diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index bb1b8f8a732b..1fd8e25cbc54 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -362,7 +362,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' @@ -449,7 +449,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' } } stage("status") { diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 000000000000..5ace4600a1f2 --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,6 @@ +version: 2 +updates: + - package-ecosystem: "github-actions" + directory: "/" + schedule: + interval: "weekly" diff --git a/.github/workflows/cflite_batch.yml b/.github/workflows/cflite_batch.yml deleted file mode 100644 index d1321cc6b7fb..000000000000 --- a/.github/workflows/cflite_batch.yml +++ /dev/null @@ -1,34 +0,0 @@ -name: ClusterFuzzLite batch fuzzing -on: - schedule: - - cron: '0 6 * * *' # UTC 6am every day. -permissions: read-all -jobs: - BatchFuzzing: - runs-on: ubuntu-latest - strategy: - fail-fast: false - matrix: - sanitizer: - - address - - undefined - - memory - steps: - - name: Build Fuzzers (${{ matrix.sanitizer }}) - id: build - uses: google/clusterfuzzlite/actions/build_fuzzers@v1 - with: - sanitizer: ${{ matrix.sanitizer }} - - name: Run Fuzzers (${{ matrix.sanitizer }}) - id: run - uses: google/clusterfuzzlite/actions/run_fuzzers@v1 - with: - github-token: ${{ secrets.GITHUB_TOKEN }} - fuzz-seconds: 1800 # 30 mins - mode: 'batch' - sanitizer: ${{ matrix.sanitizer }} - # Optional but recommended: For storing certain artifacts from fuzzing. - # See later section on "Git repo for storage". - # storage-repo: https://${{ secrets.PERSONAL_ACCESS_TOKEN }}@github.com/OWNER/STORAGE-REPO-NAME.git - # storage-repo-branch: main # Optional. Defaults to "main" - # storage-repo-branch-coverage: gh-pages # Optional. Defaults to "gh-pages". diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index ef15d321717e..3775602e33b1 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -31,7 +31,7 @@ jobs: image: px4io/px4-dev-nuttx-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -43,7 +43,7 @@ jobs: run: make ${{matrix.check}} - name: upload coverage if: contains(matrix.check, 'coverage') - uses: codecov/codecov-action@v1 + uses: codecov/codecov-action@v3 with: token: ${{ secrets.CODECOV_TOKEN }} flags: unittests diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index a8cba7ce6bd7..af6b812bd6e6 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-clang:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml index d14b533475f9..7c2463b3ffa5 100644 --- a/.github/workflows/compile_linux.yml +++ b/.github/workflows/compile_linux.yml @@ -21,7 +21,7 @@ jobs: scumaker_pilotpi_default, ] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} @@ -32,7 +32,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml index 81ecc1256905..c67d8153969e 100644 --- a/.github/workflows/compile_linux_arm64.yml +++ b/.github/workflows/compile_linux_arm64.yml @@ -18,7 +18,7 @@ jobs: scumaker_pilotpi_arm64, ] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} @@ -29,7 +29,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 45f6f5a61521..71db6ce4a312 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -19,7 +19,7 @@ jobs: #tests, # includes px4_sitl ] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} @@ -33,7 +33,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: macos_${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 67b055fdd97b..d3c49f96f7db 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -73,7 +73,7 @@ jobs: uvify_core ] steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} @@ -84,7 +84,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -121,7 +121,7 @@ jobs: run: ccache -s - name: Upload px4 package - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: px4_package_${{matrix.config}} path: | diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml index 8b0925e68809..611e0b46b9a5 100644 --- a/.github/workflows/deploy_all.yml +++ b/.github/workflows/deploy_all.yml @@ -14,7 +14,7 @@ jobs: outputs: matrix: ${{ steps.set-matrix.outputs.matrix }} steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} - id: set-matrix @@ -26,7 +26,7 @@ jobs: matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}} container: px4io/px4-dev-${{ matrix.container }}:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{secrets.ACCESS_TOKEN}} diff --git a/.github/workflows/ekf_functional_change_indicator.yml b/.github/workflows/ekf_functional_change_indicator.yml index a5fa9db59955..b52cb8fe4b7b 100644 --- a/.github/workflows/ekf_functional_change_indicator.yml +++ b/.github/workflows/ekf_functional_change_indicator.yml @@ -7,7 +7,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: checkout newest version of branch diff --git a/.github/workflows/ekf_update_change_indicator.yml b/.github/workflows/ekf_update_change_indicator.yml index 7de35207f096..ba095d8bfb7f 100644 --- a/.github/workflows/ekf_update_change_indicator.yml +++ b/.github/workflows/ekf_update_change_indicator.yml @@ -10,7 +10,7 @@ jobs: GIT_COMMITTER_EMAIL: bot@px4.io GIT_COMMITTER_NAME: PX4BuildBot steps: - - uses: actions/checkout@v2.3.1 + - uses: actions/checkout@v4 with: fetch-depth: 0 - name: main test updates change indication files @@ -19,7 +19,7 @@ jobs: run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_ENV working-directory: src/modules/ekf2/test/change_indication - name: auto-commit any changes to change indication - uses: stefanzweifel/git-auto-commit-action@v4 + uses: stefanzweifel/git-auto-commit-action@v5 with: commit_message: '[AUTO COMMIT] update change indication' commit_user_name: ${GIT_COMMITTER_NAME} diff --git a/.github/workflows/failsafe_sim.yml b/.github/workflows/failsafe_sim.yml index 6d5e73d5a521..02f553d9c29e 100644 --- a/.github/workflows/failsafe_sim.yml +++ b/.github/workflows/failsafe_sim.yml @@ -24,7 +24,7 @@ jobs: image: px4io/px4-dev-nuttx-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 7e95b1fff082..a50fa120e288 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -28,7 +28,7 @@ jobs: image: px4io/px4-dev-ros-melodic:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -39,7 +39,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -86,7 +86,7 @@ jobs: run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - name: Upload px4 coredump if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3.1.3 with: name: coredump path: px4.core @@ -101,21 +101,21 @@ jobs: - name: Upload px4 binary if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: binary path: build/px4_sitl_default/bin/px4 - name: Store PX4 log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: px4_log path: ~/.ros/log/*/*.ulg - name: Store ROS log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: ros_log path: ~/.ros/**/rostest-*.log @@ -132,7 +132,7 @@ jobs: lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - name: Upload coverage information to Codecov if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 + uses: codecov/codecov-action@v3 with: token: ${{ secrets.CODECOV_TOKEN }} flags: mavros_mission diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index 812f63fec406..72f1f87a62d9 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -23,7 +23,7 @@ jobs: image: px4io/px4-dev-ros-melodic:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -34,7 +34,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -81,7 +81,7 @@ jobs: run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - name: Upload px4 coredump if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3.1.3 with: name: coredump path: px4.core @@ -96,21 +96,21 @@ jobs: - name: Upload px4 binary if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: binary path: build/px4_sitl_default/bin/px4 - name: Store PX4 log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: px4_log path: ~/.ros/log/*/*.ulg - name: Store ROS log if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v3.1.3 with: name: ros_log path: ~/.ros/**/rostest-*.log @@ -127,7 +127,7 @@ jobs: lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - name: Upload coverage information to Codecov if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 + uses: codecov/codecov-action@v3 with: token: ${{ secrets.CODECOV_TOKEN }} flags: mavros_offboard diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 10799ba38f54..d9c90eda4772 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -13,7 +13,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -39,7 +39,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -54,7 +54,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -78,7 +78,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -105,7 +105,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-nuttx-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -120,7 +120,7 @@ jobs: runs-on: ubuntu-latest container: px4io/px4-dev-base-focal:2021-09-08 steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml index 1d9cda083e66..0ca89a87c5e7 100644 --- a/.github/workflows/python_checks.yml +++ b/.github/workflows/python_checks.yml @@ -12,7 +12,7 @@ jobs: build: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} - name: Install Python3 diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml index 3282c797c5f7..8729ad8b2ba4 100644 --- a/.github/workflows/sitl_tests.yml +++ b/.github/workflows/sitl_tests.yml @@ -24,7 +24,7 @@ jobs: image: px4io/px4-dev-simulation-focal:2021-09-08 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - - uses: actions/checkout@v1 + - uses: actions/checkout@v4 with: token: ${{ secrets.ACCESS_TOKEN }} @@ -40,7 +40,7 @@ jobs: string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC) message("::set-output name=timestamp::${current_date}") - name: ccache cache files - uses: actions/cache@v2 + uses: actions/cache@v3 with: path: ~/.ccache key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} @@ -104,14 +104,14 @@ jobs: run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit" - name: Upload px4 coredump if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3.1.3 with: name: coredump path: px4.core - name: Upload px4 binary if: failure() - uses: actions/upload-artifact@v2-preview + uses: actions/upload-artifact@v3.1.3 with: name: binary path: build/px4_sitl_default/bin/px4 @@ -128,7 +128,7 @@ jobs: lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info - name: Upload coverage information to Codecov if: contains(matrix.config.build_type, 'Coverage') - uses: codecov/codecov-action@v1 + uses: codecov/codecov-action@v3 with: token: ${{ secrets.CODECOV_TOKEN }} flags: mavsdk diff --git a/.github/workflows/tiiuae-blackduck-scan.yaml b/.github/workflows/tiiuae-blackduck-scan.yaml new file mode 100644 index 000000000000..47c9d72601c5 --- /dev/null +++ b/.github/workflows/tiiuae-blackduck-scan.yaml @@ -0,0 +1,56 @@ +name: tiiuae-blackduck-scan + +on: + push: + tags: + - 'v1.14.0-*' + workflow_dispatch: + +jobs: + scan: + name: run blackduck scan + runs-on: ubuntu-latest + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + submodules: 'recursive' + path: px4-firmware + fetch-depth: 0 + - name: Run the scan + run: | + set -exu + + # initialise configuration values + bd_project_name=px4-1.14.0 + bd_project_version=${{ github.ref_name }} + bd_project_phase=DEVELOPMENT + workdir=$(pwd) + blackduck_url="https://blackduck.ssrc.fi" + blackduck_token=${{ secrets.BLACKDUCK_ACCESS_TOKEN }} + project_id="7900cb41-3b7e-4985-8935-5d6c6b996db3" + bdscan_output_file=bdscan_output.log + + # run the actual scan + bash <(curl -s -L https://detect.synopsys.com/detect8.sh) \ + --blackduck.api.token=$blackduck_token \ + --blackduck.trust.cert=true \ + --blackduck.url=$blackduck_url \ + --detect.blackduck.signature.scanner.snippet.matching=FULL_SNIPPET_MATCHING \ + --detect.blackduck.signature.scanner.upload.source.mode=true \ + --detect.excluded.detector.types=PEAR \ + --detect.impact.analysis.enabled=true \ + --detect.project.application.id=$bd_project_name \ + --detect.project.name=$bd_project_name \ + --detect.project.version.name=$bd_project_version \ + --detect.project.version.phase=$bd_project_phase \ + --detect.source.path=$workdir \ + --detect.target.type=SOURCE \ + --detect.timeout=6000 \ + --detect.tools=ALL \ + --detect.wait.for.results=true \ + | tee ${bdscan_output_file} + + # find blackduck link from output log: + grep 'Black Duck Project BOM:' ${bdscan_output_file} |sed 's/^.*Black Duck Project BOM: //g' >> $GITHUB_STEP_SUMMARY diff --git a/.github/workflows/tiiuae-builder-images.yaml b/.github/workflows/tiiuae-builder-images.yaml new file mode 100644 index 000000000000..14f699106c87 --- /dev/null +++ b/.github/workflows/tiiuae-builder-images.yaml @@ -0,0 +1,42 @@ +name: tiiuae-builder-images + +on: + # Run only manually + workflow_dispatch: + +permissions: + contents: read + packages: write + +jobs: + hw-builder-image: + name: create docker builder base image + runs-on: ubuntu-latest + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + - name: Docker meta + id: meta + uses: docker/metadata-action@v5 + with: + images: ghcr.io/tiiuae/px4-firmware-builder-base + tags: | + type=raw,value=latest + type=sha + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Build builder base image and push + uses: docker/build-push-action@v5 + with: + context: . + file: ./px4-firmware/packaging/Dockerfile.build_env_pre + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} diff --git a/.github/workflows/tiiuae-check-format.yml b/.github/workflows/tiiuae-check-format.yml new file mode 100644 index 000000000000..717e5b4189ef --- /dev/null +++ b/.github/workflows/tiiuae-check-format.yml @@ -0,0 +1,18 @@ +# Make check_format before PULLREQUEST. + + +name: check_format + +on: [pull_request] + +jobs: + main: + name: Validate testfile + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Lint + run: | + sudo apt update -y && sudo apt install -y --no-install-recommends make astyle + make check_format diff --git a/.github/workflows/tiiuae-coverity-scan-image.yaml b/.github/workflows/tiiuae-coverity-scan-image.yaml new file mode 100644 index 000000000000..34db9e3a188c --- /dev/null +++ b/.github/workflows/tiiuae-coverity-scan-image.yaml @@ -0,0 +1,47 @@ +name: tiiuae-coverity-scan-image + +on: + # Run only manually + workflow_dispatch: + +permissions: + contents: read + packages: write + +jobs: + coverity-scan-image: + name: create coverity scan image + runs-on: ubuntu-latest + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + - name: Docker meta + id: meta + uses: docker/metadata-action@v5 + with: + images: ghcr.io/tiiuae/px4-coverity-scan-image + tags: | + type=raw,value=latest + type=sha + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Build and push coverity scan image + uses: docker/build-push-action@v5 + with: + context: . + file: ./px4-firmware/packaging/Dockerfile.coverity + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + build-args: | + "COVERITY_DOCKER_REGISTRY_USERNAME=${{ secrets.COVERITY_DOCKER_REGISTRY_USERNAME }}" + "COVERITY_DOCKER_REGISTRY_ACCESS_TOKEN=${{ secrets.COVERITY_DOCKER_REGISTRY_ACCESS_TOKEN }}" + "COVERITY_LICENSE_DAT_B64=${{ secrets.COVERITY_LICENSE_DAT_B64 }}" + "COVERITY_ACCESS_TOKEN_B64=${{ secrets.COVERITY_ACCESS_TOKEN_B64 }}" diff --git a/.github/workflows/tiiuae-coverity-scan.yaml b/.github/workflows/tiiuae-coverity-scan.yaml new file mode 100644 index 000000000000..13b6ec68f898 --- /dev/null +++ b/.github/workflows/tiiuae-coverity-scan.yaml @@ -0,0 +1,36 @@ +name: tiiuae-px4-coverity-scan + +on: + workflow_dispatch: + pull_request: + branches: [ main ] +jobs: + coverity: + runs-on: px4-self-hosted-coverity + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + submodules: 'recursive' + fetch-depth: 0 + - name: Fetch submodule tags + run: | + git submodule foreach --recursive git fetch --tags + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Run coverity scan + run: + docker run --rm -v "$(pwd):/main_ws" ghcr.io/tiiuae/px4-coverity-scan-image:latest + - name: Update summary + run: + cat cov-analyze-result.txt >> $GITHUB_STEP_SUMMARY + - name: Upload coverity scan results + uses: actions/upload-artifact@v3.1.3 + with: + name: coverity-html-report-${{ github.event.repository.name }} + path: coverity-output diff --git a/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml b/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml new file mode 100644 index 000000000000..79d45e6bebf5 --- /dev/null +++ b/.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml @@ -0,0 +1,47 @@ +on: + workflow_call: + inputs: + product: + required: true + type: string + keys: + required: true + type: string + enabled: + required: false + type: boolean + default: true + secrets: + GH_REPO_TOKEN: + required: false +jobs: + build: + runs-on: ubuntu-latest + if: ${{ inputs.enabled }} + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + # if we clone_public, then do not clone submodules, otherwise 'recursive' + submodules: ${{ inputs.clone_public == true && 'false' || 'recursive' }} + path: px4-firmware + fetch-depth: 0 + - name: Run px4-firmware ${{ inputs.product }} build + run: | + set -eux + if [ -n ${{ inputs.keys }} ]; then + export SIGNING_ARGS=${{ inputs.keys }} + fi + mkdir -p bin + cd px4-firmware/ + # run clone_public.sh if clone_public flag is provided + ./clone_public.sh + ./build.sh ../bin/ ${{ inputs.product }} + ls ../bin + - name: Upload ${{ inputs.product }} + uses: actions/upload-artifact@v3.1.3 + with: + name: pixhawk + path: bin/ + retention-days: 1 diff --git a/.github/workflows/tiiuae-pixhawk-and-saluki.yaml b/.github/workflows/tiiuae-pixhawk-and-saluki.yaml new file mode 100644 index 000000000000..8560e6a356cc --- /dev/null +++ b/.github/workflows/tiiuae-pixhawk-and-saluki.yaml @@ -0,0 +1,503 @@ +name: tiiuae-pixhawk-and-saluki + +on: + push: + branches: [ main ] + tags: + - 'v1.14.0-*' + pull_request: + branches: [ main ] + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + inputs: + jfrog-upload: + description: 'upload to Artifactory' + required: false + default: true + type: boolean + saluki-v2-manual-fpga-version: + description: 'saluki-v2 optional fpga version (e.g. sha-3b85ccc)' + required: false + default: '' + type: string + saluki-v3-manual-fpga-version: + description: 'saluki-v3 optional fpga version (e.g. sha-3b85ccc)' + required: false + default: '' + type: string + saluki-pi-manual-fpga-version: + description: 'saluki-pi optional fpga version (e.g. sha-cd7bb6b)' + required: false + default: '' + type: string + +permissions: + contents: read + packages: write + +env: + saluki_pi_fpga_version: "sha-b7e842d" + saluki_v2_fpga_version: "sha-b7e842d" + saluki_v3_fpga_version: "sha-b7e842d" + +jobs: + fc_matrix: + strategy: + fail-fast: false + matrix: + product: [pixhawk, saluki-v2_default, saluki-v2_amp, saluki-v2_kernel, saluki-pi_default, saluki-pi_amp, saluki-v3_default, saluki-v3_amp] + include: + - product: saluki-v2_custom_keys + keys: Tools/saluki-sec-scripts/custom_keys/saluki-v2/px4_bin_ed25519_private.pem + - product: saluki-v3_custom_keys + keys: Tools/saluki-sec-scripts/custom_keys/saluki-v3/px4_bin_ed25519_private.pem + - product: saluki-pi_custom_keys + keys: Tools/saluki-sec-scripts/custom_keys/saluki-pi/px4_bin_ed25519_private.pem + + uses: ./.github/workflows/tiiuae-pixhawk-and-saluki-builder.yaml + with: + product: ${{ matrix.product }} + keys: ${{ matrix.keys }} + # old workflow had condition to run only if PR is done to current repo (or triggered with other event) + enabled: ${{ github.event.pull_request.head.repo.full_name == github.repository || github.event_name == 'push' || github.event_name == 'workflow_dispatch' }} + secrets: inherit + + variables: + runs-on: ubuntu-latest + outputs: + jfrog_upload: ${{ steps.variables.outputs.jfrog_upload }} + saluki_v2_fpga_version: ${{ steps.variables.outputs.saluki_v2_fpga_version }} + saluki_v3_fpga_version: ${{ steps.variables.outputs.saluki_v3_fpga_version }} + saluki_pi_fpga_version: ${{ steps.variables.outputs.saluki_pi_fpga_version }} + steps: + - name: Print input variables + id: variables + run: | + # use saluki-v2-fpga default version if custom is not provided + saluki_v2_fpga_version=${{ env.saluki_v2_fpga_version }} + if [ -n "${{ github.event.inputs.saluki-v2-manual-fpga-version }}" ]; then + saluki_v2_fpga_version=${{ github.event.inputs.saluki-v2-manual-fpga-version }} + fi + + # use saluki-v3-fpga default version if custom is not provided + saluki_v3_fpga_version=${{ env.saluki_v3_fpga_version }} + if [ -n "${{ github.event.inputs.saluki-v3-manual-fpga-version }}" ]; then + saluki_v3_fpga_version=${{ github.event.inputs.saluki-v3-manual-fpga-version }} + fi + + # use saluki-pi-fpga default version if custom is not provided + saluki_pi_fpga_version=${{ env.saluki_pi_fpga_version }} + if [ -n "${{ github.event.inputs.saluki-pi-manual-fpga-version }}" ]; then + saluki_pi_fpga_version=${{ github.event.inputs.saluki-pi-manual-fpga-version }} + fi + + echo "saluki_v2_fpga_version=${saluki_v2_fpga_version}" >> $GITHUB_OUTPUT + echo "saluki_v3_fpga_version=${saluki_v3_fpga_version}" >> $GITHUB_OUTPUT + echo "saluki_pi_fpga_version=${saluki_pi_fpga_version}" >> $GITHUB_OUTPUT + echo "jfrog_upload=${{ github.event.inputs.jfrog-upload }}" >> $GITHUB_OUTPUT + + echo "saluki_v2_fpga_version: ${saluki_v2_fpga_version}" + echo "saluki_v3_fpga_version: ${saluki_v3_fpga_version}" + echo "saluki_pi_fpga_version: ${saluki_pi_fpga_version}" + echo "jfrog_upload: ${{ github.event.inputs.jfrog-upload }}" + + px4fwupdater: + name: build px4fwupdater + runs-on: ubuntu-latest + needs: + - fc_matrix + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Run px4-firmware px4fwupdater build + run: | + set -eux + mkdir -p bin + cd px4-firmware/ + ./clone_public.sh + ./build.sh ../bin/ px4fwupdater + ls ../bin + - name: Upload px4fwupdater to tmp storage + uses: actions/upload-artifact@v3.1.3 + with: + name: pixhawk + path: bin/ + retention-days: 1 + + upload-px4fwupdater: + name: upload px4fwupdater to docker registry + runs-on: ubuntu-latest + needs: + - px4fwupdater + - variables + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Firmware flasher - Container metadata + id: containermeta # referenced from later step + uses: docker/metadata-action@v5 + with: + images: ghcr.io/tiiuae/px4-firmware + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Firmware flasher - Build and push + uses: docker/build-push-action@v5 + with: + push: true + context: . + file: px4-firmware/Tools/px_uploader.Dockerfile + tags: ${{ steps.containermeta.outputs.tags }} + labels: ${{ steps.containermeta.outputs.labels }} + build-args: | + "saluki_pi_fpga_version=${{ needs.variables.outputs.saluki_pi_fpga_version }}" + "saluki_v2_fpga_version=${{ needs.variables.outputs.saluki_v2_fpga_version }}" + "saluki_v3_fpga_version=${{ needs.variables.outputs.saluki_v3_fpga_version }}" + - name: Build overview + run: | + echo "### Build overview:" >> $GITHUB_STEP_SUMMARY + echo "Build version: ${{ steps.containermeta.outputs.tags }}" + echo "Build labels: ${{ steps.containermeta.outputs.labels }}" + echo "Build args:" + echo " saluki_pi_fpga_version: ${{ needs.variables.outputs.saluki_pi_fpga_version }}" + echo " saluki_v2_fpga_version: ${{ needs.variables.outputs.saluki_v2_fpga_version }}" + echo " saluki_v3_fpga_version: ${{ needs.variables.outputs.saluki_v3_fpga_version }}" + + # in case more than one tag is generated, use the one which mentions commit sha + if (( $(echo "${{ steps.containermeta.outputs.tags }}" | wc -l) > 1 )); then + container_name=$(echo "${{ steps.containermeta.outputs.tags }}" | grep ':sha-' | head -n 1) + else + container_name="${{ steps.containermeta.outputs.tags }}" + fi + echo "Container name: $container_name" + + # display mermaid flowchart + echo '```mermaid' >> $GITHUB_STEP_SUMMARY + echo "flowchart LR" >> $GITHUB_STEP_SUMMARY + + # inputs + echo "FPGA-V2[(Saluki-v2 FPGA\n${{ needs.variables.outputs.saluki_v2_fpga_version }})]" >> $GITHUB_STEP_SUMMARY + echo "FPGA-V3[(Saluki-v3 FPGA\n${{ needs.variables.outputs.saluki_v3_fpga_version }})]" >> $GITHUB_STEP_SUMMARY + echo "FPGA-PI[(Saluki-pi FPGA\n${{ needs.variables.outputs.saluki_pi_fpga_version }})]" >> $GITHUB_STEP_SUMMARY + echo "PX4-SHA[${{ github.repository }}\n$GITHUB_REF]" >> $GITHUB_STEP_SUMMARY + + # build + echo "BUILD[build PX4 fwupdater]" >> $GITHUB_STEP_SUMMARY + + # outputs + echo "OUTPUT[(${container_name})]" >> $GITHUB_STEP_SUMMARY + + # links + echo "FPGA-V2 --> BUILD" >> $GITHUB_STEP_SUMMARY + echo "FPGA-V3 --> BUILD" >> $GITHUB_STEP_SUMMARY + echo "FPGA-PI --> BUILD" >> $GITHUB_STEP_SUMMARY + echo "PX4-SHA --> BUILD" >> $GITHUB_STEP_SUMMARY + echo "BUILD --> OUTPUT" >> $GITHUB_STEP_SUMMARY + # end mermaid flowchart + echo '```' >> $GITHUB_STEP_SUMMARY + + # set variables for container name and date + CONTAINER_DATE=$(date +%s) + PX4_TMP_CONTAINER_NAME="tmp_px4_container_$CONTAINER_DATE" + px4_github_sha=$(echo "${{ github.sha }}" | cut -c1-7) + + echo "# Flashing this package to your Saluki" >> $GITHUB_STEP_SUMMARY + echo "## Get these px4 firmware files to your computer" >> $GITHUB_STEP_SUMMARY + echo "To get these files to you computer, you can use the following command" >> $GITHUB_STEP_SUMMARY + echo "The command will create a temporary \`$PX4_TMP_CONTAINER_NAME\` container, copy the firmware files to directory \`px4-firmware_${px4_github_sha}\` and remove the temporary container" >> $GITHUB_STEP_SUMMARY + + # compose docker cp command + docker_cp_cmd='docker cp $(docker create --name ' + docker_cp_cmd+="$PX4_TMP_CONTAINER_NAME $container_name" + docker_cp_cmd+='):/firmware ' + docker_cp_cmd+=px4-firmware_${px4_github_sha} + docker_cp_cmd+=' && docker rm ' + docker_cp_cmd+=$PX4_TMP_CONTAINER_NAME + + # echo docker cp command to summary + echo '```shell' >> $GITHUB_STEP_SUMMARY + echo "${docker_cp_cmd}" >> $GITHUB_STEP_SUMMARY + echo '```' >> $GITHUB_STEP_SUMMARY + + # This flashing is not currently working, just keeping it here for the future + # echo "## Flash this px4 firmware to your Saluki" >> $GITHUB_STEP_SUMMARY + # echo "To flash this firmware to your Saluki, you can use the following command:" >> $GITHUB_STEP_SUMMARY + # docker_flash_cmd='docker run --rm --network=host --entrypoint= --device=${dev}:/dev/px4serial ' + # docker_flash_cmd+=$container_name + # docker_flash_cmd+=' sh -c "/bin/px_uploader.py ssrc_saluki-v2_default-*.px4"' + # echo '```shell' >> $GITHUB_STEP_SUMMARY + # echo "${docker_flash_cmd}" >> $GITHUB_STEP_SUMMARY + # echo '```' >> $GITHUB_STEP_SUMMARY + + echo "## Flash this px4 firmware to your Saluki by using fpga-flashing tool" >> $GITHUB_STEP_SUMMARY + echo "fpga-flashing is separate tool: https://github.com/tiiuae/fpga-flashing/" >> $GITHUB_STEP_SUMMARY + echo "To flash this px4 firmware and FPGA to your Saluki with fpga-flashing, you can use the following command:" >> $GITHUB_STEP_SUMMARY + flash_tool_cmd='./flash.sh --update-package ' + flash_tool_cmd+=$container_name + flash_tool_cmd+=' --fpga --px4' + echo '```shell' >> $GITHUB_STEP_SUMMARY + echo "${flash_tool_cmd}" >> $GITHUB_STEP_SUMMARY + echo '```' >> $GITHUB_STEP_SUMMARY + echo 'For more info please see: https://github.com/tiiuae/fpga-flashing/#usage' >> $GITHUB_STEP_SUMMARY + + + upload-px4fwupdater-uae: + name: upload px4fwupdater to UAE docker registry + # temporarily disabled until we get new token from UAR + if: false + runs-on: ubuntu-latest + needs: + - px4fwupdater + - variables + steps: + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - name: Firmware flasher - Container metadata + id: containermeta # referenced from later step + uses: docker/metadata-action@v5 + with: + images: artifactory.ssrcdevops.tii.ae/tiiuae/px4-firmware + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + - name: Login to SSRC JFrog Container Registry + uses: docker/login-action@v3 + with: + registry: artifactory.ssrcdevops.tii.ae + username: ${{ secrets.UAE_RT_USER }} + password: ${{ secrets.UAE_RT_APIKEY }} + # have to login to ghcr as well to download fpga and BL + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + - name: Firmware flasher - Build and push + uses: docker/build-push-action@v5 + with: + push: true + context: . + file: px4-firmware/Tools/px_uploader.Dockerfile + tags: ${{ steps.containermeta.outputs.tags }} + labels: ${{ steps.containermeta.outputs.labels }} + build-args: | + "saluki_pi_fpga_version=${{ needs.variables.outputs.saluki_pi_fpga_version }}" + "saluki_v2_fpga_version=${{ needs.variables.outputs.saluki_v2_fpga_version }}" + "saluki_v3_fpga_version=${{ needs.variables.outputs.saluki_v3_fpga_version }}" + + artifactory: + name: upload builds to artifactory + if: ${{ github.event_name != 'workflow_dispatch' || inputs.jfrog-upload == true }} + runs-on: ubuntu-latest + needs: + - px4fwupdater + - fc_matrix + steps: + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - uses: jfrog/setup-jfrog-cli@v3 + env: + JF_ENV_1: ${{ secrets.ARTIFACTORY_CLOUD_TOKEN }} + - name: Upload px4-firmware build to Artifactory + env: + ARTIFACTORY_GEN_REPO: ssrc-gen-private-local + BUILD_NAME_PX4: px4-firmware + CI: true + run: | + set -exu + + pr_or_empty="" + latest_link="" + if [ ${{ github.event_name }} == 'pull_request' ]; then + latest_link="pr/" + pr_or_empty="pr/${{ github.head_ref || github.ref_name }}/" + fi + + newline=$'\n' + artifactory_links="| target | link | + |--------|------|" + artifactory_base_url="https://ssrc.jfrog.io/artifactory/" + + for pkg in $(find bin -type f|sort); do + + file_name=$(basename $pkg) + ext="${file_name##*.}" + target_path="" + pkg_name=$(echo $file_name | sed -r -e 's/-[0-9]+\.[0-9]+\.[0-9]+-.*//g') + + if [[ $file_name = px4_fmu* ]]; then + target_path="pixhawk" + elif [[ $file_name = ssrc_saluki* ]]; then + target_path="saluki" + else + echo "$pkg ignored" + continue + fi + + artifactory_path=$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty} + artifactory_latest_path=$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${latest_link}latest/${pkg_name}.${ext} + + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_PX4" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "${artifactory_path}$file_name" + + # link to latest + jfrog rt cp --flat \ + "${artifactory_path}$file_name" \ + "${artifactory_latest_path}" + + # append every file to artifactory_links + artifactory_links+="${newline}| ${pkg_name} | ${artifactory_base_url}${artifactory_path}${file_name} |" + done + + jfrog rt build-publish "$BUILD_NAME_PX4" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_PX4" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + + # save upload path to output for later use + echo "### Cloud Artifactory links:" >> $GITHUB_STEP_SUMMARY + echo "${artifactory_links}" >> $GITHUB_STEP_SUMMARY + + artifactory-uae: + name: upload builds to UAE artifactory + # temporarily disabled until we get new token from UAR + if: false + runs-on: ubuntu-latest + needs: + - px4fwupdater + - fc_matrix + steps: + - name: Download pixhawk artifacts + uses: actions/download-artifact@v3 + with: + name: pixhawk + path: bin + - uses: jfrog/setup-jfrog-cli@v3 + env: + JF_ENV_1: ${{ secrets.UAE_ARTIFACTORY_TOKEN }} + - name: Upload px4-firmware build to Artifactory + env: + ARTIFACTORY_GEN_REPO: gen-public-local + BUILD_NAME_PX4: px4-firmware + CI: true + run: | + set -exu + + pr_or_empty="" + latest_link="" + if [ ${{ github.event_name }} == 'pull_request' ]; then + latest_link="pr/" + pr_or_empty="pr/${{ github.head_ref || github.ref_name }}/" + fi + + newline=$'\n' + artifactory_links="| target | link | + |--------|------|" + artifactory_base_url="https://artifactory.ssrcdevops.tii.ae/artifactory/" + + for pkg in $(find bin -type f|sort); do + + file_name=$(basename $pkg) + ext="${file_name##*.}" + target_path="" + pkg_name=$(echo $file_name | sed -r -e 's/-[0-9]+\.[0-9]+\.[0-9]+-.*//g') + + if [[ $file_name = px4_fmu* ]]; then + target_path="pixhawk" + elif [[ $file_name = ssrc_saluki* ]]; then + target_path="saluki" + else + echo "$pkg ignored" + continue + fi + + artifactory_path=$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${pr_or_empty} + artifactory_latest_path=$ARTIFACTORY_GEN_REPO/builds/px4-firmware/${target_path}/${latest_link}latest/${pkg_name}.${ext} + + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_PX4" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "${artifactory_path}$file_name" + + # link to latest + jfrog rt cp --flat \ + "${artifactory_path}$file_name" \ + "${artifactory_latest_path}" + + # append every file to artifactory_links + artifactory_links+="${newline}| ${pkg_name} | ${artifactory_base_url}${artifactory_path}${file_name} |" + done + + jfrog rt build-publish "$BUILD_NAME_PX4" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_PX4" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + + # export artifactory linds as gh step summary + echo "### UAE Artifactory links:" >> $GITHUB_STEP_SUMMARY + echo "${artifactory_links}" >> $GITHUB_STEP_SUMMARY + + - name: Upload px4-fwupdater build to Artifactory + env: + ARTIFACTORY_DEB_REPO: debian-public-local + DISTRIBUTION: focal + COMPONENT: fog-sw + ARCHITECTURE: amd64 + BUILD_NAME_DEB: px4-fwupdater + CI: true + run: | + set -exu + pkg=$(find bin -name 'px4fwupdater*.deb') + pkg_name=$(basename $pkg) + jfrog rt u --deb "$DISTRIBUTION/$COMPONENT/$ARCHITECTURE" \ + --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME_DEB" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_DEB_REPO/$pkg_name" + jfrog rt build-publish "$BUILD_NAME_DEB" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME_DEB" "$GITHUB_SHA" "$ARTIFACTORY_DEB_REPO" \ + --status dev \ + --comment "development build" diff --git a/.github/workflows/tiiuae-sitl-tests.yml b/.github/workflows/tiiuae-sitl-tests.yml new file mode 100644 index 000000000000..2b05a5ee7cdf --- /dev/null +++ b/.github/workflows/tiiuae-sitl-tests.yml @@ -0,0 +1,72 @@ +name: SITL Tests + +on: + push: + branches: [ main ] + pull_request: + branches: [ main ] + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +permissions: + contents: read + packages: write + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + config: + - {model: "iris", latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "RelWithDebInfo" } # Alaska + container: + image: ghcr.io/tiiuae/px4-firmware-builder-base:latest + credentials: + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + steps: + - uses: actions/checkout@v4 + with: + token: ${{ secrets.GH_REPO_TOKEN }} + fetch-depth: 0 + - name: Install dependencies + run: | + apt update -y && apt install -y --no-install-recommends wget + pip3 install requests + git config --global --add safe.directory '*' + - name: Download MAVSDK + run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" + - name: Install MAVSDK + run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb" + - name: check environment + env: + PX4_HOME_LAT: ${{matrix.config.latitude}} + PX4_HOME_LON: ${{matrix.config.longitude}} + PX4_HOME_ALT: ${{matrix.config.altitude}} + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: | + export + - name: Build PX4 + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: make px4_sitl_default + - name: Build SITL Gazebo + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: | + . /opt/ros/humble/setup.sh + make px4_sitl_default sitl_gazebo-classic + - name: Build MAVSDK tests + env: + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + DONT_RUN: 1 + run: make px4_sitl_default sitl_gazebo-classic mavsdk_tests + - name: Run SITL tests + env: + PX4_HOME_LAT: ${{matrix.config.latitude}} + PX4_HOME_LON: ${{matrix.config.longitude}} + PX4_HOME_ALT: ${{matrix.config.altitude}} + PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}} + run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose + timeout-minutes: 45 diff --git a/.github/workflows/tiiuae-sitl.yaml b/.github/workflows/tiiuae-sitl.yaml new file mode 100644 index 000000000000..30491e4617ce --- /dev/null +++ b/.github/workflows/tiiuae-sitl.yaml @@ -0,0 +1,102 @@ +name: tii-px4-sitl + +on: + push: + branches: [ main ] + pull_request: + +permissions: + contents: read + packages: write + +jobs: + tii-px4-sitl: + runs-on: ubuntu-latest + services: + registry: + image: registry:2 + ports: + - 5000:5000 + steps: + + - name: Checkout px4-firmware + uses: actions/checkout@v4 + with: + path: px4-firmware + fetch-depth: 0 + + - uses: docker/setup-buildx-action@v3 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + with: + driver-opts: network=host + + # Run docker build + - name: Run fog-sw docker build + run: | + set -eux + mkdir bin + cd px4-firmware + ./clone_public.sh + ./build_sitl.sh ../bin/ + ls ../bin/ + + - name: Docker meta + id: meta + uses: docker/metadata-action@v5 + with: + images: ghcr.io/tiiuae/tii-px4-sitl + tags: | + type=ref,event=branch + type=ref,event=pr + type=semver,pattern={{version}} + type=sha + type=raw,value=latest + + - name: Login to GitHub Container Registry + uses: docker/login-action@v3 + with: + registry: ghcr.io + username: ${{ github.actor }} + password: ${{ secrets.GITHUB_TOKEN }} + + - name: Build tii-px4-sitl image and push + uses: docker/build-push-action@v5 + with: + context: . + file: ./px4-firmware/packaging/Dockerfile.sitl + pull: true + push: true + tags: ${{ steps.meta.outputs.tags }} + labels: ${{ steps.meta.outputs.labels }} + + - uses: jfrog/setup-jfrog-cli@v3 + if: github.event_name == 'push' + env: + JF_ENV_1: ${{ secrets.ARTIFACTORY_CLOUD_TOKEN }} + + - name: Upload to Artifactory (px4-sitl) + env: + ARTIFACTORY_GEN_REPO: ssrc-gen-private-local + BUILD_NAME: px4-sitl + CI: true + if: github.event_name == 'push' + run: | + set -exu + jfrog rt ping + pkg=$(find bin -name 'px4_sitl_build*.tar.gz') + pkg_name=$(basename $pkg) + jfrog rt u --target-props COMMIT="$GITHUB_SHA" \ + --build-name "$BUILD_NAME" \ + --build-number "$GITHUB_SHA" \ + "$pkg" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/$pkg_name" + jfrog rt build-publish "$BUILD_NAME" "$GITHUB_SHA" + jfrog rt bpr "$BUILD_NAME" "$GITHUB_SHA" "$ARTIFACTORY_GEN_REPO" \ + --status dev \ + --comment "development build" + jfrog rt cp --flat \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/$pkg_name" \ + "$ARTIFACTORY_GEN_REPO/builds/px4-firmware/sitl/latest/px4_sitl_build.tar.gz" + diff --git a/.gitmodules b/.gitmodules index 6ecc529b165e..acaa2b540e73 100644 --- a/.gitmodules +++ b/.gitmodules @@ -20,12 +20,12 @@ branch = main [submodule "platforms/nuttx/NuttX/nuttx"] path = platforms/nuttx/NuttX/nuttx - url = https://github.com/PX4/NuttX.git - branch = px4_firmware_nuttx-10.3.0+-v1.14 + url = https://github.com/tiiuae/nuttx.git + branch = master [submodule "platforms/nuttx/NuttX/apps"] path = platforms/nuttx/NuttX/apps - url = https://github.com/PX4/NuttX-apps.git - branch = px4_firmware_nuttx-10.3.0+ + url = https://github.com/tiiuae/nuttx-apps.git + branch = master [submodule "Tools/flightgear_bridge"] path = Tools/simulation/flightgear/flightgear_bridge url = https://github.com/PX4/PX4-FlightGear-Bridge.git @@ -60,5 +60,34 @@ branch = px4 [submodule "src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client"] path = src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client - url = https://github.com/PX4/Micro-XRCE-DDS-Client.git - branch = px4 + url = https://github.com/tiiuae/Micro-XRCE-DDS-Client.git +[submodule "boards/ssrc/saluki-v1"] + path = boards/ssrc/saluki-v1 + url = ../saluki-v1.git +[submodule "boards/ssrc/saluki-v2"] + path = boards/ssrc/saluki-v2 + url = ../saluki-v2.git +[submodule "boards/ssrc/saluki-pi"] + path = boards/ssrc/saluki-pi + url = ../saluki-pi.git +[submodule "boards/ssrc/saluki-v3"] + path = boards/ssrc/saluki-v3 + url = ../saluki-v3.git +[submodule "src/drivers/pfsoc_crypto"] + path = src/drivers/pfsoc_crypto + url = ../pfsoc_crypto.git +[submodule "src/drivers/pfsoc_keystore"] + path = src/drivers/pfsoc_keystore + url = ../pfsoc_keystore.git +[submodule "platforms/nuttx/NuttX/extern/pf_crypto"] + path = platforms/nuttx/NuttX/extern/pf_crypto + url = ../pf_crypto.git +[submodule "platforms/nuttx/src/px4/common/process"] + path = platforms/nuttx/src/px4/common/process + url = git@github.com:tiiuae/px4-kernel.git +[submodule "Tools/saluki-sec-scripts"] + path = Tools/saluki-sec-scripts + url = git@github.com:tiiuae/saluki-sec-scripts.git +[submodule "boards/ssrc/common"] + path = boards/ssrc/common + url = git@github.com:tiiuae/px4_boards_ssrc.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 741e8e574aec..b1587b157add 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -414,6 +414,8 @@ endif() # add_library(parameters_interface INTERFACE) add_library(kernel_parameters_interface INTERFACE) +add_library(events_interface INTERFACE) +add_library(kernel_events_interface INTERFACE) include(px4_add_library) add_subdirectory(src/lib EXCLUDE_FROM_ALL) @@ -440,8 +442,11 @@ add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL) if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) target_link_libraries(parameters_interface INTERFACE usr_parameters) target_link_libraries(kernel_parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE usr_events) + target_link_libraries(kernel_events_interface INTERFACE events) else() target_link_libraries(parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE events) endif() # firmware added last to generate the builtin for included modules diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index 532b659b05e0..d6c404241514 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -140,6 +140,20 @@ add_custom_command( # copy extras into ROMFS set(extras_dependencies) +# sysinit script for kernel mode +if(CONFIG_BUILD_KERNEL) + add_custom_command(OUTPUT ${romfs_gen_root_dir}/init.d/rc.sysinit + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${PX4_BINARY_DIR}/NuttX/rc.sysinit ${romfs_gen_root_dir}/init.d/rc.sysinit + DEPENDS + ${PX4_BINARY_DIR}/NuttX/rc.sysinit + romfs_copy.stamp + COMMENT "ROMFS: copying rc.sysinit" + ) + list(APPEND extras_dependencies + ${romfs_gen_root_dir}/init.d/rc.sysinit + ) +endif() + # optional board architecture defaults set(board_arch_rc_file "rc.board_arch_defaults") if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/init/${CONFIG_ARCH_CHIP}/${board_arch_rc_file}") @@ -167,6 +181,7 @@ endif() # board custom init files set(OPTIONAL_BOARD_RC) list(APPEND OPTIONAL_BOARD_RC + rc.board_paths rc.board_defaults rc.board_sensors rc.board_extras diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 1ba7ccde27e6..dc4f593dabce 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -38,5 +38,7 @@ px4_add_romfs_files( px4-rc.params px4-rc.simulator rc.replay + rcS.sim_tcp_server + rcS.sim_udp rcS ) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index e87e013e5d77..bfecc1a0ca87 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -11,14 +11,13 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_RUN_GZSIM 0 param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 - - param set-default FW_LND_ANG 8 param set-default NPFG_PERIOD 12 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 39f5f8f3e25d..9841c988c614 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -12,9 +12,10 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_RUN_GZSIM 0 param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x new file mode 100644 index 000000000000..73ed0882fe41 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4400_ssrc_fog_x @@ -0,0 +1,57 @@ +#!/bin/sh +# +# @name SSRC Quad X on holybro x500 frame +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 +param set MC_ROLLRATE_P 0.14 +param set MC_PITCHRATE_P 0.14 +param set MC_ROLLRATE_I 0.3 +param set MC_PITCHRATE_I 0.3 +param set MC_ROLLRATE_D 0.004 +param set MC_PITCHRATE_D 0.004 + +param set BAT_N_CELLS 4 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + + +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +param set MAV_0_CONFIG 0 +param set RTPS_MAV_CONFIG 101 +param set SER_TEL1_BAUD 3000000 + +# Disable Multi-EKF +param set EKF2_MULTI_IMU 0 +param set SENS_IMU_MODE 1 +param set EKF2_MULTI_MAG 0 +param set SENS_MAG_MODE 1 + +# Logger used only while flying +param set SDLOG_MODE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x b/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x new file mode 100644 index 000000000000..708253259623 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x @@ -0,0 +1,71 @@ +#!/bin/sh +# +# @name Gazebo x500 +# +# @type Quadrotor x +# + +. ${R}etc/init.d/rc.mc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=holybro-x500} + +param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_RUN_GZSIM 0 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 + +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 4 + +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR0_KM 0.05 + +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR1_KM 0.05 + +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 + +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 + +param set-default SIM_GZ_EC_MIN1 150 +param set-default SIM_GZ_EC_MIN2 150 +param set-default SIM_GZ_EC_MIN3 150 +param set-default SIM_GZ_EC_MIN4 150 + +param set-default SIM_GZ_EC_MAX1 1000 +param set-default SIM_GZ_EC_MAX2 1000 +param set-default SIM_GZ_EC_MAX3 1000 +param set-default SIM_GZ_EC_MAX4 1000 + +param set-default MPC_THR_HOVER 0.60 + +# extra +param set COM_RCL_EXCEPT 4 +param set NAV_DLL_ACT 0 +param set NAV_RCL_ACT 0 +param set MAV_0_BROADCAST 1 +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 +param set MC_ROLLRATE_P 0.14 +param set MC_PITCHRATE_P 0.14 +param set MC_ROLLRATE_I 0.3 +param set MC_PITCHRATE_I 0.3 +param set MC_ROLLRATE_D 0.004 +param set MC_PITCHRATE_D 0.004 +param set BAT_N_CELLS 4 +param set SDLOG_MODE 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8 b/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8 new file mode 100644 index 000000000000..d2ede5ef2686 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8 @@ -0,0 +1,95 @@ +#!/bin/sh +# +# @name SSRC Skywalker X8 +# +# @type Flying Wing +# @class Plane +# + +. /etc/init.d/rc.fw_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=skywalker_x8} + +param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_RUN_GZSIM 0 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 1 + +# Control allocator parameters +param set-default CA_AIRFRAME 1 +param set-default CA_ROTOR_COUNT 1 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 6 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_R 0.5 + +# GZ SIM +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MAX1 1000 + +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_FUNC2 202 + +# Airspeed parameters +param set-default ASPD_PRIMARY 1 +# param set-default FW_AIRSPD_MAX 22.0 +# param set-default FW_AIRSPD_MIN 14.0 +# param set-default FW_AIRSPD_STALL 12.0 +# param set-default FW_AIRSPD_TRIM 18.0 + +# Maximum landing slope angle in deg +param set-default FW_LND_ANG 8 + +# RC loss failsafe to HOLD mode +param set-default COM_RC_IN_MODE 1 + +# Fixed wing control +# Pitch rate +param set-default FW_PR_P 0.9 +param set-default FW_PR_FF 0.5 +param set-default FW_PR_I 0.5 +param set-default TRIM_PITCH -0.15 +# Pitch angle in deg +param set-default FW_PSP_OFF 0 +param set-default FW_P_LIM_MIN -15 +# Roll rate +param set-default FW_RR_FF 0.5 +param set-default FW_RR_P 0.3 +param set-default FW_RR_I 0.5 +# Yaw rate +param set-default FW_YR_FF 0.5 +param set-default FW_YR_P 0.6 +param set-default FW_YR_I 0.5 +# Throttle limit +# param set-default FW_THR_MAX 0.6 +# param set-default FW_THR_MIN 0.05 +# param set-default FW_THR_TRIM 0.25 +# Climb and sink rate +param set-default FW_T_CLMB_MAX 8 +param set-default FW_T_SINK_MAX 2.7 +param set-default FW_T_SINK_MIN 2.2 + +# Navigation +param set-default NAV_ACC_RAD 15 +param set-default NAV_DLL_ACT 2 + +# Misc +param set-default MIS_TAKEOFF_ALT 30.0 +param set-default RTL_RETURN_ALT 30.0 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# Catapult launch with acc threshold trigger +param set-default FW_LAUN_DETCN_ON 1 +param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming +param set-default FW_LAUN_AC_THLD 10 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover new file mode 100644 index 000000000000..37689eeb9e8e --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover @@ -0,0 +1,77 @@ +#!/bin/sh +# +# @name SSRC SCOUT MINI UGV +# +# @url https://global.agilex.ai/products/scout-mini +# +# @type Rover +# @class Rover +# + +. ${R}etc/init.d/rc.rover_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=scout_mini} + +param set-default SIM_GZ_EN 1 +param set-default SIM_GZ_RUN_GZSIM 0 + +param set-default CA_AIRFRAME 6 + +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 1 +param set-default SENS_EN_MAGSIM 1 + +param set IMU_GYRO_CUTOFF 60 +param set IMU_DGYRO_CUTOFF 30 + +param set-default SYS_HAS_BARO 0 + +param set-default BAT1_N_CELLS 4 + +param set-default MIS_TAKEOFF_ALT 0.01 + +param set-default NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 + +# EKF2 +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default EKF2_MAG_TYPE 1 +param set-default EKF2_REQ_SACC 1.0 +param set-default EKF2_REQ_VDRIFT 0.4 +param set-default EKF2_REQ_HDRIFT 0.2 + + +################################# +# Rover Position Control Module # +################################# + +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 3 +param set-default GND_THR_CRUISE 1 +param set-default GND_THR_MAX 1 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set-default GND_MAX_ANG 3.142 +param set-default GND_WHEEL_BASE 0.45 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set-default GND_THR_MIN 0 +param set-default GND_SPEED_P 0.4 +param set-default GND_SPEED_I 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_MAX 3.0 +param set-default GND_SPEED_TRIM 3.0 +param set-default GND_SPEED_THR_SC 1.0 +param set-default GND_VEL_CTRL 1 +param set-default GND_ANG_VEL_CTRL 1 +param set-default GND_ACC_LIMIT 10 +param set-default GND_DEC_LIMIT 50 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index d54134e437e5..ed2d57ae77bc 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -71,12 +71,17 @@ px4_add_romfs_files( 3010_jsbsim_quadrotor_x 3011_jsbsim_hexarotor_x + 4400_ssrc_fog_x + 4001_gz_x500 4002_gz_x500_depth 4003_gz_rc_cessna 4004_gz_standard_vtol 4005_gz_x500_vision 4006_gz_px4vision + 4401_gz_ssrc_fog_x + 4440_gz_ssrc_skywalker_x8 + 50005_gz_ssrc_scout_mini_rover 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params index d05f50146a42..6402f8c39fb9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params @@ -3,3 +3,23 @@ #param set-default MAV_SYS_ID $((px4_instance+1)) #param set-default IMU_INTEG_RATE 250 + +if [ -f "/enclave/fog_env" ]; +then + # read and set SSRC_CONFIG env variable + . /enclave/fog_env +fi + +if [ -d "/ssrc_config" ] && [ -z "$SSRCFILE" ]; +then + SSRCFILE="/ssrc_config/config_${SSRC_CONFIG}.txt" + # Use environment variable SSRC_CONFIG to choose config.txt file. + if [ -f "$SSRCFILE" ]; then + echo "SSRC_CONFIG: load $SSRCFILE" + . "$SSRCFILE" + else + echo "config file '$SSRCFILE' not found." + fi +else + echo "No SSRC config given." +fi diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 1f4693d4c1b3..73f226713a7f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -36,46 +36,52 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" exit 1 fi -elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then +elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then - # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH - if [ -f ./gz_env.sh ]; then - . ./gz_env.sh + # allow starting of gz sim optionally + if [ "$(param show -q SIM_GZ_RUN_GZSIM)" -eq "1" ]; + then + # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH + if [ -f ./gz_env.sh ]; then + . ./gz_env.sh - elif [ -f ../gz_env.sh ]; then - . ../gz_env.sh - fi + elif [ -f ../gz_env.sh ]; then + . ../gz_env.sh + fi - # "gz sim" only avaiilable in Garden and later - GZ_SIM_VERSIONS=$(gz sim --versions 2>&1) - if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ] - then - # "gz sim" from Garden on - gz_command="gz" - gz_sub_command="sim" - else - echo "ERROR [init] Gazebo gz please install gz-garden" - exit 1 - fi + # "gz sim" only avaiilable in Garden and later + GZ_SIM_VERSIONS=$(gz sim --versions 2>&1) + if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ] + then + # "gz sim" from Garden on + gz_command="gz" + gz_sub_command="sim" + else + echo "ERROR [init] Gazebo gz please install gz-garden" + exit 1 + fi - # look for running ${gz_command} gazebo world - gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) + # look for running ${gz_command} gazebo world + gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) - # shellcheck disable=SC2153 - if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then + # shellcheck disable=SC2153 + if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then - echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" + echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" - ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & + ${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" & - if [ -z "${HEADLESS}" ]; then - # HEADLESS not set, starting gui - ${gz_command} ${gz_sub_command} -g & - fi + if [ -z "${HEADLESS}" ]; then + # HEADLESS not set, starting gui + ${gz_command} ${gz_sub_command} -g & + fi + else + echo "INFO [init] gazebo already running world: ${gz_world}" + PX4_GZ_WORLD=${gz_world} + fi else - echo "INFO [init] gazebo already running world: ${gz_world}" - PX4_GZ_WORLD=${gz_world} + echo "INFO [init] gazebo run disabled" fi # start gz_bridge @@ -116,6 +122,43 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th exit 1 fi + elif [ -n "${PX4_GZ_MODEL}" ] && [ -n "${PX4_GZ_MODEL_NAME}" ]; then + # model type and name specified, gz_bridge will spawn model with name + + if [ -n "${PX4_GZ_MODEL_POSE}" ]; then + # Clean potential input line formatting. + model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' )" + echo "INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}" + + else + echo "WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin." + model_pose="0,0,0,0,0,0" + fi + + # start gz bridge with pose arg. + if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then + if param compare -s SENS_EN_BAROSIM 1 + then + sensor_baro_sim start + fi + if param compare -s SENS_EN_GPSSIM 1 + then + sensor_gps_sim start + fi + if param compare -s SENS_EN_MAGSIM 1 + then + sensor_mag_sim start + fi + if param compare -s SENS_EN_ARSPDSIM 1 + then + sensor_airspeed_sim start + fi + + else + echo "ERROR [init] gz_bridge failed to start" + exit 1 + fi + elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then # model name specificed, gz_bridge will attach to existing model @@ -191,6 +234,7 @@ elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" else # otherwise start simulator (mavlink) module simulator_tcp_port=$((4560+px4_instance)) + simulator_udp_port=$((14560+px4_instance)) # Check if PX4_SIM_HOSTNAME environment variable is empty # If empty check if PX4_SIM_HOST_ADDR environment variable is empty @@ -198,8 +242,16 @@ else if [ -z "${PX4_SIM_HOSTNAME}" ]; then if [ -z "${PX4_SIM_HOST_ADDR}" ]; then - echo "INFO [init] PX4_SIM_HOSTNAME: localhost" - simulator_mavlink start -c $simulator_tcp_port + if [ ! -z "${PX4_SIM_USE_TCP_SERVER}" ]; then + echo "INFO [init] PX4_SIM_USE_TCP_SERVER" + simulator_mavlink start -s $simulator_tcp_port + elif [ ! -z "${PX4_SIM_USE_UDP}" ]; then + echo "INFO [init] PX4_SIM_USE_UDP" + simulator_mavlink start -u $simulator_udp_port + else + echo "INFO [init] PX4_SIM_HOSTNAME: localhost" + simulator_mavlink start -c $simulator_tcp_port + fi else echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}" simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}" diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 7ecbfc3dcb57..1665b4be354d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -29,7 +29,7 @@ fi set VEHICLE_TYPE none set LOGGER_ARGS "" set LOGGER_BUF 1000 - +set SDCARD_AVAILABLE no set RUN_MINIMAL_SHELL no set SYS_AUTOSTART=0 @@ -131,6 +131,20 @@ fi param set MAV_SYS_ID $((px4_instance+1)) param set UXRCE_DDS_KEY $((px4_instance+1)) +simulator_tcp_port=$((4560+px4_instance)) +udp_offboard_port_local=$((14580+px4_instance)) +udp_offboard_port_remote=$((14540+px4_instance)) +[ $px4_instance -gt 9 ] && udp_offboard_port_remote=14549 # use the same ports for more than 10 instances to avoid port overlaps +udp_onboard_payload_port_local=$((14280+px4_instance)) +udp_onboard_payload_port_remote=$((14030+px4_instance)) +udp_onboard_gimbal_port_local=$((13030+px4_instance)) +udp_onboard_gimbal_port_remote=$((13280+px4_instance)) +udp_gcs_port_local=$((18570+px4_instance)) +udp_gcs_port_remote=14550 + +udp_gcs_addr_remote=127.0.0.1 +[ -n "$PX4_QGC_REMOTE_ADDRESS" ] && udp_gcs_addr_remote=$PX4_QGC_REMOTE_ADDRESS + if [ $AUTOCNF = yes ] then param set SYS_AUTOSTART $SYS_AUTOSTART @@ -251,9 +265,12 @@ manual_control start sensors start commander start -if ! pwm_out_sim start -m sim +if [ "$VEHICLE_TYPE" != "rover" ] then - tune_control play error + if ! pwm_out_sim start -m sim + then + tune_control play error + fi fi # @@ -282,14 +299,26 @@ then else param set UXRCE_DDS_DOM_ID 0 fi -uxrce_dds_port=8888 +uxrce_dds_port=2020 if [ -n "$PX4_UXRCE_DDS_PORT" ] then # Override port if environment variable is defined uxrce_dds_port="$PX4_UXRCE_DDS_PORT" fi +uxrce_dds_local_port=2019 +if [ -n "$PX4_UXRCE_DDS_LOCAL_PORT" ] +then + # Override port if environment variable is defined + uxrce_dds_local_port="$PX4_UXRCE_DDS_LOCAL_PORT" +fi +uxrce_dds_ip="127.0.0.1" +if [ -n "$PX4_UXRCE_DDS_IP" ] +then + # Override port if environment variable is defined + uxrce_dds_ip="$PX4_UXRCE_DDS_IP" +fi -uxrce_dds_client start -t udp -h 127.0.0.1 -p $uxrce_dds_port $uxrce_dds_ns +uxrce_dds_client start -t udp -h $uxrce_dds_ip -r $uxrce_dds_local_port -p $uxrce_dds_port $uxrce_dds_ns if param greater -s MNT_MODE_IN -1 then @@ -327,9 +356,9 @@ fi # Run script to start logging if param compare SYS_MC_EST_GROUP 2 then - set LOGGER_ARGS "-p ekf2_timestamps" + set LOGGER_ARGS "-p ekf2_timestamps -m file" else - set LOGGER_ARGS "-p vehicle_attitude" + set LOGGER_ARGS "-p vehicle_attitude -m file" fi . ${R}etc/init.d/rc.logging diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server new file mode 100644 index 000000000000..b66f0b9610b4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_tcp_server @@ -0,0 +1,10 @@ +#!/bin/sh + +# search path for sourcing rcS +PATH="$PATH:${R}etc/init.d-posix" + +# Define TCP server +export PX4_SIM_USE_TCP_SERVER=1 + +# Continue to the main startup script +. rcS diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS.sim_udp b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_udp new file mode 100644 index 000000000000..7338f9f5d59b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/rcS.sim_udp @@ -0,0 +1,10 @@ +#!/bin/sh + +# search path for sourcing rcS +PATH="$PATH:${R}etc/init.d-posix" + +# Define UDP +export PX4_SIM_USE_UDP=1 + +# Continue to the main startup script +. rcS diff --git a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross index e918ea0f3c06..3b51c32f61be 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross @@ -17,6 +17,7 @@ # @maintainer Andreas Antener # # @board bitcraze_crazyflie exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x new file mode 100644 index 000000000000..680c874c476b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4400_ssrc_fog_x @@ -0,0 +1,115 @@ +#!/bin/sh +# +# @name SSRC Quad X on holybro x500 frame +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. /etc/init.d/rc.mc_defaults + +# Default rates +param set-default IMU_GYRO_CUTOFF 30 +param set-default IMU_GYRO_NF0_FRQ 75 +param set-default IMU_DGYRO_CUTOFF 30 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.3 +param set-default MC_PITCHRATE_I 0.3 +param set-default MC_ROLLRATE_D 0.004 +param set-default MC_PITCHRATE_D 0.004 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.175 +param set-default CA_ROTOR0_PY 0.175 +param set-default CA_ROTOR1_PX -0.175 +param set-default CA_ROTOR1_PY -0.175 +param set-default CA_ROTOR2_PX 0.175 +param set-default CA_ROTOR2_PY -0.175 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.175 +param set-default CA_ROTOR3_PY 0.175 +param set-default CA_ROTOR3_KM -0.05 + +# PWM functions +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +# Set minimum PWM control values +param set-default PWM_MAIN_MIN1 1050 +param set-default PWM_MAIN_MIN2 1050 +param set-default PWM_MAIN_MIN3 1050 +param set-default PWM_MAIN_MIN4 1050 + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +# UAVCAN_EC functions +param set-default UAVCAN_EC_FUNC1 101 +param set-default UAVCAN_EC_FUNC2 102 +param set-default UAVCAN_EC_FUNC3 103 +param set-default UAVCAN_EC_FUNC4 104 + +# Increase velocity controller P gain +param set-default MPC_XY_VEL_P_ACC 2.4 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_EMPTY 3.6 +param set-default BAT1_V_DIV 18.1 +param set-default BAT1_R_INTERNAL -1.0 +param set-default BAT1_V_LOAD_DROP 0.3000 +param set-default BAT1_V_EMPTY 3.5000 +param set-default BAT1_SOURCE 0 + +param set-default BAT2_N_CELLS 4 +param set-default BAT2_V_CHARGED 4.2 +param set-default BAT2_V_EMPTY 3.6 +param set-default BAT2_V_DIV 18.1 +param set-default BAT2_R_INTERNAL -1.0 +param set-default BAT2_V_LOAD_DROP 0.3000 +param set-default BAT2_V_EMPTY 3.5000 +param set-default BAT2_SOURCE 0 + +# Enable LL40LS in i2c +param set-default SENS_EN_LL40LS 2 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Set default for disarm after land to 4s +param set-default COM_DISARM_LAND 4.0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Set takeoff ramp to disabled for a more decisive takeoff action +param set-default MPC_TKO_RAMP_T 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor new file mode 100644 index 000000000000..a3e50962904f --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4401_ssrc_fog_x_tmotor @@ -0,0 +1,98 @@ +#!/bin/sh +# +# @name SSRC Quad X on T-Motor M690 series +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Jukka Laitinen +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. /etc/init.d/rc.mc_defaults + +# Default rates +param set-default IMU_GYRO_CUTOFF 30 +param set-default IMU_DGYRO_CUTOFF 15 +param set-default IMU_GYRO_RATEMAX 800 +param set-default MC_ROLLRATE_P 0.14 +param set-default MC_PITCHRATE_P 0.14 +param set-default MC_ROLLRATE_I 0.350 +param set-default MC_PITCHRATE_I 0.300 +param set-default MC_ROLLRATE_D 0.0052 +param set-default MC_PITCHRATE_D 0.0048 +param set-default MC_PITCHRATE_K 1.10 +param set-default MC_PITCH_P 6.50 +param set-default MC_ROLLRATE_K 1.15 +param set-default MC_ROLL_P 6.00 +param set-default MC_YAWRATE_I 0.24 +param set-default MC_YAWRATE_K 1.75 +param set-default MC_YAWRATE_P 0.20 +param set-default MC_YAW_P 3.50 + +# Control allocator parameters +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.25 +param set-default CA_ROTOR0_PY 0.25 +param set-default CA_ROTOR1_PX -0.25 +param set-default CA_ROTOR1_PY -0.25 +param set-default CA_ROTOR2_PX 0.25 +param set-default CA_ROTOR2_PY -0.25 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.25 +param set-default CA_ROTOR3_PY 0.25 +param set-default CA_ROTOR3_KM -0.05 + +# PWM functions +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 + +# HITL PWM functions +param set-default HIL_ACT_FUNC1 101 +param set-default HIL_ACT_FUNC2 102 +param set-default HIL_ACT_FUNC3 103 +param set-default HIL_ACT_FUNC4 104 + +# Increase velocity controller P gain +param set-default MPC_XY_VEL_P_ACC 2.4 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_EMPTY 3.6 +param set-default BAT1_V_DIV 18.1 + +# Enable LL40LS in i2c +param set-default SENS_EN_LL40LS 2 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Set default for disarm after land to 4s +param set-default COM_DISARM_LAND 4.0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Change current sense shunt resistor value +param set-default INA226_SHUNT 0.00025 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing b/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing new file mode 100644 index 000000000000..b8ed145b1170 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4420_ssrc_arwing @@ -0,0 +1,67 @@ +#!/bin/sh +# +# @name SSRC ARWing +# +# @type Flying Wing +# @class Plane +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. /etc/init.d/rc.fw_defaults + +# Control allocator parameters +param set-default CA_AIRFRAME 1 +param set-default CA_ROTOR_COUNT 1 +param set-default CA_SV_CS_COUNT 2 +param set-default CA_SV_CS0_TYPE 5 +param set-default CA_SV_CS0_TRQ_P 0.5 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 6 +param set-default CA_SV_CS1_TRQ_P 0.5 +param set-default CA_SV_CS1_TRQ_R 0.5 + +# PWM +param set-default PWM_MAIN_FUNC1 201 +param set-default PWM_MAIN_FUNC2 202 +param set-default PWM_MAIN_FUNC4 101 +param set-default PWM_AUX_FUNC1 201 +param set-default PWM_AUX_FUNC2 202 +param set-default PWM_AUX_FUNC4 101 +param set-default PWM_MAIN_REV 2 +param set-default PWM_AUX_REV 2 + +# Airspeed parameters +sdp3x start -X -f 400 +param set-default ASPD_DO_CHECKS 15 +param set-default FW_AIRSPD_MAX 22.0 +param set-default FW_AIRSPD_MIN 14.0 +param set-default FW_AIRSPD_STALL 12.0 +param set-default FW_AIRSPD_TRIM 18.0 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_R_INTERNAL 0.0050 +param set-default BAT1_V_EMPTY 3.6000 +param set-default BAT1_V_LOAD_DROP 0.1000 + +param set-default BAT2_R_INTERNAL 0.0050 +param set-default BAT2_V_EMPTY 3.6000 +param set-default BAT2_V_LOAD_DROP 0.1000 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# Launch detection +param set-default FW_LAUN_DETCN_ON 1 + +# Maximum manual roll angle +param set-default FW_MAN_R_MAX 60.0 + +# Rate control +param set-default FW_RR_IMAX 0.4000 +param set-default FW_YR_IMAX 0.4000 + +# Misc +param set-default RTL_RETURN_ALT 30.0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini new file mode 100644 index 000000000000..0963ba0733a4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4430_ssrc_strivermini @@ -0,0 +1,126 @@ +#!/bin/sh +# +# @name SSRC Striver Mini +# +# @type Standard VTOL +# @class VTOL +# +# @maintainer +# +# @output MAIN1 motor 1 +# @output MAIN2 motor 2 +# @output MAIN3 motor 3 +# @output MAIN4 motor 4 +# @output AUX1 Aileron 1 +# @output AUX2 Aileron 2 +# @output AUX3 Elevator 1 +# @output AUX4 Elevator 2 +# @output AUX5 Rudder +# @output AUX6 Throttle +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# @board holybro_kakutef7 exclude +# + +. ${R}etc/init.d/rc.vtol_defaults + +# Control allocator parameters +param set-default CA_AIRFRAME 2 +param set-default CA_ROTOR_COUNT 5 +param set-default CA_ROTOR0_PX 0.37 +param set-default CA_ROTOR0_PY 0.42 +param set-default CA_ROTOR1_PX -0.41 +param set-default CA_ROTOR1_PY -0.42 +param set-default CA_ROTOR2_PX 0.37 +param set-default CA_ROTOR2_PY -0.42 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.41 +param set-default CA_ROTOR3_PY 0.42 +param set-default CA_ROTOR3_KM -0.05 +param set-default CA_ROTOR4_AX 1.0 +param set-default CA_ROTOR4_AZ 0.0 +param set-default CA_SV_CS_COUNT 5 +param set-default CA_SV_CS0_TYPE 1 +param set-default CA_SV_CS0_TRQ_R -0.5 +param set-default CA_SV_CS1_TYPE 2 +param set-default CA_SV_CS1_TRQ_R -0.5 +param set-default CA_SV_CS2_TYPE 3 +param set-default CA_SV_CS2_TRQ_P 1.0 +param set-default CA_SV_CS3_TYPE 3 +param set-default CA_SV_CS3_TRQ_P -1.0 +param set-default CA_SV_CS4_TYPE 4 +param set-default CA_SV_CS4_TRQ_Y 1.0 + +# PWM functions +param set-default PWM_MAIN_FUNC1 101 +param set-default PWM_MAIN_FUNC2 102 +param set-default PWM_MAIN_FUNC3 103 +param set-default PWM_MAIN_FUNC4 104 +param set-default PWM_MAIN_FUNC5 201 +param set-default PWM_MAIN_FUNC6 202 +param set-default PWM_MAIN_FUNC7 203 +param set-default PWM_MAIN_FUNC8 204 +param set-default PWM_MAIN_FUNC9 205 +param set-default PWM_MAIN_FUNC10 105 +param set-default PWM_AUX_FUNC1 201 +param set-default PWM_AUX_FUNC2 202 +param set-default PWM_AUX_FUNC3 203 +param set-default PWM_AUX_FUNC4 204 +param set-default PWM_AUX_FUNC5 205 +param set-default PWM_AUX_FUNC6 105 + +# Start airspeed sensor driver +ms4525do start -X -f 500 + +# Airspeed parameters +param set-default ASPD_DO_CHECKS 15 +param set-default FW_AIRSPD_MAX 22.0 +param set-default FW_AIRSPD_MIN 15.0 +param set-default FW_AIRSPD_STALL 12.0 +param set-default FW_AIRSPD_TRIM 18.0 + +# Battery parameters +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_EMPTY 3.6 +param set-default BAT1_V_DIV 18.1 + +# Disable internal magnetometer +param set CAL_MAG0_PRIO 0 + +# Return mode return altitude +param set-default RTL_RETURN_ALT 30.0 + +# Fixed wing specific +param set-default FW_MAN_R_MAX 60.0 +param set-default FW_RR_IMAX 0.4000 +param set-default FW_YR_IMAX 0.4000 + +# VTOL specific +# VTOL type +param set-default VT_TYPE 2 + +# Airspeed at which we can start blending both fw and mc controls. +param set-default VT_ARSP_BLEND 10 + +# Airspeed at which we can switch to fw mode +param set-default VT_ARSP_TRANS 12 + +# Back-transition duration +param set-default VT_B_TRANS_DUR 4 +param set-default VT_B_TRANS_RAMP 3 + +# Transition duration +param set-default VT_F_TRANS_DUR 6 +param set-default VT_F_TRANS_THR 1 + +# VTOL takeoff +param set-default VTO_LOITER_ALT 20 + +# QuadChute altitude (transition to quad mode as a failsafe) +param set-default VT_FW_MIN_ALT 5 + +# QuadChute angle limits +param set-default VT_FW_QC_P 35 +param set-default VT_FW_QC_R 60 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 211feb492489..994ff023833d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 3462b7575f75..e340fb8f4462 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50005_ssrc_scout_mini_rover b/ROMFS/px4fmu_common/init.d/airframes/50005_ssrc_scout_mini_rover new file mode 100644 index 000000000000..f0ebcf96f368 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50005_ssrc_scout_mini_rover @@ -0,0 +1,103 @@ +#!/bin/sh +# +# @name SSRC SCOUT MINI UGV +# +# @url https://global.agilex.ai/products/scout-mini +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_defaults + +# Rover interface +param set-default RI_ROVER_TYPE 0 +param set-default RI_MAN_SPD_SC 1.0 + +# Battery setting +param set-default BAT1_N_CELLS 7 +param set-default BAT1_V_EMPTY 3.3 +param set-default BAT1_V_CHARGED 4.2 + +# EKF2 +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default EKF2_MAG_TYPE 1 +param set-default EKF2_REQ_SACC 1.0 +param set-default EKF2_REQ_VDRIFT 0.4 +param set-default EKF2_REQ_HDRIFT 0.2 + + +param set-default FW_AIRSPD_MIN 0 +param set-default FW_AIRSPD_TRIM 1 +param set-default FW_AIRSPD_MAX 3 + +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 3 +param set-default GND_THR_CRUISE 1 +param set-default GND_THR_MAX 1 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set-default GND_MAX_ANG 3.142 +param set-default GND_WHEEL_BASE 0.45 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set-default GND_THR_MIN 0 +param set-default GND_SPEED_P 0.4 +param set-default GND_SPEED_I 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_MAX 3.0 +param set-default GND_SPEED_TRIM 3.0 +param set-default GND_SPEED_THR_SC 1.0 +param set-default GND_VEL_CTRL 1 +param set-default GND_ANG_VEL_CTRL 1 +param set-default GND_ACC_LIMIT 10 +param set-default GND_DEC_LIMIT 50 + +param set-default MIS_TAKEOFF_ALT 0.01 + +param set-default NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 + +# Set geometry & output configration. This is just the place holder for rover type. We dont actually use control allocation for scout mini. +param set-default CA_AIRFRAME 6 + +# Disable UAVCAN since rover use rover_interface module instead +param set UAVCAN_ENABLE 0 + + +###### ROVER/DRONE COMMON PARAMS ###### +# Disable internal magnetometer +param set-default CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Set default logfile encryption key indecies +param set-default SDLOG_EXCH_KEY 2 +param set-default SDLOG_KEY 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50006_ssrc_bunker_rover b/ROMFS/px4fmu_common/init.d/airframes/50006_ssrc_bunker_rover new file mode 100644 index 000000000000..cfce96cc0ab4 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50006_ssrc_bunker_rover @@ -0,0 +1,103 @@ +#!/bin/sh +# +# @name SSRC BUNKER UGV +# +# @url https://global.agilex.ai/chassis/4 +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_defaults + +# Rover interface +param set-default RI_ROVER_TYPE 5 +param set-default RI_MAN_SPD_SC 1.0 + +# Battery setting +param set-default BAT1_N_CELLS 14 +param set-default BAT1_V_EMPTY 3.3 +param set-default BAT1_V_CHARGED 4.2 + +# EKF2 +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default EKF2_MAG_TYPE 1 +param set-default EKF2_REQ_SACC 1.0 +param set-default EKF2_REQ_VDRIFT 0.4 +param set-default EKF2_REQ_HDRIFT 0.2 + + +param set-default FW_AIRSPD_MIN 0 +param set-default FW_AIRSPD_TRIM 1 +param set-default FW_AIRSPD_MAX 3 + +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 3 +param set-default GND_THR_CRUISE 1 +param set-default GND_THR_MAX 1 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set-default GND_MAX_ANG 3.142 +param set-default GND_WHEEL_BASE 0.778 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set-default GND_THR_MIN 0 +param set-default GND_SPEED_P 0.4 +param set-default GND_SPEED_I 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_MAX 1.5 +param set-default GND_SPEED_TRIM 1.5 +param set-default GND_SPEED_THR_SC 1.0 +param set-default GND_VEL_CTRL 1 +param set-default GND_ANG_VEL_CTRL 1 +param set-default GND_ACC_LIMIT 10 +param set-default GND_DEC_LIMIT 50 + +param set-default MIS_TAKEOFF_ALT 0.01 + +param set-default NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 + +# Set geometry & output configration. This is just the place holder for rover type. We dont actually use control allocation for scout mini. +param set-default CA_AIRFRAME 6 + +# Disable UAVCAN since rover use rover_interface module instead +param set UAVCAN_ENABLE 0 + + +###### ROVER/DRONE COMMON PARAMS ###### +# Disable internal magnetometer +param set-default CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Set default logfile encryption key indecies +param set-default SDLOG_EXCH_KEY 2 +param set-default SDLOG_KEY 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50007_ssrc_bunker_mini_rover b/ROMFS/px4fmu_common/init.d/airframes/50007_ssrc_bunker_mini_rover new file mode 100644 index 000000000000..2005a6f961a5 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50007_ssrc_bunker_mini_rover @@ -0,0 +1,103 @@ +#!/bin/sh +# +# @name SSRC BUNKER UGV +# +# @url https://global.agilex.ai/chassis/5 +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_defaults + +# Rover interface +param set-default RI_ROVER_TYPE 6 +param set-default RI_MAN_SPD_SC 1.0 + +# Battery setting +param set-default BAT1_N_CELLS 14 +param set-default BAT1_V_EMPTY 3.3 +param set-default BAT1_V_CHARGED 4.2 + +# EKF2 +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 +param set-default EKF2_MAG_TYPE 1 +param set-default EKF2_REQ_SACC 1.0 +param set-default EKF2_REQ_VDRIFT 0.4 +param set-default EKF2_REQ_HDRIFT 0.2 + + +param set-default FW_AIRSPD_MIN 0 +param set-default FW_AIRSPD_TRIM 1 +param set-default FW_AIRSPD_MAX 3 + +param set-default GND_SP_CTRL_MODE 1 +param set-default GND_L1_DIST 5 +param set-default GND_L1_PERIOD 3 +param set-default GND_THR_CRUISE 1 +param set-default GND_THR_MAX 1 + +# Because this is differential drive, it can make a turn with radius 0. +# This corresponds to a turn angle of pi radians. +# If a special case is made for differential-drive, this will need to change. +param set-default GND_MAX_ANG 3.142 +param set-default GND_WHEEL_BASE 0.584 + +# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase +# to support negative throttle. +param set-default GND_THR_MIN 0 +param set-default GND_SPEED_P 0.4 +param set-default GND_SPEED_I 1 +param set-default GND_SPEED_D 0.001 +param set-default GND_SPEED_MAX 1.5 +param set-default GND_SPEED_TRIM 1.5 +param set-default GND_SPEED_THR_SC 1.0 +param set-default GND_VEL_CTRL 1 +param set-default GND_ANG_VEL_CTRL 1 +param set-default GND_ACC_LIMIT 10 +param set-default GND_DEC_LIMIT 50 + +param set-default MIS_TAKEOFF_ALT 0.01 + +param set-default NAV_ACC_RAD 0.5 # reached when within 0.5m of waypoint + +# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor +param set-default CBRK_AIRSPD_CHK 162128 + +# Set geometry & output configration. This is just the place holder for rover type. We dont actually use control allocation for scout mini. +param set-default CA_AIRFRAME 6 + +# Disable UAVCAN since rover use rover_interface module instead +param set UAVCAN_ENABLE 0 + + +###### ROVER/DRONE COMMON PARAMS ###### +# Disable internal magnetometer +param set-default CAL_MAG0_PRIO 0 + +# LEDs on TELEMETRY 1 +param set-default SER_TEL1_BAUD 57600 +param set-default MAV_1_CONFIG 101 +param set-default MAV_1_MODE 7 +param set-default MAV_1_RATE 1000 + +# Disable MAV_0 and MAV_2 +param set-default MAV_0_CONFIG 0 +param set-default MAV_2_CONFIG 0 + +# Enable safety switch +param set-default CBRK_IO_SAFETY 0 + +# Enable satellite info by default +param set-default GPS_SAT_INFO 1 + +# Set sticks movement not to switch to RC, we use mode switch for this +param set-default COM_RC_OVERRIDE 0 + +# Set default logfile encryption key indecies +param set-default SDLOG_EXCH_KEY 2 +param set-default SDLOG_KEY 3 diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 8e9931aa89f8..11425887ba24 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -66,6 +66,10 @@ px4_add_romfs_files( 4061_atl_mantis_edu 4071_ifo 4073_ifo-s + 4400_ssrc_fog_x + 4401_ssrc_fog_x_tmotor + 4420_ssrc_arwing + 4430_ssrc_strivermini 4500_clover4 4900_crazyflie 4901_crazyflie21 @@ -119,6 +123,9 @@ px4_add_romfs_files( 50000_generic_ground_vehicle 50004_nxpcup_car_dfrobot_gpx 50003_aion_robotics_r1_rover + 50005_ssrc_scout_mini_rover + 50006_ssrc_bunker_rover + 50007_ssrc_bunker_mini_rover # [60000, 61000] (Unmanned) Underwater Robots 60000_uuv_generic diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index d5a7bf293a5a..3493880fd43d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -23,12 +23,16 @@ then set LOGGER_ARGS "${LOGGER_ARGS} -x" fi +if [ $SDCARD_AVAILABLE = no ] +then + set LOGGER_ARGS "${LOGGER_ARGS} -m mavlink" +fi + if param compare SDLOG_MODE 4 then set LOGGER_ARGS "${LOGGER_ARGS} -a" fi - if ! param compare SDLOG_MODE -1 then logger start -b ${LOGGER_BUF} -t ${LOGGER_ARGS} diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index 4d04b53faa09..f039a9b8dedd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -5,6 +5,16 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # +# +# Start the rover interface CAN driver if rover type is one of SSRC supported +# rover types (scout mini, bunker, ..) +# +if param greater RI_ROVER_TYPE -1 +then + ifup can0 + rover_interface start +fi + # # Start the attitude and position estimator. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4a391d39f9a9..f2c2b9acf039 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -35,6 +35,25 @@ set SDCARD_EXT_PATH /fs/microsd/ext_autostart set SDCARD_FORMAT no set STARTUP_TUNE 1 set VEHICLE_TYPE none +set SDCARD_DEVPATH /dev/mmcsd0 +set SDCARD_SCRIPT_ENABLED no + +# +# Optional board path defaults: rc.board_paths +# +set BOARD_RC_PATHS ${R}etc/init.d/rc.board_paths +if [ -f $BOARD_RC_PATHS ] +then + echo "Board path defaults: ${BOARD_RC_PATHS}" + . $BOARD_RC_PATHS +fi +unset BOARD_RC_PATHS + +if rdfeat cmp FEATURE_ENABLE_PX4_SDCARD_SCRIPT +then + echo "SD card script execution enabled" + set SDCARD_SCRIPT_ENABLED yes +fi # # Print full system version. @@ -44,13 +63,13 @@ ver all # # Try to mount the microSD card. # -if [ -b "/dev/mmcsd0" ] +if [ -b $SDCARD_DEVPATH ] then - if mount -t vfat /dev/mmcsd0 /fs/microsd + if mount -t vfat $SDCARD_DEVPATH /fs/microsd then if [ -f "/fs/microsd/.format" ] then - echo "INFO [init] format /dev/mmcsd0 requested (/fs/microsd/.format)" + echo "INFO [init] format ${SDCARD_DEVPATH} requested (/fs/microsd/.format)" set SDCARD_FORMAT yes rm /fs/microsd/.format umount /fs/microsd @@ -62,14 +81,14 @@ then if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ] then - echo "INFO [init] formatting /dev/mmcsd0" + echo "INFO [init] formatting ${SDCARD_DEVPATH}" set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds) - if mkfatfs -F 32 /dev/mmcsd0 + if mkfatfs -F 32 $SDCARD_DEVPATH then echo "INFO [init] card formatted" - if mount -t vfat /dev/mmcsd0 /fs/microsd + if mount -t vfat $SDCARD_DEVPATH /fs/microsd then set SDCARD_AVAILABLE yes set STARTUP_TUNE 14 # tune 14 = SD_INIT @@ -97,11 +116,13 @@ then set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson" fi +unset SDCARD_DEVPATH + # # Look for an init script on the microSD card. # Disable autostart if the script found. # -if [ -f $FRC ] +if [ -f $FRC -a $SDCARD_SCRIPT_ENABLED = yes ] then . $FRC else @@ -132,7 +153,7 @@ else bsondump $PARAM_FILE - if [ -d "/fs/microsd" ] + if [ -d "/fs/microsd" -a $SDCARD_SCRIPT_ENABLED = yes ] then # try to make a backup copy cp $PARAM_FILE /fs/microsd/param_import_fail.bson @@ -157,7 +178,7 @@ else fi fi - if [ $SDCARD_AVAILABLE = yes ] + if [ $SDCARD_AVAILABLE = yes -a $SDCARD_SCRIPT_ENABLED = yes ] then param select-backup $PARAM_BACKUP_FILE fi @@ -207,7 +228,7 @@ else if param greater SYS_AUTOSTART 1000000 then # Use external startup file - if [ $SDCARD_AVAILABLE = yes ] + if [ $SDCARD_AVAILABLE = yes -a $SDCARD_SCRIPT_ENABLED = yes ] then set AUTOSTART_PATH etc/init.d/rc.autostart_ext else @@ -260,7 +281,7 @@ else # # Override parameters from user configuration file. # - if [ -f $FCONFIG ] + if [ -f $FCONFIG -a $SDCARD_SCRIPT_ENABLED = yes ] then echo "Custom: ${FCONFIG}" . $FCONFIG @@ -397,17 +418,6 @@ else mag_bias_estimator start fi - # - # Optional board mavlink streams: rc.board_mavlink - # - set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink - if [ -f $BOARD_RC_MAVLINK ] - then - echo "Board mavlink: ${BOARD_RC_MAVLINK}" - . $BOARD_RC_MAVLINK - fi - unset BOARD_RC_MAVLINK - # # Start UART/Serial device drivers. # Note: rc.serial is auto-generated from Tools/serial/generate_config.py @@ -486,7 +496,7 @@ else # # Start any custom addons from the sdcard. # - if [ -f $FEXTRAS ] + if [ -f $FEXTRAS -a $SDCARD_SCRIPT_ENABLED = yes ] then echo "Addons script: ${FEXTRAS}" . $FEXTRAS @@ -530,11 +540,27 @@ else fi fi + # + # Start the CDC ACM monitor + # + cdcacm start + # # End of autostart. # fi +# +# Optional board mavlink streams: rc.board_mavlink +# +set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink +if [ -f $BOARD_RC_MAVLINK ] +then + echo "Board mavlink: ${BOARD_RC_MAVLINK}" + . $BOARD_RC_MAVLINK +fi +unset BOARD_RC_MAVLINK + # # Unset all script parameters to free RAM. # diff --git a/Tools/HIL/run_nsh_cmd.py b/Tools/HIL/run_nsh_cmd.py index dcdd9ec1e7b3..7c37679adf43 100755 --- a/Tools/HIL/run_nsh_cmd.py +++ b/Tools/HIL/run_nsh_cmd.py @@ -39,7 +39,7 @@ def print_line(line): print('{0}'.format(line), end='') -def do_nsh_cmd(port_url, baudrate, cmd): +def do_nsh_cmd(port_url, baudrate, cmd, ignore_stdout_errors=False): ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) timeout_start = time.monotonic() @@ -106,7 +106,7 @@ def do_nsh_cmd(port_url, baudrate, cmd): if success_cmd in serial_line: sys.exit(return_code) else: - if "ERROR " in serial_line: + if "ERROR " in serial_line and not ignore_stdout_errors: return_code = -1 print_line(serial_line) @@ -148,6 +148,8 @@ def main(): parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) parser.add_argument("--cmd", "-c", dest="cmd", help="Command to run") + parser.add_argument('--ignore-stdout-errors', action='store_true', + help='Ignore errors printed to stdout') args = parser.parse_args() tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) @@ -155,7 +157,7 @@ def main(): print("pyserial url: {0}".format(port_url)) - do_nsh_cmd(port_url, args.baudrate, args.cmd) + do_nsh_cmd(port_url, args.baudrate, args.cmd, args.ignore_stdout_errors) if __name__ == "__main__": main() diff --git a/Tools/cryptotools.py b/Tools/cryptotools.py index 6e4bd42d63e5..73b90c0ab35e 100755 --- a/Tools/cryptotools.py +++ b/Tools/cryptotools.py @@ -1,8 +1,7 @@ #!/usr/bin/env python3 -import nacl.encoding -import nacl.signing -import nacl.hash +from cryptography.hazmat.primitives.asymmetric.ed25519 import Ed25519PrivateKey +from cryptography.hazmat.primitives import serialization import struct import binascii import json @@ -11,65 +10,25 @@ from pathlib import Path import sys -def make_public_key_h_file(signing_key,key_name): - """ - This file generate the public key header file - to be included into the bootloader build. - """ - public_key_c='\n' - for i,c in enumerate(signing_key.verify_key.encode(encoder=nacl.encoding.RawEncoder)): - public_key_c+= hex(c) - public_key_c+= ', ' - if((i+1)%8==0): - public_key_c+= '\n' - with open(key_name+'.pub' ,mode='w') as f: - f.write("//Public key to verify signed binaries") - f.write(public_key_c) - -def make_key_file(signing_key, key_name): - """ - Writes the key.json file. - Attention do not override your existing key files. - Do not publish your private key!! - """ - - key_file = Path(key_name+'.json') - if key_file.is_file(): - print("ATTENTION: key.json already exists, are you sure you want to overwrite it?") - print("Remove file and run script again.") - print("Script aborted!") - sys.exit(1) - - keys={} - keys["date"] = time.asctime() - keys["public"] = (signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder)).decode() - keys["private"] = binascii.hexlify(signing_key._seed).decode() - #print (keys) - with open(key_name+'.json', "w") as write_file: - json.dump(keys, write_file) - return keys - def ed25519_sign(private_key, signee_bin): """ This function creates the signature. It takes the private key and the binary file and returns the tuple (signature, public key) """ - signing_key = nacl.signing.SigningKey(private_key, encoder=nacl.encoding.HexEncoder) - # Sign a message with the signing key - signed = signing_key.sign(signee_bin,encoder=nacl.encoding.RawEncoder) + signature = private_key.sign(signee_bin) # Obtain the verify key for a given signing key - verify_key = signing_key.verify_key - - # Serialize the verify key to send it to a third party - verify_key_hex = verify_key.encode(encoder=nacl.encoding.HexEncoder) + public_key = private_key.public_key() + verify_key = public_key.public_bytes( + encoding=serialization.Encoding.Raw, + format=serialization.PublicFormat.Raw + ) - return signed.signature, verify_key_hex + return signature, verify_key - -def sign(bin_file_path, key_file_path=None, generated_key_file=None): +def sign(bin_file_path, key_file_path=None): """ reads the binary file and the key file. If the key file does not exist, it generates a @@ -85,14 +44,14 @@ def sign(bin_file_path, key_file_path=None, generated_key_file=None): signee_bin += bytearray(b'\xff')*(4-len(signee_bin)%4) try: - with open(key_file_path,mode='r') as f: - keys = json.load(f) - #print(keys) + with open(key_file_path,mode='rb') as f: + private_key = serialization.load_pem_private_key(f.read(), None) + except: print('ERROR: Key file',key_file_path,'not found') sys.exit(1) - signature, public_key = ed25519_sign(keys["private"], signee_bin) + signature, public_key = ed25519_sign(private_key, signee_bin) # Do a sanity check. This type of signature is always 64 bytes long assert len(signature) == 64 @@ -104,28 +63,6 @@ def sign(bin_file_path, key_file_path=None, generated_key_file=None): return signee_bin + signature, public_key -def generate_key(key_file): - """ - Generate two files: - "key_file.pub" containing the public key in C-format to be included in the bootloader build - "key_file.json, containt both private and public key. - Do not leak or loose the key file. This is mandatory for signing - all future binaries you want to deploy! - """ - - # Generate a new random signing key - signing_key = nacl.signing.SigningKey.generate() - # Serialize the verify key to send it to a third party - verify_key_hex = signing_key.verify_key.encode(encoder=nacl.encoding.HexEncoder) - print("public key :",verify_key_hex) - - private_key_hex=binascii.hexlify(signing_key._seed) - print("private key :",private_key_hex) - - keys = make_key_file(signing_key,key_file) - make_public_key_h_file(signing_key,key_file) - return keys - if(__name__ == "__main__"): parser = argparse.ArgumentParser(description="""CLI tool to calculate and add signature to px4. bin files\n @@ -134,18 +71,10 @@ def generate_key(key_file): parser.add_argument("signee", help=".bin file to add signature", nargs='?', default=None) parser.add_argument("signed", help="signed output .bin", nargs='?', default=None) - parser.add_argument("--key", help="key.json file", default="Tools/test_keys/test_keys.json") + parser.add_argument("--key", help="key.json file", default="Tools/test_keys/ed25519_test_key.pem") parser.add_argument("--rdct", help="binary R&D certificate file", default=None) - parser.add_argument("--genkey", help="new generated key", default=None) args = parser.parse_args() - # Only generate a key pair, don't sign - if args.genkey: - # Only create a key file, don't sign - generate_key(args.genkey) - print('New key file generated:',args.genkey) - sys.exit(0); - # Check that both signee and signed exist if not args.signee or not args.signed: print("ERROR: Must either provide file names for both signee and signed") @@ -153,11 +82,11 @@ def generate_key(key_file): sys.exit(1) # Issue a warning when signing with testing key - if args.key=='Tools/test_keys/test_keys.json': + if args.key=='Tools/test_keys/ed25519_test_key.pem': print("WARNING: Signing with PX4 test key") # Sign the binary - signed, public_key = sign(args.signee, args.key, args.genkey) + signed, public_key = sign(args.signee, args.key) with open(args.signed, mode='wb') as fs: # Write signed binary @@ -170,4 +99,3 @@ def generate_key(key_file): fs.write(f.read()) except: pass - diff --git a/Tools/px_uploader.Dockerfile b/Tools/px_uploader.Dockerfile new file mode 100644 index 000000000000..e4227bd57008 --- /dev/null +++ b/Tools/px_uploader.Dockerfile @@ -0,0 +1,49 @@ +ARG saluki_pi_fpga_version +ARG saluki_v2_fpga_version +ARG saluki_v3_fpga_version + +FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_pi_fpga_version AS SALUKI_PI +FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_v2_fpga_version AS SALUKI_V2 +FROM ghcr.io/tiiuae/saluki-pi-fpga:$saluki_v3_fpga_version AS SALUKI_V3 +FROM ghcr.io/tiiuae/saluki_bootloader_v2:master AS SALUKI_BOOTLOADER_v2 + +FROM python:alpine3.14 + +# run this with something like: +# +# $ docker run --rm -it --network=host --device=/dev/ttyS7:/dev/px4serial px4-fw-updater \ +# --udp-addr=192.168.200.101 \ +# --udp-port=14541 \ +# --port=/dev/px4serial \ +# --baud-bootloader=2000000 \ +# px4_fmu-v5_ssrc.px4 + +# This gets built in environment with somewhat unorthodox paths: +# - The build context is at / +# - The repository itself is mounted in /px4-firmware/ +# - Built firmware files are in /bin/ +# +# ("/" above is relative to GH action runner home dir) +# (see .github/workflows/tiiuae-pixhawk.yaml) + +# copy fpga files from separate saluki containers +COPY --from=SALUKI_PI /firmware/saluki_pi-fpga /firmware/fpga/saluki_pi +COPY --from=SALUKI_V2 /firmware/saluki_v2-fpga /firmware/fpga/saluki_v2 +COPY --from=SALUKI_V3 /firmware/saluki_v3-fpga /firmware/fpga/saluki_v3 +# copy px_uploader.py from saluki_bootloader_v2 container +COPY --from=SALUKI_BOOTLOADER_v2 /firmware/bootloader_v2/px_uploader.py /firmware/px_uploader.py + +WORKDIR /firmware + +ENTRYPOINT ["/entrypoint.sh"] + +# dependency of px_uploader.py +RUN pip3 install --user pyserial + +ADD px4-firmware/Tools/px_uploader.py /bin/ +ADD px4-firmware/Tools/px_uploader.entrypoint /entrypoint.sh + +# copy /bin/* -> /firmware/* +ADD bin/ /firmware/ + +ADD px4-firmware/ssrc_config /flight_modes diff --git a/Tools/px_uploader.entrypoint b/Tools/px_uploader.entrypoint new file mode 100755 index 000000000000..1d9b7f87b36b --- /dev/null +++ b/Tools/px_uploader.entrypoint @@ -0,0 +1,13 @@ +#!/bin/sh -e + +aframe="" +if [ ! -z "$AIRFRAME" ]; then + aframe="--airframe ${AIRFRAME}" +fi + +mode="" +if [ ! -z "$FLIGHT_MODE" ]; then + mode="--flightmode ${FLIGHT_MODE}" +fi + +/bin/px_uploader.py --port=/dev/px4serial --udp-addr=192.168.200.101 --baud-flightstack=57600,115200,1000000,2000000 --baud-bootloader=2000000 px4_fmu-v5x_ssrc-*.px4 ssrc_saluki-v1_default-*.px4 ssrc_saluki-v2_default-*.px4 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d479be314837..2cbc3bb309f5 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -212,8 +212,9 @@ class uploader(object): NSH_INIT = bytearray(b'\x0d\x0d\x0d') NSH_REBOOT_BL = b"reboot -b\n" NSH_REBOOT = b"reboot\n" - MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x53\x6b') - MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xcc\x37') + # Reboot commands incl. 4-byte Sp2 header + MAVLINK_REBOOT_ID1 = bytearray(b'\x53\x00\x29\x29\xfe\x21\x72\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x53\x6b') + MAVLINK_REBOOT_ID0 = bytearray(b'\x53\x00\x29\x29\xfe\x21\x45\xff\x00\x4c\x00\x00\x40\x40\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xcc\x37') MAX_FLASH_PRGRAM_TIME = 0.001 # Time on an F7 to send SYNC, RESULT from last data in multi RXed @@ -550,8 +551,15 @@ def __verify_v3(self, label, fw): self.__drawProgressBar(label, 1, 100) expect_crc = fw.crc(self.fw_maxsize) self.__send(uploader.GET_CRC + uploader.EOC) - time.sleep(0.5) - report_crc = self.__recv_int() + # wait for maximum 5 seconds for response + for i in range(10): + time.sleep(0.5) + try: + report_crc = self.__recv_int() + break + except: + pass + self.__getSync() if report_crc != expect_crc: print("Expected 0x%x" % expect_crc) @@ -743,10 +751,21 @@ def send_protocol_splitter_format(self, data): self.__send(header_bytes) self.__send(data) - def send_reboot(self, use_protocol_splitter_format=False): + def send_reboot(self, ipaddr, portnum, use_protocol_splitter_format=False): if (not self.__next_baud_flightstack()): return False + print("Attempting reboot on ethernet") + # initialize an UDP socket + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + # send reboot request + s.sendto(self.MAVLINK_REBOOT_ID1, (ipaddr, int(portnum))) + s.sendto(self.MAVLINK_REBOOT_ID0, (ipaddr, int(portnum))) + + # close the socket + s.close() + print("Attempting reboot on %s with baudrate=%d..." % (self.port.port, self.port.baudrate), file=sys.stderr) if "ttyS" in self.port.port: print("If the board does not respond, check the connection to the Flight Controller") @@ -786,6 +805,8 @@ def main(): # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") + parser.add_argument('--udp-addr', action="store", default="192.168.200.100", help="UDP address of PX4 flight controller") + parser.add_argument('--udp-port', action="store", default=14541, help="UDP port of PX4 mavlink") parser.add_argument('--port', action="store", required=True, help="Comma-separated list of serial port(s) to which the FMU may be attached") parser.add_argument('--baud-bootloader', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200) when communicating with bootloader, only required for true serial ports.") parser.add_argument('--baud-flightstack', action="store", default="57600", help="Comma-separated list of baud rate of the serial port (default is 57600) when communicating with flight stack (Mavlink or NSH), only required for true serial ports.") @@ -889,7 +910,7 @@ def main(): except Exception: - if not up.send_reboot(args.use_protocol_splitter_format): + if not up.send_reboot(args.udp_addr, args.udp_port, args.use_protocol_splitter_format): break # wait for the reboot, without we might run into Serial I/O Error 5 diff --git a/Tools/saluki-sec-scripts b/Tools/saluki-sec-scripts new file mode 160000 index 000000000000..d90d605514f0 --- /dev/null +++ b/Tools/saluki-sec-scripts @@ -0,0 +1 @@ +Subproject commit d90d605514f0cf6ba5b468deeaff2d235a25b053 diff --git a/Tools/ssrc-sim-tester.sh b/Tools/ssrc-sim-tester.sh new file mode 100755 index 000000000000..bb51c4b8681b --- /dev/null +++ b/Tools/ssrc-sim-tester.sh @@ -0,0 +1,157 @@ +#!/bin/bash + +set -e + +########################################################### +# Argument parsing + +usage() { + echo " +Usage: $(basename "$0") [-h] [-d] [-n count] [-t tag] + -- Run multiple containerized px4_sitl instances in gazebo simulation +Params: + -h Show help text. + -d Delete running simulation + -n Count of px4_sitl instances launched into simulation. Default: 1 + -t Use specific tii-px4-sitl: image for sitl instances. Default: master + -g gzserver image name. Default: ghcr.io/tiiuae/tii-gzserver:tcp + -i gazebo-data image name. Default: ghcr.io/tiiuae/tii-gazebo-data:tcp_test3 +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +delete_current_sim=0 +instance_count=1 +image_tag="master" +gzserver_image=ghcr.io/tiiuae/tii-gzserver:tcp +data_image=ghcr.io/tiiuae/tii-gazebo-data:tcp_test3 + +while getopts "hdn:t:g:i:" opt +do + case $opt in + h) + usage + ;; + d) + delete_current_sim=1 + ;; + n) + check_arg $OPTARG && instance_count=$OPTARG || error_arg $opt + ;; + t) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + g) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + i) + check_arg $OPTARG && image_tag=$OPTARG || error_arg $opt + ;; + \?) + usage + ;; + esac +done + + +########################################################### +# Main + +delete_simulation() { + echo "Delete simulation:" + pids=() + dronestr=$(docker ps | grep px4_sitl_drone | awk '{ print $1 }') + docker stop gzserver >/dev/null 2>&1 & + pid=$! + pids+=($pid) + echo " Removing gzserver (pid $pid)" + + if [ "$dronestr" != "" ]; then + readarray -t drone_list <<<"$dronestr" + for dr in ${drone_list[@]} + do + docker stop $dr >/dev/null 2>&1 & + pid=$! + pids+=($pid) + echo " Removing drone '$dr' (pid $pid)" + done + fi + + for p in ${pids[@]} + do + wait $p + echo " pid $p removed." + done + echo "Done." +} + + +if [ $delete_current_sim = 1 ]; then + delete_simulation + exit 0 +fi + + +echo "Run simulation with px4_sitl image: ghcr.io/tiiuae/tii-px4-sitl:${image_tag}" + +if [ ! -d "/tmp/gazebo-data" ]; then + echo "gazebo-data not found, download to '/tmp/gazebo-data'" + docker create --name gdata_cont ${data_image} + docker cp gdata_cont:/gazebo-data /tmp + docker rm gdata_cont + echo "gazebo-data download done." +fi + +if [ "$(docker ps | grep px4_sitl_drone)" != "" ] || [ "$(docker ps | grep gzserver)" != "" ]; then + echo "Old simulation found running, removing..." + delete_simulation +fi + +echo "Start gzserver" +docker run -d --rm --name gzserver --env PX4_SIM_USE_TCP_SERVER=1 -v /tmp/gazebo-data:/data ${gzserver_image} empty.world + +echo "Starting ${instance_count} PX4 instances" +echo + +for i in $(seq 1 $instance_count) +do + drone=px4_sitl_drone${i} + echo "Start px4 instance #${i}/${instance_count}" + docker run -d --rm --name $drone --env PX4_SIM_USE_TCP_SERVER=1 --env PX4_SIM_MODE=ssrc_fog_x ghcr.io/tiiuae/tii-px4-sitl:${image_tag} + echo " Wait 1 sec.." + sleep 1 + drone_ip=$(docker inspect -f '{{ .NetworkSettings.IPAddress }}' ${drone}) + echo " Spawn ${drone} into gazebo simulation instance" + docker exec gzserver /gzserver-api/scripts/spawn-drone.sh ${drone_ip} 12460 4560 5600 ${drone} 0 0 0 0 0 0 + echo +done + +cat << EOF + +To get logs: + $ docker logs -f gzserver + $ docker logs -f px4_sitl_drone1 + $ docker logs -f px4_sitl_drone2 + .. + $ docker logs -f px4_sitl_droneN + +QGC connection: + udp:14550 + +To stop simulation: + $ $0 -d + +EOF diff --git a/Tools/test_keys/ed25519_test_key.pem b/Tools/test_keys/ed25519_test_key.pem new file mode 100644 index 000000000000..ddcf3233a19c --- /dev/null +++ b/Tools/test_keys/ed25519_test_key.pem @@ -0,0 +1,3 @@ +-----BEGIN PRIVATE KEY----- +MC4CAQAwBQYDK2VwBCIEIHNNWX59irCh0NZLlQg6pr7jS0a55udtrB42OvEU8S0V +-----END PRIVATE KEY----- diff --git a/Tools/test_keys/key0.pub b/Tools/test_keys/key0.pub index 86153ac96535..59472655b5f9 100644 --- a/Tools/test_keys/key0.pub +++ b/Tools/test_keys/key0.pub @@ -1,5 +1,7 @@ //Public key to verify signed binaries -0x4d, 0xb0, 0xc2, 0x1, 0x5, 0x55, 0x2a, 0x3c, -0xd7, 0xfb, 0xaf, 0x5c, 0xba, 0x7a, 0xb0, 0x81, -0x1b, 0x36, 0x63, 0xdb, 0x28, 0x52, 0x5e, 0xdb, -0x14, 0x36, 0xf2, 0x57, 0x8d, 0x2, 0xb7, 0xfd, +0x30, 0x2a, 0x30, 0x05, 0x06, 0x03, 0x2b, 0x65, +0x70, 0x03, 0x21, 0x00, 0x4d, 0xb0, 0xc2, 0x01, +0x05, 0x55, 0x2a, 0x3c, 0xd7, 0xfb, 0xaf, 0x5c, +0xba, 0x7a, 0xb0, 0x81, 0x1b, 0x36, 0x63, 0xdb, +0x28, 0x52, 0x5e, 0xdb, 0x14, 0x36, 0xf2, 0x57, +0x8d, 0x02, 0xb7, 0xfd diff --git a/Tools/test_keys/test_keys.json b/Tools/test_keys/test_keys.json deleted file mode 100644 index 127a5be598d5..000000000000 --- a/Tools/test_keys/test_keys.json +++ /dev/null @@ -1 +0,0 @@ -{"date": "Tue Nov 3 13:02:09 2020", "public": "4db0c20105552a3cd7fbaf5cba7ab0811b3663db28525edb1436f2578d02b7fd", "private": "734d597e7d8ab0a1d0d64b95083aa6bee34b46b9e6e76dac1e363af114f12d15"} \ No newline at end of file diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 6655e8e3887e..5a70721a0efd 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -36,7 +36,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y -CONFIG_SYSTEMCMDS_MFT=y CONFIG_SYSTEMCMDS_MTD=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y diff --git a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp index b3c020bd7619..797df2e632f8 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -133,6 +133,15 @@ enum INT_CONFIG_BIT : uint8_t { INT1_POLARITY = Bit0, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, +}; + // FIFO_CONFIG enum FIFO_CONFIG_BIT : uint8_t { // 7:6 FIFO_MODE diff --git a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp index c2405de1fede..c1ebf31a2ffc 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/spektrum_rc.cpp @@ -187,7 +187,7 @@ void task_main(int argc, char *argv[]) } else { if (print_msg) { PX4_INFO("Spektrum RC: Publishing input_rc"); } - orb_publish(ORB_ID(input_rc), rc_pub, &input_rc); + orb_publish(ORB_ID(input_rc), &rc_pub, &input_rc); } } diff --git a/boards/mpfs/icicle/bootloader.px4board b/boards/mpfs/icicle/bootloader.px4board new file mode 100644 index 000000000000..36034e3299d2 --- /dev/null +++ b/boards/mpfs/icicle/bootloader.px4board @@ -0,0 +1,7 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_ROMFSROOT="" +CONFIG_BOARD_CRYPTO=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" diff --git a/boards/mpfs/icicle/bootloader_readme.txt b/boards/mpfs/icicle/bootloader_readme.txt new file mode 100644 index 000000000000..2da9d2c503a3 --- /dev/null +++ b/boards/mpfs/icicle/bootloader_readme.txt @@ -0,0 +1,13 @@ +1. build icicle bootloader: + +- Set PATH to point to a working riscv64-unknown-elf-gcc +- $ make mpfs_icicle_bootloader + +2. flash the icicle bootloader to eNVM + + + +$ cp /build/mpfs_icicle_bootloader/mpfs_icicle_bootloader.elf . +$ sudo SC_INSTALL_DIR= FPGENPROG=/Libero/bin64/fpgenprog java -jar $SC_INSTALL_DIR/extras/mpfs/mpfsBootmodeProgrammer.jar --bootmode 1 mpfs_icicle_bootloader.elf + +3. use px_uploader.py to flash the px4 binary diff --git a/boards/mpfs/icicle/default.px4board b/boards/mpfs/icicle/default.px4board new file mode 100644 index 000000000000..964dea31e0b5 --- /dev/null +++ b/boards/mpfs/icicle/default.px4board @@ -0,0 +1,85 @@ +CONFIG_BOARD_TOOLCHAIN="riscv64-unknown-elf" +CONFIG_BOARD_ARCHITECTURE="rv64gc" +CONFIG_BOARD_CRYPTO=y +CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS4" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS2" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PWM_ESC=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_POWER_MONITOR_INA228=y +CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_EVENTS=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_FW_ATT_CONTROL=y +CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_FW_POS_CONTROL=y +CONFIG_MODULES_FW_RATE_CONTROL=y +CONFIG_MODULES_GIMBAL=y +CONFIG_MODULES_GYRO_CALIBRATION=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SYSTEM_TIME=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_TUNE_CONTROL=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY2="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/mpfs/icicle/firmware.prototype b/boards/mpfs/icicle/firmware.prototype new file mode 100644 index 000000000000..c370b014daf3 --- /dev/null +++ b/boards/mpfs/icicle/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1500, + "magic": "Icicle", + "description": "Firmware for the Icicle board", + "image": "", + "build_time": 0, + "summary": "SSRC/Icicle", + "version": "0.1", + "image_size": 0, + "image_maxsize": 5242880, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job b/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job new file mode 100644 index 000000000000..80f45aacebd0 Binary files /dev/null and b/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN.job differ diff --git a/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest b/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest new file mode 100644 index 000000000000..b7585fad8255 --- /dev/null +++ b/boards/mpfs/icicle/fpga/icicle/MPFS_ICICLE_KIT_BASE_DESIGN_job.digest @@ -0,0 +1,5 @@ +BITS component bitstream digest: aee355e728bed157183d65b414803ca9422e59d42b06365e61ddf1ed5ec41d74 +Security component bitstream digest: d0b05cff123c1dc5e8d169e7592aa10eb08c57ebe80322cbc8e674e466523457 +Fabric component bitstream digest: c86c1db8daa0326a234a8420929ae46b4ecf9442121b36faf3c986486efb01df +sNVM component bitstream digest: aff56888b71e479e97ce28d42cd956ce55c4e225da7fafe92e84c8317e8f043f +EOB component bitstream digest: c7efe4578036a5b394aed06c53b217533bca0b865b16069e1901c282a8d782f5 diff --git a/boards/mpfs/icicle/fpga/icicle/README.md b/boards/mpfs/icicle/fpga/icicle/README.md new file mode 100644 index 000000000000..c2f14b8e6577 --- /dev/null +++ b/boards/mpfs/icicle/fpga/icicle/README.md @@ -0,0 +1 @@ +# FPGA configurations for flying with Microchip PolarFire SoC Icicle Kit + TII connector board diff --git a/boards/mpfs/icicle/include/board_type.h b/boards/mpfs/icicle/include/board_type.h new file mode 100644 index 000000000000..4fe9de57e978 --- /dev/null +++ b/boards/mpfs/icicle/include/board_type.h @@ -0,0 +1,17 @@ +#ifndef _BOARD_TYPE_H_ +#define _BOARD_TYPE_H_ + +#include + +#define BOARD_TYPE 1500 + +#define BOOTLOADER_SIGNING_ALGORITHM CRYPTO_ED25519 +#define BOOTLOADER_VERIFY_UBOOT 0 +#define BOOTLOADER_BOOT_HART_1 0 +#define BOOTLOADER_BOOT_HART_4 0 + +#define TOC_VERIFICATION_KEY 0 +#define BOOT_VERIFICATION_KEY 0 + +#define ARB_COUNT 0 +#endif diff --git a/boards/mpfs/icicle/init/rc.board_defaults b/boards/mpfs/icicle/init/rc.board_defaults new file mode 100644 index 000000000000..fbb4c5527296 --- /dev/null +++ b/boards/mpfs/icicle/init/rc.board_defaults @@ -0,0 +1,15 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +set OUTPUT_MODE pwm_esc +set AUX_MODE none + +pwm_esc start + +param set-default COM_PREARM_MODE 1 +param set-default CBRK_IO_SAFETY 0 +param set-default SYS_DM_BACKEND 1 + +safety_button start diff --git a/boards/mpfs/icicle/init/rc.board_mavlink b/boards/mpfs/icicle/init/rc.board_mavlink new file mode 100644 index 000000000000..8b22ad41837a --- /dev/null +++ b/boards/mpfs/icicle/init/rc.board_mavlink @@ -0,0 +1,4 @@ +#!/bin/sh +# +# PX4 FMUv5 specific board MAVLink startup script. +#------------------------------------------------------------------------------ diff --git a/boards/mpfs/icicle/init/rc.board_paths b/boards/mpfs/icicle/init/rc.board_paths new file mode 100644 index 000000000000..000d68e186f6 --- /dev/null +++ b/boards/mpfs/icicle/init/rc.board_paths @@ -0,0 +1 @@ +set PARAM_FILE /fs/lfs/params diff --git a/boards/mpfs/icicle/init/rc.board_sensors b/boards/mpfs/icicle/init/rc.board_sensors new file mode 100644 index 000000000000..51eb6551b7aa --- /dev/null +++ b/boards/mpfs/icicle/init/rc.board_sensors @@ -0,0 +1,24 @@ +#!/bin/sh +# +# Icicle specific board sensors init +#------------------------------------------------------------------------------ + +#board_adc start + +# Internal I2C bus +bmp388 -I -a 0x76 -f 400 start +bmp388 -I -a 0x77 -f 400 start +bmm150 -I -a 0x10 -f 400 -R 14 start +#bmm150 -I -a 0x11 -f 400 -R 14 start +ads1115 -I start + +# External I2C +ll40ls -X -f 400 start + +# Internal SPI bus +icm42688p -s -f 10000 -R 10 start +icm20649 -s -f 7000 -R 6 start + +# External compass with lights, safety button, and buzzer +ist8310 -X -a 0x0e -b 2 -f 400 -R 10 start +rgbled_ncp5623c -X -b 2 -f 400 start diff --git a/boards/mpfs/icicle/nuttx-config/Kconfig b/boards/mpfs/icicle/nuttx-config/Kconfig new file mode 100644 index 000000000000..ae2bf31307d0 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/Kconfig @@ -0,0 +1,4 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# diff --git a/boards/mpfs/icicle/nuttx-config/bootloader/defconfig b/boards/mpfs/icicle/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..d54b11160402 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/bootloader/defconfig @@ -0,0 +1,91 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_ARCH_FPU is not set +CONFIG_ALLOW_BSD_COMPONENTS=y +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mpfs/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL=y +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=12500 +CONFIG_CDCACM=y +CONFIG_CDCACM_BULKIN_REQLEN=96 +CONFIG_CDCACM_COMPOSITE=y +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_COMPOSITE_IAD=y +CONFIG_COMPOSITE_PRODUCTID=0x05e1 +CONFIG_COMPOSITE_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_COMPOSITE_VENDORID=0x16c0 +CONFIG_COMPOSITE_VENDORSTR="SSRC" +CONFIG_DEBUG_FEATURES=y +CONFIG_DEFAULT_SMALL=y +CONFIG_FS_FAT=y +CONFIG_FS_LARGEFILE=y +CONFIG_GPT_PARTITION=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=8096 +CONFIG_INTELHEX_BINARY=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_BOOTLOADER=y +CONFIG_MPFS_DDR_INIT=y +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_EMMCSD_MUX_EMMC=y +CONFIG_MPFS_HART1_ENTRYPOINT=0x80000000 +CONFIG_MPFS_HART1_SBI=y +CONFIG_MPFS_HART2_ENTRYPOINT=0xA0000000 +CONFIG_MPFS_HART2_SBI=y +CONFIG_MPFS_HART3_ENTRYPOINT=0x80000000 +CONFIG_MPFS_HART3_SBI=y +CONFIG_MPFS_HART4_ENTRYPOINT=0x80000000 +CONFIG_MPFS_HART4_SBI=y +CONFIG_MPFS_IHC_NUTTX_ON_HART1=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART2=1 +CONFIG_MPFS_IHC_NUTTX_ON_HART3=0 +CONFIG_MPFS_IHC_NUTTX_ON_HART4=0 +CONFIG_MPFS_IHC_LINUX_ON_HART1=0 +CONFIG_MPFS_IHC_LINUX_ON_HART2=0 +CONFIG_MPFS_IHC_LINUX_ON_HART3=1 +CONFIG_MPFS_IHC_LINUX_ON_HART4=0 +CONFIG_MPFS_IHC_SBI=y +CONFIG_MPFS_OPENSBI=y +CONFIG_MPFS_UART0=y +CONFIG_OPENSBI=y +CONFIG_OPENSBI_DOMAINS=y +CONFIG_RAM_SIZE=1048576 +CONFIG_RAM_START=0x08000000 +CONFIG_RAW_BINARY=y +CONFIG_SCHED_HPWORK=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_COMPOSITE=y +CONFIG_TASK_NAME_SIZE=20 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_USBDEV=y +CONFIG_USBDEV_COMPOSITE=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USBMSC=y +CONFIG_USBMSC_COMPOSITE=y +CONFIG_USBMSC_IFNOBASE=2 +CONFIG_USBMSC_NRDREQS=16 +CONFIG_USBMSC_NWRREQS=16 +CONFIG_USBMSC_WRMULTIPLE=y +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/mpfs/icicle/nuttx-config/include/board.h b/boards/mpfs/icicle/nuttx-config/include/board.h new file mode 100644 index 000000000000..4f22e221b356 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/include/board.h @@ -0,0 +1,113 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H +#define __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "mpfs_gpio.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Clocking TODO: */ + +#define MPFS_MSS_EXT_SGMII_REF_CLK (125000000UL) +#define MPFS_MSS_COREPLEX_CPU_CLK (600000000UL) +#define MPFS_MSS_SYSTEM_CLK (600000000UL) +#define MPFS_MSS_RTC_TOGGLE_CLK (1000000UL) +#define MPFS_MSS_AXI_CLK (300000000UL) +#define MPFS_MSS_APB_AHB_CLK (150000000UL) +#define MPFS_FPGA_PERIPHERAL_CLK (62500000UL) +#define MPFS_FPGA_BCLK (3000000UL) + +/* LED definitions **********************************************************/ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_LED4 3 +#define BOARD_NLEDS 4 + +#define BOARD_LED_ORANGE1 BOARD_LED1 +#define BOARD_LED_ORANGE2 BOARD_LED2 +#define BOARD_LED_RED1 BOARD_LED3 +#define BOARD_LED_RED2 BOARD_LED4 + +#define LED_STARTED 0 /* LED1 */ +#define LED_HEAPALLOCATE 1 /* LED2 */ +#define LED_IRQSENABLED 2 /* LED1 + LED2 */ +#define LED_STACKCREATED 3 /* LED3 */ +#define LED_INIRQ 4 /* LED3 + LED1 */ +#define LED_SIGNAL 5 /* LED3 + LED2 */ +#define LED_ASSERTION 6 /* LED3 + LED2 + LED1 */ +#define LED_PANIC 7 /* LED4 */ + +/* Button definitions *******************************************************/ + +/* The Icicle supports 3 buttons: */ + +#define BUTTON_USER1 0 +#define BUTTON_USER2 1 +#define BUTTON_USER3 2 +#define NUM_BUTTONS 3 +#define BUTTON_USER1_BIT (1 << BUTTON_USER1) +#define BUTTON_USER2_BIT (1 << BUTTON_USER2) +#define BUTTON_USER3_BIT (1 << BUTTON_USER3) + +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_boardinitialize + ****************************************************************************/ + +void mpfs_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif +#endif /* __ASSEMBLY__ */ +#endif /* __BOARDS_RISCV_ICICLE_MPFS_INCLUDE_BOARD_H */ diff --git a/boards/mpfs/icicle/nuttx-config/include/board_liberodefs.h b/boards/mpfs/icicle/nuttx-config/include/board_liberodefs.h new file mode 100644 index 000000000000..8a94f3c5f061 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/include/board_liberodefs.h @@ -0,0 +1,618 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board_liberodefs.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H +#define __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H + +/* These are board specific constants provided by the vendor. Values have + * been synced from hart-software-services (HSS) with the tag: 2021.08. + * Different memories have their own subset of defines. + */ + +#define LIBERO_SETTING_MSS_EXT_SGMII_REF_CLK 125000000 +#define LIBERO_SETTING_MSS_RTC_TOGGLE_CLK 1000000 +#define LIBERO_SETTING_MSS_ENVM_CR 0x40050006 +#define LIBERO_SETTING_MSS_CLOCK_CONFIG_CR 0x00000024 +#define LIBERO_SETTING_MSS_MSSCLKMUX 0x00000003 +#define LIBERO_SETTING_MSS_PLL_CKMUX 0x00000155 +#define LIBERO_SETTING_MSS_BCLKMUX 0x00000208 +#define LIBERO_SETTING_MSS_FMETER_ADDR 0x00000000 +#define LIBERO_SETTING_MSS_FMETER_DATAW 0x00000000 +#define LIBERO_SETTING_MSS_FMETER_DATAR 0x00000000 +#define LIBERO_SETTING_MSS_PLL_REF_FB 0x00000500 +#define LIBERO_SETTING_MSS_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_MSS_PLL_DIV_0_1 0x06000200 +#define LIBERO_SETTING_MSS_PLL_DIV_2_3 0x0f000600 +#define LIBERO_SETTING_MSS_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_MSS_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_MSS_SSCG_REG_2 0x000000c0 +#define LIBERO_SETTING_MSS_SSCG_REG_3 0x00000001 +#define LIBERO_SETTING_MSS_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_MSS_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_MSS_PLL_PHADJ 0x00004003 + +#define LIBERO_SETTING_CH0_CNTL 0x37f07770 +#define LIBERO_SETTING_CH1_CNTL 0x37f07770 +#define LIBERO_SETTING_SPARE_CNTL 0xff000000 + +#define LIBERO_SETTING_SGMII_SGMII_CLKMUX 0x00000005 +#define LIBERO_SETTING_SGMII_CLK_XCVR 0x00002c30 +#define LIBERO_SETTING_SGMII_REFCLKMUX 0x00000005 +#define LIBERO_SETTING_SGMII_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_SGMII_PLL_REF_FB 0x00000100 +#define LIBERO_SETTING_SGMII_PLL_DIV_0_1 0x01000100 +#define LIBERO_SETTING_SGMII_PLL_DIV_2_3 0x01000100 +#define LIBERO_SETTING_SGMII_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_SGMII_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_SGMII_SSCG_REG_2 0x00000014 +#define LIBERO_SETTING_SGMII_SSCG_REG_3 0x00000001 +#define LIBERO_SETTING_SGMII_PLL_PHADJ 0x00007443 +#define LIBERO_SETTING_SGMII_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_SGMII_MODE 0x08c0e6ff + +#define LIBERO_SETTING_RECAL_CNTL 0x000020c8 +#define LIBERO_SETTING_CLK_CNTL 0xf00050cc +#define LIBERO_SETTING_PLL_CNTL 0x80140101 + +#define LIBERO_SETTING_TRAINING_SKIP_SETTING 0x00000000 + +#define LIBERO_SETTING_DDRPHY_MODE_OFF 0x00000000 +#define LIBERO_SETTING_DPC_BITS_OFF_MODE 0x00000000 +#define LIBERO_SETTING_DDRPHY_MODE 0x00014a24 +#define LIBERO_SETTING_RPC_ODT_DQ 0x00000006 +#define LIBERO_SETTING_RPC_ODT_DQS 0x00000006 +#define LIBERO_SETTING_RPC_ODT_ADDCMD 0x00000002 +#define LIBERO_SETTING_RPC_ODT_CLK 0x00000002 +#define LIBERO_SETTING_RPC_156_VALUE 0x00000001 +#define LIBERO_SETTING_RPC_EN_ADDCMD0_OVRT9 0x00000f00 +#define LIBERO_SETTING_RPC_EN_ADDCMD1_OVRT10 0x00000fff +#define LIBERO_SETTING_RPC_EN_ADDCMD2_OVRT11 0x00000fe6 +#define LIBERO_SETTING_RPC_EN_DATA0_OVRT12 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA1_OVRT13 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA2_OVRT14 0x00000000 +#define LIBERO_SETTING_RPC_EN_DATA3_OVRT15 0x00000000 +#define LIBERO_SETTING_RPC_EN_ECC_OVRT16 0x0000007f +#define LIBERO_SETTING_RPC235_WPD_ADD_CMD0 0x00000000 +#define LIBERO_SETTING_RPC236_WPD_ADD_CMD1 0x00000000 +#define LIBERO_SETTING_RPC237_WPD_ADD_CMD2 0x00000120 +#define LIBERO_SETTING_RPC238_WPD_DATA0 0x00000000 +#define LIBERO_SETTING_RPC239_WPD_DATA1 0x00000000 +#define LIBERO_SETTING_RPC240_WPD_DATA2 0x00000000 +#define LIBERO_SETTING_RPC241_WPD_DATA3 0x00000000 +#define LIBERO_SETTING_RPC242_WPD_ECC 0x00000000 +#define LIBERO_SETTING_RPC243_WPU_ADD_CMD0 0x00000fff +#define LIBERO_SETTING_RPC244_WPU_ADD_CMD1 0x00000fff +#define LIBERO_SETTING_RPC245_WPU_ADD_CMD2 0x00000edf +#define LIBERO_SETTING_RPC246_WPU_DATA0 0x000007ff +#define LIBERO_SETTING_RPC247_WPU_DATA1 0x000007ff +#define LIBERO_SETTING_RPC248_WPU_DATA2 0x000007ff +#define LIBERO_SETTING_RPC249_WPU_DATA3 0x000007ff +#define LIBERO_SETTING_RPC250_WPU_ECC 0x0000007f +#define LIBERO_SETTING_RPC_EN_ADDCMD1_OVRT10 0x00000fff +#define LIBERO_SETTING_RPC_EN_ADDCMD2_OVRT11 0x00000fe6 + +#define LIBERO_SETTING_DDR_SOFT_RESET 0x00000000 +#define LIBERO_SETTING_DDR_PLL_CTRL 0x0100003f +#define LIBERO_SETTING_DDR_PLL_REF_FB 0x00000500 +#define LIBERO_SETTING_DDR_PLL_FRACN 0x00000000 +#define LIBERO_SETTING_DDR_PLL_DIV_0_1 0x02000100 +#define LIBERO_SETTING_DDR_PLL_DIV_2_3 0x01000100 +#define LIBERO_SETTING_DDR_PLL_CTRL2 0x00001020 +#define LIBERO_SETTING_DDR_PLL_CAL 0x00000d06 +#define LIBERO_SETTING_DDR_PLL_PHADJ 0x00005003 +#define LIBERO_SETTING_DDR_SSCG_REG_0 0x00000000 +#define LIBERO_SETTING_DDR_SSCG_REG_1 0x00000000 +#define LIBERO_SETTING_DDR_SSCG_REG_2 0x00000080 +#define LIBERO_SETTING_DDR_SSCG_REG_3 0x00000001 + +#define LIBERO_SETTING_CFG_MANUAL_ADDRESS_MAP 0x00000000 +#define LIBERO_SETTING_CFG_CHIPADDR_MAP 0x0000001d +#define LIBERO_SETTING_CFG_CIDADDR_MAP 0x00000000 +#define LIBERO_SETTING_CFG_BANKADDR_MAP_0 0x0000c2ca +#define LIBERO_SETTING_CFG_BANKADDR_MAP_1 0x00000000 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_0 0x9140f38d +#define LIBERO_SETTING_CFG_ROWADDR_MAP_1 0x75955134 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_2 0x71b69961 +#define LIBERO_SETTING_CFG_ROWADDR_MAP_3 0x00000000 +#define LIBERO_SETTING_CFG_COLADDR_MAP_0 0x440c2040 +#define LIBERO_SETTING_CFG_COLADDR_MAP_1 0x02481c61 +#define LIBERO_SETTING_CFG_COLADDR_MAP_2 0x00000000 +#define LIBERO_SETTING_CFG_VRCG_ENABLE 0x00000140 +#define LIBERO_SETTING_CFG_VRCG_DISABLE 0x000000a0 +#define LIBERO_SETTING_CFG_WRITE_LATENCY_SET 0x00000000 + +#define LIBERO_SETTING_CFG_THERMAL_OFFSET 0x00000000 +#define LIBERO_SETTING_CFG_SOC_ODT 0x00000006 +#define LIBERO_SETTING_CFG_ODTE_CK 0x00000000 +#define LIBERO_SETTING_CFG_ODTE_CS 0x00000000 +#define LIBERO_SETTING_CFG_ODTD_CA 0x00000000 +#define LIBERO_SETTING_CFG_LPDDR4_FSP_OP 0x00000001 +#define LIBERO_SETTING_CFG_DBI_CL 0x00000016 +#define LIBERO_SETTING_CFG_NON_DBI_CL 0x00000016 +#define LIBERO_SETTING_CFG_WRITE_CRC 0x00000000 +#define LIBERO_SETTING_CFG_MPR_READ_FORMAT 0x00000000 +#define LIBERO_SETTING_CFG_WR_CMD_LAT_CRC_DM 0x00000000 +#define LIBERO_SETTING_CFG_FINE_GRAN_REF_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TEMP_SENSOR_READOUT 0x00000000 +#define LIBERO_SETTING_CFG_PER_DRAM_ADDR_EN 0x00000000 +#define LIBERO_SETTING_CFG_GEARDOWN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_WR_PREAMBLE 0x00000001 +#define LIBERO_SETTING_CFG_RD_PREAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_RD_PREAMB_TRN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_SR_ABORT 0x00000000 + +#define LIBERO_SETTING_CFG_INT_VREF_MON 0x00000000 +#define LIBERO_SETTING_CFG_MAX_PWR_DOWN_MODE 0x00000000 +#define LIBERO_SETTING_CFG_READ_DBI 0x00000000 +#define LIBERO_SETTING_CFG_WRITE_DBI 0x00000000 +#define LIBERO_SETTING_CFG_DATA_MASK 0x00000001 +#define LIBERO_SETTING_CFG_RTT_PARK 0x00000000 +#define LIBERO_SETTING_CFG_ODT_INBUF_4_PD 0x00000000 +#define LIBERO_SETTING_CFG_CCD_S 0x00000005 +#define LIBERO_SETTING_CFG_CCD_L 0x00000006 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_RANGE 0x00000000 +#define LIBERO_SETTING_CFG_VREFDQ_TRN_VALUE 0x00000000 +#define LIBERO_SETTING_CFG_RRD_S 0x00000004 +#define LIBERO_SETTING_CFG_RRD_L 0x00000003 +#define LIBERO_SETTING_CFG_WTR_S 0x00000003 +#define LIBERO_SETTING_CFG_WTR_L 0x00000003 +#define LIBERO_SETTING_CFG_WTR_S_CRC_DM 0x00000003 +#define LIBERO_SETTING_CFG_WTR_L_CRC_DM 0x00000003 +#define LIBERO_SETTING_CFG_WR_CRC_DM 0x00000006 +#define LIBERO_SETTING_CFG_RFC1 0x00000036 +#define LIBERO_SETTING_CFG_RFC2 0x00000036 +#define LIBERO_SETTING_CFG_RFC4 0x00000036 +#define LIBERO_SETTING_CFG_NIBBLE_DEVICES 0x00000000 + +#define LIBERO_SETTING_CFG_MB_AUTOPCH_COL_BIT_POS_LOW 0x00000004 +#define LIBERO_SETTING_CFG_MB_AUTOPCH_COL_BIT_POS_HIGH 0x0000000a +#define LIBERO_SETTING_CFG_CA_PARITY_ERR_STATUS 0x00000000 +#define LIBERO_SETTING_CFG_CRC_ERROR_CLEAR 0x00000000 +#define LIBERO_SETTING_CFG_CA_PARITY_LATENCY 0x00000000 +#define LIBERO_SETTING_CFG_CA_PARITY_PERSIST_ERR 0x00000000 +#define LIBERO_SETTING_CFG_GENERATE_REFRESH_ON_SRX 0x00000001 +#define LIBERO_SETTING_CFG_CS_TO_CMDADDR_LATENCY 0x00000000 + +#define LIBERO_SETTING_CFG_TEMP_CTRL_REF_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TEMP_CTRL_REF_RANGE 0x00000000 + +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS0_0 0x81881881 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS0_1 0x00008818 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS1_0 0xa92a92a9 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS1_1 0x00002a92 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS2_0 0xc28c28c2 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS2_1 0x00008c28 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS3_0 0xea2ea2ea +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS3_1 0x00002ea2 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS4_0 0x03903903 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS4_1 0x00009039 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS5_0 0x2b32b32b +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS5_1 0x000032b3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS6_0 0x44944944 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS6_1 0x00009449 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS7_0 0x6c36c36c +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS7_1 0x000036c3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS8_0 0x85985985 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS8_1 0x00009859 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS9_0 0xad3ad3ad +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS9_1 0x00003ad3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS10_0 0xc69c69c6 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS10_1 0x00009c69 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS11_0 0xee3ee3ee +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS11_1 0x00003ee3 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS12_0 0x07a07a07 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS12_1 0x0000a07a +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS13_0 0x2f42f42f +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS13_1 0x000042f4 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS14_0 0x48a48a48 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS14_1 0x0000a48a +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS15_0 0x70470470 +#define LIBERO_SETTING_CFG_BIT_MAP_INDEX_CS15_1 0x00004704 + +#define LIBERO_SETTING_CFG_NUM_LOGICAL_RANKS_PER_3DS 0x00000000 +#define LIBERO_SETTING_CFG_ADVANCE_ACTIVATE_READY 0x00000000 + +#define LIBERO_SETTING_CFG_RFC_DLR1 0x00000048 +#define LIBERO_SETTING_CFG_RFC_DLR2 0x0000002c +#define LIBERO_SETTING_CFG_RFC_DLR4 0x00000020 +#define LIBERO_SETTING_CFG_RRD_DLR 0x00000004 +#define LIBERO_SETTING_CFG_FAW_DLR 0x00000010 +#define LIBERO_SETTING_CTRLR_SOFT_RESET_N 0x00000001 +#define LIBERO_SETTING_CFG_LOOKAHEAD_PCH 0x00000000 +#define LIBERO_SETTING_CFG_LOOKAHEAD_ACT 0x00000000 +#define LIBERO_SETTING_CFG_BL 0x00000000 +#define LIBERO_SETTING_CTRLR_INIT 0x00000000 +#define LIBERO_SETTING_CFG_AUTO_REF_EN 0x00000001 +#define LIBERO_SETTING_CFG_RAS 0x00000022 +#define LIBERO_SETTING_CFG_RCD 0x0000000f +#define LIBERO_SETTING_CFG_RRD 0x00000008 +#define LIBERO_SETTING_CFG_RP 0x00000011 +#define LIBERO_SETTING_CFG_RC 0x00000033 +#define LIBERO_SETTING_CFG_FAW 0x00000020 +#define LIBERO_SETTING_CFG_RFC 0x00000130 +#define LIBERO_SETTING_CFG_RTP 0x00000008 +#define LIBERO_SETTING_CFG_WR 0x00000010 +#define LIBERO_SETTING_CFG_WTR 0x00000008 +#define LIBERO_SETTING_CFG_PASR 0x00000000 +#define LIBERO_SETTING_CFG_XP 0x00000006 +#define LIBERO_SETTING_CFG_XSR 0x0000001f +#define LIBERO_SETTING_CFG_CL 0x00000005 +#define LIBERO_SETTING_CFG_READ_TO_WRITE 0x0000000f +#define LIBERO_SETTING_CFG_WRITE_TO_WRITE 0x0000000f +#define LIBERO_SETTING_CFG_READ_TO_READ 0x0000000f +#define LIBERO_SETTING_CFG_WRITE_TO_READ 0x0000001f +#define LIBERO_SETTING_CFG_READ_TO_WRITE_ODT 0x00000001 +#define LIBERO_SETTING_CFG_WRITE_TO_WRITE_ODT 0x00000000 +#define LIBERO_SETTING_CFG_READ_TO_READ_ODT 0x00000001 +#define LIBERO_SETTING_CFG_WRITE_TO_READ_ODT 0x00000001 +#define LIBERO_SETTING_CFG_MIN_READ_IDLE 0x00000007 +#define LIBERO_SETTING_CFG_MRD 0x0000000c +#define LIBERO_SETTING_CFG_BT 0x00000000 +#define LIBERO_SETTING_CFG_DS 0x00000006 +#define LIBERO_SETTING_CFG_QOFF 0x00000000 +#define LIBERO_SETTING_CFG_RTT 0x00000002 +#define LIBERO_SETTING_CFG_DLL_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_REF_PER 0x00000c34 +#define LIBERO_SETTING_CFG_STARTUP_DELAY 0x00027100 +#define LIBERO_SETTING_CFG_MEM_COLBITS 0x0000000a +#define LIBERO_SETTING_CFG_MEM_ROWBITS 0x00000010 +#define LIBERO_SETTING_CFG_MEM_BANKBITS 0x00000003 + +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS0 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS1 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS2 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS3 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS4 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS5 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS6 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_MAP_CS7 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS0 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS1 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS2 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS3 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS4 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS5 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS6 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_MAP_CS7 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_TURN_ON 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_TURN_ON 0x00000000 +#define LIBERO_SETTING_CFG_ODT_RD_TURN_OFF 0x00000000 +#define LIBERO_SETTING_CFG_ODT_WR_TURN_OFF 0x00000000 + +#define LIBERO_SETTING_CFG_EMR3 0x00000000 +#define LIBERO_SETTING_CFG_TWO_T 0x00000000 +#define LIBERO_SETTING_CFG_TWO_T_SEL_CYCLE 0x00000001 +#define LIBERO_SETTING_CFG_REGDIMM 0x00000000 +#define LIBERO_SETTING_CFG_MOD 0x0000000c +#define LIBERO_SETTING_CFG_XS 0x00000005 +#define LIBERO_SETTING_CFG_XSDLL 0x00000200 +#define LIBERO_SETTING_CFG_XPR 0x00000005 +#define LIBERO_SETTING_CFG_AL_MODE 0x00000000 +#define LIBERO_SETTING_CFG_CWL 0x00000005 +#define LIBERO_SETTING_CFG_BL_MODE 0x00000000 +#define LIBERO_SETTING_CFG_TDQS 0x00000000 +#define LIBERO_SETTING_CFG_RTT_WR 0x00000000 +#define LIBERO_SETTING_CFG_LP_ASR 0x00000000 +#define LIBERO_SETTING_CFG_AUTO_SR 0x00000000 +#define LIBERO_SETTING_CFG_SRT 0x00000000 +#define LIBERO_SETTING_CFG_ADDR_MIRROR 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_TYPE 0x00000001 +#define LIBERO_SETTING_CFG_ZQ_CAL_PER 0x00027100 +#define LIBERO_SETTING_CFG_AUTO_ZQ_CAL_EN 0x00000000 +#define LIBERO_SETTING_CFG_MEMORY_TYPE 0x00000400 +#define LIBERO_SETTING_CFG_ONLY_SRANK_CMDS 0x00000000 +#define LIBERO_SETTING_CFG_NUM_RANKS 0x00000001 +#define LIBERO_SETTING_CFG_QUAD_RANK 0x00000000 +#define LIBERO_SETTING_CFG_EARLY_RANK_TO_WR_START 0x00000000 +#define LIBERO_SETTING_CFG_EARLY_RANK_TO_RD_START 0x00000000 +#define LIBERO_SETTING_CFG_PASR_BANK 0x00000000 +#define LIBERO_SETTING_CFG_PASR_SEG 0x00000000 +#define LIBERO_SETTING_CFG_INIT_DURATION 0x00000640 +#define LIBERO_SETTING_CFG_ZQINIT_CAL_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_L_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_S_DURATION 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_R_DURATION 0x00000028 +#define LIBERO_SETTING_CFG_MRR 0x00000008 +#define LIBERO_SETTING_CFG_MRW 0x0000000a +#define LIBERO_SETTING_CFG_ODT_POWERDOWN 0x00000000 +#define LIBERO_SETTING_CFG_WL 0x00000008 +#define LIBERO_SETTING_CFG_RL 0x0000000e +#define LIBERO_SETTING_CFG_CAL_READ_PERIOD 0x00000000 +#define LIBERO_SETTING_CFG_NUM_CAL_READS 0x00000001 + +#define LIBERO_SETTING_INIT_FORCE_WRITE_DATA_0 0x00000000 +#define LIBERO_SETTING_INIT_AUTOINIT_DISABLE 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_RESET 0x00000000 +#define LIBERO_SETTING_INIT_GEARDOWN_EN 0x00000000 +#define LIBERO_SETTING_INIT_DISABLE_CKE 0x00000000 +#define LIBERO_SETTING_INIT_CS 0x00000000 +#define LIBERO_SETTING_INIT_PRECHARGE_ALL 0x00000000 +#define LIBERO_SETTING_INIT_REFRESH 0x00000000 +#define LIBERO_SETTING_INIT_ZQ_CAL_REQ 0x00000000 +#define LIBERO_SETTING_INIT_MRR_MODE 0x00000000 +#define LIBERO_SETTING_INIT_MR_W_REQ 0x00000000 +#define LIBERO_SETTING_INIT_MR_ADDR 0x00000000 +#define LIBERO_SETTING_INIT_MR_WR_DATA 0x00000000 +#define LIBERO_SETTING_INIT_MR_WR_MASK 0x00000000 +#define LIBERO_SETTING_INIT_NOP 0x00000000 +#define LIBERO_SETTING_INIT_SELF_REFRESH 0x00000000 +#define LIBERO_SETTING_INIT_POWER_DOWN 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_WRITE 0x00000000 +#define LIBERO_SETTING_INIT_FORCE_WRITE_CS 0x00000000 +#define LIBERO_SETTING_INIT_RDIMM_COMPLETE 0x00000000 +#define LIBERO_SETTING_INIT_MEMORY_RESET_MASK 0x00000000 +#define LIBERO_SETTING_INIT_CAL_SELECT 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_R_REQ 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_B_SIZE 0x00000000 +#define LIBERO_SETTING_INIT_RWFIFO 0x00000000 +#define LIBERO_SETTING_INIT_RD_DQCAL 0x00000000 +#define LIBERO_SETTING_INIT_START_DQSOSC 0x00000000 +#define LIBERO_SETTING_INIT_STOP_DQSOSC 0x00000000 +#define LIBERO_SETTING_INIT_ZQ_CAL_START 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_ADDR_0 0x00000000 +#define LIBERO_SETTING_INIT_CAL_L_ADDR_1 0x00000000 +#define LIBERO_SETTING_INIT_ODT_FORCE_EN 0x00000000 +#define LIBERO_SETTING_INIT_ODT_FORCE_RANK 0x00000000 +#define LIBERO_SETTING_INIT_PDA_MR_W_REQ 0x00000000 +#define LIBERO_SETTING_INIT_PDA_NIBBLE_SELECT 0x00000000 +#define LIBERO_SETTING_INIT_WRITE_DATA_1B_ECC_ERROR_GEN 0x00000000 +#define LIBERO_SETTING_INIT_WRITE_DATA_2B_ECC_ERROR_GEN 0x00000000 +#define LIBERO_SETTING_INIT_READ_CAPTURE_ADDR 0x00000000 +#define LIBERO_SETTING_INIT_CA_PARITY_ERROR_GEN_REQ 0x00000000 +#define LIBERO_SETTING_INIT_CA_PARITY_ERROR_GEN_CMD 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_DATA_REQ 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_CTRL_REQ 0x00000000 +#define LIBERO_SETTING_INIT_DFI_LP_WAKEUP 0x00000000 +#define LIBERO_SETTING_INIT_DFI_DRAM_CLK_DISABLE 0x00000000 + +#define LIBERO_SETTING_CFG_CTRLR_INIT_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_RDIMM_LAT 0x00000000 +#define LIBERO_SETTING_CFG_RDIMM_BSIDE_INVERT 0x00000001 +#define LIBERO_SETTING_CFG_LRDIMM 0x00000000 +#define LIBERO_SETTING_CFG_RD_PREAMB_TOGGLE 0x00000000 +#define LIBERO_SETTING_CFG_RD_POSTAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_PU_CAL 0x00000001 +#define LIBERO_SETTING_CFG_DQ_ODT 0x00000002 +#define LIBERO_SETTING_CFG_CA_ODT 0x00000004 +#define LIBERO_SETTING_CFG_ZQLATCH_DURATION 0x00000018 + +#define LIBERO_SETTING_CFG_WR_POSTAMBLE 0x00000000 +#define LIBERO_SETTING_CFG_CTRLUPD_TRIG 0x00000000 +#define LIBERO_SETTING_CFG_CTRLUPD_START_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_CTRLUPD_MAX 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_SEL 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_VALUE 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_TURN_OFF_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_SLOW_RESTART_WIN 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_RESTART_HOLDOFF 0x00000000 +#define LIBERO_SETTING_CFG_PARITY_RDIMM_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_CTRLR_BUSY_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_ASYNC_ODT 0x00000000 +#define LIBERO_SETTING_CFG_ZQ_CAL_DURATION 0x00000320 +#define LIBERO_SETTING_CFG_MRRI 0x00000012 +#define LIBERO_SETTING_CFG_PHYUPD_ACK_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_MIRROR_X16_BG0_BG1 0x00000000 +#define LIBERO_SETTING_CFG_DRAM_CLK_DISABLE_IN_SELF_RFH 0x00000000 +#define LIBERO_SETTING_CFG_CKSRE 0x00000008 +#define LIBERO_SETTING_CFG_CKSRX 0x0000000b +#define LIBERO_SETTING_CFG_RCD_STAB 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_CTRL_DELAY 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_DRAM_CLK_ENABLE 0x00000000 +#define LIBERO_SETTING_CFG_IDLE_TIME_TO_SELF_REFRESH 0x00000000 +#define LIBERO_SETTING_CFG_IDLE_TIME_TO_POWER_DOWN 0x00000000 +#define LIBERO_SETTING_CFG_BURST_RW_REFRESH_HOLDOFF 0x00000000 +#define LIBERO_SETTING_CFG_BG_INTERLEAVE 0x00000001 +#define LIBERO_SETTING_CFG_REFRESH_DURING_PHY_TRAINING 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P0 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P1 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P2 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P3 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P4 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P5 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P6 0x00000000 +#define LIBERO_SETTING_CFG_STARVE_TIMEOUT_P7 0x00000000 +#define LIBERO_SETTING_CFG_REORDER_EN 0x00000001 +#define LIBERO_SETTING_CFG_REORDER_QUEUE_EN 0x00000001 +#define LIBERO_SETTING_CFG_INTRAPORT_REORDER_EN 0x00000000 +#define LIBERO_SETTING_CFG_MAINTAIN_COHERENCY 0x00000001 +#define LIBERO_SETTING_CFG_Q_AGE_LIMIT 0x000000ff +#define LIBERO_SETTING_CFG_RO_CLOSED_PAGE_POLICY 0x00000000 +#define LIBERO_SETTING_CFG_REORDER_RW_ONLY 0x00000000 +#define LIBERO_SETTING_CFG_RO_PRIORITY_EN 0x00000000 +#define LIBERO_SETTING_CFG_DM_EN 0x00000001 +#define LIBERO_SETTING_CFG_RMW_EN 0x00000000 +#define LIBERO_SETTING_CFG_ECC_CORRECTION_EN 0x00000000 +#define LIBERO_SETTING_CFG_ECC_BYPASS 0x00000000 +#define LIBERO_SETTING_CFG_ECC_1BIT_INT_THRESH 0x00000000 +#define LIBERO_SETTING_CFG_ERROR_GROUP_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DATA_SEL 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MODE 0x00000000 +#define LIBERO_SETTING_CFG_POST_TRIG_CYCS 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MASK 0x00000000 +#define LIBERO_SETTING_CFG_EN_MASK 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MT_ADDR_0 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_MT_ADDR_1 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_0 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_1 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_2 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_3 0x00000000 +#define LIBERO_SETTING_CFG_TRIG_ERR_MASK_4 0x00000000 + +#define LIBERO_SETTING_MTC_ACQ_ADDR 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_0 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_1 0x00000000 +#define LIBERO_SETTING_MTC_ACQ_WR_DATA_2 0x00000000 + +#define LIBERO_SETTING_CFG_PRE_TRIG_CYCS 0x00000000 +#define LIBERO_SETTING_CFG_DATA_SEL_FIRST_ERROR 0x00000000 +#define LIBERO_SETTING_CFG_DQ_WIDTH 0x00000000 +#define LIBERO_SETTING_CFG_ACTIVE_DQ_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DFI_T_RDDATA_EN 0x00000015 +#define LIBERO_SETTING_CFG_DFI_T_PHY_RDLAT 0x00000006 +#define LIBERO_SETTING_CFG_DFI_T_PHY_WRLAT 0x00000003 +#define LIBERO_SETTING_CFG_DFI_PHYUPD_EN 0x00000001 +#define LIBERO_SETTING_CFG_DFI_DATA_BYTE_DISABLE 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_SEL 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_PERIODIC 0x00000000 +#define LIBERO_SETTING_CFG_DFI_LVL_PATTERN 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI1_0 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI2_0 0x00000000 +#define LIBERO_SETTING_CFG_AXI_START_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI1_0 0x7fffffff +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI2_0 0x7fffffff +#define LIBERO_SETTING_CFG_AXI_END_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI1_0 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI1_1 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI2_0 0x00000000 +#define LIBERO_SETTING_CFG_MEM_START_ADDRESS_AXI2_1 0x00000000 +#define LIBERO_SETTING_CFG_ENABLE_BUS_HOLD_AXI1 0x00000000 +#define LIBERO_SETTING_CFG_ENABLE_BUS_HOLD_AXI2 0x00000000 +#define LIBERO_SETTING_CFG_AXI_AUTO_PCH 0x00000000 + +#define LIBERO_SETTING_PHY_DFI_INIT_START 0x00000001 +#define LIBERO_SETTING_PHY_RESET_CONTROL 0x00008001 +#define LIBERO_SETTING_PHY_PC_RANK 0x00000001 +#define LIBERO_SETTING_PHY_RANKS_TO_TRAIN 0x00000001 +#define LIBERO_SETTING_PHY_WRITE_REQUEST 0x00000000 +#define LIBERO_SETTING_PHY_READ_REQUEST 0x00000000 +#define LIBERO_SETTING_PHY_WRITE_LEVEL_DELAY 0x00000000 +#define LIBERO_SETTING_PHY_GATE_TRAIN_DELAY 0x0000003f +#define LIBERO_SETTING_PHY_EYE_TRAIN_DELAY 0x0000003f +#define LIBERO_SETTING_PHY_EYE_PAT 0x00000000 +#define LIBERO_SETTING_PHY_START_RECAL 0x00000000 +#define LIBERO_SETTING_PHY_CLR_DFI_LVL_PERIODIC 0x00000000 +#define LIBERO_SETTING_PHY_TRAIN_STEP_ENABLE 0x00000018 +#define LIBERO_SETTING_PHY_LPDDR_DQ_CAL_PAT 0x00000000 +#define LIBERO_SETTING_PHY_INDPNDT_TRAINING 0x00000001 +#define LIBERO_SETTING_PHY_ENCODED_QUAD_CS 0x00000000 +#define LIBERO_SETTING_PHY_HALF_CLK_DLY_ENABLE 0x00000000 + +#define LIBERO_SETTING_SEG0_0 0x80007f80 +#define LIBERO_SETTING_SEG0_1 0x80007030 +#define LIBERO_SETTING_SEG0_2 0x00000000 +#define LIBERO_SETTING_SEG0_3 0x00000000 +#define LIBERO_SETTING_SEG0_4 0x00000000 +#define LIBERO_SETTING_SEG0_5 0x00000000 +#define LIBERO_SETTING_SEG0_6 0x00000000 +#define LIBERO_SETTING_SEG0_7 0x00000000 +#define LIBERO_SETTING_SEG1_0 0x00000000 +#define LIBERO_SETTING_SEG1_1 0x00000000 +#define LIBERO_SETTING_SEG1_2 0x80007fb0 +#define LIBERO_SETTING_SEG1_3 0x80000000 +#define LIBERO_SETTING_SEG1_4 0x80007fa0 +#define LIBERO_SETTING_SEG1_5 0x80000000 +#define LIBERO_SETTING_SEG1_6 0x00000000 +#define LIBERO_SETTING_SEG1_7 0x00000000 + +#define LIBERO_SETTING_TIP_CONFIG_PARAMS_BCLK_VCOPHS_OFFSET 0x00000002 +#define LIBERO_SETTING_DDR_CLK 1600000000 + +#define LIBERO_SETTING_REFCLK_DDR3_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR4_1600_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_NUM_OFFSETS 4 +#define LIBERO_SETTING_REFCLK_DDR3_1067_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_DDR4_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_NUM_OFFSETS 2 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_NUM_OFFSETS 3 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3_1333_OFFSET_3 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_0 7 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_1 0 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_2 7 +#define LIBERO_SETTING_REFCLK_DDR4_1600_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_0 7 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_1 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_2 1 +#define LIBERO_SETTING_REFCLK_LPDDR3_1600_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_1 5 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_2 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1600_OFFSET_3 5 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3_1067_OFFSET_3 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_2 0 +#define LIBERO_SETTING_REFCLK_DDR3L_1067_OFFSET_3 2 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_2 7 +#define LIBERO_SETTING_REFCLK_DDR4_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_0 0 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_1 1 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_2 6 +#define LIBERO_SETTING_REFCLK_LPDDR3_1333_OFFSET_3 0 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_0 1 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_1 2 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_2 3 +#define LIBERO_SETTING_REFCLK_LPDDR4_1333_OFFSET_3 0 + +#define LIBERO_SETTING_TIP_CFG_PARAMS 0x07cfe02f + +#define LIBERO_SETTING_DDR_32_CACHE 0x80000000 +#define LIBERO_SETTING_DDR_32_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_64_CACHE 0x1000000000 +#define LIBERO_SETTING_DDR_64_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_32_NON_CACHE 0xc0000000 +#define LIBERO_SETTING_DDR_32_NON_CACHE_SIZE 0x100000 +#define LIBERO_SETTING_DDR_64_NON_CACHE 0x1400000000 +#define LIBERO_SETTING_DDR_64_NON_CACHE_SIZE 0x100000 + +#define LIBERO_SETTING_DPC_BITS 0x00050422 +#define LIBERO_SETTING_DATA_LANES_USED 0x00000004 + +/* Cache settings */ + +#define LIBERO_SETTING_WAY_MASK_DMA 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_0 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_1 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_2 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_AXI4_PORT_3 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_E51_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_E51_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_1_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_1_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_2_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_2_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_3_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_3_ICACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_4_DCACHE 0x0000f0ff +#define LIBERO_SETTING_WAY_MASK_U54_4_ICACHE 0x0000f0ff +#define LIBERO_SETTING_NUM_SCRATCH_PAD_WAYS 0x00000004 +#define LIBERO_SETTING_L2_SHUTDOWN_CR 0x00000000 +#define LIBERO_SETTING_WAY_ENABLE 0x0000000b + +#endif /* __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_LIBERODEFS_H */ diff --git a/boards/mpfs/icicle/nuttx-config/include/board_memorymap.h b/boards/mpfs/icicle/nuttx-config/include/board_memorymap.h new file mode 100644 index 000000000000..0f0f92e3a9de --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/include/board_memorymap.h @@ -0,0 +1,108 @@ +/**************************************************************************** + * boards/risc-v/mpfs/icicle/include/board_memorymap.h + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#ifndef __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H +#define __BOARDS_RISCV_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* DDR start address */ + +#define MPFS_DDR_BASE (0x80000000) +#define MPFS_DDR_SIZE (0x40000000) + +/* Kernel code memory (RX) */ + +#define KFLASH_START (uintptr_t)&__kflash_start +#define KFLASH_SIZE (uintptr_t)&__kflash_size +#define KSRAM_START (uintptr_t)&__ksram_start +#define KSRAM_SIZE (uintptr_t)&__ksram_size +#define KSRAM_END (uintptr_t)&__ksram_end + +/* Kernel RAM (RW) */ + +#define PGPOOL_START (uintptr_t)&__pgheap_start +#define PGPOOL_SIZE (uintptr_t)&__pgheap_size + +/* Page pool (RWX) */ + +#define PGPOOL_START (uintptr_t)&__pgheap_start +#define PGPOOL_SIZE (uintptr_t)&__pgheap_size +#define PGPOOL_END (PGPOOL_START + PGPOOL_SIZE) + +/* User flash */ + +#define UFLASH_START (uintptr_t)&__uflash_start +#define UFLASH_SIZE (uintptr_t)&__uflash_size + +/* User RAM */ + +#define USRAM_START (uintptr_t)&__usram_start +#define USRAM_SIZE (uintptr_t)&__usram_size + +/* User IO */ + +#define USRIO_START (uintptr_t)&__usrio_start +#define USRIO_SIZE (uintptr_t)&__usrio_size + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* Kernel code memory (RX) */ + +extern uintptr_t __kflash_start; +extern uintptr_t __kflash_size; + +/* Kernel RAM (RW) */ + +extern uintptr_t __ksram_start; +extern uintptr_t __ksram_size; +extern uintptr_t __ksram_end; + +/* Page pool (RWX) */ + +extern uintptr_t __pgheap_start; +extern uintptr_t __pgheap_size; + +/* User code memory (RX) */ + +extern uintptr_t __uflash_start; +extern uintptr_t __uflash_size; + +/* User RAM (RW) */ + +extern uintptr_t __usram_start; +extern uintptr_t __usram_size; + +/* User IO (R) */ + +extern uintptr_t __usrio_start; +extern uintptr_t __usrio_size; + +#endif /* __BOARDS_RISC_V_MPFS_ICICLE_INCLUDE_BOARD_MEMORYMAP_H */ diff --git a/boards/mpfs/icicle/nuttx-config/nsh/defconfig b/boards/mpfs/icicle/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..d2c7a035a157 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/nsh/defconfig @@ -0,0 +1,179 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_FS_ROMFS_CACHE_NODE is not set +# CONFIG_MPFS_MAC_AUTONEG is not set +CONFIG_ARCH="risc-v" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/mpfs/icicle/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="mpfs" +CONFIG_ARCH_CHIP_MPFS250T_FCVG484=y +CONFIG_ARCH_CHIP_MPFS=y +CONFIG_ARCH_INTERRUPTSTACK=2048 +CONFIG_ARCH_RISCV=y +CONFIG_ARCH_STACKDUMP=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_LOOPSPERMSEC=54000 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_EPBULKIN=3 +CONFIG_CDCACM_EPINTIN=4 +CONFIG_CDCACM_PRODUCTID=0x05e1 +CONFIG_CDCACM_PRODUCTSTR="SSRC Icicle v1.0" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x16c0 +CONFIG_CDCACM_VENDORSTR="SSRC" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEV_ZERO=y +CONFIG_DISABLE_MQUEUE=y +CONFIG_DISABLE_POSIX_TIMERS=y +CONFIG_EE24XX_FREQUENCY=400000 +CONFIG_EEPROM=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_EXCLUDE_ENVIRON=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_DRIVER=y +CONFIG_I2C_EE_24XX=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=2048 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=8192 +CONFIG_INTELHEX_BINARY=y +CONFIG_LIBC_NETDB=y +CONFIG_LIBC_STRERROR=y +CONFIG_M25P_SUBSECTOR_ERASE=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MPFS_COREPWM0=y +CONFIG_MPFS_COREPWM1=y +CONFIG_MPFS_COREPWM1_NCHANNELS=1 +CONFIG_MPFS_EMMCSD=y +CONFIG_MPFS_ENABLE_DPFPU=y +CONFIG_MPFS_ETHMAC_0=y +CONFIG_MPFS_HAVE_COREPWM=y +CONFIG_MPFS_I2C0=y +CONFIG_MPFS_I2C1=y +CONFIG_MPFS_MAC_ETHFD=y +CONFIG_MPFS_PHYADDR=9 +CONFIG_MPFS_PHYINIT=y +CONFIG_MPFS_SPI0=y +CONFIG_MPFS_SPI1=y +CONFIG_MPFS_UART0=y +CONFIG_MPFS_UART1=y +CONFIG_MPFS_UART2=y +CONFIG_MPFS_UART3=y +CONFIG_MPFS_UART4=y +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_M25P=y +CONFIG_MTD_PARTITION=y +CONFIG_NAME_MAX=48 +CONFIG_NET=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DRIPADDR=0xc0a8c864 +CONFIG_NETINIT_IPADDR=0xc0a8c865 +CONFIG_NETINIT_MACADDR_1=0x554b4b41 +CONFIG_NETINIT_MACADDR_2=0x024a +CONFIG_NETINIT_NOMAC=y +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_PROMPT_STRING="icicle> " +CONFIG_NSH_READLINE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTABLE_PARTITION=y +CONFIG_PTHREAD_STACK_MIN=1024 +CONFIG_PWM=y +CONFIG_PWM_NCHANNELS=10 +CONFIG_RAM_SIZE=2097152 +CONFIG_RAM_START=0xA0600000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RR_INTERVAL=200 +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=4096 +CONFIG_SCHED_LPNTHREADS=2 +CONFIG_SCHED_WAITPID=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_NPOLLWAITERS=2 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI_DRIVER=y +CONFIG_STACK_COLORATION=y +CONFIG_START_MONTH=4 +CONFIG_START_YEAR=2021 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_MDIO=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_NSH_STACKSIZE=2304 +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART0_SERIAL_CONSOLE=y +CONFIG_UART1_RXBUFSIZE=600 +CONFIG_UART1_TXBUFSIZE=1500 +CONFIG_UART3_RXBUFSIZE=600 +CONFIG_UART3_TXBUFSIZE=3000 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_DUALSPEED=y +CONFIG_USBDEV_MAXPOWER=500 diff --git a/boards/mpfs/icicle/nuttx-config/scripts/bootloader_script.ld b/boards/mpfs/icicle/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..a1711e2f7d4a --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,201 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + + +/*----------------------------------------------------------------------------- + + -- PolarFire SoC Memorp map + + + 0x0800_0000 +-----------------------+ + | DTIM Len=8K | + +-----------------------+ + + 0x0180_0000 +-----------------------+ + | ITIM Hart 0 | + +-----------------------+ + eNVM detail + 0x0180_8000 +-----------------------+ +-------------------+ - + | ITIM Hart 1 Len=28K | |Sector 2 | ^ + +-----------------------+ |0x2022_0000 8K | | + +-------------------+ | + 0x0181_0000 +-----------------------+ |Sector 0 | 1 + | ITIM Hart 2 Len=28K | |0x2022_2000 56K | 2 + +-----------------------+ +-------------------+ 8 + |Sector 1 | k + 0x0181_8000 +-----------------------+ |0x2022_3000 56K | | + | ITIM Hart 3 Len=28K | +-------------------+ | + +-----------------------+ |Sector 3 | | + |0x2022_3E00 8K | v + 0x0182_0000 +-----------------------+ +-------------------+ - + | ITIM Hart 4 Len=28K | + +-----------------------+ + + 0x0800_0000 +-----------------------+ + | LIM Len max=1920K | +------>+------+ + +-----------------------+ | | | + | | | + 0x2022_0000 +-----------------------+ | | | + | eNVM 128K | | | | + +-----------------------+ | | | + | | | + 0x8000_0000 +-----------------------+--+ | DDR | + 0x10_0000_0000 | DDR cached | | | + | SEG0 | +--->| | + +-----------------------+ | | | + | | | + 0xC000_0000 +-----------------------+-----+ | | + 0x14_0000_0000 | DDR non-cached | | | + | SEG1 | | | + +-----------------------+ +-->+ | + | | | + 0xD000_0000 +-----------------------+------+ | | + 0x18_0000_0000 | Write Combine buffer | +------+ + | SEG1 | + +-----------------------+ + ----------------------------------------------------------------------------*/ + +MEMORY +{ + envm (rx) : ORIGIN = 0x20220100, LENGTH = 110K - 256 /* 256 reserved for hss headers */ + envm_sbi (rx) : ORIGIN = 0x2023B800, LENGTH = 18K + l2lim (rwx) : ORIGIN = 0x08000000, LENGTH = 128k + l2lim_sec (rw) : ORIGIN = 0x08020000, LENGTH = 32k + l2lim_info(rw) : ORIGIN = 0x08028000, LENGTH = 4k + l2lim_free (!rwx) : ORIGIN = 0x08029000, LENGTH = 348k + l2lim_rsvd (!rwx) : ORIGIN = 0x08080000, LENGTH = 512K /* Cache ways for L2 ZeroDevice */ + l2_cache (!rwx) : ORIGIN = 0x08100000, LENGTH = 1024K + l2zerodevice (!rwx) : ORIGIN = 0x0A000000, LENGTH = 24k + opensbi (rwx) : ORIGIN = 0x0A006000, LENGTH = 488k +} + +OUTPUT_ARCH("riscv") + +ENTRY(_stext) + +EXTERN(__start) + +SECTIONS +{ + PROVIDE(__l2lim_start = ORIGIN(l2lim)); + PROVIDE(__l2lim_end = ORIGIN(l2lim) + LENGTH(l2lim) + LENGTH(l2lim_sec) + LENGTH(l2lim_rsvd)); + + .l2_scratchpad : ALIGN(0x10) + { + __l2_scratchpad_load = LOADADDR(.l2_scratchpad); + __l2_scratchpad_start = .; + __l2_scratchpad_vma_start = .; + *(.l2_scratchpad) + + . = ALIGN(0x10); + __l2_scratchpad_end = .; + __l2_scratchpad_vma_end = .; + } > l2zerodevice + + PROVIDE(__mpfs_nuttx_start = ORIGIN(l2lim)); + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } + .debug_loc 0 : { *(.debug_loc) } + .debug_ranges 0 : { *(.debug_ranges) } + .debug_str 0 : { *(.debug_str) } + .debug_frame 0 : { *(.debug_frame) } + + .text.sbi : ALIGN(4) { + _ssbi_zerodev = ABSOLUTE(.); + _sbi_zerodev_loadaddr = LOADADDR(.text.sbi); + sbi*(.text* .rodata*) + riscv_atomic* /* In purpose everything */ + riscv_locks*(.text.* .rodata*) + riscv_asm*(.text.*) + mpfs_opensbi_trap* /* In purpose everything */ + mpfs_opensbi*(.text.* .rodata*) + mpfs_ihc_sbi*(.text.* .rodata*) + aclint_mtimer*(.text.* .rodata*) + aclint_mswi*(.text.* .rodata*) + plic.*(.text.* .rodata*) + _esbi_zerodev = ABSOLUTE(.); + } > opensbi AT > envm_sbi + + .text : { + _stext = ABSOLUTE(.); + *(.start .start.*) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > envm + + .init_section : ALIGN(4) { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > envm + + _eronly = ABSOLUTE(.); + + .data : ALIGN(4) { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + *(.gnu.linkonce.s.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > l2lim AT > envm + + PROVIDE(__global_pointer$ = _sdata + ((_edata - _sdata) / 2)); + + .bss : ALIGN(4) { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(.gnu.linkonce.sb.*) + *(COMMON) + . = ALIGN(32); + _ebss = ABSOLUTE(.); + } > l2lim + + PROVIDE(__mpfs_nuttx_end = .); + + .secmem (NOLOAD) : ALIGN(4) { + } > l2lim_sec +} diff --git a/boards/mpfs/icicle/nuttx-config/scripts/script.ld b/boards/mpfs/icicle/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..d29305316621 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/scripts/script.ld @@ -0,0 +1,133 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + l2lim_sec (rw) : ORIGIN = 0x08020000, LENGTH = 32k + l2lim_info (r) : ORIGIN = 0x08028000, LENGTH = 4k + progmem (rx) : ORIGIN = 0xA0000000, LENGTH = 6M /* w/ cache */ + sram (rwx) : ORIGIN = 0xA0600000, LENGTH = 4M /* w/ cache */ +} + +OUTPUT_ARCH("riscv") + +ENTRY(__start) +EXTERN(__start) +EXTERN(_main_toc) +EXTERN(_main_toc_sig) +EXTERN(board_get_manifest) + +SECTIONS +{ + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(8); + } > progmem + + .start : { + *(.start .start.*) + } > progmem + + .text : { + _stext = ABSOLUTE(.); + /* + This signature provides the bootloader with a way to delay booting + */ + . = 0x400; + _bootdelay_signature = ABSOLUTE(.); + FILL(0x01ecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.* .srodata .srodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + } > progmem + + .init_section : ALIGN(4) { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > progmem + + _eronly = ABSOLUTE(.); + + .data : ALIGN(4) { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.sdata .sdata.* .sdata2.*) + *(.gnu.linkonce.d.*) + *(.gnu.linkonce.s.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > progmem + + PROVIDE(__global_pointer$ = _sdata + ((_edata - _sdata) / 2)); + + .bss : ALIGN(4) { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.sbss .sbss.*) + *(.gnu.linkonce.b.*) + *(.gnu.linkonce.sb.*) + *(COMMON) + . = ALIGN(32); + _ebss = ABSOLUTE(.); + } > sram + + .secmem (NOLOAD) : ALIGN(4) { + } > l2lim_sec + + .deviceinfo (NOLOAD) : ALIGN(4) { + } > l2lim_info + + /* Stabs debugging sections. */ + + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/boards/mpfs/icicle/nuttx-config/scripts/toc.ld b/boards/mpfs/icicle/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..859fbff8c1e0 --- /dev/null +++ b/boards/mpfs/icicle/nuttx-config/scripts/toc.ld @@ -0,0 +1,63 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + /* -64 <- Leave room for the signature */ + progmem (r) : ORIGIN = 0xA0000000, LENGTH = 128M - 64 +} + +OUTPUT_ARCH("riscv") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = ALIGN(8); + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/mpfs/icicle/src/CMakeLists.txt b/boards/mpfs/icicle/src/CMakeLists.txt new file mode 100644 index 000000000000..5d01ce4c1843 --- /dev/null +++ b/boards/mpfs/icicle/src/CMakeLists.txt @@ -0,0 +1,101 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + mpfs_emmcsd.c + mpfs_domain.c + mpfs_composite.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_apps + nuttx_drivers + bootloader + ) + + target_include_directories(drivers_board + PRIVATE + ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/risc-v/src/opensbi/opensbi-3rdparty/include + ) + +else() + add_library(board_bus_info + i2c.cpp + spi.cpp + ) + add_dependencies(board_bus_info nuttx_context) + + add_library(drivers_board + init.c + led.c + mpfs_emmcsd.c + mpfs_ihc.c + mpfs_spinor.c + mpfs_pwm.c + manifest.c + mtd.cpp + spi_drv.cpp + ) + add_dependencies(drivers_board nuttx_context) + + target_link_libraries(drivers_board + PRIVATE + drivers__led # drv_led_start + nuttx_arch + nuttx_drivers + ) + + if (CONFIG_OPENAMP) + target_link_libraries(drivers_board PRIVATE nuttx_openamp) + endif() + + if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(drivers_board PRIVATE px4_kernel_layer board_bus_info) + target_compile_options(drivers_board PRIVATE -D__KERNEL__) + else() + target_link_libraries(drivers_board PRIVATE px4_layer board_bus_info) + endif() + + if (CONFIG_BUILD_PROTECTED) + add_library(drivers_userspace + init_entrypt.c + mpfs_userspace.c + ) + add_dependencies(drivers_userspace nuttx_context) + endif() + +endif() diff --git a/boards/mpfs/icicle/src/board_config.h b/boards/mpfs/icicle/src/board_config.h new file mode 100644 index 000000000000..6da3cd0239dd --- /dev/null +++ b/boards/mpfs/icicle/src/board_config.h @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file board_config.h + * + * Icicle internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include +#include "board_type.h" + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#define BOARD_REVISIONS {{"PolarFire MPFS250T", '1', NULL}, \ + {"PolarFire MPFS250T", '1', NULL}} + +/* Configuration ************************************************************************************/ + +/* Icicle GPIOs *************************************************************************************/ + +#define BOARD_HAS_HW_VERSIONING +#define BOARD_HAS_MULTIPURPOSE_VERSION_PINS +#define GPIO_HW_VERSION_PIN1 (GPIO_BANK2 | GPIO_PIN20 | GPIO_INPUT) +#define GPIO_HW_VERSION_PIN2 (GPIO_BANK2 | GPIO_PIN21 | GPIO_INPUT) +#define GPIO_HW_VERSION_PIN3 (GPIO_BANK2 | GPIO_PIN22 | GPIO_INPUT) +#define HW_INFO_INIT_PREFIX "Icicle " + +#define GPIO_nSAFETY_SWITCH_LED_OUT (GPIO_BANK2 | GPIO_PIN26 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // GPS safety switch LED +#define GPIO_LED_SAFETY GPIO_nSAFETY_SWITCH_LED_OUT + +#define GPIO_SAFETY_SWITCH_IN (GPIO_BANK2 | GPIO_PIN25 | GPIO_INPUT) // GPS safety switch +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN + +#define TONE_ALARM_PWM_OUT_PATH "/dev/pwm1" + +/* LEDS */ +//#define GPIO_nSAFETY_SWITCH_LED_OUT (GPIO_BANK2 | GPIO_PIN16 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 1 +#define GPIO_nLED_RED (GPIO_BANK2 | GPIO_PIN17 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 2 +// NB: These are both yellow on Icicle board: +#define GPIO_nLED_GREEN (GPIO_BANK2 | GPIO_PIN18 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 3 +#define GPIO_nLED_BLUE (GPIO_BANK2 | GPIO_PIN19 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) // ICICLE LED 4 + +//#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +//#define BOARD_OVERLOAD_LED LED_RED +//#define BOARD_ARMED_STATE_LED LED_BLUE + +/* I2C */ +#define I2C_RESET_SPEED I2C_SPEED_FAST +#define BOARD_I2C_BUS_CLOCK_INIT {I2C_SPEED_FAST, I2C_SPEED_FAST} + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS2" +#define RC_SERIAL_SINGLEWIRE + +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 0 + +#define BOARD_DSHOT_MOTOR_ASSIGNMENT {3, 2, 1, 0, 4, 5, 6, 7}; + +/* eMMC/SD */ + +#define SDIO_SLOTNO 0 +#define SDIO_MINOR 0 + +/* Battery ADC */ +#define ADC_CHANNELS (1 << 1) | (1 << 2) +#define ADC_BATTERY_VOLTAGE_CHANNEL 1 +#define ADC_BATTERY_CURRENT_CHANNEL 2 + +/* PX4 Boot image type; 0 to boot directly, 1 through SBI */ +#define INFO_BIT_USE_SBI 1 + +#ifdef CONFIG_BUILD_KERNEL +#define PX4_VENDOR_BOOT_FLAGS INFO_BIT_USE_SBI +#else +#define PX4_VENDOR_BOOT_FLAGS 0 +#endif + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +extern void board_peripheral_reset(int ms); +extern int mpfs_board_emmcsd_init(void); +extern int mpfs_board_register_partition(unsigned partition); +extern int mpfs_board_ihc_init(void); +extern int mpfs_board_spinor_init(struct spi_dev_s *spinor); +extern int mpfs_pwm_setup(void); +extern void board_spidev_initialize(void); +extern int board_spibus_initialize(void); +extern int board_domains_init(void); + +#ifdef CONFIG_USBDEV +extern void mpfs_usbinitialize(void); +#endif + +#ifdef CONFIG_NET_RPMSG_DRV +extern int net_rpmsg_drv_init(const char *cpuname, const char *devname, enum net_lltype_e lltype); +#endif + +#if defined(CONFIG_FAT_DMAMEMORY) && defined(CONFIG_GRAN) +int mpfs_dma_alloc_init(void); +#endif + +#include + +const char *board_bl_version_string(void); +#define PX4_BL_VERSION board_bl_version_string() + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/mpfs/icicle/src/bootloader_main.c b/boards/mpfs/icicle/src/bootloader_main.c new file mode 100644 index 000000000000..6e942d1f401c --- /dev/null +++ b/boards/mpfs/icicle/src/bootloader_main.c @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include +#include "riscv_internal.h" + +extern int mpfs_dma_alloc_init(void); +extern int sercon_main(int c, char **argv); +__EXPORT void board_on_reset(int status) {} + +static void configure_pmp(void) +{ +#if 0 + // SDCARD DMA: NB! sizes must be power of 2 + const uint64_t mode_bits = 0x1Full << 56; + + // 1MB of LIM ram (bootloader RAM_START & SIZE) + uint64_t base_range = (0x08000000ull | (0x100000ull - 1ull)) >> 2; + putreg64(mode_bits | base_range, 0x20005700); + + // 4MB from the start of DDR + base_range = (0x80000000ull | (0x400000ull - 1ull)) >> 2; + putreg64(mode_bits | base_range, 0x20005708); + +#else + +#define MPFS_PMPCFG_MMC_0 (MPFS_MPUCFG_BASE + 0x700) +#define MPFS_PMPCFG_MMC_1 (MPFS_MPUCFG_BASE + 0x708) +#define MPFS_PMPCFG_MMC_2 (MPFS_MPUCFG_BASE + 0x710) +#define MPFS_PMPCFG_MMC_3 (MPFS_MPUCFG_BASE + 0x718) + +#define MPFS_PMPCFG_ETH0_0 (MPFS_MPUCFG_BASE + 0x400) +#define MPFS_PMPCFG_ETH0_1 (MPFS_MPUCFG_BASE + 0x408) +#define MPFS_PMPCFG_ETH0_2 (MPFS_MPUCFG_BASE + 0x410) +#define MPFS_PMPCFG_ETH0_3 (MPFS_MPUCFG_BASE + 0x418) +#define MPFS_PMPCFG_ETH1_0 (MPFS_MPUCFG_BASE + 0x500) +#define MPFS_PMPCFG_ETH1_1 (MPFS_MPUCFG_BASE + 0x508) +#define MPFS_PMPCFG_ETH1_2 (MPFS_MPUCFG_BASE + 0x510) +#define MPFS_PMPCFG_ETH1_3 (MPFS_MPUCFG_BASE + 0x518) + + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_0); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_1); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_2); + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_MMC_3); + + // Ethernet MPU, MAC 0 + putreg64(0x1f00000fffffffff, MPFS_PMPCFG_ETH0_0); + +#endif + +} + +__EXPORT void mpfs_boardinitialize(void) +{ + _alert("Icicle bootloader\n"); + + /* this call exists to fix a weird linking issue */ + up_udelay(0); + + /* Enable clocks, TIMER is used for PX4 HRT, others are common + for many peripherals */ + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SUBBLK_CLOCK_CR_OFFSET, 0, + ( + SYSREG_SUBBLK_CLOCK_CR_TIMER + | SYSREG_SUBBLK_CLOCK_CR_GPIO2 + | SYSREG_SUBBLK_CLOCK_CR_CFM + | SYSREG_SUBBLK_CLOCK_CR_FIC3 + ) + ); + + /* Take peripheral out of reset */ + + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SOFT_RESET_CR_OFFSET, + ( + SYSREG_SOFT_RESET_CR_TIMER + | SYSREG_SOFT_RESET_CR_GPIO2 + | SYSREG_SOFT_RESET_CR_CFM + | SYSREG_SOFT_RESET_CR_FIC3 + | SYSREG_SOFT_RESET_CR_FPGA + ), 0); + + /* configure PMP */ + + configure_pmp(); +} + +void board_late_initialize(void) +{ + +#if defined(CONFIG_FAT_DMAMEMORY) && defined(CONFIG_GRAN) + /* configure the DMA allocator */ + + if (mpfs_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#endif +} + +int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +extern void sys_tick_handler(void); + +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/mpfs/icicle/src/hw_config.h b/boards/mpfs/icicle/src/hw_config.h new file mode 100644 index 000000000000..a8d238c236f3 --- /dev/null +++ b/boards/mpfs/icicle/src/hw_config.h @@ -0,0 +1,133 @@ +/* + * hw_config.h + * + * Created on: May 17, 2015 + * Author: david_s5 + */ + +#ifndef HW_CONFIG_H_ +#define HW_CONFIG_H_ + +/**************************************************************************** + * 10-8--2016: + * To simplify the ripple effect on the tools, we will be using + * /dev/serial/by-id/PX4 to locate PX4 devices. Therefore + * moving forward all Bootloaders must contain the prefix "PX4 BL " + * in the USBDEVICESTRING + * This Change will be made in an upcoming BL release + ****************************************************************************/ +/* + * Define usage to configure a bootloader + * + * + * Constant example Usage + * APP_LOAD_ADDRESS 0x08004000 - The address in Linker Script, where the app fw is org-ed + * BOOTLOADER_DELAY 5000 - Ms to wait while under USB pwr or bootloader request + * BOARD_FMUV2 + * INTERFACE_USB 1 - (Optional) Scan and use the USB interface for bootloading + * INTERFACE_USART 1 - (Optional) Scan and use the Serial interface for bootloading + * USBDEVICESTRING "PX4 BL FMU v2.x" - USB id string + * USBPRODUCTID 0x0011 - PID Should match defconfig + * BOOT_DELAY_ADDRESS 0x000001a0 - (Optional) From the linker script from Linker Script to get a custom + * delay provided by an APP FW + * BOARD_TYPE 9 - Must match .prototype boad_id + * _FLASH_KBYTES (*(uint16_t *)0x1fff7a22) - Run time flash size detection + * BOARD_FLASH_SECTORS ((_FLASH_KBYTES == 0x400) ? 11 : 23) - Run time determine the physical last sector + * BOARD_FLASH_SECTORS 11 - Hard coded zero based last sector + * BOARD_FLASH_SIZE (_FLASH_KBYTES*1024)- Total Flash size of device, determined at run time. + * (1024 * 1024) - Hard coded Total Flash of device - The bootloader and app reserved will be deducted + * programmatically + * + * BOARD_FIRST_FLASH_SECTOR_TO_ERASE 2 - Optional sectors index in the flash_sectors table (F4 only), to begin erasing. + * This is to allow sectors to be reserved for app fw usage. That will NOT be erased + * during a FW upgrade. + * The default is 0, and selects the first sector to be erased, as the 0th entry in the + * flash_sectors table. Which is the second physical sector of FLASH in the device. + * The first physical sector of FLASH is used by the bootloader, and is not defined + * in the table. + * + * APP_RESERVATION_SIZE (BOARD_FIRST_FLASH_SECTOR_TO_ERASE * 16 * 1024) - Number of bytes reserved by the APP FW. This number plus + * BOOTLOADER_RESERVATION_SIZE will be deducted from + * BOARD_FLASH_SIZE to determine the size of the App FW + * and hence the address space of FLASH to erase and program. + * USBMFGSTRING "PX4 AP" - Optional USB MFG string (default is '3D Robotics' if not defined.) + * SERIAL_BREAK_DETECT_DISABLED - Optional prevent break selection on Serial port from entering or staying in BL + * + * * Other defines are somewhat self explanatory. + */ + +/* Boot device selection list*/ +#define USB0_DEV 0x01 +#define SERIAL0_DEV 0x02 +#define SERIAL1_DEV 0x04 + +#define APP_LOAD_ADDRESS 0xACA00000llu +#define FLASH_START_ADDRESS APP_LOAD_ADDRESS +#define TOC_ADDRESS FLASH_START_ADDRESS +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS3,2000000" +#define BOOT_DELAY_ADDRESS 0x00000400 +#define _FLASH_KBYTES (5 * 1024) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED + +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +/* + * Uncommenting this allows to force the bootloader through + * a PWM output pin. As this can accidentally initialize + * an ESC prematurely, it is not recommended. This feature + * has not been used and hence defaults now to off. + * + * # define BOARD_FORCE_BL_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN14) + * # define BOARD_FORCE_BL_PIN_IN (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN11) + * + * # define BOARD_POWER_PIN_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) + * # define BOARD_POWER_ON 1 + * # define BOARD_POWER_OFF 0 + * # undef BOARD_POWER_PIN_RELEASE // Leave pin enabling Power - un comment to release (disable power) + * +*/ + +#define BOARD_FORCE_BL_PIN GPIO_SAFETY_SWITCH_IN + +#define BOOTLOADER_USE_TOC +#define BOOTLOADER_USE_SECURITY +#define BOOTLOADER_SIGNING_ALGORITHM CRYPTO_ED25519 +#define BOOTLOADER_VERIFY_UBOOT 0 + +#define IMAGE_FN "icicle.bin" + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 16 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 0 +#endif + +#if !defined(USB_DATA_ALIGN) +# define USB_DATA_ALIGN +#endif + +#ifndef BOOT_DEVICES_SELECTION +# define BOOT_DEVICES_SELECTION USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#ifndef BOOT_DEVICES_FILTER_ONUSB +# define BOOT_DEVICES_FILTER_ONUSB USB0_DEV|SERIAL0_DEV|SERIAL1_DEV +#endif + +#define BOARD_PIN_CS_SPINOR (GPIO_BANK2 | GPIO_PIN15 | GPIO_OUTPUT | GPIO_BUFFER_ENABLE) + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/mpfs/icicle/src/i2c.cpp b/boards/mpfs/icicle/src/i2c.cpp new file mode 100644 index 000000000000..c0d791bb54d5 --- /dev/null +++ b/boards/mpfs/icicle/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)), + initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)), +}; diff --git a/boards/mpfs/icicle/src/init.c b/boards/mpfs/icicle/src/init.c new file mode 100644 index 000000000000..2f7f09fc1963 --- /dev/null +++ b/boards/mpfs/icicle/src/init.c @@ -0,0 +1,337 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file init.c + * + * Icicle-specific early startup code. This file implements the + * board_app_initializ() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* + * Ideally we'd be able to get these from arm_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(void); +extern void led_on(int led); +extern void led_off(int led); +__END_DECLS + + +/************************************************************************************ + * Name: board_peripheral_reset + * + * Description: + * + ************************************************************************************/ +__EXPORT void board_peripheral_reset(int ms) +{ + syslog(LOG_DEBUG, "board_peripheral_reset\n"); +} + +/************************************************************************************ + * Name: board_on_reset + * + * Description: + * Optionally provided function called on entry to board_system_reset + * It should perform any house keeping prior to the rest. + * + * status - 1 if resetting to boot loader + * 0 if just resetting + * + ************************************************************************************/ +__EXPORT void board_on_reset(int status) +{ + syslog(LOG_DEBUG, "board_on_reset\n"); +} + +/************************************************************************************ + * Name: mpfs_boardinitialize + * + * Description: + * This entry point is called early in the initialization -- after all memory + * has been configured and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +mpfs_boardinitialize(void) +{ + syslog(LOG_DEBUG, "mpfs_boardinitialize\n"); + board_autoled_initialize(); + + /* this call exists to fix a weird linking issue */ + up_udelay(0); + + /* Configure Safety button GPIO */ + mpfs_configgpio(GPIO_BTN_SAFETY); + +} + +/**************************************************************************** + * Name: board_app_initialize + * + * Description: + * Perform application specific initialization. This function is never + * called directly from application code, but only indirectly via the + * (non-standard) boardctl() interface using the command BOARDIOC_INIT. + * + * Input Parameters: + * arg - The boardctl() argument is passed to the board_app_initialize() + * implementation without modification. The argument has no + * meaning to NuttX; the meaning of the argument is a contract + * between the board-specific initalization logic and the the + * matching application logic. The value cold be such things as a + * mode enumeration value, a set of DIP switch switch settings, a + * pointer to configuration data read from a file or serial FLASH, + * or whatever you would like to do with it. Every implementation + * should accept zero/NULL as a default configuration. + * + * Returned Value: + * Zero (OK) is returned on success; a negated errno value is returned on + * any failure to indicate the nature of the failure. + * + ****************************************************************************/ + + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + int ret; + + /* Power on Interfaces */ +#ifdef CONFIG_USBDEV + mpfs_usbinitialize(); +#endif + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + if (OK == board_determine_hw_info()) { + syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(), + board_get_hw_type_name()); + + } else { + syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n"); + } + + /* configure SPI interfaces and devices (after we determined the HW version) */ + board_spidev_initialize(); + board_spibus_initialize(); + +#if defined(CONFIG_FAT_DMAMEMORY) && defined(CONFIG_GRAN) + /* configure the DMA allocator */ + + if (mpfs_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#endif + +#if defined(SERIAL_HAVE_RXDMA) + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + struct timespec ts; + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)mpfs_serial_dma_poll, + NULL); +#endif + + /* initial LED state */ + + drv_led_start(); + led_off(LED_RED); + led_on(LED_GREEN); // Indicate Power. + led_off(LED_BLUE); + + /* + if (board_hardfault_init(2, true) != 0) { + led_on(LED_RED); + } + */ + +#ifdef CONFIG_MMCSD + ret = mpfs_board_emmcsd_init(); + + if (ret != OK) { + led_on(LED_RED); + syslog(LOG_ERR, "ERROR: Failed to initialize SD card"); + } + +#endif /* CONFIG_MMCSD */ + +#ifdef CONFIG_MPFS_HAVE_COREPWM + /* Configure PWM peripheral interfaces */ + + ret = mpfs_pwm_setup(); + + if (ret < 0) { + syslog(LOG_ERR, "Failed to initialize CorePWM driver: %d\n", ret); + } + +#endif + +#ifdef CONFIG_FS_PROCFS + /* Mount the procfs file system */ + + ret = mount(NULL, "/proc", "procfs", 0, NULL); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to mount procfs at %s: %d\n", "/proc", ret); + } + +#endif + +#ifdef CONFIG_MTD_M25P + + ret = mpfs_board_spinor_init(mpfs_spibus_initialize(1)); + + if (ret < 0) { + return ret; + } + +#endif + +#ifdef CONFIG_I2C_EE_24XX + + struct i2c_master_s *bus1 = mpfs_i2cbus_initialize(1); /* carrier board EEPROM is on bus 1 */ + + if (!bus1) { + syslog(LOG_ERR, "ERROR: Failed to initialize I2C bus 1\n"); + return -ENODEV; + } + + ret = ee24xx_initialize(bus1, 0x51, "/dev/eeprom1", EEPROM_AT24C04, false); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: Failed to init EEPROM of Carrier board\n"); + mpfs_i2cbus_uninitialize(bus1); + return ret; + } + +#endif + +#ifdef CONFIG_MPFS_IHC_CLIENT + + /* Initialize MPFS IHC and register the net rpmsg driver. */ + if (mpfs_board_ihc_init() != 0) { + syslog(LOG_ERR, "ERROR: Failed to initialize IHC"); + } + +#ifdef CONFIG_NET_RPMSG_DRV + + ret = net_rpmsg_drv_init("mpfs-ihc", "rpnet", NET_LL_TUN); + + if (ret < 0) { + syslog(LOG_ERR, "ERROR: net_rpmsg_drv_init() failed: %d\n", ret); + } + +#endif /* CONFIG_NET_RPMSG_DRV */ + +#endif /* CONFIG_MPFS_IHC_CLIENT */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} + +int board_read_VBUS_state(void) +{ + return 0; +} + +#ifdef CONFIG_MPFS_PHYINIT + +void mpfs_phy_boardinitialize(int arg) +{ +} + +#endif diff --git a/boards/mpfs/icicle/src/init_entrypt.c b/boards/mpfs/icicle/src/init_entrypt.c new file mode 100644 index 000000000000..021a479e16f7 --- /dev/null +++ b/boards/mpfs/icicle/src/init_entrypt.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#if !defined(CONFIG_BUILD_FLAT) && !defined(__KERNEL__) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#ifdef CONFIG_NSH_ARCHINIT +# error "CONFIG_NSH_ARCHINIT must not be defined!" +#endif + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +void px4_userspace_init(void); +int nsh_main(int argc, char *argv[]); + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +int CONFIG_INIT_ENTRYPOINT(int argc, char *argv[]) +{ + boardctl(BOARDIOC_INIT, 0); + + px4_userspace_init(); + + return nsh_main(argc, argv); +} + +#endif diff --git a/boards/mpfs/icicle/src/led.c b/boards/mpfs/icicle/src/led.c new file mode 100644 index 000000000000..ae6ada578939 --- /dev/null +++ b/boards/mpfs/icicle/src/led.c @@ -0,0 +1,101 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file led.c + * + * Icicle board LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "mpfs_gpio.h" +#include "board_config.h" + +#include +#include + +static uint32_t g_leds[] = { + GPIO_nLED_BLUE, + GPIO_nLED_RED, + GPIO_nSAFETY_SWITCH_LED_OUT, + GPIO_nLED_GREEN, +}; + +/* LED_ACTIVITY == 1, LED_BOOTLOADER == 2 */ +static bool g_led_state[3]; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_leds) / sizeof(g_leds[0])); l++) { + if (g_leds[l] != 0) { + g_led_state[l] = false; + mpfs_configgpio(g_leds[l]); + } + } +} + +static void set_led(int led, bool state) +{ + if (g_leds[led] != 0) { + g_led_state[led] = state; + mpfs_gpiowrite(g_leds[led], state); + } +} + +static bool get_led(int led) +{ + if (g_leds[led] != 0) { + return g_led_state[led]; + } + + return false; +} + +__EXPORT void led_on(int led) +{ + set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + set_led(led, !get_led(led)); +} diff --git a/boards/mpfs/icicle/src/manifest.c b/boards/mpfs/icicle/src/manifest.c new file mode 100644 index 000000000000..aba41217cad1 --- /dev/null +++ b/boards/mpfs/icicle/src/manifest.c @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file manifest.c + * + * This module supplies the interface to the manifest of hardware that is + * optional and dependent on the HW REV and HW VER IDs + * + * The manifest allows the system to know whether a hardware option + * say for example the PX4IO is an no-pop option vs it is broken. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include + +#include "systemlib/px4_macros.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +typedef struct { + uint32_t hw_ver_rev; /* the version and revision */ + const px4_hw_mft_item_t *mft; /* The first entry */ + uint32_t entries; /* the lenght of the list */ +} px4_hw_mft_list_entry_t; + +typedef px4_hw_mft_list_entry_t *px4_hw_mft_list_entry; +#define px4_hw_mft_list_uninitialized (px4_hw_mft_list_entry) -1 + +static const px4_hw_mft_item_t device_unsupported = {0, 0, 0}; + +// List of components on a specific board configuration +// The index of those components is given by the enum (px4_hw_mft_item_id_t) +// declared in board_common.h + + +static const px4_hw_mft_item_t hw_mft_list_v0000[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static const px4_hw_mft_item_t hw_mft_list_v0100[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver/rev + {0x0000, hw_mft_list_v0000, arraySize(hw_mft_list_v0000)}, + {0x0100, hw_mft_list_v0100, arraySize(hw_mft_list_v0100)}, +}; + +/************************************************************************************ + * Name: board_query_manifest + * + * Description: + * Optional returns manifest item. + * + * Input Parameters: + * manifest_id - the ID for the manifest item to retrieve + * + * Returned Value: + * 0 - item is not in manifest => assume legacy operations + * pointer to a manifest item + * + ************************************************************************************/ + +__EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id) +{ + + static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized; + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + uint32_t ver_rev = board_get_hw_version() << 8; + ver_rev |= board_get_hw_revision(); + + for (unsigned i = 0; i < arraySize(mft_lists); i++) { + if (mft_lists[i].hw_ver_rev == ver_rev) { + boards_manifest = &mft_lists[i]; + break; + } + } + + if (boards_manifest == px4_hw_mft_list_uninitialized) { + syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev); + } + } + + px4_hw_mft_item rv = &device_unsupported; + + if (boards_manifest != px4_hw_mft_list_uninitialized && + id < boards_manifest->entries) { + rv = &boards_manifest->mft[id]; + } + + return rv; +} diff --git a/boards/mpfs/icicle/src/mpfs_composite.c b/boards/mpfs/icicle/src/mpfs_composite.c new file mode 100644 index 000000000000..832f927dec10 --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_composite.c @@ -0,0 +1,305 @@ +/**************************************************************************** + * boards/risc-v/mpfs/common/src/mpfs_composite.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "mpfs.h" + +#if defined(CONFIG_BOARDCTL_USBDEVCTRL) && defined(CONFIG_USBDEV_COMPOSITE) + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static void *g_mschandle; +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_mscclassobject + * + * Description: + * If the mass storage class driver is part of composite device, then + * its instantiation and configuration is a multi-step, board-specific + * process (See comments for usbmsc_configure below). In this case, + * board-specific logic must provide board_mscclassobject(). + * + * board_mscclassobject() is called from the composite driver. It must + * encapsulate the instantiation and configuration of the mass storage + * class and the return the mass storage device's class driver instance + * to the composite driver. + * + * Input Parameters: + * classdev - The location to return the mass storage class' device + * instance. + * + * Returned Value: + * 0 on success; a negated errno on failure + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static int board_mscclassobject(int minor, + struct usbdev_devinfo_s *devinfo, + struct usbdevclass_driver_s **classdev) +{ + int ret; + + DEBUGASSERT(g_mschandle == NULL); + + /* Configure the mass storage device */ + + uinfo("Configuring with NLUNS=1\n"); + + ret = usbmsc_configure(1, &g_mschandle); + + if (ret < 0) { + uerr("ERROR: usbmsc_configure failed: %d\n", -ret); + return ret; + } + + uinfo("MSC handle=%p\n", g_mschandle); + + /* Bind the LUN(s) */ + + uinfo("Bind LUN=0 to /dev/mmcsd0\n"); + + ret = usbmsc_bindlun(g_mschandle, "/dev/mmcsd0", 0, 0, 0, false); + + if (ret < 0) { + uerr("ERROR: usbmsc_bindlun failed for LUN 1 at /dev/mmcsd0: %d\n", + ret); + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + return ret; + } + + /* Get the mass storage device's class object */ + + ret = usbmsc_classobject(g_mschandle, devinfo, classdev); + + if (ret < 0) { + uerr("ERROR: usbmsc_classobject failed: %d\n", -ret); + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + } + + return ret; +} +#endif + +/**************************************************************************** + * Name: board_mscuninitialize + * + * Description: + * Un-initialize the USB storage class driver. This is just an application + * specific wrapper aboutn usbmsc_unitialize() that is called form the + * composite device logic. + * + * Input Parameters: + * classdev - The class driver instrance previously give to the composite + * driver by board_mscclassobject(). + * + * Returned Value: + * None + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +static void board_mscuninitialize(struct usbdevclass_driver_s *classdev) +{ + if (g_mschandle != NULL) { + usbmsc_uninitialize(g_mschandle); + g_mschandle = NULL; + } +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_composite_initialize + * + * Description: + * Perform architecture specific initialization of a composite USB device. + * + * Input Parameters: + * port - port number, unused + * + * Returned Value: + * OK always + * + ****************************************************************************/ + +#ifdef CONFIG_USBMSC_COMPOSITE +int board_composite_initialize(int port) +{ + return OK; +} +#endif + +/**************************************************************************** + * Name: board_usbmsc_initialize + * + * Description: + * Perform architecture specific initialization of a mass storage USB + * device. + * + * Input Parameters: + * port - port number, unused + * + * Returned Value: + * OK always + * + ****************************************************************************/ + +int board_usbmsc_initialize(int port) +{ + return OK; +} + +/**************************************************************************** + * Name: board_composite_connect + * + * Description: + * Connect the USB composite device on the specified USB device port using + * the specified configuration. The interpretation of the configid is + * board specific. + * + * Input Parameters: + * port - The USB device port + * configid - The USB composite configuration + * + * Returned Value: + * A non-NULL handle value is returned on success. NULL is returned on + * any failure. + * + ****************************************************************************/ + +void *board_composite_connect(int port, int configid) +{ + /* Here we are composing the configuration of the usb composite device. + * The standard is to use one CDC/ACM and one USB mass storage device. + */ + + if (configid == 0) { +#ifdef CONFIG_USBMSC_COMPOSITE + struct composite_devdesc_s dev[2]; + int ifnobase = 0; + int strbase = COMPOSITE_NSTRIDS; + + /* Configure the CDC/ACM device */ + + /* Ask the cdcacm driver to fill in the constants we didn't + * know here. + */ + + cdcacm_get_composite_devdesc(&dev[0]); + + /* Overwrite and correct some values... */ + + /* The callback functions for the CDC/ACM class */ + + dev[0].classobject = cdcacm_classobject; + dev[0].uninitialize = cdcacm_uninitialize; + + /* Interfaces */ + + dev[0].devinfo.ifnobase = ifnobase; /* Offset to Interface-IDs */ + dev[0].minor = 0; /* The minor interface number */ + + /* Strings */ + + dev[0].devinfo.strbase = strbase; /* Offset to String Numbers */ + + /* Endpoints */ + + dev[0].devinfo.epno[CDCACM_EP_BULKIN_IDX] = 3; + dev[0].devinfo.epno[CDCACM_EP_BULKOUT_IDX] = 3; + dev[0].devinfo.epno[CDCACM_EP_INTIN_IDX] = 4; + + /* Count up the base numbers */ + + ifnobase += dev[0].devinfo.ninterfaces; + strbase += dev[0].devinfo.nstrings; + + /* Configure the mass storage device device */ + + /* Ask the usbmsc driver to fill in the constants we didn't + * know here. + */ + + usbmsc_get_composite_devdesc(&dev[1]); + + /* Overwrite and correct some values... */ + + /* The callback functions for the USBMSC class */ + + dev[1].classobject = board_mscclassobject; + dev[1].uninitialize = board_mscuninitialize; + + /* Interfaces */ + + dev[1].devinfo.ifnobase = ifnobase; /* Offset to Interface-IDs */ + dev[1].minor = 0; /* The minor interface number */ + + /* Strings */ + + dev[1].devinfo.strbase = strbase; /* Offset to String Numbers */ + + /* Endpoints */ + + dev[1].devinfo.epno[USBMSC_EP_BULKIN_IDX] = 1; + dev[1].devinfo.epno[USBMSC_EP_BULKOUT_IDX] = 2; + + /* Count up the base numbers */ + + ifnobase += dev[1].devinfo.ninterfaces; + strbase += dev[1].devinfo.nstrings; + + return composite_initialize(2, dev); +#else + return NULL; +#endif + + } else { + return NULL; + } +} + +#endif /* CONFIG_BOARDCTL_USBDEVCTRL && CONFIG_USBDEV_COMPOSITE */ diff --git a/boards/mpfs/icicle/src/mpfs_domain.c b/boards/mpfs/icicle/src/mpfs_domain.c new file mode 100644 index 000000000000..90e2cb9aa72b --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_domain.c @@ -0,0 +1,190 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#undef NULL /* To please compiler */ + +#include +#include +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define MPFS_DOMAIN_MAX_COUNT 4 +#define MPFS_DOMAIN_REGION_MAX_COUNT 12 + +struct mpfs_domain { + const char *domain_name; + const int *harts; + const unsigned n_of_harts; + const uintptr_t bootaddress; + const bool reset_allowed; + const bool domain_enabled; +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static struct sbi_domain mpfs_domains[MPFS_DOMAIN_MAX_COUNT]; +static struct sbi_hartmask mpfs_masks[MPFS_DOMAIN_MAX_COUNT]; +static struct sbi_domain_memregion + mpfs_regions[MPFS_DOMAIN_REGION_MAX_COUNT + 1] = { + 0 +}; + +static const int linux_harts[] = {1, 3, 4}; +static const int px4_harts[] = {2}; + +static const struct mpfs_domain domains[] = { + { + .domain_name = "Linux-Ree-Domain", + .harts = linux_harts, + .n_of_harts = sizeof(linux_harts) / sizeof(linux_harts[0]), + .bootaddress = CONFIG_MPFS_HART3_ENTRYPOINT, + .reset_allowed = false, + .domain_enabled = true, + }, + { + .domain_name = "PX4-Ree-Domain", + .harts = px4_harts, + .n_of_harts = sizeof(px4_harts) / sizeof(px4_harts[0]), + .bootaddress = CONFIG_MPFS_HART2_ENTRYPOINT, + .reset_allowed = true, +#ifdef CONFIG_BUILD_KERNEL + .domain_enabled = true, +#else + .domain_enabled = false, +#endif + }, +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_domains_init + * + * Description: + * Initializes the domain structures from the hard coded domains table + * + * Input Parameters: + * None + * + * Returned Value: + * Zero (OK) is returned on success. + * + ****************************************************************************/ + +int board_domains_init(void) +{ + int err = -1; + unsigned i; + unsigned j; + struct sbi_domain_memregion *reg; + + /* All memory, all access */ + + sbi_domain_memregion_init(0, ~0UL, + (SBI_DOMAIN_MEMREGION_READABLE | + SBI_DOMAIN_MEMREGION_WRITEABLE | + SBI_DOMAIN_MEMREGION_EXECUTABLE), + &mpfs_regions[0]); + + /* Add to root domain */ + + sbi_domain_root_add_memregion(&mpfs_regions[0]); + + i = 1; + sbi_domain_for_each_memregion(&root, reg) { + if ((reg->flags & SBI_DOMAIN_MEMREGION_READABLE) || + (reg->flags & SBI_DOMAIN_MEMREGION_WRITEABLE) || + (reg->flags & SBI_DOMAIN_MEMREGION_EXECUTABLE)) { + continue; + } + + if (MPFS_DOMAIN_REGION_MAX_COUNT <= i) { + return SBI_EINVAL; + } + + sbi_memcpy(&mpfs_regions[i++], reg, sizeof(*reg)); + } + + /* Go through the constant configuration list */ + + for (i = 0; i < sizeof(domains) / sizeof(domains[0]); i++) { + + if (!domains[i].domain_enabled) { + continue; + } + + /* Set first hart id in the list as boot hart */ + mpfs_domains[i].boot_hartid = domains[i].harts[0]; + sbi_strncpy(mpfs_domains[i].name, + domains[i].domain_name, sizeof(mpfs_domains[i].name)); + + mpfs_domains[i].next_addr = domains[i].bootaddress; + mpfs_domains[i].next_mode = PRV_S; + mpfs_domains[i].next_arg1 = 0; + mpfs_domains[i].regions = mpfs_regions; + + for (j = 0; j < domains[i].n_of_harts; j++) { + sbi_hartmask_set_hart(domains[i].harts[j], &mpfs_masks[i]); + } + + mpfs_domains[i].possible_harts = &mpfs_masks[i]; + mpfs_domains[i].system_reset_allowed = domains[i].reset_allowed; + + /* Register the domain */ + + err = sbi_domain_register(&mpfs_domains[i], &mpfs_masks[i]); + + if (err) { + sbi_printf("%s register failed %d\n", domains[i].domain_name, err); + return err; + } + } + + return 0; +} diff --git a/boards/mpfs/icicle/src/mpfs_emmcsd.c b/boards/mpfs/icicle/src/mpfs_emmcsd.c new file mode 100644 index 000000000000..f063a31e8a35 --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_emmcsd.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include "mpfs_emmcsd.h" +#include "board_config.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *g_sdio_dev; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_emmcsd_init + * + * Description: + * Configure the eMMCSD driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate the nature of any failure. + * + ****************************************************************************/ + +static void partition_handler(FAR struct partition_s *part, FAR void *arg) +{ + unsigned partition = *(int *)arg; + char devname[] = "/dev/mmcsd0p0"; + + if (partition < 10 && part->index == partition) { + devname[sizeof(devname) - 2] = partition + 48; + register_blockpartition(devname, 0, "/dev/mmcsd0", part->firstblock, part->nblocks); + } +} + +int mpfs_board_register_partition(unsigned partition) +{ + return parse_block_partition("/dev/mmcsd0", partition_handler, &partition); +} + +int mpfs_board_emmcsd_init(void) +{ + int ret; + + /* Mount the SDIO-based MMC/SD block driver */ + + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + g_sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!g_sdio_dev) { + ferr("ERROR: Failed to initialize SDIO slot %d\n", SDIO_SLOTNO); + return -ENODEV; + } + + /* Now bind the SDIO interface to the MMC/SD driver */ + + finfo("Bind SDIO to the MMC/SD driver, minor=%d\n", SDIO_MINOR); + + ret = mmcsd_slotinitialize(SDIO_MINOR, g_sdio_dev); + + if (ret != OK) { + ferr("ERROR: Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + sdio_mediachange(g_sdio_dev, true); + + return OK; +} diff --git a/boards/mpfs/icicle/src/mpfs_ihc.c b/boards/mpfs/icicle/src/mpfs_ihc.c new file mode 100644 index 000000000000..3792b37130d5 --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_ihc.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include + +#include "mpfs_ihc.h" +#include "board_config.h" + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_board_ihc_init + * + * Description: + * Starts the Inter-Hart Communication (IHC) driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate any failure. + * + ****************************************************************************/ + +int mpfs_board_ihc_init(void) +{ + int ret = 0; + + /* With OpenSBI, initilization comes via mpfs_opensbi.c, not here */ + +#ifndef CONFIG_MPFS_OPENSBI + + ret = mpfs_ihc_init(); + +#endif + + return ret; +} diff --git a/boards/mpfs/icicle/src/mpfs_pwm.c b/boards/mpfs/icicle/src/mpfs_pwm.c new file mode 100644 index 000000000000..e3300da1e227 --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_pwm.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "mpfs_corepwm.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: mpfs_pwm_setup + * + * Description: + * + * Initialize PWM and register PWM devices + * + ****************************************************************************/ + +#define PWM_DEV_NAME "/dev/pwm%d" + +int mpfs_pwm_setup(void) +{ + int npwm = 0; + char devname[sizeof(PWM_DEV_NAME)]; /* Buffer for device name */ + struct pwm_lowerhalf_s *lower_half = NULL; /* lower-half handle */ + int config_npwm = 0; /* Number of channels in use */ + + /* The underlying CorePWM driver "knows" there are up to 16 channels + * available for each timer device, so we don't have to do anything + * special here. + */ + +#ifdef CONFIG_MPFS_COREPWM0 + config_npwm++; +#endif +#ifdef CONFIG_MPFS_COREPWM1 + config_npwm++; +#endif + + for (npwm = 0; npwm < config_npwm; npwm++) { + lower_half = mpfs_corepwm_init(npwm); + + /* If we can't get the lower-half handle, skip and keep going. */ + + if (lower_half) { + /* Translate the peripheral number to a device name. */ + snprintf(devname, sizeof(devname), PWM_DEV_NAME, npwm); + pwm_register(devname, lower_half); + } + } + + return 0; +} diff --git a/boards/mpfs/icicle/src/mpfs_spinor.c b/boards/mpfs/icicle/src/mpfs_spinor.c new file mode 100644 index 000000000000..4d5dc9ce8802 --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_spinor.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include + +#include "board_config.h" +#include "hw_config.h" + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_spinor_init + * + * Description: + * Configure the SPI NOR flash driver. + * + * Returned Value: + * Zero (OK) is returned on success; A negated errno value is returned + * to indicate the nature of any failure. + * + ****************************************************************************/ + +#ifdef CONFIG_MTD_M25P +int mpfs_board_spinor_init(struct spi_dev_s *spinor) +{ + struct mtd_dev_s *mtd, *mtd_p0, *mtd_p1; + + const char *block0 = "/dev/mtdblock0"; + const char *block1 = "/dev/mtdblock1"; + struct mtd_geometry_s geo; + int ret; + + if (!spinor) { + syslog(LOG_ERR, "EROR: FAILED to initialize SPI port 1\n"); + return -ENODEV; + } + + mtd = m25p_initialize(spinor); + + if (!mtd) { + syslog(LOG_ERR, "ERROR: Failed to bind SPI port 1 to the SPI NOR driver\n"); + return -ENODEV; + } + + // Find the MTD geometry and calculate partition sizes + if (mtd->ioctl(mtd, MTDIOC_GEOMETRY, + (unsigned long)((uintptr_t)&geo))) { + _alert("ERROR: MTD geometry unknown\n"); + return -ENODEV; + } + + unsigned all_pages = (geo.neraseblocks * geo.erasesize) / geo.blocksize; + unsigned boot_pages = BOARD_FLASH_SIZE / geo.blocksize; + + // Allocate boot partition + mtd_p0 = mtd_partition(mtd, 0, boot_pages); + + if (!mtd_p0) { + syslog(LOG_ERR, "ERROR: Failed to create boot partition\n"); + return -ENODEV; + } + + // Allocate all the rest for lfs partition + mtd_p1 = mtd_partition(mtd, boot_pages, all_pages - boot_pages); + + if (!mtd_p1) { + syslog(LOG_ERR, "ERROR: Failed to create lfs partition\n"); + return -ENODEV; + } + + ret = register_mtddriver(block0, mtd_p0, 0777, NULL); + + if (ret != 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver for boot partition: %d\n", ret); + return ret; + } + + ret = register_mtddriver(block1, mtd_p1, 0777, NULL); + + if (ret != 0) { + syslog(LOG_ERR, "ERROR: Failed to register MTD driver for lfs partition: %d\n", ret); + return ret; + } + + return ret; +} +#endif diff --git a/boards/mpfs/icicle/src/mpfs_userspace.c b/boards/mpfs/icicle/src/mpfs_userspace.c new file mode 100755 index 000000000000..8988d5b919fd --- /dev/null +++ b/boards/mpfs/icicle/src/mpfs_userspace.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * boards/risc-v/mpfs/common/kernel/mpfs_userspace.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include + +#include +#include +#include +#include + +#if defined(CONFIG_BUILD_PROTECTED) && !defined(__KERNEL__) + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +#ifndef CONFIG_NUTTX_USERSPACE +# error "CONFIG_NUTTX_USERSPACE not defined" +#endif + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/* These 'addresses' of these values are setup by the linker script. + * They are not actual uint32_t storage locations! + * They are only used meaningfully in the following way: + * + * - The linker script defines, for example, the symbol_sdata. + * - The declaration extern uint32_t _sdata; makes C happy. C will believe + * that the value _sdata is the address of a uint32_t variable _data + * (it is not!). + * - We can recover the linker value then by simply taking the address of + * of _data. like: uint32_t *pdata = &_sdata; + */ + +extern uint32_t _stext; /* Start of .text */ +extern uint32_t _etext; /* End_1 of .text + .rodata */ +extern const uint32_t _eronly; /* End+1 of read only section */ +extern uint32_t _sdata; /* Start of .data */ +extern uint32_t _edata; /* End+1 of .data */ +extern uint32_t _sbss; /* Start of .bss */ +extern uint32_t _ebss; /* End+1 of .bss */ + +extern uintptr_t *__ld_usram_end; /* End+1 of user ram section */ + +/* This is the user space entry point */ + +int CONFIG_INIT_ENTRYPOINT(int argc, char *argv[]); + +const struct userspace_s userspace locate_data(".userspace") = { + /* General memory map */ + + .us_entrypoint = (main_t)CONFIG_INIT_ENTRYPOINT, + .us_textstart = (uintptr_t) &_stext, + .us_textend = (uintptr_t) &_etext, + .us_datasource = (uintptr_t) &_eronly, + .us_datastart = (uintptr_t) &_sdata, + .us_dataend = (uintptr_t) &_edata, + .us_bssstart = (uintptr_t) &_sbss, + .us_bssend = (uintptr_t) &_ebss, + + .us_heapend = (uintptr_t) &__ld_usram_end, + + /* Memory manager heap structure */ + + .us_heap = &g_mmheap, + + /* Task/thread startup routines */ + + .task_startup = nxtask_startup, + + /* Signal handler trampoline */ + + .signal_handler = up_signal_handler, + + /* User-space work queue support (declared in include/nuttx/wqueue.h) */ + +#ifdef CONFIG_LIBC_USRWORK + .work_usrstart = work_usrstart, +#endif +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +#endif /* CONFIG_BUILD_PROTECTED && !__KERNEL__ */ diff --git a/boards/mpfs/icicle/src/mtd.cpp b/boards/mpfs/icicle/src/mtd.cpp new file mode 100644 index 000000000000..fd8d20e9bbbc --- /dev/null +++ b/boards/mpfs/icicle/src/mtd.cpp @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +// KiB BS nB +static const px4_mft_device_t i2c0 = { // 24xx64 on Base 8K 32 X 256 + .bus_type = px4_mft_device_t::I2C, + .devid = PX4_MK_I2C_DEVID(1, 0x50) +}; + +static const px4_mtd_entry_t eeprom = { + .device = &i2c0, + .npart = 1, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 256 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &eeprom + } +}; + +static const px4_mft_entry_s mtd_mft = { + .type = MTD, + .pmft = (void *) &board_mtd_config, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mtd_mft + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/mpfs/icicle/src/spi.cpp b/boards/mpfs/icicle/src/spi.cpp new file mode 100644 index 000000000000..d500af88b36e --- /dev/null +++ b/boards/mpfs/icicle/src/spi.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBusInternal(SPI::Bus::SPI0, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, + SPI::CS{GPIO::Bank2, GPIO::Pin8}, + SPI::DRDY{}), + /* NOTE: Not in use + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, + SPI::CS{GPIO::Bank2, GPIO::Pin11}, + SPI::DRDY{}), + */ + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, + SPI::CS{GPIO::Bank2, GPIO::Pin9}, + SPI::DRDY{GPIO::Bank2, GPIO::Pin1}) + }), + initSPIBusInternal(SPI::Bus::SPI1, { + initSPIDevice(SPIDEV_FLASH(0), + SPI::CS{GPIO::Bank2, GPIO::Pin15}, + SPI::DRDY{}) + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/mpfs/icicle/src/spi_drv.cpp b/boards/mpfs/icicle/src/spi_drv.cpp new file mode 100644 index 000000000000..d0e59d46993f --- /dev/null +++ b/boards/mpfs/icicle/src/spi_drv.cpp @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +#include +#include +#include + +static const px4_spi_bus_t *_spi_bus0; +static const px4_spi_bus_t *_spi_bus1; + +static inline void board_spix_select(const px4_spi_bus_t *bus, struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (bus->devices[i].cs_gpio == 0) { + break; + } + + if (devid == bus->devices[i].devid) { + // SPI select is active low, so write !selected to select the device + mpfs_gpiowrite(bus->devices[i].cs_gpio, !selected); + } + } +} + +#if defined(CONFIG_MPFS_SPI0) +__EXPORT void mpfs_spi0_select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + board_spix_select(_spi_bus0, dev, devid, selected); +} +#endif + +#if defined(CONFIG_MPFS_SPI1) +__EXPORT void mpfs_spi1_select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + board_spix_select(_spi_bus1, dev, devid, selected); +} +#endif + + +__EXPORT void board_spidev_initialize(void) +{ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + px4_arch_configgpio(px4_spi_buses[bus].devices[i].cs_gpio); + } + } + } +} + +__EXPORT int board_spibus_initialize(void) +{ + // NOTE: MPFS uses 0based bus numbering + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + switch (px4_spi_buses[i].bus) { + case 1: _spi_bus0 = &px4_spi_buses[i]; break; + + case 2: _spi_bus1 = &px4_spi_buses[i]; break; + } + } + + struct spi_dev_s *spi_bus0 = px4_spibus_initialize(1); + + if (!spi_bus0) { + return -ENODEV; + } + + struct spi_dev_s *spi_bus1 = px4_spibus_initialize(2); + + if (!spi_bus1) { + return -ENODEV; + } + + /* deselect all */ + for (int bus = 0; bus < SPI_BUS_MAX_BUS_ITEMS; ++bus) { + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (px4_spi_buses[bus].devices[i].cs_gpio != 0) { + SPI_SELECT(spi_bus0, px4_spi_buses[bus].devices[i].devid, false); + SPI_SELECT(spi_bus1, px4_spi_buses[bus].devices[i].devid, false); + } + } + } + + return OK; +} diff --git a/boards/mpfs/icicle/src/toc.c b/boards/mpfs/icicle/src/toc.c new file mode 100644 index 000000000000..0c4c0d01ce75 --- /dev/null +++ b/boards/mpfs/icicle/src/toc.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +#include "board_config.h" +#include "board_type.h" + +/* Size of the signature */ + +#define SIGNATURE_SIZE PX4_SIGNATURE_SIZE(BOOTLOADER_SIGNING_ALGORITHM) + +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; + +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) + +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) + +/* Boot signature start and end are defined by the + * signature definition below +*/ +extern const uintptr_t _boot_signature; + +#define BOOTSIG_ADDR ((const void *)&_boot_signature) +#define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) + +/* RD certifcate may follow boot signature */ + +#define RDCT_ADDR BOOTSIG_END +#define RDCT_END ((const void *)((const uint8_t*)BOOTSIG_END+sizeof(image_cert_t))) + +/* RD certificate signature follows the certificate */ + +#define RDCTSIG_ADDR RDCT_END +#define RDCTSIG_END ((const void *)((const uint8_t*)RDCTSIG_ADDR+SIGNATURE_SIZE)) + +/* The table of contents */ + +IMAGE_MAIN_TOC(6) = { + {TOC_START_MAGIC, TOC_VERSION}, + { + {"TOC", TOC_ADDR, TOC_END, 0, 1, TOC_VERIFICATION_KEY, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, BOOT_VERIFICATION_KEY, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE, PX4_VENDOR_BOOT_FLAGS, IMAGE_TYPE_PX4, ARB_COUNT}, + {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, + }, + TOC_END_MAGIC +}; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/px4/fmu-v4/test.px4board b/boards/px4/fmu-v4/test.px4board index 6f18488d5ca0..c1c005f90524 100644 --- a/boards/px4/fmu-v4/test.px4board +++ b/boards/px4/fmu-v4/test.px4board @@ -1,4 +1,6 @@ CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n diff --git a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig index ff995b6cf5bb..8b9bceb4cbb4 100644 --- a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig @@ -134,6 +134,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/debug/defconfig b/boards/px4/fmu-v5/nuttx-config/debug/defconfig index 74f4ecac3c7c..a7ea4e784241 100644 --- a/boards/px4/fmu-v5/nuttx-config/debug/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/debug/defconfig @@ -178,6 +178,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index b0453967b2a9..4f92a79ff28f 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -90,6 +90,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 CONFIG_EXAMPLES_CALIB_UDELAY=y +CONFIG_DISABLE_PTHREAD=n CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y @@ -104,6 +105,7 @@ CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y CONFIG_FS_ROMFS=y +CONFIG_FS_SHMSF=y CONFIG_GRAN=y CONFIG_GRAN_INTR=y CONFIG_HAVE_CXX=y @@ -132,6 +134,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/protected/defconfig b/boards/px4/fmu-v5/nuttx-config/protected/defconfig index 1902014e3b08..24bbc8cf4c77 100644 --- a/boards/px4/fmu-v5/nuttx-config/protected/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/protected/defconfig @@ -137,6 +137,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5/nuttx-config/scripts/script.ld index d73a97deb9ce..4aa8307a9b8f 100644 --- a/boards/px4/fmu-v5/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v5/nuttx-config/scripts/script.ld @@ -82,7 +82,7 @@ MEMORY OUTPUT_ARCH(arm) EXTERN(_vectors) -ENTRY(_stext) +ENTRY(_vectors) /* * Ensure that abort() is present in the final object. The exception handling @@ -91,20 +91,39 @@ ENTRY(_stext) EXTERN(abort) EXTERN(_bootdelay_signature) EXTERN(_main_toc) +EXTERN(_main_toc_sig) SECTIONS { - .text : { + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(0x1000); + } > FLASH_AXIM + + .vectors : { _stext = ABSOLUTE(.); *(.vectors) . = ALIGN(32); + } > FLASH_AXIM + + .text : { /* This signature provides the bootloader with a way to delay booting */ _bootdelay_signature = ABSOLUTE(.); FILL(0xffecc2925d7d05c5) . += 8; - *(.main_toc) *(.text .text.*) *(.fixup) *(.gnu.warning) @@ -180,11 +199,4 @@ SECTIONS } > ITCM_RAM AT > FLASH_AXIM _framfuncs = LOADADDR(.ramfunc); - - /* Start of the image signature. This - * has to be in the end of the image - */ - .signature : { - _boot_signature = ALIGN(4); - } > FLASH_AXIM } diff --git a/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld b/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..b68869ab9fb6 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/scripts/toc.ld @@ -0,0 +1,63 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + /* -64 <- Leave room for the signature */ + progmem (r) : ORIGIN = 0x08008000, LENGTH = 2016K - 64 +} + +OUTPUT_ARCH("arm") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = 0x1000 - 64; + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig b/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig new file mode 100644 index 000000000000..91633794bf38 --- /dev/null +++ b/boards/px4/fmu-v5/nuttx-config/ssrc/defconfig @@ -0,0 +1,265 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_FS_PROCFS_EXCLUDE_MEMINFO is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_UPTIME is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0032 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3D Robotics" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FILE_STREAM=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBM_TOOLCHAIN=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=48 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC1_SDIO_MODE=y +CONFIG_SDMMC1_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32F7_ADC1=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC1=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI4=y +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_SERIAL_CONSOLE=y +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_RXDMA=y +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index c5450ab1f4f9..8ef7d34d1f13 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -133,6 +133,34 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5/src/toc.c b/boards/px4/fmu-v5/src/toc.c index 5f3fec31f412..7a1bf1ee53e7 100644 --- a/boards/px4/fmu-v5/src/toc.c +++ b/boards/px4/fmu-v5/src/toc.c @@ -33,21 +33,36 @@ #include /* (Maximum) size of the signature */ + #define SIGNATURE_SIZE 64 -/* Boot image starts at _vectors and ends at - * the beginning of signature -*/ +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; -extern uint32_t _vectors[]; -extern const int *_boot_signature; +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) -#define BOOT_ADDR _vectors -#define BOOT_END ((const void *)&_boot_signature) +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) /* Boot signature start and end are defined by the * signature definition below */ +extern const uintptr_t _boot_signature; #define BOOTSIG_ADDR ((const void *)&_boot_signature) #define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) @@ -64,13 +79,19 @@ extern const int *_boot_signature; /* The table of contents */ -IMAGE_MAIN_TOC(4) = { +IMAGE_MAIN_TOC(6) = { {TOC_START_MAGIC, TOC_VERSION}, { - {"BOOT", BOOT_ADDR, BOOT_END, 0, 1, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, + {"TOC", TOC_ADDR, TOC_END, 0, 1, 0, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, - {"RDCT", RDCT_ADDR, RDCT_END, 0, 3, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, }, TOC_END_MAGIC }; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/px4/fmu-v5/ssrc.px4board b/boards/px4/fmu-v5/ssrc.px4board new file mode 100644 index 000000000000..ac15a9d41529 --- /dev/null +++ b/boards/px4/fmu-v5/ssrc.px4board @@ -0,0 +1,21 @@ +CONFIG_BOARD_CRYPTO=y +CONFIG_COMMON_OPTICAL_FLOW=n +CONFIG_COMMON_OSD=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_CAMERA_FEEDBACK=n +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_EXAMPLES_FAKE_GPS=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n +CONFIG_SYSTEMCMDS_SERIAL_TEST=n +CONFIG_SYSTEMCMDS_I2CDETECT=n + +# Test keys to help local building +# These can be overridden in CI with environment variables pointing to real keys +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY2="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index 626efaf02f6b..db3e9523a07b 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -1,4 +1,7 @@ CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_BATT_SMBUS=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n @@ -14,3 +17,5 @@ CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n +CONFIG_DRIVERS_UAVCAN=n + diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index ba812e4b7bfd..b9a040d9a3a0 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -19,3 +19,5 @@ param set-default SENS_EN_INA226 1 param set-default SYS_USE_IO 1 safety_button start + +set LOGGER_BUF 8 diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index dad84376ce73..0d69e36794f6 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -8,3 +8,12 @@ then # Start MAVLink on the UART connected to the mission computer mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z fi + +# Start MC flight control mavlink to ethernet interface +mavlink start -c 192.168.200.100 -u 14541 -o 14540 -r 2000000 -x + +# Start uXRCE-DDS client on UDP +microdds_client start -t udp -h 192.168.200.100 -r 2019 -p 2020 + +# Start MC maintenance mavlink to ethernet interface +#mavlink start -c 192.168.200.100 -u 14543 -o 14542 -r 2000000 -m magic -x diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 5de6af261f5f..996e9b9ae38c 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -93,6 +93,7 @@ CONFIG_DEV_FIFO_SIZE=0 CONFIG_DEV_GPIO=y CONFIG_DEV_PIPE_MAXSIZE=1024 CONFIG_DEV_PIPE_SIZE=70 +CONFIG_DISABLE_PTHREAD=n CONFIG_ETH0_PHY_LAN8742A=y CONFIG_FAT_DMAMEMORY=y CONFIG_FAT_LCNAMES=y @@ -109,6 +110,7 @@ CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y CONFIG_FS_PROCFS_MAX_TASKS=64 CONFIG_FS_PROCFS_REGISTER=y CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y CONFIG_GRAN=y CONFIG_GRAN_INTR=y CONFIG_HAVE_CXX=y @@ -136,6 +138,7 @@ CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y CONFIG_MTD_PARTITION=y CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 CONFIG_NET=y CONFIG_NETDB_DNSCLIENT=y CONFIG_NETDB_DNSCLIENT_ENTRIES=8 @@ -169,6 +172,37 @@ CONFIG_NSH_ARGCAT=y CONFIG_NSH_BUILTIN_APPS=y CONFIG_NSH_CMDPARMS=y CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_DISABLE_CAT=n +CONFIG_NSH_DISABLE_CD=n +CONFIG_NSH_DISABLE_CP=n +CONFIG_NSH_DISABLE_DATE=n +CONFIG_NSH_DISABLE_ECHO=n +CONFIG_NSH_DISABLE_ENV=n +CONFIG_NSH_DISABLE_EXPORT=n +CONFIG_NSH_DISABLE_FREE=n +CONFIG_NSH_DISABLE_HELP=n +CONFIG_NSH_DISABLE_IFCONFIG=n +CONFIG_NSH_DISABLE_IFUPDOWN=n +CONFIG_NSH_DISABLE_KILL=n +CONFIG_NSH_DISABLE_LS=n +CONFIG_NSH_DISABLE_MKDIR=n +CONFIG_NSH_DISABLE_MOUNT=n +CONFIG_NSH_DISABLE_MV=n +CONFIG_NSH_DISABLE_PS=n +CONFIG_NSH_DISABLE_PSSTACKUSAGE=n +CONFIG_NSH_DISABLE_PWD=n +CONFIG_NSH_DISABLE_RM=n +CONFIG_NSH_DISABLE_RMDIR=n +CONFIG_NSH_DISABLE_SET=n +CONFIG_NSH_DISABLE_SOURCE=n +CONFIG_NSH_DISABLE_SLEEP=n +CONFIG_NSH_DISABLE_TEST=n +CONFIG_NSH_DISABLE_TELNETD=n +CONFIG_NSH_DISABLE_UMOUNT=n +CONFIG_NSH_DISABLE_UNSET=n +CONFIG_NSH_DISABLE_USLEEP=n CONFIG_NSH_LINELEN=128 CONFIG_NSH_MAXARGUMENTS=15 CONFIG_NSH_NESTDEPTH=8 diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld index 8d5a78da6bfc..4aa8307a9b8f 100644 --- a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld @@ -82,7 +82,7 @@ MEMORY OUTPUT_ARCH(arm) EXTERN(_vectors) -ENTRY(_stext) +ENTRY(_vectors) /* * Ensure that abort() is present in the final object. The exception handling @@ -90,14 +90,34 @@ ENTRY(_stext) */ EXTERN(abort) EXTERN(_bootdelay_signature) -EXTERN(board_get_manifest) +EXTERN(_main_toc) +EXTERN(_main_toc_sig) SECTIONS { - .text : { + /* Provide dummy relocation symbols for the dummy ToC */ + PROVIDE(_toc_start = 0); + PROVIDE(_toc_end = 0); + PROVIDE(_toc_signature = 0); + PROVIDE(_app_start = 0); + PROVIDE(_app_end = 0); + PROVIDE(_boot_signature = 0); + + /* Make a hole for the ToC and signature */ + .toc (NOLOAD) : { + *(.main_toc) + *(.main_toc_sig) + FILL(0xff); + . = ALIGN(0x1000); + } > FLASH_AXIM + + .vectors : { _stext = ABSOLUTE(.); *(.vectors) . = ALIGN(32); + } > FLASH_AXIM + + .text : { /* This signature provides the bootloader with a way to delay booting */ @@ -127,14 +147,6 @@ SECTIONS _einit = ABSOLUTE(.); } > FLASH_AXIM - /* - * Construction data for parameters. - */ - __param ALIGN(4): { - __param_start = ABSOLUTE(.); - KEEP(*(__param*)) - __param_end = ABSOLUTE(.); - } > FLASH_AXIM .ARM.extab : { *(.ARM.extab*) diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld new file mode 100644 index 000000000000..b68869ab9fb6 --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/scripts/toc.ld @@ -0,0 +1,63 @@ +/**************************************************************************** + * boards/risc-v/icicle/mpfs/scripts/ld.script + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +MEMORY +{ + /* -64 <- Leave room for the signature */ + progmem (r) : ORIGIN = 0x08008000, LENGTH = 2016K - 64 +} + +OUTPUT_ARCH("arm") + +EXTERN(_main_toc) + +SECTIONS +{ + .toc : { + /* The ToC */ + _toc_start = ABSOLUTE(.); + KEEP(*(.main_toc)); + /* Padd the rest */ + FILL(0xff); + . = 0x1000 - 64; + _toc_end = ABSOLUTE(.); + } > progmem + + /* Start of the ToC signature, appended directly after ToC */ + PROVIDE(_toc_signature = ALIGN(4)); + + .toc_sig (NOLOAD) : { + /* Make a hole for the singature */ + KEEP(*(.main_toc_sig)); + } > progmem + + .app (NOLOAD) : { + /* The application firmware payload */ + _app_start = ABSOLUTE(.); + *(.firmware) + . = ALIGN(4); + _app_end = ABSOLUTE(.); + } > progmem + + /* Start of the payload signature. This has to be in the end of the + * payload and aligned to a 4 byte boundary + */ + PROVIDE(_boot_signature = ALIGN(4)); +} diff --git a/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig b/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig new file mode 100644 index 000000000000..5ad4e11f54df --- /dev/null +++ b/boards/px4/fmu-v5x/nuttx-config/ssrc/defconfig @@ -0,0 +1,314 @@ +# +# This file is autogenerated: PLEASE DO NOT EDIT IT. +# +# You can use "make menuconfig" to make any modifications to the installed .config file. +# You can then do "make savedefconfig" to generate a new defconfig file that includes your +# modifications. +# +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_FS_PROCFS_EXCLUDE_MEMINFO is not set +# CONFIG_MMCSD_HAVE_CARDDETECT is not set +# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_SPI is not set +# CONFIG_NSH_DISABLEBG is not set +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLE_ARP is not set +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DATE is not set +# CONFIG_NSH_DISABLE_DF is not set +# CONFIG_NSH_DISABLE_DMESG is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_ENV is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_EXPORT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_IFUPDOWN is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MV is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PSSTACKUSAGE is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_SOURCE is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_TIME is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_UPTIME is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5x/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32f7" +CONFIG_ARCH_CHIP_STM32F765II=y +CONFIG_ARCH_CHIP_STM32F7=y +CONFIG_ARCH_INTERRUPTSTACK=1024 +CONFIG_ARCH_STACKDUMP=y +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_DCACHE=y +CONFIG_ARMV7M_DTCM=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARM_MPU_EARLY_RESET=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0033 +CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Auterion" +CONFIG_CRYPTO=y +CONFIG_CRYPTO_RANDOM_POOL=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_GPIO=y +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_ETH0_PHY_LAN8742A=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FILE_STREAM=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=y +CONFIG_FS_SHMFS=y +CONFIG_GRAN=y +CONFIG_GRAN_INTR=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_I2C=y +CONFIG_I2C_RESET=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="nsh_main" +CONFIG_INIT_STACKSIZE=2944 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_THROTTLE=0 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_NETDB=y +CONFIG_LIBC_STRERROR=y +CONFIG_LIBM_TOOLCHAIN=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=3 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=48 +CONFIG_NET=y +CONFIG_NETDEV_IFINDEX=y +CONFIG_NETDEV_PHY_IOCTL=y +CONFIG_NETINIT_DRIPADDR=0xc0a8c864 +CONFIG_NETINIT_IPADDR=0xc0a8c865 +CONFIG_NET_ARP_IPIN=y +CONFIG_NET_ARP_SEND=y +CONFIG_NET_BROADCAST=y +CONFIG_NET_ETH_PKTSIZE=1518 +CONFIG_NET_ICMP=y +CONFIG_NET_ICMP_SOCKET=y +CONFIG_NET_SOCKOPTS=y +CONFIG_NET_TCP=y +CONFIG_NET_TCPBACKLOG=y +CONFIG_NET_TCP_DELAYED_ACK=y +CONFIG_NET_TCP_NOTIFIER=y +CONFIG_NET_TCP_WRITE_BUFFERS=y +CONFIG_NET_UDP=y +CONFIG_NET_UDP_CHECKSUMS=y +CONFIG_NET_UDP_NOTIFIER=y +CONFIG_NET_UDP_WRITE_BUFFERS=y +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CROMFSETC=y +CONFIG_NSH_CUSTOMROMFS=y +CONFIG_NSH_CUSTOMROMFS_HEADER="../../include/nsh_romfsimg.h" +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=15 +CONFIG_NSH_NESTDEPTH=8 +CONFIG_NSH_QUOTE=y +CONFIG_NSH_ROMFSETC=y +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_VARS=y +CONFIG_OTG_ID_GPIO_DISABLE=y +CONFIG_PIPES=y +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_SETSPEED=y +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_READLINE_CMD_HISTORY=y +CONFIG_READLINE_TABCOMPLETION=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_SWITCH=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDMMC2_SDIO_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32F7_ADC1=y +CONFIG_STM32F7_ADC3=y +CONFIG_STM32F7_BBSRAM=y +CONFIG_STM32F7_BBSRAM_FILES=5 +CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_DMA1=y +CONFIG_STM32F7_DMA2=y +CONFIG_STM32F7_DMACAPABLE=y +CONFIG_STM32F7_ETHMAC=y +CONFIG_STM32F7_FLOWCONTROL_BROKEN=y +CONFIG_STM32F7_I2C1=y +CONFIG_STM32F7_I2C2=y +CONFIG_STM32F7_I2C3=y +CONFIG_STM32F7_I2C4=y +CONFIG_STM32F7_I2C_DYNTIMEO=y +CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32F7_OTGFS=y +CONFIG_STM32F7_PHYADDR=0 +CONFIG_STM32F7_PHYSR=31 +CONFIG_STM32F7_PHYSR_100MBPS=0x8 +CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10 +CONFIG_STM32F7_PHYSR_MODE=0x10 +CONFIG_STM32F7_PHYSR_SPEED=0x8 +CONFIG_STM32F7_PHY_POLLING=y +CONFIG_STM32F7_PROGMEM=y +CONFIG_STM32F7_PWR=y +CONFIG_STM32F7_RTC=y +CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32F7_RTC_MAGIC_REG=1 +CONFIG_STM32F7_SAVE_CRASHDUMP=y +CONFIG_STM32F7_SDMMC2=y +CONFIG_STM32F7_SDMMC_DMA=y +CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32F7_SPI1=y +CONFIG_STM32F7_SPI1_DMA=y +CONFIG_STM32F7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI2=y +CONFIG_STM32F7_SPI2_DMA=y +CONFIG_STM32F7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32F7_SPI3=y +CONFIG_STM32F7_SPI3_DMA=y +CONFIG_STM32F7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32F7_SPI5=y +CONFIG_STM32F7_SPI6=y +CONFIG_STM32F7_SPI_DMATHRESHOLD=8 +CONFIG_STM32F7_TIM10=y +CONFIG_STM32F7_TIM11=y +CONFIG_STM32F7_TIM3=y +CONFIG_STM32F7_TIM9=y +CONFIG_STM32F7_UART4=y +CONFIG_STM32F7_UART5=y +CONFIG_STM32F7_UART7=y +CONFIG_STM32F7_UART8=y +CONFIG_STM32F7_USART1=y +CONFIG_STM32F7_USART2=y +CONFIG_STM32F7_USART3=y +CONFIG_STM32F7_USART6=y +CONFIG_STM32F7_USART_BREAKS=y +CONFIG_STM32F7_USART_INVERT=y +CONFIG_STM32F7_USART_SINGLEWIRE=y +CONFIG_STM32F7_USART_SWAP=y +CONFIG_STM32F7_WWDG=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_SYSTEM_PING=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART5_IFLOWCONTROL=y +CONFIG_UART5_OFLOWCONTROL=y +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_RXDMA=y +CONFIG_UART5_TXBUFSIZE=3000 +CONFIG_UART5_TXDMA=y +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_IFLOWCONTROL=y +CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_RXDMA=y +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt index 684b11736eda..def8804acf95 100644 --- a/boards/px4/fmu-v5x/src/CMakeLists.txt +++ b/boards/px4/fmu-v5x/src/CMakeLists.txt @@ -31,17 +31,22 @@ # ############################################################################ +add_library(board_bus_info + i2c.cpp + spi.cpp +) +add_dependencies(board_bus_info nuttx_context) + add_library(drivers_board can.c - i2c.cpp init.cpp led.c manifest.c mtd.cpp sdio.c - spi.cpp timer_config.cpp usb.c + toc.c ) add_dependencies(drivers_board platform_gpio_mcp23009) @@ -55,5 +60,6 @@ target_link_libraries(drivers_board nuttx_arch # sdio nuttx_drivers # sdio px4_layer + board_bus_info platform_gpio_mcp23009 ) diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 281c769f2646..096f344e9c91 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -43,6 +43,7 @@ * Included Files ****************************************************************************************************/ +#include #include #include #include diff --git a/boards/px4/fmu-v5x/src/toc.c b/boards/px4/fmu-v5x/src/toc.c new file mode 100644 index 000000000000..ba9ff4d033bb --- /dev/null +++ b/boards/px4/fmu-v5x/src/toc.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * + * Copyright (C) 2020 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 + +/* (Maximum) size of the signature */ + +#define SIGNATURE_SIZE 64 + +/* ToC area boundaries */ +extern const uintptr_t _toc_start; +extern const uintptr_t _toc_end; + +#define TOC_ADDR &_toc_start +#define TOC_END ((const void *)&_toc_end) + +/* ToC signature */ +extern const uintptr_t _toc_signature; + +#define TOCSIG_ADDR ((const void *)&_toc_signature) +#define TOCSIG_END ((const void *)((const uint8_t *)TOCSIG_ADDR+SIGNATURE_SIZE)) + +/* Boot image starts at __start and ends at + * the beginning of signature, but for protected/kernel mode we don't know + * their locations. Assume binary file start and binary file end ? +*/ +extern const uintptr_t _app_start; +extern const uintptr_t _app_end; + +#define BOOT_ADDR &_app_start +#define BOOT_END ((const void *)&_app_end) + +/* Boot signature start and end are defined by the + * signature definition below +*/ +extern const uintptr_t _boot_signature; + +#define BOOTSIG_ADDR ((const void *)&_boot_signature) +#define BOOTSIG_END ((const void *)((const uint8_t *)BOOTSIG_ADDR+SIGNATURE_SIZE)) + +/* RD certifcate may follow boot signature */ + +#define RDCT_ADDR BOOTSIG_END +#define RDCT_END ((const void *)((const uint8_t*)BOOTSIG_END+sizeof(image_cert_t))) + +/* RD certificate signature follows the certificate */ + +#define RDCTSIG_ADDR RDCT_END +#define RDCTSIG_END ((const void *)((const uint8_t*)RDCTSIG_ADDR+SIGNATURE_SIZE)) + +/* The table of contents */ + +IMAGE_MAIN_TOC(6) = { + {TOC_START_MAGIC, TOC_VERSION}, + { + {"TOC", TOC_ADDR, TOC_END, 0, 1, 0, 0, TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG0", TOCSIG_ADDR, TOCSIG_END, 0, 0, 0, 0, 0}, + {"BOOT", BOOT_ADDR, BOOT_END, 0, 3, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE}, + {"SIG1", BOOTSIG_ADDR, BOOTSIG_END, 0, 0, 0, 0, 0}, + /* + {"RDCT", RDCT_ADDR, RDCT_END, 0, 5, 0, 0, TOC_FLAG1_RDCT | TOC_FLAG1_CHECK_SIGNATURE}, + {"RDSG", RDCTSIG_ADDR, RDCTSIG_END, 0, 0, 0, 0, 0}, + */ + }, + TOC_END_MAGIC +}; + +/* Define a signature area, just for sizing the ToC area */ + +const char _main_toc_sig[SIGNATURE_SIZE] __attribute__((section(".main_toc_sig"))); diff --git a/boards/px4/fmu-v5x/ssrc.px4board b/boards/px4/fmu-v5x/ssrc.px4board new file mode 100644 index 000000000000..fbeef20f68b2 --- /dev/null +++ b/boards/px4/fmu-v5x/ssrc.px4board @@ -0,0 +1,27 @@ + +# In addition to default.px4foard + +CONFIG_BOARD_CRYPTO=y +CONFIG_DRIVERS_SW_CRYPTO=y +CONFIG_DRIVERS_STUB_KEYSTORE=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y + +# Removed to save space + +CONFIG_COMMON_OPTICAL_FLOW=n +CONFIG_COMMON_OSD=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_CAMERA_FEEDBACK=n +CONFIG_MODULES_PAYLOAD_DELIVERER=n +CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_SYSTEMCMDS_NETMAN=n +CONFIG_SYSTEMCMDS_REFLECT=n +CONFIG_SYSTEMCMDS_SD_BENCH=n +CONFIG_SYSTEMCMDS_SD_STRESS=n + +# Test keys to help local building +# These can be overridden in CI with environment variables pointing to real keys +CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/key0.pub" +CONFIG_PUBLIC_KEY2="../../../Tools/test_keys/rsa2048.pub" diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board index 0b1517d173f1..5f4f13f98f8c 100644 --- a/boards/px4/fmu-v5x/test.px4board +++ b/boards/px4/fmu-v5x/test.px4board @@ -2,6 +2,8 @@ CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_INS=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 484b844f1910..cb9b676b6856 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -12,7 +12,6 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index db358c3e4d0e..bdc4dd4362a2 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -5,6 +5,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_PWM_OUT_SIM=y CONFIG_MODULES_AIRSHIP_ATT_CONTROL=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y diff --git a/boards/ssrc/common b/boards/ssrc/common new file mode 160000 index 000000000000..8d03c1a54ad4 --- /dev/null +++ b/boards/ssrc/common @@ -0,0 +1 @@ +Subproject commit 8d03c1a54ad48a665f6b287ca0ae01377901001b diff --git a/boards/ssrc/saluki-pi b/boards/ssrc/saluki-pi new file mode 160000 index 000000000000..48b65a0fd9e9 --- /dev/null +++ b/boards/ssrc/saluki-pi @@ -0,0 +1 @@ +Subproject commit 48b65a0fd9e9345551116f20219c2ec58daa0a38 diff --git a/boards/ssrc/saluki-v1 b/boards/ssrc/saluki-v1 new file mode 160000 index 000000000000..a7d26ae9a718 --- /dev/null +++ b/boards/ssrc/saluki-v1 @@ -0,0 +1 @@ +Subproject commit a7d26ae9a718411dbd3a82496904eb6727ccf5c9 diff --git a/boards/ssrc/saluki-v2 b/boards/ssrc/saluki-v2 new file mode 160000 index 000000000000..64c1c219cbf0 --- /dev/null +++ b/boards/ssrc/saluki-v2 @@ -0,0 +1 @@ +Subproject commit 64c1c219cbf0f9ba10ce889a6917780668c4cf2b diff --git a/boards/ssrc/saluki-v3 b/boards/ssrc/saluki-v3 new file mode 160000 index 000000000000..588fcdb9d617 --- /dev/null +++ b/boards/ssrc/saluki-v3 @@ -0,0 +1 @@ +Subproject commit 588fcdb9d617df982e794842b536802e4f4a9c62 diff --git a/build.sh b/build.sh new file mode 100755 index 000000000000..f8545656e931 --- /dev/null +++ b/build.sh @@ -0,0 +1,136 @@ +#!/bin/bash -eux + +usage() { + set +x + echo "" + echo " usage: $0 " + echo " output-dir : directory for output artifacts" + echo " build-target : supported build targets:" + echo " px4fwupdater" + echo " pixhawk" + echo " saluki-v1_default" + echo " saluki-v1_protected" + echo " saluki-v1_amp" + echo " saluki-v2_default" + echo " saluki-v2_amp" + echo " saluki-v2_protected" + echo " saluki-v2_kernel" + echo " saluki-pi_default" + echo " saluki-pi_amp" + echo " saluki-pi_protected" + echo " saluki-v3_default" + echo " saluki-v3_amp" + echo + exit 1 +} +if [ -z ${SIGNING_ARGS+x} ]; then + SIGNING_ARGS="" +else + echo "using custom signing keys: ${SIGNING_ARGS}" +fi + +dest_dir="${1:-}" +target="${2:-}" + +if [ -z "$dest_dir" ]; then + usage +fi + +version=$(git describe --always --tags --dirty | sed 's/^v//') +script_dir=$(dirname $(realpath $0)) +dest_dir=$(realpath $1) +iname_env=tii_px4_build + +mkdir -p ${dest_dir} +pushd ${script_dir} + +build_env="docker build --build-arg UID=$(id -u) --build-arg GID=$(id -g) --pull -f ./packaging/Dockerfile.build_env -t ${iname_env} ." +build_cmd_fw="docker run --rm -e SIGNING_ARGS=${SIGNING_ARGS} -v ${script_dir}:/px4-firmware/sources ${iname_env} ./packaging/build_px4fw.sh" +build_cmd_px4fwupdater="${script_dir}/packaging/build_px4fwupdater.sh -v ${version} -i ${dest_dir}" + +# Generate build_env +if [ "${target}" != px4fwupdater ]; then + $build_env +fi + +case $target in + "px4fwupdater") + $build_cmd_px4fwupdater + ;; + "pixhawk") + $build_cmd_fw px4_fmu-v5x_ssrc + cp ${script_dir}/build/px4_fmu-v5x_ssrc/px4_fmu-v5x_ssrc.px4 ${dest_dir}/px4_fmu-v5x_ssrc-${version}.px4 + ;; + "saluki-v1_default") + $build_cmd_fw ssrc_saluki-v1_default + cp ${script_dir}/build/ssrc_saluki-v1_default/ssrc_saluki-v1_default.px4 ${dest_dir}/ssrc_saluki-v1_default-${version}.px4 + ;; + "saluki-v1_protected") + $build_cmd_fw ssrc_saluki-v1_protected + cp ${script_dir}/build/ssrc_saluki-v1_protected/ssrc_saluki-v1_protected.px4 ${dest_dir}/ssrc_saluki-v1_protected-${version}.px4 + ;; + "saluki-v1_amp") + $build_cmd_fw ssrc_saluki-v1_amp + cp ${script_dir}/build/ssrc_saluki-v1_amp/ssrc_saluki-v1_amp.bin ${dest_dir}/ssrc_saluki-v1_amp-${version}.bin + ;; + "saluki-v2_default") + $build_cmd_fw ssrc_saluki-v2_default + cp ${script_dir}/build/ssrc_saluki-v2_default/ssrc_saluki-v2_default.px4 ${dest_dir}/ssrc_saluki-v2_default-${version}.px4 + ;; + "saluki-v2_protected") + $build_cmd_fw ssrc_saluki-v2_protected + cp ${script_dir}/build/ssrc_saluki-v2_protected/ssrc_saluki-v2_protected.px4 ${dest_dir}/ssrc_saluki-v2_protected-${version}.px4 + ;; + "saluki-v2_amp") + $build_cmd_fw ssrc_saluki-v2_amp + cp ${script_dir}/build/ssrc_saluki-v2_amp/ssrc_saluki-v2_amp.bin ${dest_dir}/ssrc_saluki-v2_amp-${version}.bin + ;; + "saluki-v2_kernel") + $build_cmd_fw ssrc_saluki-v2_kernel + cp ${script_dir}/build/ssrc_saluki-v2_kernel/ssrc_saluki-v2_kernel.px4 ${dest_dir}/ssrc_saluki-v2_kernel-${version}.px4 + cp ${script_dir}/build/ssrc_saluki-v2_kernel/ssrc_saluki-v2_kernel.bin ${dest_dir}/ssrc_saluki-v2_kernel-${version}.bin + cp ${script_dir}/build/ssrc_saluki-v2_kernel/ssrc_saluki-v2_kernel_kernel.elf ${dest_dir}/ssrc_saluki-v2_kernel-${version}.elf + ;; + "saluki-v2_custom_keys") + # on custom keys case we build _default target but SIGNING_ARGS env variable is set above in build_cmd_fw + $build_cmd_fw ssrc_saluki-v2_default + cp ${script_dir}/build/ssrc_saluki-v2_default/ssrc_saluki-v2_default.px4 ${dest_dir}/ssrc_saluki-v2_custom_keys-${version}.px4 + ;; + + "saluki-v3_default") + $build_cmd_fw ssrc_saluki-v3_default + cp ${script_dir}/build/ssrc_saluki-v3_default/ssrc_saluki-v3_default.px4 ${dest_dir}/ssrc_saluki-v3_default-${version}.px4 + ;; + "saluki-v3_amp") + $build_cmd_fw ssrc_saluki-v3_amp + cp ${script_dir}/build/ssrc_saluki-v3_amp/ssrc_saluki-v3_amp.bin ${dest_dir}/ssrc_saluki-v3_amp-${version}.bin + ;; + "saluki-v3_custom_keys") + # on custom keys case we build _default target but SIGNING_ARGS env variable is set above in build_cmd_fw + $build_cmd_fw ssrc_saluki-v3_default + cp ${script_dir}/build/ssrc_saluki-v3_default/ssrc_saluki-v3_default.px4 ${dest_dir}/ssrc_saluki-v3_custom_keys-${version}.px4 + ;; + "saluki-pi_default") + $build_cmd_fw ssrc_saluki-pi_default + cp ${script_dir}/build/ssrc_saluki-pi_default/ssrc_saluki-pi_default.px4 ${dest_dir}/ssrc_saluki-pi_default-${version}.px4 + ;; + "saluki-pi_protected") + $build_cmd_fw ssrc_saluki-pi_protected + cp ${script_dir}/build/ssrc_saluki-pi_protected/ssrc_saluki-pi_protected.px4 ${dest_dir}/ssrc_saluki-pi_protected-${version}.px4 + ;; + "saluki-pi_amp") + $build_cmd_fw ssrc_saluki-pi_amp + cp ${script_dir}/build/ssrc_saluki-pi_amp/ssrc_saluki-pi_amp.bin ${dest_dir}/ssrc_saluki-pi_amp-${version}.bin + ;; + "saluki-pi_custom_keys") + # on custom keys case we build _default target but SIGNING_ARGS env variable is set above in build_cmd_fw + $build_cmd_fw ssrc_saluki-pi_default + cp ${script_dir}/build/ssrc_saluki-pi_default/ssrc_saluki-pi_default.px4 ${dest_dir}/ssrc_saluki-pi_custom_keys-${version}.px4 + ;; + + *) + usage + ;; +esac + +echo "Done" diff --git a/build_sitl.sh b/build_sitl.sh new file mode 100755 index 000000000000..2abb7f54b81f --- /dev/null +++ b/build_sitl.sh @@ -0,0 +1,41 @@ +#!/bin/bash + +set -euxo pipefail + +if [ "$1" = "" ]; then + echo "ERROR: Package output directory not given" + echo " usage: $0 " + echo " output-dir : directory for output artifacts" + exit 1 +fi + +script_dir=$(dirname $(realpath $0)) +dest_dir=$(realpath $1) + +mkdir -p ${dest_dir} + +pushd ${script_dir} + +# Generate build_env +iname_env=tii_px4_build +docker build \ + --build-arg UID=$(id -u) \ + --build-arg GID=$(id -g) \ + --pull \ + --output type=docker \ + -f ./packaging/Dockerfile.build_env -t ${iname_env} . + +# Build Saluki image +version=$(git describe --always --tags --dirty | sed 's/^v//') + +docker run \ + --rm \ + -v ${script_dir}:/px4-firmware/sources \ + ${iname_env} \ + ./packaging/build_px4_sitl.sh \ + -v ${version} \ + +mv px4_sitl_build-*.tar.gz ${dest_dir}/ + +echo "Done" +exit 0 diff --git a/clone_public.sh b/clone_public.sh new file mode 100755 index 000000000000..c5ed7d21e599 --- /dev/null +++ b/clone_public.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +set -euo pipefail + +while read -r repo +do + [[ "${repo}" == boards/ssrc/* ]] || \ + [[ "${repo}" == *saluki-sec-scripts ]] || \ + [[ "${repo}" == *pfsoc_crypto ]] || \ + [[ "${repo}" == *pfsoc_keystore ]] || \ + [[ "${repo}" == *pf_crypto ]] || \ + [[ "${repo}" == *process ]] && continue + git submodule update --init --recursive "${repo}" +done <<< "$(git submodule status | awk '{print $2}')" diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 1a54456194c1..c93b15734a52 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -107,6 +107,7 @@ function(px4_add_functional_gtest) px4_daemon work_queue parameters + events perf tinybson uorb_msgs diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index 5c0ee1153d68..5a7c5c5c123a 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -87,7 +87,7 @@ function(px4_add_module) NAME px4_add_module ONE_VALUE MODULE MAIN STACK_MAIN STACK_MAX PRIORITY MULTI_VALUE COMPILE_FLAGS LINK_FLAGS SRCS INCLUDES DEPENDS MODULE_CONFIG - OPTIONS EXTERNAL DYNAMIC UNITY_BUILD + OPTIONS EXTERNAL DYNAMIC UNITY_BUILD NO_DAEMON REQUIRED MODULE MAIN ARGN ${ARGN}) @@ -166,12 +166,13 @@ function(px4_add_module) endif() if(NOT DYNAMIC) - target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf) + target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf uORB) if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL) - target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel) + target_link_libraries(${MODULE} PRIVATE + kernel_events_interface kernel_parameters_interface px4_kernel_layer) set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE}) else() - target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB) + target_link_libraries(${MODULE} PRIVATE events_interface parameters_interface px4_layer) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) @@ -220,6 +221,10 @@ function(px4_add_module) target_compile_options(${MODULE} PRIVATE ${COMPILE_FLAGS}) endif() + if(NO_DAEMON) + set_target_properties(${MODULE} PROPERTIES NO_DAEMON NO_DAEMON) + endif() + if (KERNEL) target_compile_options(${MODULE} PRIVATE -D__KERNEL__) endif() diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 30e4e3d16cb9..313ccd7c3ce8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -60,6 +60,8 @@ set(msg_files CollisionReport.msg ControlAllocatorStatus.msg Cpuload.msg + DatamanRequest.msg + DatamanResponse.msg DebugArray.msg DebugKeyValue.msg DebugValue.msg @@ -101,10 +103,12 @@ set(msg_files GpsInjectData.msg GimbalControls.msg Gripper.msg + Guid.msg HealthReport.msg HeaterStatus.msg HomePosition.msg HoverThrustEstimate.msg + HwInfo.msg InputRc.msg InternalCombustionEngineStatus.msg IridiumsbdStatus.msg @@ -154,6 +158,7 @@ set(msg_files RateCtrlStatus.msg RcChannels.msg RcParameterMap.msg + RoverStatus.msg Rpm.msg RtlTimeEstimate.msg SatelliteInfo.msg @@ -176,6 +181,8 @@ set(msg_files SensorsStatusImu.msg SensorUwb.msg SystemPower.msg + SystemVersion.msg + SystemVersionString.msg TakeoffStatus.msg TaskStackInfo.msg TecsStatus.msg diff --git a/msg/DatamanRequest.msg b/msg/DatamanRequest.msg new file mode 100644 index 000000000000..f819771a4528 --- /dev/null +++ b/msg/DatamanRequest.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data +uint32 data_length \ No newline at end of file diff --git a/msg/DatamanResponse.msg b/msg/DatamanResponse.msg new file mode 100644 index 000000000000..ebf752db50d2 --- /dev/null +++ b/msg/DatamanResponse.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data + +uint8 STATUS_SUCCESS = 0 +uint8 STATUS_FAILURE_ID_ERR = 1 +uint8 STATUS_FAILURE_NO_DATA = 2 +uint8 STATUS_FAILURE_READ_FAILED = 3 +uint8 STATUS_FAILURE_WRITE_FAILED = 4 +uint8 STATUS_FAILURE_CLEAR_FAILED = 5 +uint8 status diff --git a/msg/Guid.msg b/msg/Guid.msg new file mode 100644 index 000000000000..2ff4d6769875 --- /dev/null +++ b/msg/Guid.msg @@ -0,0 +1,4 @@ +# Guid (serial number) + +uint64 timestamp # time since system start (microseconds) +uint8[32] mfguid # guid diff --git a/msg/HwInfo.msg b/msg/HwInfo.msg new file mode 100644 index 000000000000..eb2e25f707cf --- /dev/null +++ b/msg/HwInfo.msg @@ -0,0 +1,4 @@ +# Hardware info string + +uint64 timestamp # time since system start (microseconds) +char[64] hw_info # main autopilot hw info string diff --git a/msg/Mission.msg b/msg/Mission.msg index 70fc68ccf2c1..c7740d4f7ab8 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -3,3 +3,7 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest + +int16 mission_update_counter # indicates updates to the mission, reload from dataman if increased +int16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased +int16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased diff --git a/msg/RoverStatus.msg b/msg/RoverStatus.msg new file mode 100644 index 000000000000..9e917222dbc7 --- /dev/null +++ b/msg/RoverStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) +float32 linear_velocity +float32 angular_velocity +uint8 vehicle_state +uint8 control_mode +uint16 error_code +float32 battery_voltage +bool light_control_enable +uint8 front_light_mode +uint8 front_light_custom_value +uint8 rear_light_mode +uint8 rear_light_custom_value diff --git a/msg/SystemVersion.msg b/msg/SystemVersion.msg new file mode 100644 index 000000000000..99450150c35b --- /dev/null +++ b/msg/SystemVersion.msg @@ -0,0 +1,11 @@ +# System version information in hash (binary) format + +uint64 timestamp # time since system start (microseconds) +uint64 hw_version # main autopilot hw version +uint64 hw_revision # main autopilot hw revision +uint64 soc_arch_id # main autopilot soc architecture identifier +uint64 sw_version # main autopilot sw version +uint64 os_version # OS or middleware version +uint64 bl_version # bootloader version +uint64 component_version1 # vendor specific component version +uint64 component_version2 # vendor specific component version diff --git a/msg/SystemVersionString.msg b/msg/SystemVersionString.msg new file mode 100644 index 000000000000..e1d3be880e47 --- /dev/null +++ b/msg/SystemVersionString.msg @@ -0,0 +1,9 @@ +# System version information in user readable string format + +uint64 timestamp # time since system start (microseconds) +char[32] hw_version # main autopilot hw version +char[32] sw_version # main autopilot sw version +char[32] os_version # OS or middleware version +char[32] bl_version # bootloader version +char[32] component_version1 # vendor specific component version +char[32] component_version2 # vendor specific component version diff --git a/msg/UlogStream.msg b/msg/UlogStream.msg index d206b4a42595..471683417674 100644 --- a/msg/UlogStream.msg +++ b/msg/UlogStream.msg @@ -17,3 +17,4 @@ uint8 flags # see FLAGS_* uint8[249] data # ulog data uint8 ORB_QUEUE_LENGTH = 16 # TODO: we might be able to reduce this if mavlink polled on the topic +# TOPICS ulog_stream ulog_stream_acked diff --git a/msg/UlogStreamAck.msg b/msg/UlogStreamAck.msg index e3747fff6ea6..9816d9bc0fdc 100644 --- a/msg/UlogStreamAck.msg +++ b/msg/UlogStreamAck.msg @@ -1,4 +1,4 @@ -# Ack a previously sent ulog_stream message that had +# Ack a previously sent ulog_stream_acked message that had # the NEED_ACK flag set uint64 timestamp # time since system start (microseconds) diff --git a/packaging/Dockerfile.build_env b/packaging/Dockerfile.build_env new file mode 100644 index 000000000000..5e05bf8331c3 --- /dev/null +++ b/packaging/Dockerfile.build_env @@ -0,0 +1,20 @@ +# px4-firmware builder environment +FROM ghcr.io/tiiuae/px4-firmware-builder-base:latest + +ARG UID=1000 +ARG GID=1000 + +RUN groupadd -o -g $GID builder && \ + useradd -m -u $UID -g $GID -g builder builder && \ + usermod -aG sudo builder && \ + echo 'builder ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers + +RUN mkdir -p /px4-firmware && chown -R builder:builder /px4-firmware +RUN mkdir -p /artifacts && chown -R builder:builder /artifacts + +USER builder + +VOLUME /px4-firmware/sources +WORKDIR /px4-firmware/sources + +RUN git config --global --add safe.directory /px4-firmware/sources diff --git a/packaging/Dockerfile.build_env_pre b/packaging/Dockerfile.build_env_pre new file mode 100644 index 000000000000..9e0d12903e95 --- /dev/null +++ b/packaging/Dockerfile.build_env_pre @@ -0,0 +1,36 @@ +# px4-firmware builder environment +FROM ros:humble-ros-base + +ENV LANG C.UTF-8 +ENV LANGUAGE C.UTF-8 +ENV LC_ALL C.UTF-8 +ENV RMW_IMPLEMENTATION rmw_fastrtps_cpp + +RUN apt-get update && apt-get install -y --no-install-recommends \ + git build-essential cmake lsb-core ninja-build \ + libboost-all-dev libeigen3-dev libgstreamer-plugins-base1.0-dev libopencv-dev \ + python3-empy python3-toml python3-numpy python3-genmsg python3-setuptools \ + python3-packaging python3-jinja2 python3-yaml openjdk-11-jre \ + gazebo libgazebo-dev \ + genromfs xxd curl unzip \ + python3-nacl python3-pip python3-future \ + ros-humble-gazebo-ros \ + ros-humble-fastrtps \ + ros-humble-rmw-fastrtps-cpp \ + fakeroot \ + dh-make \ + debhelper \ + && pip3 install kconfiglib jsonschema \ + && rm -rf /var/lib/apt/lists/* + +RUN mkdir -p /tools + +RUN curl -LOs https://static.dev.sifive.com/dev-tools/freedom-tools/v2020.12/riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz && \ + tar xf riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz -C /tools && \ + rm -f riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14.tar.gz + +RUN curl -LOs https://developer.arm.com/-/media/Files/downloads/gnu-rm/9-2019q4/gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 && \ + tar xvf gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 -C /tools && \ + rm -f gcc-arm-none-eabi-9-2019-q4-major-x86_64-linux.tar.bz2 + +ENV PATH=/tools/riscv64-unknown-elf-toolchain-10.2.0-2020.12.8-x86_64-linux-ubuntu14/bin:/tools/gcc-arm-none-eabi-9-2019-q4-major/bin/:$PATH diff --git a/packaging/Dockerfile.coverity b/packaging/Dockerfile.coverity new file mode 100644 index 000000000000..6358369f5ed1 --- /dev/null +++ b/packaging/Dockerfile.coverity @@ -0,0 +1,27 @@ +FROM ghcr.io/tiiuae/px4-firmware-builder-base:latest + +# coverity docker registry access token to download coverity tool +# for more details refer to confluence page "Coverity docker scan" +# https://ssrc.atlassian.net/wiki/spaces/DRON/pages/705823997/Coverity+docker+scan +ARG COVERITY_DOCKER_REGISTRY_USERNAME +ARG COVERITY_DOCKER_REGISTRY_ACCESS_TOKEN + +# coverity license file to run coverity tool +ARG COVERITY_LICENSE_DAT_B64 +RUN echo "$COVERITY_LICENSE_DAT_B64" | base64 -d > /license.dat + +# access token for our internal server +ARG COVERITY_ACCESS_TOKEN_B64 +RUN echo "$COVERITY_ACCESS_TOKEN_B64" | base64 -d > /auth_key.txt +RUN chmod 400 /auth_key.txt + +# - download coverity installer package +# - chmod and run the installer +# - remove installation package to reduce image size +RUN curl https://sig-repo.synopsys.com/artifactory/coverity-releases/2023.6.0/cov-analysis-linux64-2023.6.0.sh -o /coverity_install.sh -u ${COVERITY_DOCKER_REGISTRY_USERNAME}:${COVERITY_DOCKER_REGISTRY_ACCESS_TOKEN} \ + && chmod +x /coverity_install.sh \ + && /coverity_install.sh -q --installation.dir=/cov --license.region=6 --license.agreement=agree --license.type.choice=0 --license.cov.path=/license.dat \ + && rm /coverity_install.sh + +COPY px4-firmware/packaging/entrypoint_coverity.sh / +ENTRYPOINT [ "/entrypoint_coverity.sh" ] diff --git a/packaging/Dockerfile.sitl b/packaging/Dockerfile.sitl new file mode 100644 index 000000000000..a60728b8a31c --- /dev/null +++ b/packaging/Dockerfile.sitl @@ -0,0 +1,40 @@ +FROM ghcr.io/tiiuae/fog-ros-baseimage-builder:v2.1.0 + +RUN apt-get update && apt-get install -y --no-install-recommends \ + wget + +RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list +RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - +RUN apt-get update && apt-get install -y --no-install-recommends \ + libignition-transport8-dev \ + && rm -rf /var/lib/apt/lists/* + +RUN apt-get update && apt-get install -y --no-install-recommends \ + wget + +RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list +RUN wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add - +RUN apt-get update && apt-get install -y --no-install-recommends \ + libignition-transport8-dev \ + && rm -rf /var/lib/apt/lists/* + +# Install PX4 SITL +WORKDIR /packages + +COPY bin/px4_sitl_build*.tar.gz ./px4_sitl_build.tar.gz + +RUN tar -xzf px4_sitl_build.tar.gz \ + && mv px4_sitl/bin/* /usr/bin/ \ + && mv px4_sitl/etc /px4_sitl_etc \ + && rm -rf px4_sitl_build/ \ + && rm px4_sitl_build.tar.gz + +WORKDIR /px4_sitl + +COPY px4-firmware/ssrc_config /ssrc_config +COPY px4-firmware/packaging/entrypoint.sh . + +ENV PACKAGE_NAME=px4_sitl +ENV PX4_SIM_MODEL=ssrc_fog_x + +ENTRYPOINT ["/px4_sitl/entrypoint.sh"] diff --git a/packaging/Dockerfile.sitl_gzsim b/packaging/Dockerfile.sitl_gzsim new file mode 100644 index 000000000000..f0944e534919 --- /dev/null +++ b/packaging/Dockerfile.sitl_gzsim @@ -0,0 +1,76 @@ +FROM docker.io/ros:humble-ros-base as builder + +# Install git for pulling the base repository +RUN apt update +RUN apt install -y \ + git \ + curl \ + lsb-release \ + gnupg + +RUN curl http://packages.osrfoundation.org/gazebo.key | apt-key add - +RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + +# Install build dependencies +RUN apt update +RUN DEBIAN_FRONTEND=noninteractive TZ=Etc/UTC apt install -y \ + libgz-transport12 \ + libgz-transport12-dev \ + astyle \ + build-essential \ + cmake \ + cppcheck \ + file \ + g++ \ + gcc \ + gdb \ + git \ + lcov \ + libfuse2 \ + libxml2-dev \ + libxml2-utils \ + make \ + ninja-build \ + python3 \ + python3-dev \ + python3-pip \ + python3-setuptools \ + python3-wheel \ + rsync \ + shellcheck \ + unzip \ + ros-humble-fastrtps \ + ros-humble-rmw-fastrtps-cpp \ + fastddsgen + +# Checkout the px4 version and build it +RUN git clone https://github.com/tiiuae/px4-firmware +RUN python3 -m pip install -r px4-firmware/Tools/setup/requirements.txt +COPY build_px4_sitl_gzsim.sh ./build.sh +RUN ./build.sh + +# ▲ runtime ──┐ +# └── build ▼ + +FROM docker.io/ros:humble-ros-base + +RUN apt update +RUN apt install -y \ + curl \ + lsb-release \ + gnupg \ + ros-humble-fastrtps \ + ros-humble-rmw-fastrtps-cpp + +RUN curl http://packages.osrfoundation.org/gazebo.key | apt-key add - +RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + +RUN apt update +RUN DEBIAN_FRONTEND=noninteractive TZ=Etc/UTC apt install -y libgz-transport12 dnsutils + +WORKDIR /px4_sitl + +COPY --from=builder /px4-firmware/build/px4_sitl_default /px4_sitl +COPY ./entrypoint_sitl_gzsim.sh /entrypoint.sh + +CMD ["/entrypoint.sh"] diff --git a/packaging/build_px4_sitl.sh b/packaging/build_px4_sitl.sh new file mode 100755 index 000000000000..1b3345ac0102 --- /dev/null +++ b/packaging/build_px4_sitl.sh @@ -0,0 +1,65 @@ +set -eo pipefail + +usage() { + echo " +Usage: $(basename "$0") [-h] [-v version] + -- Generate tar packages from px4_sitl module. +Params: + -h Show help text. + -v Git version string +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +mod_dir="$(realpath $(dirname $0)/..)" +VERSION="" + +while getopts "hv:" opt +do + case $opt in + h) + usage + ;; + v) + check_arg $OPTARG && VERSION=$OPTARG || error_arg $opt + ;; + \?) + usage + ;; + esac +done + +source /opt/ros/humble/setup.sh + + +# Remove old build output +rm -Rf build/px4_sitl_default + +# Build +make px4_sitl_default + +# tar artifacts +mkdir -p tmp_packing_dir +pushd tmp_packing_dir + +mkdir -p px4_sitl/build/px4_sitl_default + +cp -r ../build/px4_sitl_default/bin px4_sitl/bin +cp -r ../build/px4_sitl_default/etc px4_sitl/etc +tar czvf ../px4_sitl_build-v${VERSION}.tar.gz px4_sitl + +popd +rm -Rf tmp_packing_dir diff --git a/packaging/build_px4_sitl_gzsim.sh b/packaging/build_px4_sitl_gzsim.sh new file mode 100755 index 000000000000..5ee8050ace18 --- /dev/null +++ b/packaging/build_px4_sitl_gzsim.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +source /opt/ros/humble/setup.sh + +pushd px4-firmware + make px4_sitl_default +popd diff --git a/packaging/build_px4fw.sh b/packaging/build_px4fw.sh new file mode 100755 index 000000000000..eb0c7e1f4d27 --- /dev/null +++ b/packaging/build_px4fw.sh @@ -0,0 +1,42 @@ +#!/bin/bash + +source /opt/ros/humble/setup.sh + +if [ -z "$1" ]; then + echo "Usage: $0 " + echo + exit 1 +else + # go through all given arguments and build them + for arg in "$@"; do + echo "BUILDING ${arg}" + + # extract the middle part of the name between the "_"'s + NAME=${arg} + NAME=${NAME%_*} + NAME=${NAME##*_} + + # for our own HW, use HW specific siging. For pixhawks, and icicle board, + # use the PX4 default signing script and keys + if [[ $NAME = saluki* ]] + then + export SIGNING_TOOL=Tools/saluki-sec-scripts/ed25519_sign.py + + if [ -z "$SIGNING_ARGS" ]; then + export SIGNING_ARGS=Tools/saluki-sec-scripts/test_keys/$NAME/ed25519_test_key.pem + fi + else + export SIGNING_TOOL=Tools/cryptotools.py + unset SIGNING_ARGS + fi + + # Remove old build output + rm -Rf build/${arg} + # Build + make ${arg} + + if [ -n "$SIGNING_ARGS" ]; then + echo "Signing key: $SIGNING_ARGS" + fi + done +fi diff --git a/packaging/build_px4fwupdater.sh b/packaging/build_px4fwupdater.sh new file mode 100755 index 000000000000..7c2da899555f --- /dev/null +++ b/packaging/build_px4fwupdater.sh @@ -0,0 +1,82 @@ +set -eo pipefail + +usage() { + echo " +Usage: $(basename "$0") [-h] [-v version] + -- Generate debian package from px4fwupdater module. +Params: + -h Show help text. + -v Git version string +" + exit 0 +} + +check_arg() { + if [ "$(echo $1 | cut -c1)" = "-" ]; then + return 1 + else + return 0 + fi +} + +error_arg() { + echo "$0: option requires an argument -- $1" + usage +} + +mod_dir="$(realpath $(dirname $0)/..)" +VERSION="" +indir="" + +while getopts "hv:i:" opt +do + case $opt in + h) + usage + ;; + v) + check_arg $OPTARG && VERSION=$OPTARG || error_arg $opt + ;; + i) + indir=$OPTARG + ;; + \?) + usage + ;; + esac +done + +set -x + +if [ -e tmp_packaging_dir ]; then + rm -Rf tmp_packaging_dir +fi + +mkdir -p tmp_packaging_dir +pushd tmp_packaging_dir + +mkdir -p ./opt/px4fwupdater/ +if [ -n "${indir}" ] +then + cp ${indir}/*.px4 ./opt/px4fwupdater/ +else + cp ${mod_dir}/build/ssrc_saluki-v1_default/*.px4 ./opt/px4fwupdater/ + cp ${mod_dir}/build/ssrc_saluki-v2_default/*.px4 ./opt/px4fwupdater/ + cp ${mod_dir}/build/px4_fmu-v5x_ssrc/*.px4 ./opt/px4fwupdater/ +fi + +mkdir DEBIAN +cp -R ${mod_dir}/packaging/debian/* DEBIAN/ +cp ${mod_dir}/packaging/px4_update_fw.sh ./opt/px4fwupdater/px4_update_fw.sh +cp ${mod_dir}/packaging/detect_ttyS.sh ./opt/px4fwupdater/detect_ttyS.sh +cp ${mod_dir}/Tools/px_uploader.py ./opt/px4fwupdater/px_uploader.py + +if [ -e ../px4fwupdater*.deb ]; then + rm -f ../px4fwupdater*.deb +fi + +sed -i "s/Version:.*/&${VERSION}/" DEBIAN/control +fakeroot dpkg-deb --build . "${indir}"/px4fwupdater_${VERSION}_amd64.deb + +popd +rm -Rf tmp_packaging_dir diff --git a/packaging/build_saluki_only.sh b/packaging/build_saluki_only.sh new file mode 100755 index 000000000000..97fd877f6194 --- /dev/null +++ b/packaging/build_saluki_only.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +source /opt/ros/galactic/setup.sh + +export SIGNING_TOOL=Tools/cryptotools.py + +# Remove old build output +rm -Rf build/ssrc_saluki-v1_default + +# Build +make ssrc_saluki-v1_default diff --git a/packaging/coverity.yaml b/packaging/coverity.yaml new file mode 100644 index 000000000000..da2ca70fe40e --- /dev/null +++ b/packaging/coverity.yaml @@ -0,0 +1,10 @@ +capture: + build: + build-command: packaging/build_px4fw.sh ssrc_saluki-v2_default + +commit: + connect: + auth-key-file: /auth_key.txt + stream: px4-coverity-test + url: https://coverity.ssrc.fi:443/ + on-new-cert: trust diff --git a/packaging/debian/control b/packaging/debian/control new file mode 100644 index 000000000000..68453baf1c69 --- /dev/null +++ b/packaging/debian/control @@ -0,0 +1,6 @@ +Package: px4-firmware-updater +Version: +Maintainer: Jani Paalijärvi +Architecture: amd64 +Depends: python3-serial +Description: PX4 firmware updater package. Installs included firmware to PixHawk4. diff --git a/packaging/debian/postinst b/packaging/debian/postinst new file mode 100755 index 000000000000..a9bf588e2f88 --- /dev/null +++ b/packaging/debian/postinst @@ -0,0 +1 @@ +#!/bin/bash diff --git a/packaging/debian/prerm b/packaging/debian/prerm new file mode 100755 index 000000000000..a9bf588e2f88 --- /dev/null +++ b/packaging/debian/prerm @@ -0,0 +1 @@ +#!/bin/bash diff --git a/packaging/detect_ttyS.sh b/packaging/detect_ttyS.sh new file mode 100755 index 000000000000..e8a2259b18fd --- /dev/null +++ b/packaging/detect_ttyS.sh @@ -0,0 +1,4 @@ +#!/bin/sh + +port=`cat /proc/tty/driver/serial | grep DTR | cut -d':' -f1` +echo "${port}" diff --git a/packaging/entrypoint.sh b/packaging/entrypoint.sh new file mode 100755 index 000000000000..a8346d9f8726 --- /dev/null +++ b/packaging/entrypoint.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +source /opt/ros/humble/setup.bash + +if [ "$1" == "sim_tcp_server" ] || [ "$PX4_SIM_USE_TCP_SERVER" != "" ]; then + px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS.sim_tcp_server +elif [ "$1" == "sim_udp" ] || [ "$PX4_SIM_USE_UDP" != "" ]; then + px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS.sim_udp +else + px4 -d "/px4_sitl_etc" -w sitl_${PX4_SIM_MODEL} -s /px4_sitl_etc/init.d-posix/rcS +fi diff --git a/packaging/entrypoint_coverity.sh b/packaging/entrypoint_coverity.sh new file mode 100755 index 000000000000..5962684f81da --- /dev/null +++ b/packaging/entrypoint_coverity.sh @@ -0,0 +1,32 @@ +#!/bin/bash -eu + +COVERITY_SUMMARY_OUT=/main_ws/cov-analyze-result.txt +COVERITY_SCAN_OUT=/main_ws/cov-scan-output.txt +COVERITY_REPORT_OUT=/main_ws/coverity-output + +cp /main_ws/packaging/coverity.yaml /main_ws/coverity.yaml + +export PATH=$PATH:/cov/bin/ +cov-configure --gcc +coverity scan --exclude-language java |tee ${COVERITY_SCAN_OUT} +coverity list + +# find important information from coverity scan to be shown on github action step summary +# link for coverity +grep 'Results are available at' ${COVERITY_SCAN_OUT} >> ${COVERITY_SUMMARY_OUT} +echo 'send a slack message to tampere-drones if you have access issues' >> ${COVERITY_SUMMARY_OUT} + +echo "Analysis summary:">> ${COVERITY_SUMMARY_OUT} +# '```' marks the code block for output +echo '```' >> ${COVERITY_SUMMARY_OUT} +# check analysis summary output and save everything beginning from "analysis summary report" to a file +cov-analyze --dir idir --strip-path /main_ws/src/ |sed -n -E -e '/Analysis summary report:/,$ p'>>${COVERITY_SUMMARY_OUT} +# '```' ends the code block for output +echo '```' >> ${COVERITY_SUMMARY_OUT} + +echo "File findings:">> ${COVERITY_SUMMARY_OUT} +echo '------' >> ${COVERITY_SUMMARY_OUT} + +# save coverity html output +cov-format-errors --dir idir --html-output ${COVERITY_REPORT_OUT} +echo 'for more details please check attached html report from "Artifacts" -sections above' >> ${COVERITY_SUMMARY_OUT} diff --git a/packaging/entrypoint_sitl_gzsim.sh b/packaging/entrypoint_sitl_gzsim.sh new file mode 100755 index 000000000000..c42bdafe05b1 --- /dev/null +++ b/packaging/entrypoint_sitl_gzsim.sh @@ -0,0 +1,37 @@ +#!/bin/bash +# + +export PATH=/px4_sitl/bin:$PATH + +case $PX4_VEHICLE_TYPE in + mc) + export PX4_SYS_AUTOSTART=4401 + export PX4_GZ_MODEL=holybro-x500 + ;; + rover) + export PX4_SYS_AUTOSTART=50005 + export PX4_GZ_MODEL=scout_mini + ;; + vtol) + export PX4_SYS_AUTOSTART=4004 + export PX4_GZ_MODEL=standard_vtol + ;; + fw) + export PX4_SYS_AUTOSTART=4003 + export PX4_GZ_MODEL=rc_cessna + ;; + *) + echo "ERROR: unknown vehicle type: $PX4_VEHICLE_TYPE" + exit 1 + ;; +esac + +export PX4_GZ_MODEL_NAME=$DRONE_DEVICE_ID +export PX4_GZ_WORLD=${PX4_GZ_WORLD:-default} +export GZ_PARTITION=sim +export GZ_RELAY=$(dig +short gazebo-server) +export GZ_IP=$(hostname -i) + +source /opt/ros/humble/setup.sh + +/px4_sitl/bin/px4 -d -s /px4_sitl/etc/init.d-posix/rcS -w /px4_sitl diff --git a/packaging/px4_update_fw.sh b/packaging/px4_update_fw.sh new file mode 100755 index 000000000000..e58b022614c4 --- /dev/null +++ b/packaging/px4_update_fw.sh @@ -0,0 +1,33 @@ +#!/bin/bash + +port=$(/opt/px4fwupdater/detect_ttyS.sh) + +if [ -z $port ] +then + echo "Unable to detect serial port. Exiting.." + exit 1 +fi + +systemctl is-active agent_protocol_splitter.service &> /dev/null +if [ $? -eq 0 ] +then + echo "Stopping agent_protocol_splitter before flashing" + systemctl stop agent_protocol_splitter.service + echo "Start flashing.. port /dev/ttyS${port}" + /opt/px4fwupdater/px_uploader.py \ + --port /dev/ttyS${port} \ + --baud-bootloader 2000000 \ + --baud-flightstack 57600,115200,1000000,2000000 \ + --use-protocol-splitter-format \ + /opt/px4fwupdater/px4_fmu-v5_ssrc.px4 + echo "Start agent_protocol_splitter" + systemctl start agent_protocol_splitter.service +else + echo "Start flashing.. port /dev/ttyS{port}" + /opt/px4fwupdater/px_uploader.py \ + --port /dev/ttyS${port} \ + --baud-bootloader 2000000 \ + --baud-flightstack 57600,115200,1000000,2000000 \ + --use-protocol-splitter-format \ + /opt/px4fwupdater/px4_fmu-v5_ssrc.px4 +fi diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a6e06b9d4735..7f3d1e9168fc 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -43,7 +43,6 @@ endif() add_library(px4_platform STATIC board_common.c board_identity.c - events.cpp external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp @@ -60,6 +59,10 @@ if(NOT "${PX4_BOARD}" MATCHES "io-v2") add_subdirectory(uORB) endif() +if(NOT "${PX4_PLATFORM}" MATCHES "posix") + target_link_libraries(px4_platform board_bus_info) +endif() + add_subdirectory(px4_work_queue) add_subdirectory(work_queue) diff --git a/platforms/common/apps.cpp.in b/platforms/common/apps.cpp.in index 11d1c50d9f97..0c209dba7a47 100644 --- a/platforms/common/apps.cpp.in +++ b/platforms/common/apps.cpp.in @@ -2,6 +2,7 @@ #include #include #include +#include #include "apps.h" @@ -34,15 +35,16 @@ void init_app_map(apps_map_type &apps) void list_builtins(apps_map_type &apps) { - printf("Builtin Commands:\n"); + PX4_INFO("Builtin Commands:"); for (apps_map_type::iterator it = apps.begin(); it != apps.end(); ++it) { - printf(" %s\n", it->first.c_str()); + PX4_INFO(" %s", it->first.c_str()); } } int shutdown_main(int argc, char *argv[]) { - printf("Exiting NOW.\n"); + PX4_INFO("Exiting NOW."); + uORB::Manager::terminate(); system_exit(0); } @@ -66,7 +68,7 @@ int sleep_main(int argc, char *argv[]) } unsigned long usecs = 1000000UL * atol(argv[1]); - printf("Sleeping for %s s; (%lu us).\n", argv[1], usecs); + PX4_INFO("Sleeping for %s s; (%lu us).", argv[1], usecs); px4_usleep(usecs); return 0; } diff --git a/platforms/common/include/px4_platform_common/atomic.h b/platforms/common/include/px4_platform_common/atomic.h index 47b3fb6de87b..6a4e00694d4d 100644 --- a/platforms/common/include/px4_platform_common/atomic.h +++ b/platforms/common/include/px4_platform_common/atomic.h @@ -58,9 +58,7 @@ #include #include -#if defined(__PX4_NUTTX) -# include -#endif // __PX4_NUTTX +#include namespace px4 { @@ -75,26 +73,31 @@ class atomic // IRQ handlers. This might not be required everywhere though. static_assert(__atomic_always_lock_free(sizeof(T), 0), "atomic is not lock-free for the given type T"); #endif // __PX4_POSIX - - atomic() = default; - explicit atomic(T value) : _value(value) {} + atomic() + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + px4_sem_init(&_lock, 0, 1); + } + } + explicit atomic(T value) : _value(value) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + px4_sem_init(&_lock, 0, 1); + } + } /** * Atomically read the current value */ inline T load() const { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); } } @@ -104,16 +107,12 @@ class atomic */ inline void store(T value) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); _value = value; - leave_critical_section(flags); + release_lock(); - } else -#endif // __PX4_NUTTX - { + } else { __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); } } @@ -124,18 +123,14 @@ class atomic */ inline T fetch_add(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value += num; - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); } } @@ -146,18 +141,14 @@ class atomic */ inline T fetch_sub(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value -= num; - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); } } @@ -168,18 +159,14 @@ class atomic */ inline T fetch_and(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value &= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); } } @@ -190,18 +177,14 @@ class atomic */ inline T fetch_xor(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value ^= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); } } @@ -212,18 +195,14 @@ class atomic */ inline T fetch_or(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T val = _value; _value |= num; - leave_critical_section(flags); + release_lock(); return val; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); } } @@ -234,18 +213,14 @@ class atomic */ inline T fetch_nand(T num) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); T ret = _value; _value = ~(_value & num); - leave_critical_section(flags); + release_lock(); return ret; - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); } } @@ -260,37 +235,48 @@ class atomic */ inline bool compare_exchange(T *expected, T desired) { -#if defined(__PX4_NUTTX) - if (!__atomic_always_lock_free(sizeof(T), 0)) { - irqstate_t flags = enter_critical_section(); + take_lock(); if (_value == *expected) { _value = desired; - leave_critical_section(flags); + release_lock(); return true; } else { *expected = _value; - leave_critical_section(flags); + release_lock(); return false; } - } else -#endif // __PX4_NUTTX - { + } else { return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); } } private: T _value {}; + + inline void take_lock() const { do {} while (px4_sem_wait(&_lock) != 0); } + inline void release_lock() const { px4_sem_post(&_lock); } + mutable px4_sem_t _lock; }; using atomic_int = atomic; using atomic_int32_t = atomic; + +/* On riscv64-unknown-elf atomic is not quaranteed to be lock-free + * It is unclear whether it is really required. + * An optimal solution could be atomic_flag, but it doesn't seem to be available + * Just use atomic ints for now, to be safe +*/ +#if !defined(CONFIG_ARCH_RISCV) using atomic_bool = atomic; +#else +using atomic_bool = atomic; +#endif } /* namespace px4 */ #endif /* __cplusplus */ + diff --git a/platforms/common/include/px4_platform_common/atomic_from_isr.h b/platforms/common/include/px4_platform_common/atomic_from_isr.h new file mode 100644 index 000000000000..191e969dfdf1 --- /dev/null +++ b/platforms/common/include/px4_platform_common/atomic_from_isr.h @@ -0,0 +1,266 @@ +/**************************************************************************** + * + * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file atomic_from_isr.h + * + * Provides atomic integers and counters. Each method is executed atomically and thus + * can be used to prevent data races and add memory synchronization between threads. + * + * In addition to the atomicity, each method serves as a memory barrier (sequential + * consistent ordering). This means all operations that happen before and could + * potentially have visible side-effects in other threads will happen before + * the method is executed. + * + * The implementation uses the built-in methods from GCC (supported by Clang as well). + * @see https://gcc.gnu.org/onlinedocs/gcc/_005f_005fatomic-Builtins.html. + * + * @note: on ARM, the instructions LDREX and STREX might be emitted. To ensure correct + * behavior, the exclusive monitor needs to be cleared on a task switch (via CLREX). + * This happens automatically e.g. on ARMv7-M as part of an exception entry or exit + * sequence. + */ + +#pragma once + +#ifdef __cplusplus + +#include +#include + + +#if !defined(__PX4_NUTTX) + +/* For non-NuttX targets forward this to the generic atomic implementation */ +#include "atomic.h" + +namespace px4 +{ + +template +using atomic_from_isr = atomic; + +} + +#else + +/* For NuttX targets, implement an IRQ safe way for atomic data */ +#include + +namespace px4 +{ + +template +class atomic_from_isr +{ +public: + + atomic_from_isr() = default; + explicit atomic_from_isr(T value) : _value(value) {} + + /** + * Atomically read the current value + */ + inline T load() const + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + leave_critical_section(flags); + return val; + + } else { + return __atomic_load_n(&_value, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically store a value + */ + inline void store(T value) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + _value = value; + leave_critical_section(flags); + + } else { + __atomic_store(&_value, &value, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically add a number and return the previous value. + * @return value prior to the addition + */ + inline T fetch_add(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value += num; + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_add(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomically substract a number and return the previous value. + * @return value prior to the substraction + */ + inline T fetch_sub(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value -= num; + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_sub(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic AND with a number + * @return value prior to the operation + */ + inline T fetch_and(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value &= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_and(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic XOR with a number + * @return value prior to the operation + */ + inline T fetch_xor(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value ^= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_xor(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic OR with a number + * @return value prior to the operation + */ + inline T fetch_or(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T val = _value; + _value |= num; + leave_critical_section(flags); + return val; + + } else { + return __atomic_fetch_or(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic NAND (~(_value & num)) with a number + * @return value prior to the operation + */ + inline T fetch_nand(T num) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + T ret = _value; + _value = ~(_value & num); + leave_critical_section(flags); + return ret; + + } else { + return __atomic_fetch_nand(&_value, num, __ATOMIC_SEQ_CST); + } + } + + /** + * Atomic compare and exchange operation. + * This compares the contents of _value with the contents of *expected. If + * equal, the operation is a read-modify-write operation that writes desired + * into _value. If they are not equal, the operation is a read and the current + * contents of _value are written into *expected. + * @return If desired is written into _value then true is returned + */ + inline bool compare_exchange(T *expected, T desired) + { + if (!__atomic_always_lock_free(sizeof(T), 0)) { + irqstate_t flags = enter_critical_section(); + + if (_value == *expected) { + _value = desired; + leave_critical_section(flags); + return true; + + } else { + *expected = _value; + leave_critical_section(flags); + return false; + } + + } else { + return __atomic_compare_exchange(&_value, expected, &desired, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST); + } + } + +private: + T _value {}; +}; + +} /* namespace px4 */ + +#endif /* __PX4_NUTTX */ +#endif /* __cplusplus */ diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 7a6af777c495..c26c53985f03 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -353,6 +353,8 @@ typedef enum PX4_SOC_ARCH_ID_t { PX4_SOC_ARCH_ID_VOXL2 = 0x100A, + PX4_SOC_ARCH_ID_MPFS = 0x2000, + } PX4_SOC_ARCH_ID_t; diff --git a/platforms/common/include/px4_platform_common/crypto.h b/platforms/common/include/px4_platform_common/crypto.h index 94016e0fa6b2..f82e5f1530d3 100644 --- a/platforms/common/include/px4_platform_common/crypto.h +++ b/platforms/common/include/px4_platform_common/crypto.h @@ -197,6 +197,9 @@ class PX4Crypto * cipher: pointer to a buffer for encrypted data * cipher_size: size of the buffer reserved for cipher and actual cipher length * after the encryption + * mac: pointer to the buffer for authentication code + * mac_size: pointer to the size of the authentication code buffer + * the size is updated to match the actual size after encryption * returns true on success, false on failure */ @@ -204,7 +207,30 @@ class PX4Crypto const uint8_t *message, size_t message_size, uint8_t *cipher, - size_t *cipher_size); + size_t *cipher_size, + uint8_t *mac, + size_t *mac_size); + + /* + * Decrypt data. This always supports decryption in place + * + * De-crypts the given cipher using the given nonce and key index. + * handle: session handle, returned by oepn + * key_index: index to the key used for decryption + * cipher: the ciphertext to be decrypted + * mac: pointer to the buffer for authentication code + * mac_size: size of the authentication code buffer + * message: output buffer given by the client + * message_size: in: size of "message" buffer, out: actual result size + * returns + */ + bool decrypt_data(uint8_t key_index, + const uint8_t *cipher, + size_t cipher_size, + const uint8_t *mac, + size_t mac_size, + uint8_t *message, + size_t *message_size); size_t get_min_blocksize(uint8_t key_idx); diff --git a/platforms/common/include/px4_platform_common/crypto_algorithms.h b/platforms/common/include/px4_platform_common/crypto_algorithms.h index 32a4a44f5f48..6c5d04f957f0 100644 --- a/platforms/common/include/px4_platform_common/crypto_algorithms.h +++ b/platforms/common/include/px4_platform_common/crypto_algorithms.h @@ -43,4 +43,15 @@ typedef enum { CRYPTO_XCHACHA20 = 2, CRYPTO_AES = 3, CRYPTO_RSA_OAEP = 4, + CRYPTO_ECDSA_P256 = 5, + CRYPTO_RSA_SIG = 6, /* openssl dgst -sha256 -sign */ } px4_crypto_algorithm_t; + +/* Define the expected size of the signature for signing algorithms */ + +#define PX4_SIGNATURE_SIZE(x) PX4_SIGNATURE_SIZE_(x) +#define PX4_SIGNATURE_SIZE_(x) PX4_SIGNATURE_SIZE_##x + +#define PX4_SIGNATURE_SIZE_CRYPTO_ED25519 64 +#define PX4_SIGNATURE_SIZE_CRYPTO_ECDSA_P256 64 /* Raw R+S, both 32 bytes */ +#define PX4_SIGNATURE_SIZE_CRYPTO_RSA_SIG 256 /* Assume key length of 2048 bits */ diff --git a/platforms/common/include/px4_platform_common/crypto_backend.h b/platforms/common/include/px4_platform_common/crypto_backend.h index ea8ad85b0699..5fdd6a548b3f 100644 --- a/platforms/common/include/px4_platform_common/crypto_backend.h +++ b/platforms/common/include/px4_platform_common/crypto_backend.h @@ -48,6 +48,7 @@ extern "C" { */ void keystore_init(void); +void keystore_deinit(void); /* * Open a session for accessing security keys @@ -142,6 +143,19 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle, size_t *max_len, uint8_t encryption_key_idx); +/* + * Re-create or set nonce. + * + * A nonce or intialization vector value for the selected algortithm is + * automatically generated when the crypto session is opened. If needed, the + * nonce can be set by this function. + * If this is called with NULL pointer, a new nonce is automatically random + * generated + */ +bool crypto_renew_nonce(crypto_session_handle_t handle, + const uint8_t *nonce, + size_t nonce_size); + /* * Get the generated nonce value * @@ -175,7 +189,9 @@ bool crypto_encrypt_data(crypto_session_handle_t handle, const uint8_t *message, size_t message_size, uint8_t *cipher, - size_t *cipher_size); + size_t *cipher_size, + uint8_t *mac, + size_t *mac_size); /* * Returns a minimum data block size on which the crypto operations can be @@ -189,6 +205,27 @@ bool crypto_encrypt_data(crypto_session_handle_t handle, size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx); +/* + * Decrypt data. This always supports decryption in place + * + * De-crypts the given cipher using the given nonce and key index. + * handle: session handle, returned by oepn + * key_index: index to the key used for decryption + * cipher: the ciphertext to be decrypted + * nonce: the nonce used for decryption. If NULL, using the previously set nonce + * nonce_size: size of the given nonce value (note. caller is responsible of giving a suitable nonce for the algorithm) + * message: output buffer given by the client + * message_size: in: size of "message" buffer, out: actual result size + * returns + */ +bool crypto_decrypt_data(crypto_session_handle_t handle, + uint8_t key_index, + const uint8_t *cipher, + size_t cipher_size, + const uint8_t *mac, + size_t mac_size, + uint8_t *message, + size_t *message_size); /* Crypto IOCTLs, to access backend from user space */ @@ -210,6 +247,8 @@ typedef struct cryptoiocencrypt { size_t message_size; uint8_t *cipher; size_t *cipher_size; + uint8_t *mac; + size_t *mac_size; bool ret; } cryptoiocencrypt_t; @@ -246,6 +285,37 @@ typedef struct cryptoiocgetblocksz { size_t ret; } cryptoiocgetblocksz_t; +#define CRYPTOIOCRENEWNONCE _CRYPTOIOC(8) +typedef struct cryptoiocrenewnonce { + crypto_session_handle_t *handle; + const uint8_t *nonce; + size_t nonce_size; + size_t ret; +} cryptoiocrenewnonce_t; + +#define CRYPTOIOCSIGNATURECHECK _CRYPTOIOC(9) +typedef struct cryptoiocsignaturecheck { + crypto_session_handle_t *handle; + uint8_t key_index; + const uint8_t *signature; + const uint8_t *message; + size_t message_size; + size_t ret; +} cryptoiocsignaturecheck_t; + +#define CRYPTOIOCDECRYPTDATA _CRYPTOIOC(10) +typedef struct cryptoiocdecryptdata { + crypto_session_handle_t *handle; + uint8_t key_index; + const uint8_t *cipher; + size_t cipher_size; + const uint8_t *mac; + size_t mac_size; + uint8_t *message; + size_t *message_size; + size_t ret; +} cryptoiocdecryptdata_t; + #if defined(__cplusplus) } // extern "C" #endif diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 38d7b4201f4c..f33c8df3421e 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -121,10 +121,21 @@ __END_DECLS #define M_LOG10E_F 0.43429448f #define M_LN2_F 0.69314718f #define M_LN10_F 2.30258509f + +#ifndef M_PI_F #define M_PI_F 3.14159265f +#endif + #define M_TWOPI_F 6.28318531f + +#ifndef M_PI_2_F #define M_PI_2_F 1.57079632f +#endif + +#ifndef M_PI_4_F #define M_PI_4_F 0.78539816f +#endif + #define M_3PI_4_F 2.35619449f #define M_SQRTPI_F 1.77245385f #define M_1_PI_F 0.31830989f diff --git a/platforms/common/include/px4_platform_common/mmap.h b/platforms/common/include/px4_platform_common/mmap.h new file mode 100644 index 000000000000..7bcf2f095f8b --- /dev/null +++ b/platforms/common/include/px4_platform_common/mmap.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#if defined(__PX4_NUTTX) +#include +#endif + +#if defined (__PX4_NUTTX) && defined(CONFIG_BUILD_KERNEL) + +/* For size_t */ +#include + +__BEGIN_DECLS + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset); +int px4_munmap(void *start, size_t length); + +__END_DECLS + +#else +#define px4_mmap mmap +#define px4_munmap munmap +#endif diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h index e4d639919072..221757c08a91 100644 --- a/platforms/common/include/px4_platform_common/posix.h +++ b/platforms/common/include/px4_platform_common/posix.h @@ -45,19 +45,21 @@ #include #include #include +#include #include "sem.h" #define PX4_F_RDONLY 1 #define PX4_F_WRONLY 2 +typedef orb_poll_struct_t px4_pollfd_struct_t; +typedef orb_pollevent_t px4_pollevent_t; +#define px4_poll orb_poll + #ifdef __PX4_NUTTX #include -typedef struct pollfd px4_pollfd_struct_t; -typedef pollevent_t px4_pollevent_t; - #if defined(__cplusplus) #define _GLOBAL :: #else @@ -68,7 +70,6 @@ typedef pollevent_t px4_pollevent_t; #define px4_ioctl _GLOBAL ioctl #define px4_write _GLOBAL write #define px4_read _GLOBAL read -#define px4_poll _GLOBAL poll #define px4_access _GLOBAL access #define px4_getpid _GLOBAL getpid @@ -83,19 +84,6 @@ typedef pollevent_t px4_pollevent_t; __BEGIN_DECLS -typedef short px4_pollevent_t; - -typedef struct { - /* This part of the struct is POSIX-like */ - int fd; /* The descriptor being polled */ - px4_pollevent_t events; /* The input event flags */ - px4_pollevent_t revents; /* The output event flags */ - - /* Required for PX4 compatibility */ - px4_sem_t *sem; /* Pointer to semaphore used to post output event */ - void *priv; /* For use by drivers */ -} px4_pollfd_struct_t; - #ifndef POLLIN #define POLLIN (0x01) #endif @@ -118,7 +106,6 @@ __EXPORT int px4_close(int fd); __EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); -__EXPORT int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout); __EXPORT int px4_access(const char *pathname, int mode); __EXPORT px4_task_t px4_getpid(void); diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h index 7313bd78a93f..d7c356176621 100644 --- a/platforms/common/include/px4_platform_common/px4_mtd.h +++ b/platforms/common/include/px4_platform_common/px4_mtd.h @@ -37,6 +37,9 @@ __BEGIN_DECLS #define MAX_MTD_INSTANCES 5u +// Forward decalarations +FAR struct i2c_master_s; + // The data needed to interface with mtd device's typedef struct { diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp index f2a66bb8983c..5381e76a5f14 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueue.hpp @@ -43,6 +43,10 @@ #include #include +#ifdef __PX4_NUTTX +# include +#endif + namespace px4 { @@ -84,9 +88,16 @@ class WorkQueue : public IntrusiveSortedListNode #ifdef __PX4_NUTTX // In NuttX work can be enqueued from an ISR +#ifdef CONFIG_BUILD_FLAT void work_lock() { _flags = enter_critical_section(); } void work_unlock() { leave_critical_section(_flags); } irqstate_t _flags; +#else + // For non-flat targets, work is enqueued by user threads as well + void work_lock() { _atomic.start(); } + void work_unlock() { _atomic.finish(); } + atomic_block _atomic; +#endif #else // loop as the wait may be interrupted by a signal void work_lock() { do {} while (px4_sem_wait(&_qlock) != 0); } diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index 9bee390323bc..11e6c0f953a7 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -50,13 +50,13 @@ namespace wq_configurations { static constexpr wq_config_t rate_ctrl{"wq:rate_ctrl", 3150, 0}; // PX4 inner loop highest priority -static constexpr wq_config_t SPI0{"wq:SPI0", 2392, -1}; -static constexpr wq_config_t SPI1{"wq:SPI1", 2392, -2}; -static constexpr wq_config_t SPI2{"wq:SPI2", 2392, -3}; -static constexpr wq_config_t SPI3{"wq:SPI3", 2392, -4}; -static constexpr wq_config_t SPI4{"wq:SPI4", 2392, -5}; -static constexpr wq_config_t SPI5{"wq:SPI5", 2392, -6}; -static constexpr wq_config_t SPI6{"wq:SPI6", 2392, -7}; +static constexpr wq_config_t SPI0{"wq:SPI0", 2568, -1}; +static constexpr wq_config_t SPI1{"wq:SPI1", 2568, -2}; +static constexpr wq_config_t SPI2{"wq:SPI2", 2568, -3}; +static constexpr wq_config_t SPI3{"wq:SPI3", 2568, -4}; +static constexpr wq_config_t SPI4{"wq:SPI4", 2568, -5}; +static constexpr wq_config_t SPI5{"wq:SPI5", 2568, -6}; +static constexpr wq_config_t SPI6{"wq:SPI6", 2568, -7}; static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8}; static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9}; @@ -76,18 +76,20 @@ static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; -static constexpr wq_config_t ttyS0{"wq:ttyS0", 1728, -21}; -static constexpr wq_config_t ttyS1{"wq:ttyS1", 1728, -22}; -static constexpr wq_config_t ttyS2{"wq:ttyS2", 1728, -23}; -static constexpr wq_config_t ttyS3{"wq:ttyS3", 1728, -24}; -static constexpr wq_config_t ttyS4{"wq:ttyS4", 1728, -25}; -static constexpr wq_config_t ttyS5{"wq:ttyS5", 1728, -26}; -static constexpr wq_config_t ttyS6{"wq:ttyS6", 1728, -27}; -static constexpr wq_config_t ttyS7{"wq:ttyS7", 1728, -28}; -static constexpr wq_config_t ttyS8{"wq:ttyS8", 1728, -29}; -static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30}; -static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31}; -static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32}; +static constexpr wq_config_t rover_interface{"wq:rover_interface", 2000, -19}; + +static constexpr wq_config_t ttyS0{"wq:ttyS0", 1732, -21}; +static constexpr wq_config_t ttyS1{"wq:ttyS1", 1732, -22}; +static constexpr wq_config_t ttyS2{"wq:ttyS2", 1732, -23}; +static constexpr wq_config_t ttyS3{"wq:ttyS3", 1732, -24}; +static constexpr wq_config_t ttyS4{"wq:ttyS4", 1732, -25}; +static constexpr wq_config_t ttyS5{"wq:ttyS5", 1732, -26}; +static constexpr wq_config_t ttyS6{"wq:ttyS6", 1732, -27}; +static constexpr wq_config_t ttyS7{"wq:ttyS7", 1732, -28}; +static constexpr wq_config_t ttyS8{"wq:ttyS8", 1732, -29}; +static constexpr wq_config_t ttyS9{"wq:ttyS9", 1732, -30}; +static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1732, -31}; +static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1732, -32}; static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50}; diff --git a/platforms/common/include/px4_platform_common/sem.h b/platforms/common/include/px4_platform_common/sem.h index 5e4234142397..d14301006737 100644 --- a/platforms/common/include/px4_platform_common/sem.h +++ b/platforms/common/include/px4_platform_common/sem.h @@ -89,7 +89,11 @@ typedef sem_t px4_sem_t; __BEGIN_DECLS -#define px4_sem_init sem_init +#define px4_sem_init(s, p, v) ({ \ + int __ret; \ + do { __ret = sem_init(s, p, v); sem_setprotocol(s, SEM_PRIO_INHERIT); } while(0); \ + __ret; \ + }) #define px4_sem_setprotocol sem_setprotocol #define px4_sem_wait sem_wait #define px4_sem_trywait sem_trywait diff --git a/platforms/common/include/px4_platform_common/shutdown.h b/platforms/common/include/px4_platform_common/shutdown.h index f4bd2e9cf6b3..6f26e5d900ae 100644 --- a/platforms/common/include/px4_platform_common/shutdown.h +++ b/platforms/common/include/px4_platform_common/shutdown.h @@ -83,7 +83,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook); * @return 0 on success, <0 on error */ #if defined(CONFIG_BOARDCTL_RESET) -__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0); +__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0, bool continue_boot = false); #endif // CONFIG_BOARDCTL_RESET diff --git a/platforms/common/include/px4_platform_common/tasks.h b/platforms/common/include/px4_platform_common/tasks.h index ccdc108aa4ba..b2fee387d543 100644 --- a/platforms/common/include/px4_platform_common/tasks.h +++ b/platforms/common/include/px4_platform_common/tasks.h @@ -187,4 +187,10 @@ __EXPORT int px4_prctl(int option, const char *arg2, px4_task_t pid); /** return the name of the current task */ __EXPORT const char *px4_get_taskname(void); +/** Execute a named task from filesystem */ +__EXPORT int px4_exec(const char *appname, + char *const *argv, + const char *redirfile, + int oflags); + __END_DECLS diff --git a/platforms/common/px4_log.cpp b/platforms/common/px4_log.cpp index 8e733b5ee4c1..84480d3ca7a6 100644 --- a/platforms/common/px4_log.cpp +++ b/platforms/common/px4_log.cpp @@ -55,7 +55,7 @@ static LogHistory g_log_history; #endif -static orb_advert_t orb_log_message_pub = nullptr; +static orb_advert_t orb_log_message_pub = ORB_ADVERT_INVALID; __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_PANIC + 1] = { "DEBUG", "INFO", "WARN", "ERROR", "PANIC" }; @@ -178,7 +178,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char } /* publish an orb log message */ - if (level >= _PX4_LOG_LEVEL_INFO && orb_log_message_pub) { //publish all messages + if (level >= _PX4_LOG_LEVEL_INFO && orb_advert_valid(orb_log_message_pub)) { //publish all messages log_message_s log_message; @@ -199,7 +199,7 @@ __EXPORT void px4_log_modulename(int level, const char *module_name, const char va_end(argptr); log_message.text[max_length - 1] = 0; //ensure 0-termination log_message.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message); + orb_publish(ORB_ID(log_message), &orb_log_message_pub, &log_message); } } diff --git a/platforms/common/px4_work_queue/WorkItemSingleShot.cpp b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp index d31d51ab7b02..77d24e2c2a87 100644 --- a/platforms/common/px4_work_queue/WorkItemSingleShot.cpp +++ b/platforms/common/px4_work_queue/WorkItemSingleShot.cpp @@ -41,6 +41,7 @@ WorkItemSingleShot::WorkItemSingleShot(const px4::wq_config_t &config, worker_me : px4::WorkItem("", config), _argument(argument), _method(method) { px4_sem_init(&_sem, 0, 0); + px4_sem_setprotocol(&_sem, SEM_PRIO_NONE); } WorkItemSingleShot::WorkItemSingleShot(const px4::WorkItem &work_item, worker_method method, void *argument) diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 8d961c14b3d7..324d3cec7ee8 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -62,6 +62,33 @@ static BlockingQueue *_wq_manager_create_queue{nullptr}; static px4::atomic_bool _wq_manager_should_exit{true}; +#if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_KERNEL) +static void WorkQueueManagerLazyStart(void) +{ + static px4::atomic_int _wq_manager_start_issued{0}; + + // Start the manager if it is not running yet + if (_wq_manager_create_queue == nullptr) { + int ret = PX4_OK; + + // Only the first one to arrive gets to do this, others block + if (_wq_manager_start_issued.fetch_or(1) == 0) { + ret = WorkQueueManagerStart(); + + } + + // Start command issued, wait until it is running + if (ret == PX4_OK) { + int retries = 0; + + // Ok make sure the manager is truly running + while (_wq_manager_create_queue == nullptr && retries++ < 100) { + px4_usleep(1_ms); + } + } + } +} +#endif static WorkQueue * FindWorkQueueByName(const char *name) @@ -86,6 +113,10 @@ FindWorkQueueByName(const char *name) WorkQueue * WorkQueueFindOrCreate(const wq_config_t &new_wq) { +#if defined(__PX4_NUTTX) && defined(CONFIG_BUILD_KERNEL) + WorkQueueManagerLazyStart(); +#endif + if (_wq_manager_create_queue == nullptr) { PX4_ERR("not running"); return nullptr; diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index dcbd733ceb29..5c0be97a009c 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -108,6 +108,8 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor #define SHUTDOWN_ARG_IN_PROGRESS (1<<0) #define SHUTDOWN_ARG_REBOOT (1<<1) #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2) +#define SHUTDOWN_ARG_BL_CONTINUE_BOOT (1<<3) + static uint8_t shutdown_args = 0; static constexpr int max_shutdown_hooks = 1; @@ -175,7 +177,18 @@ static void shutdown_worker(void *arg) if (shutdown_args & SHUTDOWN_ARG_REBOOT) { #if defined(CONFIG_BOARDCTL_RESET) PX4_INFO_RAW("Reboot NOW."); - boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0); + uintptr_t reboot_arg = 0; + + if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) { + if (shutdown_args & SHUTDOWN_ARG_BL_CONTINUE_BOOT) { + reboot_arg = 2; + + } else { + reboot_arg = 1; + } + } + + boardctl(BOARDIOC_RESET, reboot_arg); #else PX4_PANIC("board reset not available"); #endif @@ -206,7 +219,7 @@ static void shutdown_worker(void *arg) } #if defined(CONFIG_BOARDCTL_RESET) -int px4_reboot_request(bool to_bootloader, uint32_t delay_us) +int px4_reboot_request(bool to_bootloader, uint32_t delay_us, bool continue_boot) { pthread_mutex_lock(&shutdown_mutex); @@ -215,10 +228,19 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us) return 0; } +#ifdef CONFIG_BUILD_KERNEL + // Must start the worker as it is not automatically started by the system + work_usrstart(); +#endif + shutdown_args |= SHUTDOWN_ARG_REBOOT; if (to_bootloader) { shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER; + + if (continue_boot) { + shutdown_args |= SHUTDOWN_ARG_BL_CONTINUE_BOOT; + } } shutdown_time_us = hrt_absolute_time(); diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index c151d051c747..b044acb9ae7c 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -34,9 +34,7 @@ # this includes the generated topics directory include_directories(${CMAKE_CURRENT_BINARY_DIR}) -set(SRCS) - -set(SRCS_COMMON +px4_add_library(uORB ORBSet.hpp Publication.hpp PublicationMulti.hpp @@ -50,56 +48,19 @@ set(SRCS_COMMON uORBCommon.hpp uORBCommunicator.hpp uORBManager.hpp + uORBManager.cpp uORBUtils.cpp uORBUtils.hpp - uORBDeviceMaster.hpp - uORBDeviceNode.hpp - ) - -set(SRCS_KERNEL - uORBDeviceMaster.cpp uORBDeviceNode.cpp - uORBManager.cpp - ) - -set(SRCS_USER - uORBManagerUsr.cpp + uORBDeviceNode.hpp ) -if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") - # Kernel side library in nuttx kernel/protected build - px4_add_library(uORB_kernel - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB_kernel PRIVATE cdev uorb_msgs nuttx_mm) - target_compile_options(uORB_kernel PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -D__KERNEL__) - - # User side library in nuttx kernel/protected build - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_USER} - ) -elseif("${PX4_PLATFORM}" MATCHES "nuttx") - - # Library for NuttX (flat build, posix...) - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB PRIVATE cdev nuttx_mm) -else() - - # Library for all other targets (posix...) - px4_add_library(uORB - ${SRCS_COMMON} - ${SRCS_KERNEL} - ) - target_link_libraries(uORB PRIVATE cdev) +if ("${PX4_PLATFORM}" MATCHES "posix") + target_link_libraries(uORB PRIVATE rt) endif() -target_link_libraries(uORB PRIVATE uorb_msgs) target_compile_options(uORB PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) +target_link_libraries(uORB PRIVATE uorb_msgs) if(PX4_TESTING) add_subdirectory(uORB_tests) diff --git a/platforms/common/uORB/IndexedStack.hpp b/platforms/common/uORB/IndexedStack.hpp new file mode 100644 index 000000000000..73d94272464e --- /dev/null +++ b/platforms/common/uORB/IndexedStack.hpp @@ -0,0 +1,143 @@ +#pragma once + +#include + +/* + * This is a single-linked list/stack which items can be accessed via handle of + * type H. The handle can be either an index (int8_t) to a pre-allocated list, + * or a direct pointer to the item. The items should have a "next" member + * variable of the intended handle type. +*/ + +template class IndexedStack; + +/* The list can be used via the IndexedStackHandle element */ + +template +class IndexedStackHandle +{ +public: + IndexedStackHandle(class IndexedStack &stack) : _stack(stack) {} + + void push(H handle) { _stack.push(handle);} + H pop() { return _stack.pop(); } + void push_free(H handle) { _stack.push_free(handle); } + H pop_free() { return _stack.pop_free(); } + bool rm(H handle) { return _stack.rm(handle); } + H head() {return _stack._head;} + H next(H handle) {return peek(handle)->next;} + bool empty() {return !_stack.handle_valid(_stack.head());} + T *peek(H handle) { return _stack.handle_valid(handle) ? _stack.peek(handle) : nullptr; } + bool handle_valid(H handle) {return _stack.handle_valid(handle); } + +private: + + class IndexedStack &_stack; +}; + + +/* The actual implementation of the list. This supports two types of handles; + * void * for NuttX flat build / single process environment + * int8_t for share memory / communication between processes + */ + +template +class IndexedStack +{ +public: + friend class IndexedStackHandle; + + IndexedStack() + { + clear_handle(_head); + clear_handle(_free_head); + init_freelist(_free_head); + } + +private: + void push(H handle) { push(handle, _head);} + H pop() { return pop(_head); } + void push_free(H handle) { push(handle, _free_head); } + H pop_free() { return pop(_free_head); } + + bool rm(H handle) + { + H p = _head; + H r; clear_handle(r); + + if (!handle_valid(handle) || + !handle_valid(p)) { + return false; + } + + if (p == handle) { + // remove the first item + T *item = peek(_head); + _head = item->next; + r = p; + + } else { + while (handle_valid((r = peek(p)->next))) { + if (r == handle) { + T *prev = peek(p); + T *item = peek(r); + // remove the item + prev->next = item->next; + break; + } + + p = r; + } + } + + return handle_valid(r) ? true : false; + } + + /* Helper functions for constructor; initialize the list of free items */ + void init_freelist(int8_t handle) + { + /* Add all items into free list */ + IndexedStackHandle self(*this); + + for (int8_t i = 0; i < S ; i++) { + self.push_free(i); + } + + _free_head = S - 1; + } + + void init_freelist(void *handle) {} + + /* Push & pop internal implementations for actual and freelist */ + void push(H handle, H &head) + { + if (handle_valid(handle)) { + T *item = peek(handle); + item->next = head; + head = handle; + } + } + + H pop(H &head) + { + H ret = head; + + if (handle_valid(ret)) { + T *item = peek(head); + head = item->next; + } + + return ret; + } + + T *peek(int8_t handle) { return &_item[handle]; } + T *peek(void *handle) { return static_cast(handle); } + static bool handle_valid(int8_t handle) { return handle >= 0; } + static bool handle_valid(void *handle) { return handle != nullptr; } + static void clear_handle(int8_t &x) { x = -1; }; + static void clear_handle(void *&x) { x = nullptr; }; + + H _head; + H _free_head; + T _item[S]; +}; diff --git a/platforms/common/uORB/ORBSet.hpp b/platforms/common/uORB/ORBSet.hpp index 28e283c7b9e7..118284308852 100644 --- a/platforms/common/uORB/ORBSet.hpp +++ b/platforms/common/uORB/ORBSet.hpp @@ -33,12 +33,14 @@ #pragma once +#include + class ORBSet { public: struct Node { Node *next{nullptr}; - const char *node_name{nullptr}; + orb_advert_t handle{ORB_ADVERT_INVALID}; }; ORBSet() = default; @@ -49,8 +51,6 @@ class ORBSet unlinkNext(_top); if (_top->next == nullptr) { - free((void *)_top->node_name); - _top->node_name = nullptr; delete _top; _top = nullptr; @@ -58,7 +58,7 @@ class ORBSet } } - void insert(const char *node_name) + void insert(const orb_advert_t &handle) { Node **p; @@ -79,36 +79,31 @@ class ORBSet } _end->next = nullptr; - _end->node_name = strdup(node_name); + _end->handle = handle; } - bool find(const char *node_name) + orb_advert_t find(const char *node_name) { Node *p = _top; while (p) { - if (strcmp(p->node_name, node_name) == 0) { - return true; + if (strcmp(uORB::DeviceNode::get_name(p->handle), node_name) == 0) { + return p->handle; } p = p->next; } - return false; + return ORB_ADVERT_INVALID; } bool erase(const char *node_name) { Node *p = _top; - if (_top && (strcmp(_top->node_name, node_name) == 0)) { + if (_top && (strcmp(uORB::DeviceNode::get_name(_top->handle), node_name) == 0)) { p = _top->next; - if (_top->node_name) { - free((void *)_top->node_name); - _top->node_name = nullptr; - } - delete _top; _top = p; @@ -120,7 +115,7 @@ class ORBSet } while (p->next) { - if (strcmp(p->next->node_name, node_name) == 0) { + if (strcmp(uORB::DeviceNode::get_name(p->next->handle), node_name) == 0) { unlinkNext(p); return true; } @@ -142,11 +137,6 @@ class ORBSet a->next = b->next; - if (b->node_name) { - free((void *)b->node_name); - b->node_name = nullptr; - } - delete b; b = nullptr; } diff --git a/platforms/common/uORB/Publication.hpp b/platforms/common/uORB/Publication.hpp index 7547e3b7faf3..9f7763c033eb 100644 --- a/platforms/common/uORB/Publication.hpp +++ b/platforms/common/uORB/Publication.hpp @@ -70,19 +70,19 @@ class PublicationBase { public: - bool advertised() const { return _handle != nullptr; } + bool advertised() const { return orb_advert_valid(_handle); } bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); } - orb_id_t get_topic() const { return get_orb_meta(_orb_id); } + orb_id_t get_topic() const { return _meta; } protected: - PublicationBase(ORB_ID id) : _orb_id(id) {} + PublicationBase(ORB_ID id) : _meta(get_orb_meta(id)) {} ~PublicationBase() { - if (_handle != nullptr) { + if (orb_advert_valid(_handle)) { // don't automatically unadvertise queued publications (eg vehicle_command) if (Manager::orb_get_queue_size(_handle) == 1) { unadvertise(); @@ -90,8 +90,8 @@ class PublicationBase } } - orb_advert_t _handle{nullptr}; - const ORB_ID _orb_id; + orb_advert_t _handle{ORB_ADVERT_INVALID}; + const orb_id_t _meta; }; /** diff --git a/platforms/common/uORB/PublicationMulti.hpp b/platforms/common/uORB/PublicationMulti.hpp index bc275940b604..3510013880cc 100644 --- a/platforms/common/uORB/PublicationMulti.hpp +++ b/platforms/common/uORB/PublicationMulti.hpp @@ -89,7 +89,7 @@ class PublicationMulti : public PublicationBase advertise(); } - return (orb_publish(get_topic(), _handle, &data) == PX4_OK); + return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK); } int get_instance() diff --git a/platforms/common/uORB/Subscription.cpp b/platforms/common/uORB/Subscription.cpp index 9c17cab378d2..6fe7ffb1ee44 100644 --- a/platforms/common/uORB/Subscription.cpp +++ b/platforms/common/uORB/Subscription.cpp @@ -36,26 +36,24 @@ * */ +#include #include "Subscription.hpp" -#include +#include "uORBManager.hpp" namespace uORB { -bool Subscription::subscribe() +bool Subscription::subscribe(bool advertise) { - // check if already subscribed - if (_node != nullptr) { + if (orb_advert_valid(_node)) { return true; } - if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) { - unsigned initial_generation; - void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation); + if (_orb_id != ORB_ID::INVALID) { + _node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &_last_generation, advertise); - if (node) { - _node = node; - _last_generation = initial_generation; + if (orb_advert_valid(_node)) { + _advertiser = advertise; return true; } } @@ -65,22 +63,30 @@ bool Subscription::subscribe() void Subscription::unsubscribe() { - if (_node != nullptr) { - uORB::Manager::orb_remove_internal_subscriber(_node); + if (orb_advert_valid(_node)) { + uORB::Manager::orb_remove_internal_subscriber(_node, _advertiser); } - _node = nullptr; _last_generation = 0; } bool Subscription::ChangeInstance(uint8_t instance) { if (instance != _instance) { - if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) { - // if desired new instance exists, unsubscribe from current + // Subscribe to the new existing node + unsigned generation; + + if (orb_exists(get_topic(), instance) != PX4_OK) { + return false; + } + + orb_advert_t new_node = uORB::Manager::orb_add_internal_subscriber(_orb_id, instance, &generation, false); + + if (orb_advert_valid(new_node)) { unsubscribe(); + _node = new_node; _instance = instance; - subscribe(); + _last_generation = generation; return true; } diff --git a/platforms/common/uORB/Subscription.hpp b/platforms/common/uORB/Subscription.hpp index 71c8809ad229..6850e280d5e3 100644 --- a/platforms/common/uORB/Subscription.hpp +++ b/platforms/common/uORB/Subscription.hpp @@ -45,13 +45,10 @@ #include #include "uORBManager.hpp" -#include "uORBUtils.hpp" namespace uORB { -class SubscriptionCallback; - // Base subscription wrapper class class Subscription { @@ -107,30 +104,18 @@ class Subscription return *this; } - ~Subscription() + virtual ~Subscription() { unsubscribe(); } - bool subscribe(); + bool subscribe(bool advertise = false); void unsubscribe(); - bool valid() const { return _node != nullptr; } + bool valid() const { return orb_advert_valid(_node); } bool advertised() { - if (valid()) { - return Manager::is_advertised(_node); - } - - // try to initialize - if (subscribe()) { - // check again if valid - if (valid()) { - return Manager::is_advertised(_node); - } - } - - return false; + return Manager::has_publisher(_orb_id, _instance); } /** @@ -186,16 +171,18 @@ class Subscription protected: friend class SubscriptionCallback; + friend class SubscriptionPollable; friend class SubscriptionCallbackWorkItem; - void *get_node() { return _node; } + orb_advert_t &get_node() { return _node; } - void *_node{nullptr}; + orb_advert_t _node{ORB_ADVERT_INVALID}; unsigned _last_generation{0}; /**< last generation the subscriber has seen */ ORB_ID _orb_id{ORB_ID::INVALID}; uint8_t _instance{0}; + bool _advertiser{false}; }; // Subscription wrapper class with data diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 7ec29e329b6e..ae6fc704faf1 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -105,7 +105,7 @@ class SubscriptionBlocking : public SubscriptionCallback */ bool updatedBlocking(uint32_t timeout_us = 0) { - if (!_registered) { + if (!registered()) { registerCallback(); } @@ -129,7 +129,11 @@ class SubscriptionBlocking : public SubscriptionCallback // Calculate an absolute time in the future struct timespec ts; +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + px4_clock_gettime(CLOCK_MONOTONIC, &ts); +#else px4_clock_gettime(CLOCK_REALTIME, &ts); +#endif uint64_t nsecs = ts.tv_nsec + (timeout_us * 1000); static constexpr unsigned billion = (1000 * 1000 * 1000); ts.tv_sec += nsecs / billion; diff --git a/platforms/common/uORB/SubscriptionCallback.hpp b/platforms/common/uORB/SubscriptionCallback.hpp index 942a9fbe17d3..941a3237ff6b 100644 --- a/platforms/common/uORB/SubscriptionCallback.hpp +++ b/platforms/common/uORB/SubscriptionCallback.hpp @@ -39,14 +39,17 @@ #pragma once #include -#include #include +#include namespace uORB { // Subscription wrapper class with callbacks on new publications -class SubscriptionCallback : public SubscriptionInterval, public ListNode +class SubscriptionCallback : public SubscriptionInterval +#ifndef CONFIG_BUILD_FLAT + , public ListNode +#endif { public: /** @@ -68,36 +71,25 @@ class SubscriptionCallback : public SubscriptionInterval, public ListNode -#include - -#include "uORBDeviceNode.hpp" -#include "uORBManager.hpp" -#include "uORBUtils.hpp" +#include +#include +#include #include "Subscription.hpp" -#include - namespace uORB { @@ -83,9 +79,9 @@ class SubscriptionInterval SubscriptionInterval() : _subscription{nullptr} {} - ~SubscriptionInterval() = default; + virtual ~SubscriptionInterval() = default; - bool subscribe() { return _subscription.subscribe(); } + bool subscribe(bool create = false) { return _subscription.subscribe(create); } void unsubscribe() { _subscription.unsubscribe(); } bool advertised() { return _subscription.advertised(); } diff --git a/platforms/common/uORB/uORB.cpp b/platforms/common/uORB/uORB.cpp index 734b8d83f1b4..d09be27d6326 100644 --- a/platforms/common/uORB/uORB.cpp +++ b/platforms/common/uORB/uORB.cpp @@ -40,7 +40,7 @@ #include "uORBManager.hpp" #include "uORBCommon.hpp" - +#include "Publication.hpp" #include #include @@ -50,11 +50,11 @@ #include #endif -static uORB::DeviceMaster *g_dev = nullptr; +static bool initialized = false; int uorb_start(void) { - if (g_dev != nullptr) { + if (initialized) { PX4_WARN("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; @@ -65,51 +65,13 @@ int uorb_start(void) return -ENOMEM; } -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - /* create the driver */ - g_dev = uORB::Manager::get_instance()->get_device_master(); - - if (g_dev == nullptr) { - return -errno; - } - -#endif - + initialized = true; return OK; } -int uorb_status(void) +int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) { -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - - if (g_dev != nullptr) { - g_dev->printStatistics(); - - } else { - PX4_INFO("uorb is not running"); - } - -#else - boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_STATUS); -#endif - return OK; -} - -int uorb_top(char **topic_filter, int num_filters) -{ -#if !defined(__PX4_NUTTX) || defined(CONFIG_BUILD_FLAT) || defined(__KERNEL__) - - if (g_dev != nullptr) { - g_dev->showTop(topic_filter, num_filters); - - } else { - PX4_INFO("uorb is not running"); - } - -#else - boardctl(ORBIOCDEVMASTERCMD, ORB_DEVMASTER_TOP); -#endif - return OK; + return uORB::Manager::get_instance()->orb_poll(fds, nfds, timeout); } orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) @@ -135,63 +97,63 @@ orb_advert_t orb_advertise_multi_queue(const struct orb_metadata *meta, const vo int orb_unadvertise(orb_advert_t handle) { - return uORB::Manager::get_instance()->orb_unadvertise(handle); + return uORB::Manager::orb_unadvertise(handle); } -int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +int orb_publish(const struct orb_metadata *meta, orb_advert_t *handle, const void *data) { - return uORB::Manager::get_instance()->orb_publish(meta, handle, data); + return uORB::Manager::orb_publish(meta, *handle, data); } -int orb_subscribe(const struct orb_metadata *meta) +orb_sub_t orb_subscribe(const struct orb_metadata *meta) { return uORB::Manager::get_instance()->orb_subscribe(meta); } -int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance); } -int orb_unsubscribe(int handle) +int orb_unsubscribe(orb_sub_t handle) { return uORB::Manager::get_instance()->orb_unsubscribe(handle); } -int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) { - return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer); + return uORB::Manager::orb_copy(meta, handle, buffer); } -int orb_check(int handle, bool *updated) +int orb_check(orb_sub_t handle, bool *updated) { return uORB::Manager::get_instance()->orb_check(handle, updated); } int orb_exists(const struct orb_metadata *meta, int instance) { - return uORB::Manager::get_instance()->orb_exists(meta, instance); + return uORB::Manager::orb_exists(meta, instance); } int orb_group_count(const struct orb_metadata *meta) { unsigned instance = 0; - while (uORB::Manager::get_instance()->orb_exists(meta, instance) == OK) { + while (orb_exists(meta, instance) == OK) { ++instance; }; return instance; } -int orb_set_interval(int handle, unsigned interval) +int orb_set_interval(orb_sub_t handle, unsigned interval) { - return uORB::Manager::get_instance()->orb_set_interval(handle, interval); + return uORB::Manager::orb_set_interval(handle, interval); } -int orb_get_interval(int handle, unsigned *interval) +int orb_get_interval(orb_sub_t handle, unsigned *interval) { - return uORB::Manager::get_instance()->orb_get_interval(handle, interval); + return uORB::Manager::orb_get_interval(handle, interval); } const char *orb_get_c_type(unsigned char short_type) diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 7fdbb699d1f8..2006ef08ebcd 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -56,6 +56,7 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; + /** * Maximum number of multi topic instances. This must be <= 10 (because it's the last char of the node path) */ @@ -114,20 +115,54 @@ typedef const struct orb_metadata *orb_id_t; __BEGIN_DECLS int uorb_start(void); -int uorb_status(void); -int uorb_top(char **topic_filter, int num_filters); /** * ORB topic advertiser handle. - * - * Advertiser handles are global; once obtained they can be shared freely - * and do not need to be closed or released. - * - * This permits publication from interrupt context and other contexts where - * a file-descriptor-based handle would not otherwise be in scope for the - * publisher. */ -typedef void *orb_advert_t; + +typedef struct { + void *node; +#ifndef CONFIG_BUILD_FLAT + void *data; +#endif + size_t data_size; +} orb_advert_t; + +/** + * ORB topic subscriber handle. + */ + +typedef void *orb_sub_t; + +/** + * Helper functions to initialize and check the handles + */ + +static inline bool orb_advert_valid(orb_advert_t handle) {return handle.node != NULL;} +#ifndef CONFIG_BUILD_FLAT +static const orb_advert_t ORB_ADVERT_INVALID = {NULL, NULL, 0}; +#else +static const orb_advert_t ORB_ADVERT_INVALID = {NULL, 0}; +#endif +static inline bool orb_sub_valid(orb_sub_t handle) {return handle != NULL;} +static const orb_sub_t ORB_SUB_INVALID = NULL; + +/** + * orb_poll struct + */ + +typedef short orb_pollevent_t; +typedef struct { + /* This part of the struct is POSIX-like */ + orb_sub_t fd; /* The polling subscriber handle */ + orb_pollevent_t events; /* The input event flags */ + orb_pollevent_t revents; /* The output event flags */ +} orb_poll_struct_t; + +/** + * @see uORB::Manager::orb_poll() + */ +extern int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) __EXPORT; /** * @see uORB::Manager::orb_advertise() @@ -159,7 +194,7 @@ extern int orb_unadvertise(orb_advert_t handle) __EXPORT; /** * @see uORB::Manager::orb_publish() */ -extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t *handle, const void *data) __EXPORT; /** * Advertise as the publisher of a topic. @@ -172,15 +207,15 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance) { - if (!*handle) { + if (!orb_advert_valid(*handle)) { *handle = orb_advertise_multi(meta, data, instance); - if (*handle) { + if (orb_advert_valid(*handle)) { return 0; } } else { - return orb_publish(meta, *handle, data); + return orb_publish(meta, handle, data); } return -1; @@ -189,27 +224,27 @@ static inline int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t /** * @see uORB::Manager::orb_subscribe() */ -extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +extern orb_sub_t orb_subscribe(const struct orb_metadata *meta) __EXPORT; /** * @see uORB::Manager::orb_subscribe_multi() */ -extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; +extern orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; /** * @see uORB::Manager::orb_unsubscribe() */ -extern int orb_unsubscribe(int handle) __EXPORT; +extern int orb_unsubscribe(orb_sub_t handle) __EXPORT; /** * @see uORB::Manager::orb_copy() */ -extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT; +extern int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) __EXPORT; /** * @see uORB::Manager::orb_check() */ -extern int orb_check(int handle, bool *updated) __EXPORT; +extern int orb_check(orb_sub_t handle, bool *updated) __EXPORT; /** * @see uORB::Manager::orb_exists() @@ -227,12 +262,12 @@ extern int orb_group_count(const struct orb_metadata *meta) __EXPORT; /** * @see uORB::Manager::orb_set_interval() */ -extern int orb_set_interval(int handle, unsigned interval) __EXPORT; +extern int orb_set_interval(orb_sub_t handle, unsigned interval) __EXPORT; /** * @see uORB::Manager::orb_get_interval() */ -extern int orb_get_interval(int handle, unsigned *interval) __EXPORT; +extern int orb_get_interval(orb_sub_t handle, unsigned *interval) __EXPORT; /** * Returns the C type string from a short type in o_fields metadata, or nullptr diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 7cc5c9bd3eb0..e15851dac43a 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" @@ -43,10 +45,177 @@ #endif /* CONFIG_ORB_COMMUNICATOR */ #if defined(__PX4_NUTTX) +#include #include +#include +#endif + +#include +#include +#include + +// This is a speed optimization for nuttx flat build +#ifdef CONFIG_BUILD_FLAT +#define ATOMIC_ENTER irqstate_t flags = px4_enter_critical_section() +#define ATOMIC_LEAVE px4_leave_critical_section(flags) +#else +#define ATOMIC_ENTER lock() +#define ATOMIC_LEAVE unlock() +#endif + +// Every subscriber thread has it's own list of cached subscriptions +uORB::DeviceNode::MappingCache::MappingCacheListItem *uORB::DeviceNode::MappingCache::g_cache = + nullptr; + +// This lock protects the subscription cache list from concurrent accesses by the threads in the same process +px4_sem_t uORB::DeviceNode::MappingCache::g_cache_lock; + +orb_advert_t uORB::DeviceNode::MappingCache::get(ORB_ID orb_id, uint8_t instance) +{ + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + (orb_id != node(item->handle)->id() || + instance != node(item->handle)->get_instance())) { + item = item->next; + } + + unlock(); + + return item != nullptr ? item->handle : ORB_ADVERT_INVALID; +} + +orb_advert_t uORB::DeviceNode::MappingCache::map_node(ORB_ID orb_id, uint8_t instance, int shm_fd) +{ + + // Check if it is already mapped + orb_advert_t handle = get(orb_id, instance); + + if (orb_advert_valid(handle)) { + return handle; + } + + lock(); + + // Not mapped yet, map it + void *ptr = px4_mmap(0, sizeof(uORB::DeviceNode), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); + + if (ptr != MAP_FAILED) { + // In NuttX flat and protected builds we can just drop the mappings + // to save some kernel memory. There is no MMU, and the memory is + // there until the shm object is unlinked +#if defined(CONFIG_BUILD_FLAT) + px4_munmap(ptr, sizeof(uORB::DeviceNode)); #endif -static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast(filp->f_priv); } + // Create a list item and add to the beginning of the list + handle.node = ptr; + MappingCacheListItem *item = new MappingCacheListItem{g_cache, handle}; + + if (item) { + g_cache = item; + } + } + + unlock(); + + return handle; +} + +#if !defined(CONFIG_BUILD_FLAT) +orb_advert_t uORB::DeviceNode::MappingCache::map_data(orb_advert_t handle, int shm_fd, size_t size, bool publisher) +{ + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + handle.node != item->handle.node) { + item = item->next; + } + + if (item != nullptr) { + + if (item->handle.data != nullptr && item->handle.data_size == size) { + // Mapped already, return the mapping + handle = item->handle; + + } else { + // Drop any old mapping if exists + if (handle.data != nullptr) { + px4_munmap(handle.data, handle.data_size); + } + + // Map the data with new size + if (shm_fd >= 0 && size > 0) { + handle.data = px4_mmap(0, size, publisher ? PROT_WRITE : PROT_READ, MAP_SHARED, shm_fd, 0); + + if (handle.data == MAP_FAILED) { + handle.data = nullptr; + handle.data_size = 0; + PX4_ERR("MMAP fail\n"); + + } else { + handle.data_size = size; + } + + } else { + handle.data = nullptr; + handle.data_size = 0; + } + + item->handle = handle; + } + } + + unlock(); + + return handle; +} +#endif + +bool uORB::DeviceNode::MappingCache::del(const orb_advert_t &handle) +{ + MappingCacheListItem *prev = nullptr; + + lock(); + + MappingCacheListItem *item = g_cache; + + while (item && + handle.node != item->handle.node) { + prev = item; + item = item->next; + } + + if (item != nullptr) { + if (prev == nullptr) { + // Remove the first item + g_cache = item->next; + + } else { + prev->next = item->next; + } + + px4_munmap(handle.node, sizeof(DeviceNode)); + +#ifndef CONFIG_BUILD_FLAT + + if (handle.data) { + px4_munmap(handle.data, handle.data_size); + } + +#endif + + delete (item); + } + + unlock(); + + return item != nullptr ? true : false; +} // round up to nearest power of two // Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128 @@ -73,210 +242,414 @@ static inline uint8_t round_pow_of_two_8(uint8_t n) return value + 1; } -uORB::DeviceNode::DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, - uint8_t queue_size) : - CDev(strdup(path)), // success is checked in CDev::init - _meta(meta), - _instance(instance), - _queue_size(round_pow_of_two_8(queue_size)) +orb_advert_t uORB::DeviceNode::nodeOpen(const ORB_ID id, const uint8_t instance, bool create) { + /* + * Generate the path to the node and try to open it. + */ + + orb_advert_t handle = MappingCache::get(id, instance); + + if (orb_advert_valid(handle)) { + return handle; + } + + char nodepath[orb_maxpath]; + int inst = instance; + int ret = uORB::Utils::node_mkpath(nodepath, get_orb_meta(id), &inst); + bool created = false; + + if (ret == -ENAMETOOLONG || strlen(nodepath) > NAME_MAX) { + PX4_ERR("Device node name too long! '%s' (len: %lu vs. NAME_MAX: %lu)", + get_orb_meta(id)->o_name, ((long unsigned int) strlen(nodepath)), ((long unsigned int) NAME_MAX)); + } + + if (ret != OK) { + return handle; + } + + // First, try to create the node. This will fail if it already exists + + int shm_fd = -1; + + if (create) { + shm_fd = shm_open(nodepath, O_CREAT | O_RDWR | O_EXCL, 0666); + + if (shm_fd >= 0) { + + // If the creation succeeded, set the size of the shm region + if (ftruncate(shm_fd, sizeof(uORB::DeviceNode)) != 0) { + ::close(shm_fd); + shm_fd = -1; + PX4_ERR("truncate fail!\n"); + + } else { + created = true; + } + } + } + + if (shm_fd < 0) { + // Now try to open an existing one + + shm_fd = shm_open(nodepath, O_RDWR, 0666); + } + + if (shm_fd < 0) { + // We were not able to create a new node or open an existing one + return handle; + } + + handle = MappingCache::map_node(id, instance, shm_fd); + + // No need to keep the fd any more, close it + + ::close(shm_fd); + + if (orb_advert_valid(handle) && created) { + // construct the new node in the region + new (node(handle)) uORB::DeviceNode(id, instance, nodepath); + } + + return handle; } -uORB::DeviceNode::~DeviceNode() +int uORB::DeviceNode::nodeClose(orb_advert_t &handle) { - free(_data); + if (!orb_advert_valid(handle)) { + return PX4_ERROR; + } - const char *devname = get_devname(); + if (node(handle)->_publisher_count == 0) { + node(handle)->_queue_size = 0; + node(handle)->_data_valid = false; - if (devname) { -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) - kmm_free((void *)devname); + // Delete the data +#ifdef CONFIG_BUILD_FLAT + free(node(handle)->_data); + node(handle)->_data = nullptr; #else - free((void *)devname); + shm_unlink(node(handle)->get_devname() + 1); + MappingCache::map_data(handle, -1, 0, false); #endif + + // If there are no more subscribers, delete the node and its mapping + if (node(handle)->_subscriber_count == 0) { + + // Close the Node object + shm_unlink(node(handle)->get_devname()); + + // Uninitialize the node + delete (node(handle)); + + // Delete the mappings for this process + MappingCache::del(handle); + } } + + handle = ORB_ADVERT_INVALID; + + return PX4_OK; } -int -uORB::DeviceNode::open(cdev::file_t *filp) +orb_advert_t uORB::DeviceNode::orb_advertise(const ORB_ID id, int instance, unsigned queue_size, + bool publisher) { - /* is this a publisher? */ - if (filp->f_oflags == PX4_F_WRONLY) { + /* Open the node, if it exists or create a new one */ - lock(); - mark_as_advertised(); - unlock(); + orb_advert_t handle; + handle = nodeOpen(id, instance, true); - /* now complete the open */ - return CDev::open(filp); + if (orb_advert_valid(handle)) { + node(handle)->advertise(publisher, queue_size); } - /* is this a new subscriber? */ - if (filp->f_oflags == PX4_F_RDONLY) { + return handle; +} - /* allocate subscriber data */ - SubscriptionInterval *sd = new SubscriptionInterval(_meta, 0, _instance); +int uORB::DeviceNode::advertise(bool publisher, uint8_t queue_size) +{ + int ret = -1; - if (nullptr == sd) { - return -ENOMEM; - } + ret = ++_advertiser_count; - filp->f_priv = (void *)sd; + if (publisher) { + ret = ++_publisher_count; + } - int ret = CDev::open(filp); + update_queue_size(queue_size); - if (ret != PX4_OK) { - PX4_ERR("CDev::open failed"); - delete sd; - } + return ret; +} - return ret; +int uORB::DeviceNode::orb_unadvertise(orb_advert_t &handle, bool publisher) +{ + int ret = -1; + + if (orb_advert_valid(handle)) { + ret = node(handle)->unadvertise(publisher); + nodeClose(handle); } - if (filp->f_oflags == 0) { - return CDev::open(filp); + return ret; +} + +int uORB::DeviceNode::unadvertise(bool publisher) +{ + int ret = -1; + + ret = --_advertiser_count; + + if (publisher) { + --_publisher_count; } - /* can only be pub or sub, not both */ - return -EINVAL; + return ret; } -int -uORB::DeviceNode::close(cdev::file_t *filp) +uORB::DeviceNode::DeviceNode(const ORB_ID id, const uint8_t instance, const char *path) : + _orb_id(id), + _instance(instance) { - if (filp->f_oflags == PX4_F_RDONLY) { /* subscriber */ - SubscriptionInterval *sd = filp_to_subscription(filp); - delete sd; +#if defined(CONFIG_BUILD_FLAT) + _devname = strdup(path); +#else + + if (strnlen(path, sizeof(_devname)) == sizeof(_devname)) { + PX4_ERR("node path too long %s", path); } - return CDev::close(filp); + strncpy(_devname, path, sizeof(_devname)); + + int ret = px4_sem_init(&_lock, 1, 1); + + if (ret != 0) { + PX4_DEBUG("SEM INIT FAIL: ret %d", ret); + } + +#endif } -ssize_t -uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen) +uORB::DeviceNode::~DeviceNode() { - /* if the caller's buffer is the wrong size, that's an error */ - if (buflen != _meta->o_size) { - return -EIO; +#if defined(CONFIG_BUILD_FLAT) + + // Delete all the allocated free callback items. + // There should not be any left in use, since the node is + // deleted only if there are no more publishers or subscribers registered + + IndexedStackHandle callbacks(_callbacks); + uorb_cb_handle_t handle = callbacks.pop_free(); + + while (callbacks.handle_valid(handle)) { + delete (static_cast(callbacks.peek(handle))); + handle = callbacks.pop_free(); } - return filp_to_subscription(filp)->copy(buffer) ? _meta->o_size : 0; + free(_devname); +#else + px4_sem_destroy(&_lock); +#endif } -ssize_t -uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) +/* Map the node data to the memory space of publisher or subscriber */ + +void uORB::DeviceNode::remap_data(orb_advert_t &handle, size_t new_size, bool publisher) { - /* - * Writes are legal from interrupt context as long as the - * object has already been initialised from thread context. - * - * Writes outside interrupt context will allocate the object - * if it has not yet been allocated. - * - * Note that filp will usually be NULL. - */ - if (nullptr == _data) { + // In NuttX flat and protected builds, just malloc the data (from user heap) + // and store + // the pointer. This saves us the inodes in the + // kernel side. Otherwise the same logic would work -#ifdef __PX4_NUTTX +#ifdef CONFIG_BUILD_FLAT - if (!up_interrupt_context()) { -#endif /* __PX4_NUTTX */ + // Data size has changed, re-allocate (remap) by publisher + // The remapping may happen only on the first write, + // when the handle.data_size==0 - lock(); + if (publisher && handle.data_size == 0) { + free(_data); + _data = malloc(new_size); + } - /* re-check size */ - if (nullptr == _data) { - const size_t data_size = _meta->o_size * _queue_size; - _data = (uint8_t *) px4_cache_aligned_alloc(data_size); - memset(_data, 0, data_size); - } + if (_data != nullptr) { + handle.data_size = new_size; - unlock(); + } else { + handle.data_size = 0; + } + +#else + + // Open the data, the data shm name is the same as device node's except for leading '_' + int oflag = publisher ? O_RDWR | O_CREAT : O_RDONLY; + int shm_fd = shm_open(get_devname() + 1, oflag, 0666); + + // and mmap it + if (shm_fd >= 0) { -#ifdef __PX4_NUTTX + // For the publisher, set the new data size + if (publisher && handle.data_size == 0) { + if (ftruncate(shm_fd, new_size) != 0) { + ::close(shm_fd); + PX4_ERR("Setting advertise size failed\n"); + return; + } } -#endif /* __PX4_NUTTX */ + handle = MappingCache::map_data(handle, shm_fd, new_size, publisher); + + // Close the shm, there is no need to leave it open + ::close(shm_fd); + } + +#endif +} + +/** + * Copies data and the corresponding generation + * from a node to the buffer provided. + * + * @param dst + * The buffer into which the data is copied. + * @param generation + * The generation that was copied. + * @return bool + * Returns true if the data was copied. + */ +bool uORB::DeviceNode::copy(void *dst, orb_advert_t &handle, unsigned &generation) +{ + if (dst == nullptr || !_data_valid) { + return false; + } + + size_t o_size = get_meta()->o_size; + size_t data_size = _queue_size * o_size; + + ATOMIC_ENTER; + + if (data_size != handle.data_size) { + remap_data(handle, data_size, false); - /* failed or could not allocate */ - if (nullptr == _data) { - return -ENOMEM; + if (node_data(handle) == nullptr) { + ATOMIC_LEAVE; + return false; } } - /* If write size does not match, that is an error */ - if (_meta->o_size != buflen) { - return -EIO; + if (_queue_size == 1) { + memcpy(dst, node_data(handle), o_size); + generation = _generation.load(); + + } else { + const unsigned current_generation = _generation.load(); + + if (current_generation == generation) { + /* The subscriber already read the latest message, but nothing new was published yet. + * Return the previous message + */ + --generation; + } + + /* Compatible with normal and overflow conditions */ + if (current_generation - generation > _queue_size) { + /* Reader is too far behind: some messages are lost */ + generation = current_generation - _queue_size; + } + + memcpy(dst, ((uint8_t *)node_data(handle)) + (o_size * (generation % _queue_size)), o_size); + + ++generation; } + ATOMIC_LEAVE; + + return true; + +} + +ssize_t +uORB::DeviceNode::write(const char *buffer, const orb_metadata *meta, orb_advert_t &handle) +{ + size_t o_size = meta->o_size; + + /* If data size has changed, re-map the data */ + size_t data_size = _queue_size * o_size; + + /* Remove single unresponsive entry at a time (if any) */ + uorb_cb_handle_t remove_cb = UORB_INVALID_CB_HANDLE; + /* Perform an atomic copy. */ ATOMIC_ENTER; - /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ - unsigned generation = _generation.fetch_add(1); - memcpy(_data + (_meta->o_size * (generation % _queue_size)), buffer, _meta->o_size); + if (data_size != handle.data_size) { + remap_data(handle, data_size, true); + } - // callbacks - for (auto item : _callbacks) { - item->call(); + if (node_data(handle) == nullptr) { + ATOMIC_LEAVE; + return -ENOMEM; } + /* wrap-around happens after ~49 days, assuming a publisher rate of 1 kHz */ + unsigned generation = _generation.fetch_add(1); + + memcpy(((uint8_t *)node_data(handle)) + o_size * (generation % _queue_size), buffer, o_size); + /* Mark at least one data has been published */ _data_valid = true; - ATOMIC_LEAVE; + IndexedStackHandle callbacks(_callbacks); - /* notify any poll waiters */ - poll_notify(POLLIN); + uorb_cb_handle_t cb = callbacks.head(); - return _meta->o_size; -} + while (callbacks.handle_valid(cb)) { + EventWaitItem *item = callbacks.peek(cb); -int -uORB::DeviceNode::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case ORBIOCUPDATED: { - ATOMIC_ENTER; - *(bool *)arg = filp_to_subscription(filp)->updated(); - ATOMIC_LEAVE; - return PX4_OK; - } + if (item->interval_us == 0 || hrt_elapsed_time(&item->last_update) >= item->interval_us) { - case ORBIOCSETINTERVAL: - filp_to_subscription(filp)->set_interval_us(arg); - return PX4_OK; +#ifdef CONFIG_BUILD_FLAT - case ORBIOCGADVERTISER: - *(uintptr_t *)arg = (uintptr_t)this; - return PX4_OK; + if (item->subscriber != nullptr) { + // execute callback + item->subscriber->call(); - case ORBIOCSETQUEUESIZE: { - lock(); - int ret = update_queue_size(arg); - unlock(); - return ret; - } + } else { + // release poll + Manager::unlockThread(item->lock); + } - case ORBIOCGETINTERVAL: - *(unsigned *)arg = filp_to_subscription(filp)->get_interval_us(); - return PX4_OK; +#else - case ORBIOCISADVERTISED: - *(unsigned long *)arg = _advertised; + // Release poll waiters and callback threads + if (item->cb_triggered < CB_ALIVE_MAX_VALUE) { + __atomic_fetch_add(&item->cb_triggered, 1, __ATOMIC_SEQ_CST); + Manager::unlockThread(item->lock); - return PX4_OK; + } else { + remove_cb = cb; + } + +#endif + } - default: - /* give it to the superclass */ - return CDev::ioctl(filp, cmd, arg); + cb = callbacks.next(cb); } + + ATOMIC_LEAVE; + + if (callbacks.handle_valid(remove_cb)) { + PX4_ERR("Removing subscriber due to inactivity\n"); + unregister_callback(handle, remove_cb); + } + + return o_size; } ssize_t -uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t &handle, const void *data) { - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; + uORB::DeviceNode *devnode = node(handle); int ret; /* check if the device handle is initialized and data is valid */ @@ -286,18 +659,13 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v } /* check if the orb meta data matches the publication */ - if (devnode->_meta->o_id != meta->o_id) { + if (static_cast(devnode->id()) != meta->o_id) { errno = EINVAL; return PX4_ERROR; } - /* call the devnode write method with no file pointer */ - ret = devnode->write(nullptr, (const char *)data, meta->o_size); - - if (ret < 0) { - errno = -ret; - return PX4_ERROR; - } + /* call the devnode write method */ + ret = devnode->write((const char *)data, meta, handle); if (ret != (int)meta->o_size) { errno = EIO; @@ -322,30 +690,6 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v return PX4_OK; } -int uORB::DeviceNode::unadvertise(orb_advert_t handle) -{ - if (handle == nullptr) { - return -EINVAL; - } - - uORB::DeviceNode *devnode = (uORB::DeviceNode *)handle; - - /* - * We are cheating a bit here. First, with the current implementation, we can only - * have multiple publishers for instance 0. In this case the caller will have - * instance=nullptr and _published has no effect at all. Thus no unadvertise is - * necessary. - * In case of multiple instances, we have at most 1 publisher per instance and - * we can signal an instance as 'free' by setting _published to false. - * We never really free the DeviceNode, for this we would need reference counting - * of subscribers and publishers. But we also do not have a leak since future - * publishers reuse the same DeviceNode object. - */ - devnode->_advertised = false; - - return PX4_OK; -} - #ifdef CONFIG_ORB_COMMUNICATOR int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) { @@ -359,119 +703,136 @@ int16_t uORB::DeviceNode::topic_advertised(const orb_metadata *meta) } #endif /* CONFIG_ORB_COMMUNICATOR */ -px4_pollevent_t -uORB::DeviceNode::poll_state(cdev::file_t *filp) -{ - // If the topic appears updated to the subscriber, say so. - return filp_to_subscription(filp)->updated() ? POLLIN : 0; -} - -void -uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) -{ - // If the topic looks updated to the subscriber, go ahead and notify them. - if (filp_to_subscription((cdev::file_t *)fds->priv)->updated()) { - CDev::poll_notify_one(fds, events); - } -} - bool uORB::DeviceNode::print_statistics(int max_topic_length) { - if (!_advertised) { - return false; - } - lock(); + ATOMIC_ENTER; const uint8_t instance = get_instance(); const int8_t sub_count = subscriber_count(); const uint8_t queue_size = get_queue_size(); - unlock(); + ATOMIC_LEAVE; - PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, get_meta()->o_name, (int)instance, (int)sub_count, - queue_size, get_meta()->o_size, get_devname()); + const orb_metadata *meta = get_meta(); + + PX4_INFO_RAW("%-*s %2i %4i %2i %4i %s\n", max_topic_length, meta->o_name, (int)instance, (int)sub_count, + queue_size, meta->o_size, get_devname()); return true; } -void uORB::DeviceNode::add_internal_subscriber() +orb_advert_t uORB::DeviceNode::add_subscriber(ORB_ID orb_id, uint8_t instance, + unsigned *initial_generation, bool advertise) { - lock(); + orb_advert_t handle; + + if (advertise) { + handle = orb_advertise(orb_id, instance, 0, false); + + } else { + handle = nodeOpen(orb_id, instance, false); + } + + if (orb_advert_valid(handle)) { + node(handle)->_add_subscriber(initial_generation); + + } else { + *initial_generation = 0; + } + + return handle; +} + + +void uORB::DeviceNode::_add_subscriber(unsigned *initial_generation) +{ + *initial_generation = _generation.load() - (_data_valid ? 1 : 0); _subscriber_count++; #ifdef CONFIG_ORB_COMMUNICATOR uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); if (ch != nullptr && _subscriber_count > 0) { - unlock(); //make sure we cannot deadlock if add_subscription calls back into DeviceNode - ch->add_subscription(_meta->o_name, 1); - - } else -#endif /* CONFIG_ORB_COMMUNICATOR */ + ch->add_subscription(get_name(), 1); - { - unlock(); } + +#endif /* CONFIG_ORB_COMMUNICATOR */ } -void uORB::DeviceNode::remove_internal_subscriber() + +int8_t uORB::DeviceNode::remove_subscriber(orb_advert_t &handle, bool advertiser) { - lock(); - _subscriber_count--; + int8_t ret = _subscriber_count--; + + if (advertiser) { + orb_unadvertise(handle, false); + + } else { + nodeClose(handle); + + } #ifdef CONFIG_ORB_COMMUNICATOR uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); - if (ch != nullptr && _subscriber_count == 0) { - unlock(); //make sure we cannot deadlock if remove_subscription calls back into DeviceNode - ch->remove_subscription(_meta->o_name); + if (ch != nullptr && ret == 0) { + ch->remove_subscription(get_meta()->o_name); - } else -#endif /* CONFIG_ORB_COMMUNICATOR */ - { - unlock(); } + +#endif /* ORB_COMMUNICATOR */ + + return ret; } #ifdef CONFIG_ORB_COMMUNICATOR -int16_t uORB::DeviceNode::process_add_subscription() +int16_t uORB::DeviceNode::process_add_subscription(orb_advert_t &handle) { // if there is already data in the node, send this out to // the remote entity. // send the data to the remote entity. uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); + const orb_metadata *meta = get_meta(); - if (_data != nullptr && ch != nullptr) { // _data will not be null if there is a publisher. - ch->send_message(_meta->o_name, _meta->o_size, _data); + if (ch != nullptr) { + ch->send_message(meta->o_name, meta->o_size, (uint8_t *)node_data(handle)); } return PX4_OK; } -int16_t uORB::DeviceNode::process_remove_subscription() +int16_t uORB::DeviceNode::process_remove_subscription(orb_advert_t &handle) { return PX4_OK; } -int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data) +int16_t uORB::DeviceNode::process_received_message(orb_advert_t &handle, int32_t length, uint8_t *data) { int16_t ret = -1; - if (length != (int32_t)(_meta->o_size)) { - PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", _meta->o_name, (int)length, (int)_meta->o_size); + if (!orb_advert_valid(handle)) { + return ret; + } + + const orb_metadata *meta = get_meta(); + + if (length != (int32_t)(meta->o_size)) { + PX4_ERR("Received '%s' with DataLength[%d] != ExpectedLen[%d]", meta->o_name, (int)length, + (int)meta->o_size); return PX4_ERROR; } - /* call the devnode write method with no file pointer */ - ret = write(nullptr, (const char *)data, _meta->o_size); + /* call the devnode write method */ + ret = write((const char *)data, meta, handle); if (ret < 0) { return PX4_ERROR; } - if (ret != (int)_meta->o_size) { + if (ret != (int)meta->o_size) { errno = EIO; return PX4_ERROR; } @@ -482,57 +843,90 @@ int16_t uORB::DeviceNode::process_received_message(int32_t length, uint8_t *data int uORB::DeviceNode::update_queue_size(unsigned int queue_size) { - if (_queue_size == queue_size) { + // subscribers may advertise the node, but not set the queue_size + if (queue_size == 0) { return PX4_OK; } - //queue size is limited to 255 for the single reason that we use uint8 to store it - if (_data || _queue_size > queue_size || queue_size > 255) { + queue_size = round_pow_of_two_8(queue_size); + + // queue size is limited to 255 for the single reason that we use uint8 to store it + if (queue_size > 255) { return PX4_ERROR; } - _queue_size = round_pow_of_two_8(queue_size); + _queue_size = queue_size; + return PX4_OK; } -unsigned uORB::DeviceNode::get_initial_generation() +bool +uORB::DeviceNode::_register_callback(uORB::SubscriptionCallback *cb_sub, + int8_t poll_lock, hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle) { + int8_t cb_lock = poll_lock; + + IndexedStackHandle callbacks(_callbacks); + ATOMIC_ENTER; - // If there any previous publications allow the subscriber to read them - unsigned generation = _generation.load() - (_data_valid ? 1 : 0); + cb_handle = callbacks.pop_free(); + EventWaitItem *item = callbacks.peek(cb_handle); - ATOMIC_LEAVE; +#ifdef CONFIG_BUILD_FLAT - return generation; -} + if (!item) { + px4_leave_critical_section(flags); + item = new EventWaitItem; + flags = px4_enter_critical_section(); + cb_handle = item; + } -bool -uORB::DeviceNode::register_callback(uORB::SubscriptionCallback *callback_sub) -{ - if (callback_sub != nullptr) { - ATOMIC_ENTER; - - // prevent duplicate registrations - for (auto existing_callbacks : _callbacks) { - if (callback_sub == existing_callbacks) { - ATOMIC_LEAVE; - return true; - } - } +#endif - _callbacks.add(callback_sub); - ATOMIC_LEAVE; - return true; + if (item != nullptr) { + item->lock = cb_lock; +#ifdef CONFIG_BUILD_FLAT + item->subscriber = cb_sub; +#else + item->cb_triggered = 0; +#endif + item->last_update = last_update; + item->interval_us = interval_us; + callbacks.push(cb_handle); + + } else { + PX4_ERR("register fail\n"); } - return false; + ATOMIC_LEAVE; + + return uorb_cb_handle_valid(cb_handle); } -void -uORB::DeviceNode::unregister_callback(uORB::SubscriptionCallback *callback_sub) +int +uORB::DeviceNode::_unregister_callback(uorb_cb_handle_t &cb_handle) { + IndexedStackHandle callbacks(_callbacks); + int ret = 0; + ATOMIC_ENTER; - _callbacks.remove(callback_sub); + + bool ret_rm = callbacks.rm(cb_handle); + + if (!ret_rm) { + PX4_ERR("unregister fail\n"); + + } else { +#ifndef CONFIG_BUILD_FLAT + EventWaitItem *item = callbacks.peek(cb_handle); + ret = item->cb_triggered; +#endif + callbacks.push_free(cb_handle); + cb_handle = UORB_INVALID_CB_HANDLE; + } + ATOMIC_LEAVE; + + return ret; } diff --git a/platforms/common/uORB/uORBDeviceNode.hpp b/platforms/common/uORB/uORBDeviceNode.hpp index c5c5bb22c0c8..f7222f9932a4 100644 --- a/platforms/common/uORB/uORBDeviceNode.hpp +++ b/platforms/common/uORB/uORBDeviceNode.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. + * Copyight (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,37 +33,64 @@ #pragma once -#include "uORBCommon.hpp" -#include "uORBDeviceMaster.hpp" +#include +#include +#include +#include +#include +#include -#include +#include "uORB.h" +#include "uORBCommon.hpp" +#include +#include -#include #include #include #include -namespace uORB -{ -class DeviceNode; -class DeviceMaster; -class Manager; -class SubscriptionCallback; -} +#include "IndexedStack.hpp" + +#if defined(CONFIG_BUILD_FLAT) +#define MAX_EVENT_WAITERS 0 // dynamic +typedef void *uorb_cb_handle_t; +#define UORB_INVALID_CB_HANDLE nullptr +#define uorb_cb_handle_valid(x) ((x) != nullptr) +#else +#if defined(CONFIG_BUILD_KERNEL) +#define MAX_EVENT_WAITERS 32 +#else +#define MAX_EVENT_WAITERS 6 +#endif +#define UORB_INVALID_CB_HANDLE -1 +typedef int8_t uorb_cb_handle_t; +#define uorb_cb_handle_valid(x) ((x) >= 0) +#define CB_ALIVE_MAX_VALUE 50 +#endif + +#define CB_LIST_T struct EventWaitItem, uorb_cb_handle_t, MAX_EVENT_WAITERS namespace uORBTest { class UnitTest; } +namespace uORB +{ + +class SubscriptionCallback; + /** * Per-object device instance. */ -class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode +class DeviceNode { public: - DeviceNode(const struct orb_metadata *meta, const uint8_t instance, const char *path, uint8_t queue_size = 1); - virtual ~DeviceNode(); + // Open a node, either existing or create a new one + static orb_advert_t nodeOpen(const ORB_ID id, const uint8_t instance, bool create); + static int nodeClose(orb_advert_t &handle); + + ~DeviceNode(); // no copy, assignment, move, move assignment DeviceNode(const DeviceNode &) = delete; @@ -74,56 +101,25 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeget_name(); - static int unadvertise(orb_advert_t handle); + } else { + return nullptr; + } + } -#ifdef CONFIG_ORB_COMMUNICATOR /** * processes a request for topic advertisement from remote * @param meta @@ -140,53 +136,40 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNode(_meta->o_id); } + ORB_ID id() const { return _orb_id; } - const char *get_name() const { return _meta->o_name; } + const char *get_name() const { return get_orb_meta(_orb_id)->o_name; } uint8_t get_instance() const { return _instance; } @@ -231,82 +217,152 @@ class uORB::DeviceNode : public cdev::CDev, public IntrusiveSortedListNodeo_size); - generation = _generation.load(); - ATOMIC_LEAVE; - return true; - - } else { - ATOMIC_ENTER; - const unsigned current_generation = _generation.load(); - - if (current_generation == generation) { - /* The subscriber already read the latest message, but nothing new was published yet. - * Return the previous message - */ - --generation; - } - - // Compatible with normal and overflow conditions - if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) { - // Reader is too far behind: some messages are lost - generation = current_generation - _queue_size; - } - - memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size); - ATOMIC_LEAVE; - - ++generation; - - return true; - } - } + return node(node_handle)->_register_callback(callback_sub, poll_lock, last_update, interval_us, cb_handle); + } - return false; + static int unregister_callback(orb_advert_t &node_handle, uorb_cb_handle_t &cb_handle) + { + return node(node_handle)->_unregister_callback(cb_handle); + } + void *operator new (size_t, void *p) + { + return p; } - // add item to list of work items to schedule on node update - bool register_callback(SubscriptionCallback *callback_sub); + void operator delete (void *p) + { + } - // remove item from list of work items - void unregister_callback(SubscriptionCallback *callback_sub); + const char *get_devname() const {return _devname;} -protected: +#ifndef CONFIG_BUILD_FLAT + static bool cb_dequeue(orb_advert_t &node_handle, uorb_cb_handle_t cb) + { + IndexedStackHandle callbacks(node(node_handle)->_callbacks); + EventWaitItem *item = callbacks.peek(cb); - px4_pollevent_t poll_state(cdev::file_t *filp) override; + if (__atomic_load_n(&item->cb_triggered, __ATOMIC_SEQ_CST) > 0) { + __atomic_fetch_sub(&item->cb_triggered, 1, __ATOMIC_SEQ_CST); + return true; + } - void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) override; + return false; + } +#endif private: friend uORBTest::UnitTest; - const orb_metadata *_meta; /**< object metadata information */ + //const orb_metadata *_meta; /**< object metadata information */ + const ORB_ID _orb_id; - uint8_t *_data{nullptr}; /**< allocated object buffer */ bool _data_valid{false}; /**< At least one valid data */ px4::atomic _generation{0}; /**< object generation count */ - List _callbacks; + + struct EventWaitItem { + hrt_abstime last_update; +#ifdef CONFIG_BUILD_FLAT + class SubscriptionCallback *subscriber; +#else + uint32_t cb_triggered; +#endif + uint32_t interval_us; + uorb_cb_handle_t next; // List ptr + int8_t lock; + }; + + IndexedStack _callbacks; + + inline ssize_t write(const char *buffer, const orb_metadata *meta, orb_advert_t &handle); + static int callback_thread(int argc, char *argv[]); + struct SubscriptionCallback *callback_sub; + + class MappingCache + { + public: + struct MappingCacheListItem { + MappingCacheListItem *next; + orb_advert_t handle; + }; + + // This list is process specific in kernel build and global in in flat + static MappingCacheListItem *g_cache; + static px4_sem_t g_cache_lock; + + static void init(void) + { + static bool initialized = false; + + if (!initialized) { + px4_sem_init(&g_cache_lock, 0, 1); + initialized = true; + } + } + static orb_advert_t get(ORB_ID orb_id, uint8_t instance); + static orb_advert_t map_node(ORB_ID orb_id, uint8_t instance, int shm_fd); + static orb_advert_t map_data(orb_advert_t handle, int shm_fd, size_t size, bool publisher); + static bool del(const orb_advert_t &handle); + + static void lock() + { + init(); + do {} while (px4_sem_wait(&g_cache_lock) != 0); + } + static void unlock() { px4_sem_post(&g_cache_lock); } + }; const uint8_t _instance; /**< orb multi instance identifier */ - bool _advertised{false}; /**< has ever been advertised (not necessarily published data yet) */ - uint8_t _queue_size; /**< maximum number of elements in the queue */ - int8_t _subscriber_count{0}; + uint8_t _queue_size{0}; /**< maximum number of elements in the queue */ + int8_t _subscriber_count{0}; /**< how many subscriptions there are */ + int8_t _publisher_count{0}; /**< how many publishers have advertised this topic */ + int8_t _advertiser_count{0}; /**< how many total advertisers this topic has */ + DeviceNode(const ORB_ID id, const uint8_t instance, const char *path); -// Determine the data range - static inline bool is_in_range(unsigned left, unsigned value, unsigned right) - { - if (right > left) { - return (left <= value) && (value <= right); + int advertise(bool publisher, uint8_t queue_size); + int unadvertise(bool publisher); - } else { // Maybe the data overflowed and a wraparound occurred - return (left <= value) || (value <= right); - } - } + /** + * Change the size of the queue. + * @return PX4_OK if queue size successfully set + */ + int update_queue_size(unsigned int queue_size); + + void _add_subscriber(unsigned *initial_generation); + + void remap_data(orb_advert_t &handle, size_t new_size, bool advertiser); + + inline static DeviceNode *node(const orb_advert_t &handle) { return static_cast(handle.node); } +#ifdef CONFIG_BUILD_FLAT + inline static void *node_data(const orb_advert_t &handle) { return node(handle)->_data; } +#else + inline static void *node_data(const orb_advert_t &handle) { return handle.data; } +#endif + + bool _register_callback(SubscriptionCallback *callback_sub, int8_t poll_lock, hrt_abstime last_update, + uint32_t interval_us, uorb_cb_handle_t &cb_handle); + int _unregister_callback(uorb_cb_handle_t &cb_handle); + +#ifdef CONFIG_BUILD_FLAT + char *_devname; + void *_data{nullptr}; +#else + + /** + * Mutex for protecting node's internal data + */ + + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + void unlock() { px4_sem_post(&_lock); } + px4_sem_t _lock; /**< lock to protect access to all class members */ + + char _devname[NAME_MAX + 1]; +#endif }; +} //namespace uORB diff --git a/platforms/common/uORB/uORBManager.cpp b/platforms/common/uORB/uORBManager.cpp index 5b5530694b94..05e6319d0523 100644 --- a/platforms/common/uORB/uORBManager.cpp +++ b/platforms/common/uORB/uORBManager.cpp @@ -31,22 +31,30 @@ * ****************************************************************************/ +#include +#include #include #include +#include #include #include +#include #include #include #include - -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) -#include -#endif +#include #include "uORBDeviceNode.hpp" #include "uORBUtils.hpp" #include "uORBManager.hpp" +#include "SubscriptionCallback.hpp" + +#ifndef CONFIG_FS_SHMFS_VFS_PATH +#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm" +#endif + +static const char uORBManagerName[] = "_uORB_Manager"; #ifdef CONFIG_ORB_COMMUNICATOR pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER; @@ -54,219 +62,161 @@ pthread_mutex_t uORB::Manager::_communicator_mutex = PTHREAD_MUTEX_INITIALIZER; uORB::Manager *uORB::Manager::_Instance = nullptr; -bool uORB::Manager::initialize() -{ - if (_Instance == nullptr) { - _Instance = new uORB::Manager(); - } +// This is the per-process lock for callback thread +#ifndef CONFIG_BUILD_FLAT +int8_t uORB::Manager::per_process_lock = -1; +pid_t uORB::Manager::per_process_cb_thread = -1; +List uORB::Manager::per_process_cb_list; +px4_sem_t uORB::Manager::per_process_cb_list_mutex; -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) - px4_register_boardct_ioctl(_ORBIOCDEVBASE, orb_ioctl); +static int callback_count = 1; #endif - return _Instance != nullptr; -} -bool uORB::Manager::terminate() +void uORB::Manager::cleanup() { - if (_Instance != nullptr) { - delete _Instance; - _Instance = nullptr; - return true; - } - - return false; -} + // TODO: This is operating system dependent. Works on linux and NuttX + DIR *shm_dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *next_file; -uORB::Manager::Manager() -{ -#ifdef ORB_USE_PUBLISHER_RULES - const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules"; - int ret = readPublisherRulesFromFile(file_name, _publisher_rule); - - if (ret == PX4_OK) { - _has_publisher_rules = true; - PX4_INFO("Using orb rules from %s", file_name); - - } else { - PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret)); + // Delete all uorb shm allocations + while ((next_file = readdir(shm_dir)) != nullptr) { + // build the path for each file in the folder + if (!strncmp(next_file->d_name, "orb_", 4) || + !strncmp(next_file->d_name, "_orb_", 5)) { + shm_unlink(next_file->d_name); + } } -#endif /* ORB_USE_PUBLISHER_RULES */ + closedir(shm_dir); + // Delete manager shm allocations + shm_unlink(uORBManagerName); } -uORB::Manager::~Manager() -{ - delete _device_master; -} - -uORB::DeviceMaster *uORB::Manager::get_device_master() +bool uORB::Manager::initialize() { - if (!_device_master) { - _device_master = new DeviceMaster(); + if (_Instance == nullptr) { - if (_device_master == nullptr) { - PX4_ERR("Failed to allocate DeviceMaster"); - errno = ENOMEM; - } - } + // Cleanup from previous execution, in case some shm files are left + cleanup(); - return _device_master; -} + // Create a shared memory segment for uORB Manager and initialize a new manager into it + int shm_fd = shm_open(uORBManagerName, O_CREAT | O_RDWR, 0666); -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) -int uORB::Manager::orb_ioctl(unsigned int cmd, unsigned long arg) -{ - int ret = PX4_OK; + if (shm_fd >= 0) { + // If the creation succeeded, set the size + if (ftruncate(shm_fd, sizeof(uORB::Manager)) == 0) { + // mmap the shared memory region + void *ptr = px4_mmap(0, sizeof(uORB::Manager), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); - switch (cmd) { - case ORBIOCDEVEXISTS: { - orbiocdevexists_t *data = (orbiocdevexists_t *)arg; + if (ptr == MAP_FAILED) { + return false; + } - if (data->check_advertised) { - if (uORB::Manager::get_instance()) { - data->ret = uORB::Manager::get_instance()->orb_exists(get_orb_meta(data->orb_id), data->instance); + _Instance = new (ptr) uORB::Manager(); - } else { - data->ret = PX4_ERROR; + for (auto &publisher : _Instance->g_has_publisher) { + publisher = 0; } - - } else { - data->ret = uORB::Manager::orb_device_node_exists(data->orb_id, data->instance) ? PX4_OK : PX4_ERROR; } } - break; + } - case ORBIOCDEVADVERTISE: { - orbiocdevadvertise_t *data = (orbiocdevadvertise_t *)arg; - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); +#if defined(CONFIG_FS_SHMFS_WRPROTECT) + px4_register_boardct_ioctl(_ORBIOCDEVBASE, orb_ioctl); +#endif - if (dev) { - data->ret = dev->advertise(data->meta, data->is_advertiser, data->instance); + return _Instance != nullptr; +} - } else { - data->ret = PX4_ERROR; - } - } - break; +void uORB::Manager::map_instance() +{ + if (_Instance == nullptr) { - case ORBIOCDEVUNADVERTISE: { - orbiocdevunadvertise_t *data = (orbiocdevunadvertise_t *)arg; - data->ret = uORB::Manager::orb_unadvertise(data->handle); - } - break; + // Open the existing manager + int shm_fd = shm_open(uORBManagerName, O_RDWR, 0666); - case ORBIOCDEVPUBLISH: { - orbiocdevpublish_t *data = (orbiocdevpublish_t *)arg; - data->ret = uORB::Manager::orb_publish(data->meta, data->handle, data->data); - } - break; + if (shm_fd >= 0) { + // mmap the shared memory region + void *ptr = px4_mmap(0, sizeof(uORB::Manager), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0); - case ORBIOCDEVADDSUBSCRIBER: { - orbiocdevaddsubscriber_t *data = (orbiocdevaddsubscriber_t *)arg; - data->handle = uORB::Manager::orb_add_internal_subscriber(data->orb_id, data->instance, data->initial_generation); + if (ptr != MAP_FAILED) { + _Instance = (uORB::Manager *)ptr; + } } - break; + } - case ORBIOCDEVREMSUBSCRIBER: { - uORB::Manager::orb_remove_internal_subscriber(reinterpret_cast(arg)); - } - break; + if (_Instance == nullptr) { + PX4_ERR("FATAL: Can't get uORB manager"); + } +} - case ORBIOCDEVQUEUESIZE: { - orbiocdevqueuesize_t *data = (orbiocdevqueuesize_t *)arg; - data->size = uORB::Manager::orb_get_queue_size(data->handle); - } - break; +bool uORB::Manager::terminate() +{ + // We don't delete or unmap the Manager. Cleanup will + // unlink the SHM, and all the mappings are dropped when the + // processes exit - case ORBIOCDEVDATACOPY: { - orbiocdevdatacopy_t *data = (orbiocdevdatacopy_t *)arg; - data->ret = uORB::Manager::orb_data_copy(data->handle, data->dst, data->generation, data->only_if_updated); - } - break; + cleanup(); - case ORBIOCDEVREGCALLBACK: { - orbiocdevregcallback_t *data = (orbiocdevregcallback_t *)arg; - data->registered = uORB::Manager::register_callback(data->handle, data->callback_sub); - } - break; + if (_Instance != nullptr) { + return true; + } - case ORBIOCDEVUNREGCALLBACK: { - orbiocdevunregcallback_t *data = (orbiocdevunregcallback_t *)arg; - uORB::Manager::unregister_callback(data->handle, data->callback_sub); - } - break; + return false; +} - case ORBIOCDEVGETINSTANCE: { - orbiocdevgetinstance_t *data = (orbiocdevgetinstance_t *)arg; - data->instance = uORB::Manager::orb_get_instance(data->handle); - } - break; +uORB::Manager::Manager() +{ + int ret; - case ORBIOCDEVMASTERCMD: { - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); +#ifdef ORB_USE_PUBLISHER_RULES + const char *file_name = PX4_STORAGEDIR"/orb_publisher.rules"; + ret = readPublisherRulesFromFile(file_name, _publisher_rule); - if (dev) { - if (arg == ORB_DEVMASTER_TOP) { - dev->showTop(nullptr, 0); + if (ret == PX4_OK) { + _has_publisher_rules = true; + PX4_INFO("Using orb rules from %s", file_name); - } else { - dev->printStatistics(); - } - } - } - break; + } else { + PX4_ERR("Failed to read publisher rules file %s (%s)", file_name, strerror(-ret)); + } - case ORBIOCDEVUPDATESAVAIL: { - orbiocdevupdatesavail_t *data = (orbiocdevupdatesavail_t *)arg; - data->ret = updates_available(data->handle, data->last_generation); - } - break; +#endif /* ORB_USE_PUBLISHER_RULES */ - case ORBIOCDEVISADVERTISED: { - orbiocdevisadvertised_t *data = (orbiocdevisadvertised_t *)arg; - data->ret = is_advertised(data->handle); - } - break; + ret = px4_sem_init(&_lock, 1, 1); - default: - ret = -ENOTTY; + if (ret != 0) { + PX4_DEBUG("SEM INIT FAIL: ret %d", ret); } - return ret; + g_sem_pool.init(); } -#endif -int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +uORB::Manager::~Manager() { - if (meta == nullptr) { - return PX4_ERROR; - } - - int ret = PX4_ERROR; + px4_sem_destroy(&_lock); +} +int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) +{ // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) - if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { - return ret; + if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1)) || meta == nullptr) { + return PX4_ERROR; } - uORB::DeviceMaster *dev = uORB::Manager::get_instance()->get_device_master(); - - if (dev) { - uORB::DeviceNode *node = dev->getDeviceNode(meta, instance); - - if (node != nullptr) { - if (node->is_advertised()) { - return PX4_OK; - } - } + // meta != nullptr + // orb is advertised by a publisher + if (meta != nullptr && + has_publisher(static_cast(meta->o_id), instance)) { + return PX4_OK; } - return ret; + return PX4_ERROR; } orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) + uint8_t queue_size) { #ifdef ORB_USE_PUBLISHER_RULES @@ -278,124 +228,130 @@ orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, if (_publisher_rule.ignore_other_topics) { if (!findTopic(_publisher_rule, meta->o_name)) { PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); - return (orb_advert_t)_Instance; + return ORB_ADVERT_INVALID; } } } else { if (findTopic(_publisher_rule, meta->o_name)) { PX4_DEBUG("not allowing %s to publish topic %s", prog_name, meta->o_name); - return (orb_advert_t)_Instance; + return ORB_ADVERT_INVALID; } } } #endif /* ORB_USE_PUBLISHER_RULES */ - /* open the node as an advertiser */ - int fd = node_open(meta, true, instance); + // Calculate the wanted instance + unsigned group_tries = 0; - if (fd == PX4_ERROR) { - PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); - return nullptr; - } + lock(); - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); + while (group_tries < ORB_MULTI_MAX_INSTANCES) { - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); + // is not advertised by a publisher or is a single instance publisher + if (!has_publisher(static_cast(meta->o_id), group_tries) || !instance) { + break; + } + + group_tries++; } - /* get the advertiser handle and close the node */ - orb_advert_t advertiser; + if (group_tries == ORB_MULTI_MAX_INSTANCES) { + unlock(); + PX4_ERR("%s: too many instances (%d)", meta->o_name, group_tries); + return ORB_ADVERT_INVALID; + } - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); + orb_advert_t handle = uORB::DeviceNode::orb_advertise(static_cast(meta->o_id), group_tries, queue_size, true); - if (result == PX4_ERROR) { - PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return nullptr; + if (instance != nullptr) { + *instance = group_tries; } -#ifdef CONFIG_ORB_COMMUNICATOR + // Cache existence of this node instance globally + if (orb_advert_valid(handle)) { + set_has_publisher(static_cast(meta->o_id), group_tries); - // Advertise to the remote side, but only if it is a local topic. Otherwise - // we will generate an advertisement loop. - if (_remote_topics.find(meta->o_name) == false) { +#ifdef CONFIG_ORB_COMMUNICATOR + // For remote systems call over and inform them uORB::DeviceNode::topic_advertised(meta); +#endif /* CONFIG_ORB_COMMUNICATOR */ + + } else { + PX4_ERR("orb_advertise_multi failed %s", meta->o_name); } -#endif /* CONFIG_ORB_COMMUNICATOR */ + unlock(); /* the advertiser may perform an initial publish to initialise the object */ - if (data != nullptr) { - result = orb_publish(meta, advertiser, data); + + if (data != nullptr && orb_advert_valid(handle)) { + int result = orb_publish(meta, handle, data); if (result == PX4_ERROR) { PX4_ERR("orb_publish failed %s", meta->o_name); - return nullptr; + orb_unadvertise(handle); } } - return advertiser; + return handle; } -int uORB::Manager::orb_unadvertise(orb_advert_t handle) +int uORB::Manager::orb_unadvertise(orb_advert_t &handle) { -#ifdef ORB_USE_PUBLISHER_RULES - - if (handle == _Instance) { - return PX4_OK; //pretend success + if (!orb_advert_valid(handle)) { + return PX4_ERROR; } -#endif /* ORB_USE_PUBLISHER_RULES */ + ORB_ID id = static_cast(node(handle)->id()); + uint8_t instance = node(handle)->get_instance(); - return uORB::DeviceNode::unadvertise(handle); -} + Manager *manager = get_instance(); -int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(meta, false); -} + manager->lock(); -int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(meta, false, &inst); + bool unadvertised = uORB::DeviceNode::orb_unadvertise(handle, true) >= 0; + + // Node is deleted and handle invalidated, if the last advertiser goes away + + if (!orb_advert_valid(handle) || node(handle)->publisher_count() == 0) { + manager->unset_has_publisher(id, instance); + } + + manager->unlock(); + + return unadvertised ? PX4_OK : PX4_ERROR; } -int uORB::Manager::orb_unsubscribe(int fd) +// Should only be called from old interface +orb_sub_t uORB::Manager::orb_subscribe(const struct orb_metadata *meta) { - return px4_close(fd); + return orb_subscribe_multi(meta, 0); } -int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) +// Should only be called from old interface +orb_sub_t uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) { -#ifdef ORB_USE_PUBLISHER_RULES + uORB::SubscriptionInterval *sub = new uORB::SubscriptionPollable(meta, instance); - if (handle == _Instance) { - return PX4_OK; //pretend success + if (sub && !sub->valid()) { + // subscribe and advertise the topic + sub->subscribe(true); } -#endif /* ORB_USE_PUBLISHER_RULES */ - - return uORB::DeviceNode::publish(meta, handle, data); + return sub; } -int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) +int uORB::Manager::orb_unsubscribe(orb_sub_t handle) { - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) { - return PX4_ERROR; - } + delete (static_cast(handle)); + return PX4_OK; +} - if (ret != (int)meta->o_size) { +int uORB::Manager::orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer) +{ + if (!(static_cast(handle))->copy(buffer)) { errno = EIO; return PX4_ERROR; } @@ -403,190 +359,313 @@ int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *b return PX4_OK; } -int uORB::Manager::orb_check(int handle, bool *updated) +int uORB::Manager::orb_check(orb_sub_t handle, bool *updated) { - /* Set to false here so that if `px4_ioctl` fails to false. */ - *updated = false; - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); + *updated = ((uORB::SubscriptionInterval *)handle)->updated(); + return PX4_OK; } -int uORB::Manager::orb_set_interval(int handle, unsigned interval) +int uORB::Manager::orb_set_interval(orb_sub_t handle, unsigned interval) { - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); + ((uORB::SubscriptionInterval *)handle)->set_interval_us(interval * 1000); + return PX4_OK; } -int uORB::Manager::orb_get_interval(int handle, unsigned *interval) +int uORB::Manager::orb_get_interval(orb_sub_t handle, unsigned *interval) { - int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); - *interval /= 1000; - return ret; + *interval = ((uORB::SubscriptionInterval *)handle)->get_interval_us() / 1000; + return PX4_OK; } - -bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) +int uORB::Manager::orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout) { - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + SubscriptionPollable *sub; - return device_master != nullptr && - device_master->deviceNodeExists(orb_id, instance); -} + // Get a poll semaphore from the global pool + int8_t lock_idx = g_sem_pool.reserve(); -void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) -{ - uORB::DeviceNode *node = nullptr; - DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master(); + if (lock_idx < 0) { + PX4_ERR("Out of thread locks"); + return -1; + } + + // Any orb updated already? + bool err = false; + int count = 0; + + for (unsigned i = 0; i < nfds; i++) { + fds[i].revents = 0; - if (device_master != nullptr) { - node = device_master->getDeviceNode(get_orb_meta(orb_id), instance); + if ((fds[i].events & POLLIN) == POLLIN) { + sub = static_cast(fds[i].fd); + sub->registerPoll(lock_idx); - if (node) { - node->add_internal_subscriber(); - *initial_generation = node->get_initial_generation(); + if (sub->updated()) { + fds[i].revents = POLLIN; + count++; + } } } - return node; -} + // If none of the orbs were updated before registration, go to sleep. + // If some orb was updated after registration, but not yet refelected in "updated", the semaphore is already released. So there is no race in here. -void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) -{ - static_cast(node_handle)->remove_internal_subscriber(); -} + if (count == 0) { -uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast(node_handle)->get_queue_size(); } + // First advertiser will wake us up, or it might have happened already + // during registration above -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) -{ - if (!is_advertised(node_handle)) { - return false; + int ret; + + if (timeout < 0) { + // Wait event until interrupted by a signal + ret = g_sem_pool.take_interruptible(lock_idx); + + } else { + // Wait event for a maximum timeout time + struct timespec to; +#if defined(CONFIG_ARCH_BOARD_PX4_SITL) + px4_clock_gettime(CLOCK_MONOTONIC, &to); +#else + px4_clock_gettime(CLOCK_REALTIME, &to); +#endif + hrt_abstime now = ts_to_abstime(&to); + abstime_to_ts(&to, now + (hrt_abstime)timeout * 1000); + ret = g_sem_pool.take_timedwait(lock_idx, &to); + } + + if (ret != 0 && errno != ETIMEDOUT && errno != EINTR) { + PX4_ERR("poll on %d timeout %d FAIL errno %d\n", lock_idx, timeout, errno); + err = true; + } } - if (only_if_updated && !static_cast(node_handle)->updates_available(generation)) { - return false; + count = 0; + + for (unsigned i = 0; i < nfds; i++) { + if ((fds[i].events & POLLIN) == POLLIN) { + sub = static_cast(fds[i].fd); + sub->unregisterPoll(); + + if (sub->updated()) { + fds[i].revents |= POLLIN; + count++; + } + } } - return static_cast(node_handle)->copy(dst, generation); -} + // release the semaphore + g_sem_pool.free(lock_idx); -// add item to list of work items to schedule on node update -bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - return static_cast(node_handle)->register_callback(callback_sub); + return err ? -1 : count; } -// remove item from list of work items -void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - static_cast(node_handle)->unregister_callback(callback_sub); -} +#ifndef CONFIG_BUILD_FLAT -uint8_t uORB::Manager::orb_get_instance(const void *node_handle) +int8_t +uORB::Manager::launchCallbackThread() { - if (node_handle) { - return static_cast(node_handle)->get_instance(); + if (px4_sem_init(&per_process_cb_list_mutex, 1, 1) != 0) { + PX4_ERR("Can't initialize cb mutex"); + return -1; } - return -1; -} + per_process_lock = Manager::getThreadLock(); -/* These are optimized by inlining in NuttX Flat build */ -#if !defined(CONFIG_BUILD_FLAT) -unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) -{ - return is_advertised(node_handle) ? static_cast(node_handle)->updates_available( - last_generation) : 0; + if (per_process_lock < 0) { + PX4_ERR("Out of thread locks\n"); + return -1; + } + + /* Set the priority to 1 higher than the highest controller, which is always nav_and_controllers */ + + int priority = sched_get_priority_max(SCHED_FIFO) + px4::wq_configurations::nav_and_controllers.relative_priority + 1; + + if (per_process_cb_thread == -1) { + per_process_cb_thread = px4_task_spawn_cmd("orb_callback", + SCHED_DEFAULT, + priority, + PX4_STACK_ADJUSTED(1024), + callback_thread, + nullptr); + + if (per_process_cb_thread < 0) { + PX4_ERR("callback thread creation failed\n"); + Manager::freeThreadLock(per_process_lock); + return -1; + } + } + + return per_process_lock; } -bool uORB::Manager::is_advertised(const void *node_handle) +int +uORB::Manager::callback_thread(int argc, char *argv[]) { - return static_cast(node_handle)->is_advertised(); + int tmp = 1; + + while (true) { + /* Sleep here waiting for callbacks, lock as many times as it has been unlocked */ + + lockThread(per_process_lock, tmp); + + lock_cb_list(); + + callback_count -= tmp; + + for (auto sub : per_process_cb_list) { + /* Just in cast the callback thread has been starved, + * run all the queued callbacks now + */ + while (sub->do_call()) { + callback_count++; + } + } + + tmp = callback_count; + + unlock_cb_list(); + } + + Manager::freeThreadLock(per_process_lock); + per_process_lock = -1; + + return 0; } + #endif -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) +bool +uORB::Manager::registerCallback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, + hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle) { - char path[orb_maxpath]; - int fd = -1; - int ret = PX4_ERROR; + if (uorb_cb_handle_valid(cb_handle)) { + // double registration, ok + return true; + } - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return PX4_ERROR; + bool ret = false; + int8_t cb_lock = -1; +#ifndef CONFIG_BUILD_FLAT + cb_lock = getCallbackLock(); + + if (cb_lock >= 0) { + // The callback needs to be first added to the cb_list, before registering to the publisher. + // Otherwise there is a race condition where the cb may be triggered withouth it being handled + // by the callback thread. If the callback thread is on higher priority than this registration + // function, it may spin endlessly blocking this + + lock_cb_list(); + per_process_cb_list.add(callback_sub); + unlock_cb_list(); +#endif + ret = uORB::DeviceNode::register_callback(node_handle, callback_sub, cb_lock, last_update, + + interval_us, cb_handle); +#ifndef CONFIG_BUILD_FLAT } - /* if we have an instance and are an advertiser, we will generate a new node and set the instance, - * so we do not need to open here */ - if (!instance || !advertiser) { - /* - * Generate the path to the node and try to open it. - */ - ret = uORB::Utils::node_mkpath(path, meta, instance); + if (!ret) { + lock_cb_list(); + per_process_cb_list.remove(callback_sub); + unlock_cb_list(); + } - if (ret != OK) { - errno = -ret; - return PX4_ERROR; - } +#endif - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); + return ret; +} - } else { - *instance = 0; +void +uORB::Manager::unregisterCallback(orb_advert_t &node_handle, SubscriptionCallback *callback_sub, + uorb_cb_handle_t &cb_handle) +{ + if (!uorb_cb_handle_valid(cb_handle)) { + // not registered + return; } - /* we may need to advertise the node... */ - if (fd < 0) { +#ifndef CONFIG_BUILD_FLAT + lock_cb_list(); - ret = PX4_ERROR; + // Unregister the callback from the device node and retrieve amount of unhandled callback triggers + // The unregister from the node needs to be done callback_thread locked; otherwise we don't know + // if there are unhandled triggers left or not (due to a race between the callback thread and + // this unregistration function). - if (get_device_master()) { - ret = _device_master->advertise(meta, advertiser, instance); - } + // Unregister_callback returns the number of callbacks that has been triggered but not yet handled + // by the callback thread. This may happen if the publisher is on higher or equal priority than this - /* it's OK if it already exists */ - if ((ret != PX4_OK) && (EEXIST == errno)) { - ret = PX4_OK; - } + callback_count += DeviceNode::unregister_callback(node_handle, cb_handle); - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node advertise call */ - ret = uORB::Utils::node_mkpath(path, meta, instance); + // Remove the callback from the list - /* on success, try to open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); + per_process_cb_list.remove(callback_sub); - } else { - errno = -ret; - return PX4_ERROR; - } + unlock_cb_list(); +#else + DeviceNode::unregister_callback(node_handle, cb_handle); +#endif +} + +void uORB::Manager::GlobalSemPool::init(void) +{ + for (auto &sem : _global_sem) { + sem.init(); + } + + px4_sem_init(&_semLock, 1, 1); +} + +void uORB::Manager::GlobalSemPool::free(int8_t i) +{ + lock(); + + _global_sem[i].free(); + _global_sem[i].in_use = false; + + unlock(); +} + +int8_t uORB::Manager::GlobalSemPool::reserve() +{ + lock(); + + // Find the first free lock + int8_t i; + + for (i = 0; i < NUM_GLOBAL_SEMS; i++) { + if (!_global_sem[i].in_use) { + break; } } - if (fd < 0) { - errno = EIO; - return PX4_ERROR; + // Check that we got one + if (i == NUM_GLOBAL_SEMS) { + PX4_ERR("Out of global locks"); + unlock(); + return -1; } - /* everything has been OK, we can return the handle now */ - return fd; + // Make sure the semaphore is initialized properly for the new user + _global_sem[i].init(); + + // Mark this one as in use + _global_sem[i].in_use = true; + + unlock(); + + return i; } #ifdef CONFIG_ORB_COMMUNICATOR void uORB::Manager::set_uorb_communicator(uORBCommunicator::IChannel *channel) { - pthread_mutex_lock(&_communicator_mutex); + _comm_channel = channel; - if (channel != nullptr) { - channel->register_handler(this); - _comm_channel = channel; + if (_comm_channel != nullptr) { + _comm_channel->register_handler(this); } - - pthread_mutex_unlock(&_communicator_mutex); } uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator() @@ -598,30 +677,8 @@ uORBCommunicator::IChannel *uORB::Manager::get_uorb_communicator() return temp; } -int16_t uORB::Manager::process_remote_topic(const char *topic_name) +const struct orb_metadata *uORB::Manager::topic_name_to_meta(const char *topic_name) { - PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); - - // Look to see if we already have a node for this topic - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, topic_name); - - if (ret == OK) { - DeviceMaster *device_master = get_device_master(); - - if (device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); - - if (node) { - PX4_INFO("Marking DeviceNode(%s) as advertised in process_remote_topic", topic_name); - node->mark_as_advertised(); - _remote_topics.insert(topic_name); - return 0; - } - } - } - - // We didn't find a node so we need to create it via an advertisement const struct orb_metadata *const *topic_list = orb_get_topics(); orb_id_t topic_ptr = nullptr; @@ -632,43 +689,63 @@ int16_t uORB::Manager::process_remote_topic(const char *topic_name) } } - if (topic_ptr) { - PX4_INFO("Advertising remote topic %s", topic_name); - _remote_topics.insert(topic_name); - // Add some queue depth when advertising remote topics. These - // topics may get aggregated and thus delivered in a batch that - // requires some buffering in a queue. - orb_advertise(topic_ptr, nullptr, 5); + return topic_ptr; +} - } else { +int16_t uORB::Manager::process_remote_topic(const char *topic_name) +{ + PX4_DEBUG("entering process_remote_topic: name: %s", topic_name); + + int16_t rc = 0; + + const struct orb_metadata *meta = topic_name_to_meta(topic_name); + + if (meta == nullptr) { PX4_INFO("process_remote_topic meta not found for %s\n", topic_name); + _remote_topics.erase(topic_name); + return -1; } - return 0; + _Instance->lock(); + + orb_advert_t handle = orb_advertise(meta, nullptr); + + _Instance->unlock(); + + if (orb_advert_valid(handle)) { + PX4_INFO("Advertising remote publisher %s", topic_name); + _remote_topics.insert(handle); + + } else { + PX4_INFO("Advertisement failed"); + rc = -1; + } + + return rc; } int16_t uORB::Manager::process_add_subscription(const char *messageName) { PX4_DEBUG("entering Manager_process_add_subscription: name: %s", messageName); - int16_t rc = 0; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); + int16_t rc = -1; - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + const struct orb_metadata *meta = topic_name_to_meta(messageName); - if (node == nullptr) { - PX4_DEBUG("DeviceNode(%s) not created yet", messageName); + if (meta == nullptr) { + return rc; + } - } else { - // node is present. - node->process_add_subscription(); - } + orb_advert_t handle = DeviceNode::nodeOpen(static_cast(meta->o_id), 0, false); + + if (!orb_advert_valid(handle)) { + PX4_DEBUG("DeviceNode(%s) not created yet", messageName); } else { - rc = -1; + // node is present. + + node(handle)->process_add_subscription(handle); + rc = 0; } return rc; @@ -677,23 +754,24 @@ int16_t uORB::Manager::process_add_subscription(const char *messageName) int16_t uORB::Manager::process_remove_subscription(const char *messageName) { int16_t rc = -1; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + const struct orb_metadata *meta = topic_name_to_meta(messageName); + + if (meta == nullptr) { + PX4_DEBUG("DeviceNode(%s) meta not found", messageName); + return rc; + } + + orb_advert_t handle = DeviceNode::nodeOpen(static_cast(meta->o_id), 0, false); - // get the node name. - if (node == nullptr) { - PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", - __LINE__, messageName); + if (!orb_advert_valid(handle)) { + PX4_DEBUG("[posix-uORB::Manager::process_remove_subscription(%d)]Error No existing subscriber found for message: [%s]", + __LINE__, messageName); - } else { - // node is present. - node->process_remove_subscription(); - rc = 0; - } + } else { + // node is present. + node(handle)->process_remove_subscription(handle); + rc = 0; } return rc; @@ -702,22 +780,16 @@ int16_t uORB::Manager::process_remove_subscription(const char *messageName) int16_t uORB::Manager::process_received_message(const char *messageName, int32_t length, uint8_t *data) { int16_t rc = -1; - char nodepath[orb_maxpath]; - int ret = uORB::Utils::node_mkpath(nodepath, messageName); - DeviceMaster *device_master = get_device_master(); - if (ret == OK && device_master) { - uORB::DeviceNode *node = device_master->getDeviceNode(nodepath); + orb_advert_t handle = _remote_topics.find(messageName); - // get the node name. - if (node == nullptr) { - PX4_DEBUG("No existing subscriber found for message: [%s] nodepath:[%s]", messageName, nodepath); + if (!orb_advert_valid(handle)) { + PX4_DEBUG("No existing subscriber found for message: [%s]", messageName); - } else { - // node is present. - node->process_received_message(length, data); - rc = 0; - } + } else { + // node is present. + node(handle)->process_received_message(handle, length, data); + rc = 0; } return rc; @@ -862,4 +934,5 @@ int uORB::Manager::readPublisherRulesFromFile(const char *file_name, PublisherRu fclose(fp); return ret; } + #endif /* ORB_USE_PUBLISHER_RULES */ diff --git a/platforms/common/uORB/uORBManager.hpp b/platforms/common/uORB/uORBManager.hpp index 1c2870b1eecd..7b373d352ba2 100644 --- a/platforms/common/uORB/uORBManager.hpp +++ b/platforms/common/uORB/uORBManager.hpp @@ -31,135 +31,36 @@ * ****************************************************************************/ -#ifndef _uORBManager_hpp_ -#define _uORBManager_hpp_ +#pragma once + +#include +#include +#include #include "uORBDeviceNode.hpp" #include "uORBCommon.hpp" -#include "uORBDeviceMaster.hpp" #include // For ORB_ID enum #include #include +#include #ifdef CONFIG_ORB_COMMUNICATOR #include "ORBSet.hpp" #include "uORBCommunicator.hpp" #endif /* CONFIG_ORB_COMMUNICATOR */ +#define NUM_GLOBAL_SEMS 40 + namespace uORB { -class Manager; -class SubscriptionCallback; -} - - -/* - * IOCTLs for manager to access device nodes using - * a handle - */ - -#define _ORBIOCDEV(_n) (_PX4_IOC(_ORBIOCDEVBASE, _n)) -#define ORBIOCDEVEXISTS _ORBIOCDEV(30) -typedef struct orbiocdevexists { - const ORB_ID orb_id; - const uint8_t instance; - const bool check_advertised; - int ret; -} orbiocdevexists_t; - -#define ORBIOCDEVADVERTISE _ORBIOCDEV(31) -typedef struct orbiocadvertise { - const struct orb_metadata *meta; - bool is_advertiser; - int *instance; - int ret; -} orbiocdevadvertise_t; - -#define ORBIOCDEVUNADVERTISE _ORBIOCDEV(32) -typedef struct orbiocunadvertise { - void *handle; - int ret; -} orbiocdevunadvertise_t; - -#define ORBIOCDEVPUBLISH _ORBIOCDEV(33) -typedef struct orbiocpublish { - const struct orb_metadata *meta; - orb_advert_t handle; - const void *data; - int ret; -} orbiocdevpublish_t; - -#define ORBIOCDEVADDSUBSCRIBER _ORBIOCDEV(34) -typedef struct { - const ORB_ID orb_id; - const uint8_t instance; - unsigned *initial_generation; - void *handle; -} orbiocdevaddsubscriber_t; - -#define ORBIOCDEVREMSUBSCRIBER _ORBIOCDEV(35) - -#define ORBIOCDEVQUEUESIZE _ORBIOCDEV(36) -typedef struct { - const void *handle; - uint8_t size; -} orbiocdevqueuesize_t; - -#define ORBIOCDEVDATACOPY _ORBIOCDEV(37) -typedef struct { - void *handle; - void *dst; - unsigned generation; - bool only_if_updated; - bool ret; -} orbiocdevdatacopy_t; - -#define ORBIOCDEVREGCALLBACK _ORBIOCDEV(38) -typedef struct { - void *handle; - class uORB::SubscriptionCallback *callback_sub; - bool registered; -} orbiocdevregcallback_t; - -#define ORBIOCDEVUNREGCALLBACK _ORBIOCDEV(39) -typedef struct { - void *handle; - class uORB::SubscriptionCallback *callback_sub; -} orbiocdevunregcallback_t; - -#define ORBIOCDEVGETINSTANCE _ORBIOCDEV(40) -typedef struct { - const void *handle; - uint8_t instance; -} orbiocdevgetinstance_t; - -#define ORBIOCDEVUPDATESAVAIL _ORBIOCDEV(41) -typedef struct { - const void *handle; - unsigned last_generation; - unsigned ret; -} orbiocdevupdatesavail_t; - -#define ORBIOCDEVISADVERTISED _ORBIOCDEV(42) -typedef struct { - const void *handle; - bool ret; -} orbiocdevisadvertised_t; - -typedef enum { - ORB_DEVMASTER_STATUS = 0, - ORB_DEVMASTER_TOP = 1 -} orbiocdevmastercmd_t; -#define ORBIOCDEVMASTERCMD _ORBIOCDEV(45) - /** * This is implemented as a singleton. This class manages creating the * uORB nodes for each uORB topics and also implements the behavor of the * uORB Api's. */ -class uORB::Manager +class Manager #ifdef CONFIG_ORB_COMMUNICATOR : public uORBCommunicator::IChannelRxHandler #endif /* CONFIG_ORB_COMMUNICATOR */ @@ -181,22 +82,16 @@ class uORB::Manager /** * Method to get the singleton instance for the uORB::Manager. - * Make sure initialize() is called first. * @return uORB::Manager* */ - static uORB::Manager *get_instance() { return _Instance; } - - /** - * Get the DeviceMaster. If it does not exist, - * it will be created and initialized. - * Note: the first call to this is not thread-safe. - * @return nullptr if initialization failed (and errno will be set) - */ - uORB::DeviceMaster *get_device_master(); + static uORB::Manager *get_instance() + { + if (_Instance == nullptr) { + map_instance(); + } -#if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) - static int orb_ioctl(unsigned int cmd, unsigned long arg); -#endif + return _Instance; + } // ==== uORB interface methods ==== /** @@ -259,7 +154,7 @@ class uORB::Manager * this function will return nullptr and set errno to ENOENT. */ orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size = 1); + uint8_t queue_size = 1); /** * Unadvertise a topic. @@ -267,7 +162,7 @@ class uORB::Manager * @param handle handle returned by orb_advertise or orb_advertise_multi. * @return 0 on success */ - static int orb_unadvertise(orb_advert_t handle); + static int orb_unadvertise(orb_advert_t &handle); /** * Publish new data to a topic. @@ -282,12 +177,12 @@ class uORB::Manager * @param data A pointer to the data to be published. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - static int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data); + static int orb_publish(const struct orb_metadata *meta, orb_advert_t &handle, const void *data) {return uORB::DeviceNode::publish(meta, handle, data);} /** * Subscribe to a topic. * - * The returned value is a file descriptor that can be passed to poll() + * The returned value is a handle that can be passed to orb_poll() * in order to wait for updates to a topic, as well as topic_read, * orb_check. * @@ -312,12 +207,12 @@ class uORB::Manager * @return PX4_ERROR on error, otherwise returns a handle * that can be used to read and update the topic. */ - int orb_subscribe(const struct orb_metadata *meta); + orb_sub_t orb_subscribe(const struct orb_metadata *meta); /** * Subscribe to a multi-instance of a topic. * - * The returned value is a file descriptor that can be passed to poll() + * The returned value is a handle that can be passed to orb_poll() * in order to wait for updates to a topic, as well as topic_read, * orb_check. * @@ -344,13 +239,13 @@ class uORB::Manager * @param instance The instance of the topic. Instance 0 matches the * topic of the orb_subscribe() call, higher indices * are for topics created with orb_advertise_multi(). - * @return PX4_ERROR on error, otherwise returns a handle + * @return returns a handle * that can be used to read and update the topic. * If the topic in question is not known (due to an * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) - * this function will return -1 and set errno to ENOENT. + * this function will return an invalid handle and set errno to ENOENT. */ - int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance); + orb_sub_t orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance); /** * Unsubscribe from a topic. @@ -358,14 +253,14 @@ class uORB::Manager * @param handle A handle returned from orb_subscribe. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_unsubscribe(int handle); + static int orb_unsubscribe(orb_sub_t handle); /** * Fetch data from a topic. * * This is the only operation that will reset the internal marker that * indicates that a topic has been updated for a subscriber. Once poll - * or check return indicating that an updaet is available, this call + * or check return indicating that an update is available, this call * must be used to update the subscription. * * @param meta The uORB metadata (usually from the ORB_ID() macro) @@ -376,7 +271,7 @@ class uORB::Manager * using the data. * @return OK on success, PX4_ERROR otherwise with errno set accordingly. */ - int orb_copy(const struct orb_metadata *meta, int handle, void *buffer); + static int orb_copy(const struct orb_metadata *meta, orb_sub_t handle, void *buffer); /** * Check whether a topic has been published to since the last orb_copy. @@ -394,7 +289,7 @@ class uORB::Manager * @return OK if the check was successful, PX4_ERROR otherwise with * errno set accordingly. */ - int orb_check(int handle, bool *updated); + static int orb_check(orb_sub_t handle, bool *updated); /** * Check if a topic has already been created and published (advertised) @@ -403,7 +298,7 @@ class uORB::Manager * @param instance ORB instance * @return OK if the topic exists, PX4_ERROR otherwise. */ - int orb_exists(const struct orb_metadata *meta, int instance); + static int orb_exists(const struct orb_metadata *meta, int instance); /** * Set the minimum interval between which updates are seen for a subscription. @@ -423,8 +318,7 @@ class uORB::Manager * @param interval An interval period in milliseconds. * @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly. */ - int orb_set_interval(int handle, unsigned interval); - + static int orb_set_interval(orb_sub_t handle, unsigned interval); /** * Get the minimum interval between which updates are seen for a subscription. @@ -435,37 +329,80 @@ class uORB::Manager * @param interval The returned interval period in milliseconds. * @return OK on success, PX4_ERROR otherwise with ERRNO set accordingly. */ - int orb_get_interval(int handle, unsigned *interval); - - static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance); + static int orb_get_interval(orb_sub_t handle, unsigned *interval); - static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation); + int orb_poll(orb_poll_struct_t *fds, unsigned int nfds, int timeout); - static void orb_remove_internal_subscriber(void *node_handle); + static orb_advert_t orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation, + bool advertise) + { + if (!advertise && !has_publisher(orb_id, instance)) { + return ORB_ADVERT_INVALID; + } - static uint8_t orb_get_queue_size(const void *node_handle); + _Instance->lock(); + orb_advert_t handle = uORB::DeviceNode::add_subscriber(orb_id, instance, initial_generation, advertise); + _Instance->unlock(); - static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated); + return handle; + } - static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub); + static void orb_remove_internal_subscriber(orb_advert_t &node_handle, bool advertiser) + { + _Instance->lock(); + node(node_handle)->remove_subscriber(node_handle, advertiser); + _Instance->unlock(); + } - static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub); + static uint8_t orb_get_queue_size(const orb_advert_t &node_handle) {return node(node_handle)->get_queue_size();} - static uint8_t orb_get_instance(const void *node_handle); + static bool orb_data_copy(orb_advert_t &node_handle, void *dst, unsigned &generation, + bool only_if_updated) + { + if (!orb_advert_valid(node_handle) || + (only_if_updated && !node(node_handle)->updates_available(generation))) { + return false; + } -#if defined(CONFIG_BUILD_FLAT) - /* These are optimized by inlining in NuttX Flat build */ - static unsigned updates_available(const void *node_handle, unsigned last_generation) { return is_advertised(node_handle) ? static_cast(node_handle)->updates_available(last_generation) : 0; } + return node(node_handle)->copy(dst, node_handle, generation); + } - static bool is_advertised(const void *node_handle) { return static_cast(node_handle)->is_advertised(); } +#ifndef CONFIG_BUILD_FLAT + static uint8_t getCallbackLock() + { + int8_t ret = per_process_lock; -#else - static unsigned updates_available(const void *node_handle, unsigned last_generation); + if (ret < 0) { + _Instance->lock(); + ret = per_process_lock >= 0 ? per_process_lock : launchCallbackThread(); + _Instance->unlock(); + } - static bool is_advertised(const void *node_handle); + return ret; + } #endif + static uint8_t orb_get_instance(orb_advert_t &node_handle) + { + if (orb_advert_valid(node_handle)) { + return node(node_handle)->get_instance(); + } + + return -1; + } + + static unsigned updates_available(const orb_advert_t &node_handle, unsigned last_generation) + { + return node(node_handle)->updates_available(last_generation); + } + + static bool has_publisher(ORB_ID orb_id, uint8_t instance) + { + return (get_instance()->g_has_publisher[static_cast(orb_id)] & (1 << instance)) != 0; + } + #ifdef CONFIG_ORB_COMMUNICATOR + /** * Method to set the uORBCommunicator::IChannel instance. * @param comm_channel @@ -483,15 +420,44 @@ class uORB::Manager #endif /* CONFIG_ORB_COMMUNICATOR */ + void *operator new (size_t, void *p) + { + return p; + } + + void operator delete (void *p) + { + px4_munmap(p, sizeof(uORB::Manager)); + } + + static bool registerCallback(orb_advert_t &node_handle, class SubscriptionCallback *callback_sub, + hrt_abstime last_update, uint32_t interval_us, uorb_cb_handle_t &cb_handle); + + static void unregisterCallback(orb_advert_t &node_handle, class SubscriptionCallback *callback_sub, + uorb_cb_handle_t &cb_handle); + + static void lockThread(int idx, unsigned count = 1) + { + while (count--) { + _Instance->g_sem_pool.take(idx); + } + } + + static void unlockThread(int idx) + { + _Instance->g_sem_pool.release(idx); + } + + static void freeThreadLock(int i) {_Instance->g_sem_pool.free(i);} + + static int8_t getThreadLock() {return _Instance->g_sem_pool.reserve();} + private: // class methods + inline static uORB::DeviceNode *node(orb_advert_t handle) {return static_cast(handle.node);} - /** - * Common implementation for orb_advertise and orb_subscribe. - * - * Handles creation of the object and the initial publication for - * advertisers. - */ - int node_open(const struct orb_metadata *meta, bool advertiser, int *instance = nullptr); + static void cleanup(); + static int callback_thread(int argc, char *argv[]); + static int8_t launchCallbackThread(); private: // data members static Manager *_Instance; @@ -505,13 +471,34 @@ class uORB::Manager ORBSet _remote_topics; #endif /* CONFIG_ORB_COMMUNICATOR */ - DeviceMaster *_device_master{nullptr}; - private: //class methods Manager(); - virtual ~Manager(); + ~Manager(); + + /** + * Lock against node concurrent node creation + */ + void lock() { do {} while (px4_sem_wait(&_lock) != 0); } + + /** + * Release the node creation lock + */ + void unlock() { px4_sem_post(&_lock); } + + px4_sem_t _lock; #ifdef CONFIG_ORB_COMMUNICATOR + /** + * Helper function to find orb_metadata struct by topic name + * @param topic_name + * This represents the uORB message Name (topic); This message Name should be + * globally unique. + * @return + * pointer to struct orb_metadata + * nullptr = failure + */ + const struct orb_metadata *topic_name_to_meta(const char *topic_name); + /** * Interface to process a received topic advertisement from remote. * @param topic_name @@ -598,12 +585,87 @@ class uORB::Manager * @return 0 on success, <0 otherwise */ int readPublisherRulesFromFile(const char *file_name, PublisherRule &rule); - PublisherRule _publisher_rule; bool _has_publisher_rules = false; #endif /* ORB_USE_PUBLISHER_RULES */ -}; + /** + * Method to map the singleton instance for the uORB::Manager + * to the processes address space. Make sure initialize() is called first. + */ + static void map_instance(); -#endif /* _uORBManager_hpp_ */ + void set_has_publisher(ORB_ID orb_id, uint8_t instance) + { + static_assert(sizeof(g_has_publisher[0]) * 8 >= ORB_MULTI_MAX_INSTANCES); + g_has_publisher[static_cast(orb_id)] |= (1 << instance); + } + + void unset_has_publisher(ORB_ID orb_id, uint8_t instance) + { + static_assert(sizeof(g_has_publisher[0]) * 8 >= ORB_MULTI_MAX_INSTANCES); + g_has_publisher[static_cast(orb_id)] &= ~(1 << instance); + } + + // Global cache for advertised uORB node instances + uint16_t g_has_publisher[ORB_TOPICS_COUNT + 1]; + + // A global pool of semaphores for + // 1) poll locks + // 2) callback thread signalling (except in NuttX flat build) + + class GlobalSemPool + { + public: + void init(); + int8_t reserve(); + void free(int8_t i); + + void take(int8_t i) { do {} while (_global_sem[i].take() != 0); } + int take_interruptible(int8_t i) { return _global_sem[i].take(); } + int take_timedwait(int8_t i, struct timespec *abstime) { return _global_sem[i].take_timedwait(abstime); } + void release(int8_t i) {_global_sem[i].release(); } + int value(int8_t i) { return _global_sem[i].value(); } + + class GlobalLock + { + public: + void init() + { + px4_sem_init(&_sem, 1, 0); +#if defined(__PX4_NUTTX) + sem_setprotocol(&_sem, SEM_PRIO_NONE); +#endif + in_use = false; + } + void free() { px4_sem_destroy(&_sem); } + int take() { return px4_sem_wait(&_sem); } + int take_timedwait(struct timespec *abstime) { return px4_sem_timedwait(&_sem, abstime); } + void release() { px4_sem_post(&_sem); } + int value() { int value; px4_sem_getvalue(&_sem, &value); return value; } + bool in_use{false}; + + private: + px4_sem_t _sem; /* For signaling to the callback thread */ + }; + private: + + void lock() { do {} while (px4_sem_wait(&_semLock) != 0); } + void unlock() { px4_sem_post(&_semLock); } + + GlobalLock _global_sem[NUM_GLOBAL_SEMS]; + px4_sem_t _semLock; + } g_sem_pool; + +#ifndef CONFIG_BUILD_FLAT + static void lock_cb_list() { do {} while (px4_sem_wait(&per_process_cb_list_mutex) != 0); } + static void unlock_cb_list() { px4_sem_post(&per_process_cb_list_mutex); } + + static int8_t per_process_lock; + static pid_t per_process_cb_thread; + static List per_process_cb_list; + static px4_sem_t per_process_cb_list_mutex; +#endif +}; +} // namespace uORB diff --git a/platforms/common/uORB/uORBManagerUsr.cpp b/platforms/common/uORB/uORBManagerUsr.cpp deleted file mode 100644 index af00dbb9ba58..000000000000 --- a/platforms/common/uORB/uORBManagerUsr.cpp +++ /dev/null @@ -1,349 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. - * - * 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. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * 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 OWNER 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 -#include -#include -#include - -#include -#include -#include - -#include - -#include "uORBDeviceNode.hpp" -#include "uORBUtils.hpp" -#include "uORBManager.hpp" - -uORB::Manager *uORB::Manager::_Instance = nullptr; - -bool uORB::Manager::initialize() -{ - if (_Instance == nullptr) { - _Instance = new uORB::Manager(); - } - - return _Instance != nullptr; -} - -bool uORB::Manager::terminate() -{ - if (_Instance != nullptr) { - delete _Instance; - _Instance = nullptr; - return true; - } - - return false; -} - -uORB::Manager::Manager() -{ -} - -uORB::Manager::~Manager() -{ -} - -int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance) -{ - // instance valid range: [0, ORB_MULTI_MAX_INSTANCES) - if ((instance < 0) || (instance > (ORB_MULTI_MAX_INSTANCES - 1))) { - return PX4_ERROR; - } - - orbiocdevexists_t data = {static_cast(meta->o_id), static_cast(instance), true, PX4_ERROR}; - boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); - - return data.ret; -} - -orb_advert_t uORB::Manager::orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, - unsigned int queue_size) -{ - /* open the node as an advertiser */ - int fd = node_open(meta, true, instance); - - if (fd == PX4_ERROR) { - PX4_ERR("%s advertise failed (%i)", meta->o_name, errno); - return nullptr; - } - - /* Set the queue size. This must be done before the first publication; thus it fails if - * this is not the first advertiser. - */ - int result = px4_ioctl(fd, ORBIOCSETQUEUESIZE, (unsigned long)queue_size); - - if (result < 0 && queue_size > 1) { - PX4_WARN("orb_advertise_multi: failed to set queue size"); - } - - /* get the advertiser handle and close the node */ - orb_advert_t advertiser; - - result = px4_ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); - px4_close(fd); - - if (result == PX4_ERROR) { - PX4_WARN("px4_ioctl ORBIOCGADVERTISER failed. fd = %d", fd); - return nullptr; - } - - /* the advertiser may perform an initial publish to initialise the object */ - if (data != nullptr) { - result = orb_publish(meta, advertiser, data); - - if (result == PX4_ERROR) { - PX4_ERR("orb_publish failed %s", meta->o_name); - return nullptr; - } - } - - return advertiser; -} - -int uORB::Manager::orb_unadvertise(orb_advert_t handle) -{ - orbiocdevunadvertise_t data = {handle, PX4_ERROR}; - boardctl(ORBIOCDEVUNADVERTISE, reinterpret_cast(&data)); - - return data.ret; -} - -int uORB::Manager::orb_subscribe(const struct orb_metadata *meta) -{ - return node_open(meta, false); -} - -int uORB::Manager::orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) -{ - int inst = instance; - return node_open(meta, false, &inst); -} - -int uORB::Manager::orb_unsubscribe(int fd) -{ - return px4_close(fd); -} - -int uORB::Manager::orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) -{ - orbiocdevpublish_t d = {meta, handle, data, PX4_ERROR}; - boardctl(ORBIOCDEVPUBLISH, reinterpret_cast(&d)); - - return d.ret; -} - -int uORB::Manager::orb_copy(const struct orb_metadata *meta, int handle, void *buffer) -{ - int ret; - - ret = px4_read(handle, buffer, meta->o_size); - - if (ret < 0) { - return PX4_ERROR; - } - - if (ret != (int)meta->o_size) { - errno = EIO; - return PX4_ERROR; - } - - return PX4_OK; -} - -int uORB::Manager::orb_check(int handle, bool *updated) -{ - /* Set to false here so that if `px4_ioctl` fails to false. */ - *updated = false; - return px4_ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated); -} - -int uORB::Manager::orb_set_interval(int handle, unsigned interval) -{ - return px4_ioctl(handle, ORBIOCSETINTERVAL, interval * 1000); -} - -int uORB::Manager::orb_get_interval(int handle, unsigned *interval) -{ - int ret = px4_ioctl(handle, ORBIOCGETINTERVAL, (unsigned long)interval); - *interval /= 1000; - return ret; -} - -int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance) -{ - char path[orb_maxpath]; - int fd = -1; - int ret = PX4_ERROR; - - /* - * If meta is null, the object was not defined, i.e. it is not - * known to the system. We can't advertise/subscribe such a thing. - */ - if (nullptr == meta) { - errno = ENOENT; - return PX4_ERROR; - } - - /* if we have an instance and are an advertiser, we will generate a new node and set the instance, - * so we do not need to open here */ - if (!instance || !advertiser) { - /* - * Generate the path to the node and try to open it. - */ - ret = uORB::Utils::node_mkpath(path, meta, instance); - - if (ret != OK) { - errno = -ret; - return PX4_ERROR; - } - - /* open the path as either the advertiser or the subscriber */ - fd = px4_open(path, advertiser ? PX4_F_WRONLY : PX4_F_RDONLY); - - } else { - *instance = 0; - } - - /* we may need to advertise the node... */ - if (fd < 0) { - ret = PX4_ERROR; - - orbiocdevadvertise_t data = {meta, advertiser, instance, PX4_ERROR}; - boardctl(ORBIOCDEVADVERTISE, (unsigned long)&data); - ret = data.ret; - - /* it's OK if it already exists */ - if ((ret != PX4_OK) && (EEXIST == errno)) { - ret = PX4_OK; - } - - if (ret == PX4_OK) { - /* update the path, as it might have been updated during the node advertise call */ - ret = uORB::Utils::node_mkpath(path, meta, instance); - - /* on success, try to open again */ - if (ret == PX4_OK) { - fd = px4_open(path, (advertiser) ? PX4_F_WRONLY : PX4_F_RDONLY); - - } else { - errno = -ret; - return PX4_ERROR; - } - } - } - - if (fd < 0) { - errno = EIO; - return PX4_ERROR; - } - - /* everything has been OK, we can return the handle now */ - return fd; -} - -bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance) -{ - orbiocdevexists_t data = {orb_id, instance, false, 0}; - boardctl(ORBIOCDEVEXISTS, reinterpret_cast(&data)); - - return data.ret == PX4_OK ? true : false; -} - -void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation) -{ - orbiocdevaddsubscriber_t data = {orb_id, instance, initial_generation, nullptr}; - boardctl(ORBIOCDEVADDSUBSCRIBER, reinterpret_cast(&data)); - - return data.handle; -} - -void uORB::Manager::orb_remove_internal_subscriber(void *node_handle) -{ - boardctl(ORBIOCDEVREMSUBSCRIBER, reinterpret_cast(node_handle)); -} - -uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) -{ - orbiocdevqueuesize_t data = {node_handle, 0}; - boardctl(ORBIOCDEVQUEUESIZE, reinterpret_cast(&data)); - - return data.size; -} - -bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation, bool only_if_updated) -{ - orbiocdevdatacopy_t data = {node_handle, dst, generation, only_if_updated, false}; - boardctl(ORBIOCDEVDATACOPY, reinterpret_cast(&data)); - generation = data.generation; - - return data.ret; -} - -bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - orbiocdevregcallback_t data = {node_handle, callback_sub, false}; - boardctl(ORBIOCDEVREGCALLBACK, reinterpret_cast(&data)); - - return data.registered; -} - -void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub) -{ - orbiocdevunregcallback_t data = {node_handle, callback_sub}; - boardctl(ORBIOCDEVUNREGCALLBACK, reinterpret_cast(&data)); -} - -uint8_t uORB::Manager::orb_get_instance(const void *node_handle) -{ - orbiocdevgetinstance_t data = {node_handle, 0}; - boardctl(ORBIOCDEVGETINSTANCE, reinterpret_cast(&data)); - - return data.instance; -} - -unsigned uORB::Manager::updates_available(const void *node_handle, unsigned last_generation) -{ - orbiocdevupdatesavail_t data = {node_handle, last_generation, 0}; - boardctl(ORBIOCDEVUPDATESAVAIL, reinterpret_cast(&data)); - return data.ret; -} - -bool uORB::Manager::is_advertised(const void *node_handle) -{ - orbiocdevisadvertised_t data = {node_handle, false}; - boardctl(ORBIOCDEVISADVERTISED, reinterpret_cast(&data)); - return data.ret; -} diff --git a/platforms/common/uORB/uORBUtils.cpp b/platforms/common/uORB/uORBUtils.cpp index fc9a88095f2d..c37dcc0873ef 100644 --- a/platforms/common/uORB/uORBUtils.cpp +++ b/platforms/common/uORB/uORBUtils.cpp @@ -45,7 +45,7 @@ int uORB::Utils::node_mkpath(char *buf, const struct orb_metadata *meta, int *in index = *instance; } - len = snprintf(buf, orb_maxpath, "/%s/%s%d", "obj", meta->o_name, index); + len = snprintf(buf, orb_maxpath, "_orb_%s%d", meta->o_name, index); if (len >= orb_maxpath) { return -ENAMETOOLONG; @@ -62,7 +62,7 @@ int uORB::Utils::node_mkpath(char *buf, const char *orbMsgName) unsigned index = 0; - len = snprintf(buf, orb_maxpath, "/%s/%s%d", "obj", orbMsgName, index); + len = snprintf(buf, orb_maxpath, "_orb_%s%d", orbMsgName, index); if (len >= orb_maxpath) { return -ENAMETOOLONG; diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp index bc7cda513297..d9f2e7cd8776 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -56,7 +56,7 @@ int uORBTest::UnitTest::pubsublatency_main() /* wakeup source(s) */ px4_pollfd_struct_t fds[1] {}; - int test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); + orb_sub_t test_multi_sub = orb_subscribe_multi(ORB_ID(orb_test_medium), 0); orb_test_medium_s t{}; @@ -258,13 +258,13 @@ int uORBTest::UnitTest::test_single() t.val = 0; ptopic = orb_advertise(ORB_ID(orb_test), &t); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } - int sfd = orb_subscribe(ORB_ID(orb_test)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -289,7 +289,7 @@ int uORBTest::UnitTest::test_single() t.val = 2; - if (PX4_OK != orb_publish(ORB_ID(orb_test), ptopic, &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_test), &ptopic, &t)) { return test_fail("publish failed"); } @@ -344,18 +344,18 @@ int uORBTest::UnitTest::test_multi() t.val = 103; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[0], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[0], &t)) { return test_fail("mult. pub0 fail"); } t.val = 203; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[1], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[1], &t)) { return test_fail("mult. pub1 fail"); } /* subscribe to both topics and ensure valid data is received */ - int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); + orb_sub_t sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd0, &u)) { return test_fail("sub #0 copy failed: %d", errno); @@ -365,7 +365,7 @@ int uORBTest::UnitTest::test_multi() return test_fail("sub #0 val. mismatch: %d", u.val); } - int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + orb_sub_t sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd1, &u)) { return test_fail("sub #1 copy failed: %d", errno); @@ -422,7 +422,7 @@ int uORBTest::UnitTest::pub_test_multi2_main() data_topic.timestamp = hrt_absolute_time(); data_topic.val = data_next_idx; - orb_publish(ORB_ID(orb_test_medium_multi), orb_pub[data_next_idx], &data_topic); + orb_publish(ORB_ID(orb_test_medium_multi), &orb_pub[data_next_idx], &data_topic); // PX4_WARN("publishing msg (idx=%i, t=%" PRIu64 ")", data_next_idx, data_topic.time); data_next_idx = (data_next_idx + 1) % num_instances; @@ -449,7 +449,7 @@ int uORBTest::UnitTest::test_multi2() _thread_should_exit = false; const int num_instances = 3; - int orb_data_fd[num_instances] {-1, -1, -1}; + orb_sub_t orb_data_fd[num_instances] {ORB_SUB_INVALID, ORB_SUB_INVALID, ORB_SUB_INVALID}; int orb_data_next = 0; for (int i = 0; i < num_instances; ++i) { @@ -476,7 +476,7 @@ int uORBTest::UnitTest::test_multi2() px4_usleep(1000); bool updated = false; - int orb_data_cur_fd = orb_data_fd[orb_data_next]; + orb_sub_t orb_data_cur_fd = orb_data_fd[orb_data_next]; orb_check(orb_data_cur_fd, &updated); if (updated) { @@ -508,9 +508,9 @@ int uORBTest::UnitTest::test_multi_reversed() /* For these tests 0 and 1 instances are taken from before, therefore continue with 2 and 3. */ /* Subscribe first and advertise afterwards. */ - int sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); + orb_sub_t sfd2 = orb_subscribe_multi(ORB_ID(orb_multitest), 2); - if (sfd2 < 0) { + if (!orb_sub_valid(sfd2)) { return test_fail("sub. id2: ret: %d", sfd2); } @@ -533,13 +533,13 @@ int uORBTest::UnitTest::test_multi_reversed() t.val = 204; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[2], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[2], &t)) { return test_fail("mult. pub0 fail"); } t.val = 304; - if (PX4_OK != orb_publish(ORB_ID(orb_multitest), _pfd[3], &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_multitest), &_pfd[3], &t)) { return test_fail("mult. pub1 fail"); } @@ -551,7 +551,7 @@ int uORBTest::UnitTest::test_multi_reversed() return test_fail("sub #3 val. mismatch: %d", u.val); } - int sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); + orb_sub_t sfd3 = orb_subscribe_multi(ORB_ID(orb_multitest), 3); if (PX4_OK != orb_copy(ORB_ID(orb_multitest), sfd3, &u)) { return test_fail("sub #3 copy failed: %d", errno); @@ -570,20 +570,21 @@ int uORBTest::UnitTest::test_wrap_around() orb_test_medium_s t{}; orb_test_medium_s u{}; - orb_advert_t ptopic{nullptr}; + orb_advert_t ptopic{ORB_ADVERT_INVALID}; bool updated{false}; // Advertise but not publish topics, only generate device_node, which is convenient for modifying DeviceNode::_generation const int queue_size = 16; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_wrap_around), nullptr, queue_size); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } - auto node = uORB::Manager::get_instance()->get_device_master()->getDeviceNode(ORB_ID(orb_test_medium_wrap_around), 0); + orb_advert_t handle = uORB::DeviceNode::nodeOpen(ORB_ID::orb_test_medium_wrap_around, 0, false); + auto node = static_cast(handle.node); - if (node == nullptr) { + if (!orb_advert_valid(handle)) { return test_fail("get device node failed."); } @@ -598,11 +599,11 @@ int uORBTest::UnitTest::test_wrap_around() } t.val = 0; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); - int sfd = orb_subscribe(ORB_ID(orb_test_medium_wrap_around)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test_medium_wrap_around)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -648,7 +649,7 @@ int uORBTest::UnitTest::test_wrap_around() for (int i = 0; i < queue_size - 2; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); } for (int i = 0; i < queue_size - 2; ++i) { @@ -676,7 +677,7 @@ int uORBTest::UnitTest::test_wrap_around() for (int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); } for (int i = 0; i < queue_size; ++i) { @@ -691,7 +692,7 @@ int uORBTest::UnitTest::test_wrap_around() SET_GENERTATION(); t.val = queue_size; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -704,7 +705,7 @@ int uORBTest::UnitTest::test_wrap_around() #undef SET_GENERTATION t.val = 943; - orb_publish(ORB_ID(orb_test_medium_wrap_around), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_wrap_around), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -822,9 +823,9 @@ int uORBTest::UnitTest::test_queue() orb_advert_t ptopic{nullptr}; bool updated{false}; - int sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); + orb_sub_t sfd = orb_subscribe(ORB_ID(orb_test_medium_queue)); - if (sfd < 0) { + if (!orb_sub_valid(sfd)) { return test_fail("subscribe failed: %d", errno); } @@ -832,7 +833,7 @@ int uORBTest::UnitTest::test_queue() orb_test_medium_s t{}; ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue), &t, queue_size); - if (ptopic == nullptr) { + if (!orb_advert_valid(ptopic)) { return test_fail("advertise failed: %d", errno); } @@ -878,7 +879,7 @@ int uORBTest::UnitTest::test_queue() for (int i = 0; i < queue_size - 2; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); } for (int i = 0; i < queue_size - 2; ++i) { @@ -893,7 +894,7 @@ int uORBTest::UnitTest::test_queue() for (int i = 0; i < queue_size + overflow_by; ++i) { t.val = i; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); } for (int i = 0; i < queue_size; ++i) { @@ -911,7 +912,7 @@ int uORBTest::UnitTest::test_queue() } t.val = 943; - orb_publish(ORB_ID(orb_test_medium_queue), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue), &ptopic, &t); CHECK_UPDATED(-1); CHECK_COPY(u.val, t.val); @@ -934,10 +935,10 @@ int uORBTest::UnitTest::pub_test_queue_entry(int argc, char *argv[]) int uORBTest::UnitTest::pub_test_queue_main() { orb_test_medium_s t{}; - orb_advert_t ptopic{nullptr}; + orb_advert_t ptopic{ORB_ADVERT_INVALID}; const int queue_size = 50; - if ((ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size)) == nullptr) { + if (!orb_advert_valid(ptopic = orb_advertise_queue(ORB_ID(orb_test_medium_queue_poll), &t, queue_size))) { _thread_should_exit = true; return test_fail("advertise failed: %d", errno); } @@ -953,7 +954,7 @@ int uORBTest::UnitTest::pub_test_queue_main() int burst_counter = 0; while (burst_counter++ < queue_size / 2 + 7) { //make interval non-boundary aligned - orb_publish(ORB_ID(orb_test_medium_queue_poll), ptopic, &t); + orb_publish(ORB_ID(orb_test_medium_queue_poll), &ptopic, &t); ++t.val; } @@ -974,9 +975,9 @@ int uORBTest::UnitTest::test_queue_poll_notify() test_note("Testing orb queuing (poll & notify)"); orb_test_medium_s t{}; - int sfd = -1; + orb_sub_t sfd = ORB_SUB_INVALID; - if ((sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll))) < 0) { + if (!orb_sub_valid(sfd = orb_subscribe(ORB_ID(orb_test_medium_queue_poll)))) { return test_fail("subscribe failed: %d", errno); } @@ -1042,7 +1043,7 @@ int uORBTest::UnitTest::latency_test(bool print) orb_advert_t pfd0 = orb_advertise(ORB_ID(orb_test_medium), &t); - if (pfd0 == nullptr) { + if (!orb_advert_valid(pfd0)) { return test_fail("orb_advertise failed (%i)", errno); } @@ -1068,7 +1069,7 @@ int uORBTest::UnitTest::latency_test(bool print) ++t.val; t.timestamp = hrt_absolute_time(); - if (PX4_OK != orb_publish(ORB_ID(orb_test_medium), pfd0, &t)) { + if (PX4_OK != orb_publish(ORB_ID(orb_test_medium), &pfd0, &t)) { return test_fail("mult. pub0 timing fail"); } diff --git a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp index 908eeca9c7e2..3175481f3c9c 100644 --- a/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp +++ b/platforms/common/uORB/uORB_tests/uORBTest_UnitTest.hpp @@ -35,7 +35,6 @@ #define _uORBTest_UnitTest_hpp_ #include -#include #include #include #include @@ -75,6 +74,7 @@ class uORBTest::UnitTest // Assist in testing the wrap-around situation static void set_generation(uORB::DeviceNode &node, unsigned generation) { + node._data_valid = true; node._generation.store(generation); } diff --git a/platforms/common/work_queue/hrt_thread.c b/platforms/common/work_queue/hrt_thread.c index 8438b0ae2c1f..d1999dcfa2ec 100644 --- a/platforms/common/work_queue/hrt_thread.c +++ b/platforms/common/work_queue/hrt_thread.c @@ -296,6 +296,11 @@ void hrt_work_queue_init(void) work_hrtthread, (char *const *)NULL); + struct sigaction act; + memset(&act, 0, sizeof(struct sigaction)); + sigemptyset(&act.sa_mask); + act.sa_handler = _sighandler; - signal(SIGCONT, _sighandler); + sigaddset(&act.sa_mask, SIGCONT); + sigaction(SIGCONT, &act, NULL); } diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 3926f237336b..3b8e87da3ae7 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -36,11 +36,20 @@ if(POLICY CMP0079) endif() include(cygwin_cygpath) +include(bin_romfs) add_executable(px4 ${PX4_SOURCE_DIR}/platforms/common/empty.c) -set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) +if (CONFIG_OPENSBI AND "${PX4_BOARD_LABEL}" STREQUAL "bootloader") + set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}_int.elf) + set(FW_NAME_FLASH ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) + set(FW_NAME_SBI ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_sbi.bin) + set(RISCV_RELAXATIONS -Wl,--relax) +else() + set(FW_NAME ${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.elf) + set(RISCV_RELAXATIONS) +endif() set_target_properties(px4 PROPERTIES OUTPUT_NAME ${FW_NAME}) -add_dependencies(px4 git_nuttx) +add_dependencies(px4 git_nuttx nuttx_context) get_property(module_libraries GLOBAL PROPERTY PX4_MODULE_LIBRARIES) @@ -98,6 +107,20 @@ else() endif() endif() +# Select the correct linker script(s) for build type +if (CONFIG_BUILD_FLAT) + set(LDSCRIPT ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}script.ld) +elseif (CONFIG_BUILD_PROTECTED) + set(LDMEMORY ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld) + set(LDSCRIPT ${LDMEMORY},--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld) + set(LDKERNEL ${LDMEMORY},--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld) +elseif (CONFIG_BUILD_KERNEL) + set(LDKERNEL ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel.ld) + set(LDSCRIPT ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}gnu-elf.ld) +else() + message(FATAL_ERROR "Cannot determine linker script for build type") +endif() + list(APPEND nuttx_libs nuttx_boards nuttx_drivers @@ -105,7 +128,6 @@ list(APPEND nuttx_libs nuttx_sched nuttx_crypto nuttx_binfmt - nuttx_xx ) if (NOT CONFIG_BUILD_FLAT) @@ -115,6 +137,16 @@ if (NOT CONFIG_BUILD_FLAT) nuttx_kmm nuttx_stubs nuttx_kc + nuttx_kxx + ) + + list(APPEND nuttx_userlibs + nuttx_arch + nuttx_apps + nuttx_mm + nuttx_proxies + nuttx_c + nuttx_xx ) else() list(APPEND nuttx_libs @@ -122,12 +154,19 @@ else() nuttx_arch nuttx_mm nuttx_c + nuttx_xx ) endif() if(CONFIG_NET) list(APPEND nuttx_libs nuttx_net) target_link_libraries(nuttx_fs INTERFACE nuttx_net) + target_link_libraries(nuttx_drivers INTERFACE nuttx_net) +endif() + +if(CONFIG_OPENAMP) + list(APPEND nuttx_libs nuttx_openamp) + target_link_libraries(nuttx_drivers INTERFACE nuttx_openamp) endif() file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_DIR}) @@ -136,12 +175,16 @@ file(RELATIVE_PATH PX4_BINARY_DIR_REL ${CMAKE_CURRENT_BINARY_DIR} ${PX4_BINARY_D # because even relative linker script paths are different for linux, mac and windows CYGPATH(NUTTX_CONFIG_DIR NUTTX_CONFIG_DIR_CYG) -if((DEFINED ENV{SIGNING_TOOL}) AND (NOT NUTTX_DIR MATCHES "external")) +if((DEFINED ENV{SIGNING_TOOL}) AND (NOT "${PX4_BOARD_LABEL}" STREQUAL "bootloader") AND (NOT NUTTX_DIR MATCHES "external")) set(PX4_BINARY_OUTPUT ${PX4_BINARY_DIR}/${PX4_CONFIG}_unsigned.bin) + add_subdirectory(toc) + add_custom_command(OUTPUT ${PX4_BINARY_DIR_REL}/${PX4_CONFIG}.bin - COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin - DEPENDS ${PX4_BINARY_OUTPUT} + COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_DIR}/toc.bin ${PX4_BINARY_DIR}/toc_signed.bin + COMMAND $ENV{SIGNING_TOOL} $ENV{SIGNING_ARGS} ${PX4_BINARY_OUTPUT} ${PX4_BINARY_DIR}/${PX4_CONFIG}_signed.bin + COMMAND cat ${PX4_BINARY_DIR}/toc_signed.bin ${PX4_BINARY_DIR}/${PX4_CONFIG}_signed.bin > ${PX4_BINARY_DIR}/${PX4_CONFIG}.bin + DEPENDS toc_bin ${PX4_BINARY_OUTPUT} WORKING_DIRECTORY ${PX4_SOURCE_DIR} ) else() @@ -150,6 +193,7 @@ endif() if (NOT CONFIG_BUILD_FLAT) + # Build the px4-kernelspace target_link_libraries(nuttx_karch INTERFACE drivers_board @@ -166,7 +210,7 @@ if (NOT CONFIG_BUILD_FLAT) -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}kernel-space.ld + -Wl,--script=${LDKERNEL} -Wl,-Map=${PX4_CONFIG}_kernel.map -Wl,--warn-common @@ -174,33 +218,146 @@ if (NOT CONFIG_BUILD_FLAT) -Wl,--start-group ${nuttx_libs} - ${kernel_module_libraries} -Wl,--end-group m gcc ) + target_link_libraries(px4_kernel PRIVATE -Wl,--print-memory-usage) + + target_link_libraries(px4_kernel PRIVATE ${kernel_module_libraries}) + if (config_romfs_root) add_subdirectory(${PX4_SOURCE_DIR}/ROMFS ${PX4_BINARY_DIR}/ROMFS) target_link_libraries(px4_kernel PRIVATE romfs) endif() - target_link_libraries(px4_kernel PRIVATE -Wl,--print-memory-usage) + # Build the px4-userspace. Common set of libaries for protected / kernel + target_link_libraries(nuttx_c INTERFACE nuttx_proxies) + target_link_libraries(nuttx_c INTERFACE nuttx_mm) + target_link_libraries(nuttx_mm INTERFACE nuttx_c) + # Re-enable when direct link to nuttx_xx is removed from prebuild_targets + # otherwise kernel gets multiple definitions of g_default_pthread_attr + #target_link_libraries(nuttx_xx INTERFACE nuttx_c) + target_link_libraries(nuttx_c INTERFACE gcc) + +if (CONFIG_BUILD_KERNEL) + + # We don't need px4 target, but supress warnings from the linker + target_link_libraries(px4 PRIVATE -nostartfiles -Wl,--entry=0) + + # Every user space app needs crt0 + list(APPEND nuttx_userlibs nuttx_crt0) + + # Create the initial boot ROMFS (which contains nsh) + add_custom_target(boot_bins + COMMAND cp ${PX4_BINARY_DIR}/bin/nsh ${PX4_BINARY_DIR}/init + COMMAND install -D ${PX4_BINARY_DIR}/init -t ${PX4_BINARY_DIR}/boot + COMMAND rm -f ${PX4_BINARY_DIR}/init + DEPENDS nuttx_app_bins + ) - set(nuttx_userspace) + make_bin_romfs( + BINDIR ${PX4_BINARY_DIR}/boot + OUTDIR ${NUTTX_CONFIG_DIR}/include + OUTPREFIX boot + LABEL Px4BootVol + STRIPPED "y" + DEPS boot_bins + ) - list(APPEND nuttx_userspace - drivers_userspace - nuttx_arch - nuttx_apps - nuttx_mm - nuttx_proxies - nuttx_c - nuttx_xx + # Build and install the px4 processes + foreach(module ${module_libraries}) + get_target_property(MAIN ${module} MAIN) + get_target_property(NO_DAEMON ${module} NO_DAEMON) + + # Don't handle modules that don't have a main (entry) function + if (NOT MAIN) + continue() + endif() + + # Build module.a into module.elf + set(BIN px4-${MAIN}.elf) + add_executable(${BIN} ${PX4_SOURCE_DIR}/platforms/nuttx/src/px4/common/process/main.cpp) + set_target_properties(${BIN} PROPERTIES OUTPUT_NAME ${BIN}) + add_dependencies(${BIN} nuttx_crt0 ${module}) + target_compile_options(${BIN} PRIVATE -Dentry=${MAIN}_main) + + # Disable daemon if the module explicitly tells us to do so + if (NO_DAEMON) + target_compile_options(${BIN} PRIVATE -DNO_DAEMON) + endif() + + target_link_libraries(${BIN} PRIVATE + + -nostartfiles + -nodefaultlibs + -nostdlib + -nostdinc++ + + -fno-exceptions + -fno-rtti + + -Wl,--script=${LDSCRIPT} + -Wl,--entry=_start + -Wl,-r + -Wl,-Bstatic + -Wl,-Map=${PX4_CONFIG}.map + -Wl,--warn-common + -Wl,--gc-sections + + -Wl,--start-group + px4_layer + px4_platform + px4_work_queue + perf + uORB + ${nuttx_userlibs} + ${module_libraries} + -Wl,--end-group + + m + gcc ) - target_link_libraries(nuttx_c INTERFACE nuttx_proxies) + # The individual module deps are FUBAR, so this cannot be done + #target_link_libraries(${BIN} PRIVATE ${module}) + target_link_libraries(${BIN} INTERFACE perf nuttx_c) + # Some global deps are also FUBAR, so this must be done + target_link_libraries(px4_platform board_bus_info) + + # Install the executable to /bin + add_custom_target(${BIN}_install + COMMAND mkdir -p ${PX4_BINARY_DIR}/bin + COMMAND install -D ${PX4_BINARY_DIR}/${BIN} -t ${PX4_BINARY_DIR}/bin + COMMAND mv ${PX4_BINARY_DIR}/bin/${BIN} ${PX4_BINARY_DIR}/bin/${MAIN} + COMMAND rm -f ${PX4_BINARY_DIR}/${BIN} + DEPENDS ${BIN} + ) + + list(APPEND px4_bins ${BIN}_install) + endforeach() + + # Create the /bin ROMFS + make_bin_romfs( + BINDIR ${PX4_BINARY_DIR}/bin + OUTDIR ${NUTTX_CONFIG_DIR}/include + OUTPREFIX bin + LABEL Px4BinVol + STRIPPED "y" + DEPS nuttx_app_bins ${px4_bins} + ) + + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${KERNEL_NAME} ${PX4_BINARY_OUTPUT} + DEPENDS px4_kernel + ) + +else() # CONFIG_BUILD_PROTECTED + + # NSH needs the px4 user entry point and userspace structure + list(APPEND nuttx_userlibs drivers_userspace) target_link_libraries(px4 PRIVATE @@ -212,14 +369,14 @@ if (NOT CONFIG_BUILD_FLAT) -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}memory.ld,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}user-space.ld + -Wl,--script=${LDSCRIPT} -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common -Wl,--gc-sections -Wl,--start-group - ${nuttx_userspace} + ${nuttx_userlibs} -Wl,--end-group m @@ -228,9 +385,7 @@ if (NOT CONFIG_BUILD_FLAT) target_link_libraries(px4 PRIVATE -Wl,--print-memory-usage) - target_link_libraries(px4 PRIVATE - ${module_libraries} - ) + target_link_libraries(px4 PRIVATE ${module_libraries}) add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${PX4_BOARD}_user.bin @@ -240,9 +395,9 @@ if (NOT CONFIG_BUILD_FLAT) DEPENDS px4 px4_kernel ) -else() +endif() - target_link_libraries(nuttx_c INTERFACE nuttx_sched) # nxsched_get_streams +else() # CONFIG_BUILD_FLAT target_link_libraries(nuttx_arch INTERFACE @@ -251,8 +406,9 @@ else() arch_board_reset ) + target_link_libraries(nuttx_c INTERFACE nuttx_sched) target_link_libraries(nuttx_c INTERFACE nuttx_drivers) - target_link_libraries(nuttx_drivers INTERFACE nuttx_c) + target_link_libraries(nuttx_drivers INTERFACE nuttx_c nuttx_fs nuttx_mm) target_link_libraries(nuttx_xx INTERFACE nuttx_c) target_link_libraries(nuttx_fs INTERFACE nuttx_c) @@ -265,7 +421,8 @@ else() -fno-exceptions -fno-rtti - -Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld + ${RISCV_RELAXATIONS} + -Wl,--script=${LDSCRIPT} -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common -Wl,--gc-sections @@ -289,13 +446,24 @@ else() target_link_libraries(px4 PRIVATE romfs) endif() - add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} - COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} - DEPENDS px4 - ) + # for opensbi bootloader, strip the l2lim & ddr sections + if (CONFIG_OPENSBI AND "${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -R .l2_scratchpad ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_DIR_REL}/${FW_NAME_FLASH} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME_FLASH} ${PX4_BINARY_OUTPUT} + DEPENDS px4 + ) + else() + add_custom_command(OUTPUT ${PX4_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BINARY_OUTPUT} + DEPENDS px4 + ) + endif() endif() +add_custom_target(px4_binary DEPENDS ${PX4_BINARY_OUTPUT}) + # create .px4 with parameter and airframe metadata if (TARGET parameters_xml AND TARGET airframes_xml) @@ -343,7 +511,7 @@ if(${CONFIG_NSH_LIBRARY} MATCHES "y") endif() endif() -if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader" AND NOT CONFIG_OPENSBI) # TODO: handle opensbi add_custom_command(OUTPUT ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin COMMAND ${CMAKE_OBJCOPY} -O binary ${PX4_BINARY_DIR_REL}/${FW_NAME} ${PX4_BOARD_DIR}/extras/${PX4_BOARD}_bootloader.bin DEPENDS px4 diff --git a/platforms/nuttx/NuttX/CMakeLists.txt b/platforms/nuttx/NuttX/CMakeLists.txt index 355abf0b23f8..0c3102df172b 100644 --- a/platforms/nuttx/NuttX/CMakeLists.txt +++ b/platforms/nuttx/NuttX/CMakeLists.txt @@ -71,20 +71,24 @@ add_custom_command( ) add_custom_target(nuttx_context DEPENDS ${NUTTX_DIR}/include/nuttx/config.h) - # builtins set(builtin_apps_string) set(builtin_apps_decl_string) +set(daemon_app_strings) if(CONFIG_NSH_LIBRARY) list(SORT module_libraries) foreach(module ${module_libraries}) get_target_property(MAIN ${module} MAIN) get_target_property(STACK_MAIN ${module} STACK_MAIN) get_target_property(PRIORITY ${module} PRIORITY) + get_target_property(NO_DAEMON ${module} NO_DAEMON) if(MAIN) set(builtin_apps_string "${builtin_apps_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main },\n") set(builtin_apps_decl_string "${builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") + if (NOT NO_DAEMON) + set(daemon_app_strings "${daemon_app_strings} ${MAIN} -d &\n") + endif() endif() endforeach() endif() @@ -94,6 +98,7 @@ if (NOT CONFIG_BUILD_FLAT) set(kernel_builtin_apps_string) set(kernel_builtin_apps_proxy_string) set(kernel_builtin_apps_decl_string) + set(kernel_symlinks_string) list(SORT kernel_module_libraries) foreach(module ${kernel_module_libraries}) @@ -105,11 +110,13 @@ if (NOT CONFIG_BUILD_FLAT) set(kernel_builtin_apps_string "${kernel_builtin_apps_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, ${MAIN}_main },\n") set(kernel_builtin_apps_proxy_string "${kernel_builtin_apps_proxy_string}{ \"${MAIN}\", ${PRIORITY}, ${STACK_MAIN}, launch_kmod_main },\n") set(kernel_builtin_apps_decl_string "${kernel_builtin_apps_decl_string}int ${MAIN}_main(int argc, char *argv[]);\n") + set(kernel_symlinks_string "${kernel_symlinks_string}ln -s bin/launch_kmod lnk/${MAIN}\n") endif() endforeach() configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.bdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.bdat) configure_file(${CMAKE_CURRENT_SOURCE_DIR}/px4_kernel.pdat.in ${CMAKE_CURRENT_BINARY_DIR}/px4_kernel.pdat) + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/rc.sysinit.in ${CMAKE_CURRENT_BINARY_DIR}/rc.sysinit) add_custom_command(OUTPUT ${KERNEL_BUILTIN_DIR}/kernel_builtin_list.h ${KERNEL_BUILTIN_DIR}/kernel_builtin_proto.h WORKING_DIRECTORY ${KERNEL_BUILTIN_DIR} @@ -157,6 +164,14 @@ function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra target) set(nuttx_lib_target lib${target}.a) endif() + # Facilitate building kxx, as NuttX kernel does not contain CXX code + # and thus does not support creating libkxx natively... + if(${nuttx_lib} STREQUAL "kxx") + set(nuttx_libname "xx") + else() + set(nuttx_libname ${nuttx_lib}) + endif() + file(GLOB_RECURSE nuttx_lib_files LIST_DIRECTORIES false ${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.c ${CMAKE_CURRENT_SOURCE_DIR}/nuttx/${nuttx_lib_dir}/*.h @@ -166,7 +181,7 @@ function(add_nuttx_dir nuttx_lib nuttx_lib_dir kernel extra target) COMMAND ${CMAKE_COMMAND} -E remove -f ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a COMMAND find ${nuttx_lib_dir} -type f -name \*.o -delete COMMAND make -C ${nuttx_lib_dir} --no-print-directory --silent ${nuttx_lib_target} TOPDIR="${NUTTX_DIR}" KERNEL=${kernel} EXTRAFLAGS=${extra} - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_lib}.a ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a + COMMAND ${CMAKE_COMMAND} -E copy_if_different ${NUTTX_DIR}/${nuttx_lib_dir}/lib${nuttx_libname}.a ${CMAKE_CURRENT_BINARY_DIR}/nuttx/${nuttx_lib_dir}/lib${nuttx_lib}.a DEPENDS ${nuttx_lib_files} nuttx_context ${NUTTX_DIR}/include/nuttx/config.h @@ -196,6 +211,8 @@ if (NOT CONFIG_BUILD_FLAT) add_nuttx_dir(c libs/libc n "" c) add_dependencies(nuttx_c_build nuttx_kc_build) # can't build these in parallel add_nuttx_dir(kc libs/libc y -D__KERNEL__ kc) + add_dependencies(nuttx_xx_build nuttx_kxx_build) # can't build these in parallel + add_nuttx_dir(kxx libs/libxx y -D__KERNEL__ all xx) add_nuttx_dir(mm mm n "" mm) add_dependencies(nuttx_mm_build nuttx_kmm_build) # can't build these in parallel add_nuttx_dir(kmm mm y -D__KERNEL__ kmm) @@ -212,6 +229,56 @@ if(CONFIG_NET) add_nuttx_dir(net net y -D__KERNEL__ all) endif() +if(CONFIG_OPENAMP) + add_nuttx_dir(openamp openamp y -D__KERNEL__ all) +endif() + +if (CONFIG_BUILD_KERNEL) + include(bin_romfs) + + # For modules in kernel mode, we need the startup object files (crt0) + add_custom_command(OUTPUT ${PX4_BINARY_DIR}/startup/crt0.o + COMMAND mkdir -p ${PX4_BINARY_DIR}/startup + COMMAND make -C arch/${CONFIG_ARCH}/src export_startup --no-print-directory --silent + TOPDIR="${NUTTX_DIR}" + EXPORT_DIR="${PX4_BINARY_DIR}" > ${CMAKE_CURRENT_BINARY_DIR}/nuttx_startup.log + DEPENDS nuttx_context nuttx_arch_build nuttx_karch_build + WORKING_DIRECTORY ${NUTTX_DIR} + ) + add_custom_target(nuttx_startup DEPENDS ${PX4_BINARY_DIR}/startup/crt0.o) + add_library(nuttx_crt0 OBJECT IMPORTED GLOBAL) + set_property(TARGET nuttx_crt0 PROPERTY IMPORTED_OBJECTS ${PX4_BINARY_DIR}/startup/crt0.o) + + # Build and install the NuttX applications into build/xx/bin + set(LDSCRIPT ${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}gnu-elf.ld) + + list(APPEND nuttx_userlibs + nuttx_apps + nuttx_mm + nuttx_proxies + nuttx_c + nuttx_xx + ) + + foreach(lib ${nuttx_userlibs}) + get_property(lib_location TARGET ${lib} PROPERTY IMPORTED_LOCATION) + list(APPEND userlibs ${lib_location}) + endforeach(lib) + + get_property(CRT0_OBJ TARGET nuttx_crt0 PROPERTY IMPORTED_OBJECTS) + + add_custom_target(nuttx_app_bins + COMMAND mkdir -p ${PX4_BINARY_DIR}/bin + COMMAND make -C ${NUTTX_SRC_DIR}/apps install --no-print-directory --silent + ARCHCRT0OBJ="${CRT0_OBJ}" + BINDIR="${PX4_BINARY_DIR}/bin" + TOPDIR="${NUTTX_DIR}" + ELFLDNAME="${LDSCRIPT}" + USERLIBS="${userlibs}" > ${CMAKE_CURRENT_BINARY_DIR}/nuttx_apps_install.log + DEPENDS ${nuttx_userlibs} nuttx_startup + ) +endif() + ############################################################################### # NuttX oldconfig: Update a configuration using a provided .config as base add_custom_target(oldconfig_nuttx diff --git a/platforms/nuttx/NuttX/Make.defs.in b/platforms/nuttx/NuttX/Make.defs.in index 315bf92300ec..2f4c2351cd69 100644 --- a/platforms/nuttx/NuttX/Make.defs.in +++ b/platforms/nuttx/NuttX/Make.defs.in @@ -62,7 +62,7 @@ CXXINCPATH := $(shell $(INCDIR) -s "$(CC)" $(TOPDIR)$(DELIM)include$(DELIM)cxx) ARCHINCLUDES += $(CINCPATH) ARCHXXINCLUDES += $(CINCPATH) $(CXXINCPATH) -ARCHSCRIPT = -T$(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld +ARCHSCRIPT = $(BOARD_DIR)$(DELIM)scripts$(DELIM)flash.ld ifeq ($(CONFIG_BOARD_USE_PROBES),y) ARCHINCLUDES += -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/$(CONFIG_ARCH_CHIP) -I $(TOPDIR)/arch/$(CONFIG_ARCH)/src/common @@ -93,15 +93,18 @@ else MAXOPTIMIZATION = -Os endif -FLAGS = $(MAXOPTIMIZATION) -g2 \ +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -g2 \ -fdata-sections \ -ffunction-sections \ -fno-builtin-printf \ -fno-common \ -fno-strength-reduce \ -fno-strict-aliasing \ - -fomit-frame-pointer \ - -Wall \ + -fomit-frame-pointer + +FLAGS = -Wall \ + -Werror \ -Wextra \ -Wlogical-op \ -Wno-cpp \ @@ -126,9 +129,7 @@ ifeq ($(CONFIG_ARMV7M_STACKCHECK),y) FLAGS += -finstrument-functions -ffixed-r10 endif -CFLAGS = $(ARCHINCLUDES) \ - -std=gnu11 \ - ${CMAKE_C_FLAGS} \ +ARCHCFLAGS = -std=gnu11 \ $(FLAGS) \ -Wno-bad-function-cast \ -Wno-float-equal \ @@ -145,10 +146,10 @@ CFLAGS = $(ARCHINCLUDES) \ -Wno-type-limits \ ${CMAKE_C_COMP_DEP_FLAGS} -CXXFLAGS = $(ARCHXXINCLUDES) \ - -std=c++14 \ +ARCHCPUFLAGS = ${CMAKE_C_FLAGS} + +ARCHCXXFLAGS = -std=c++14 \ -nostdinc++ \ - ${CMAKE_CXX_FLAGS} \ $(FLAGS) \ -fcheck-new \ -fno-builtin \ @@ -159,6 +160,8 @@ CXXFLAGS = $(ARCHXXINCLUDES) \ -Wno-double-promotion \ -Wno-missing-declarations +CFLAGS = $(ARCHINCLUDES) $(ARCHCFLAGS) $(ARCHCPUFLAGS) $(ARCHOPTIMIZATION) +CXXFLAGS = $(ARCHXXINCLUDES) $(ARCHCXXFLAGS) $(ARCHCPUFLAGS) $(ARCHOPTIMIZATION) CPPFLAGS = $(ARCHINCLUDES) $(ARCHDEFINES) $(EXTRAFLAGS) AFLAGS = $(CFLAGS) -D__ASSEMBLY__ @@ -182,3 +185,18 @@ endef define ARCHIVE $(AR) $1 $(2) endef + +# ELF module definitions + +CELFFLAGS = $(CFLAGS) +CXXELFFLAGS = $(CXXFLAGS) + +# ELF linkage + +LDSTARTGROUP = --start-group +LDENDGROUP = --end-group +LDLIBPATH = $(foreach PATH, $(USERLIBS), $(addprefix -L, $(dir $(PATH)))) +LDLIBFILES = $(foreach PATH, $(USERLIBS), $(notdir $(PATH))) +LDLIBS = $(patsubst %.a,%,$(patsubst lib%,-l%,$(LDLIBFILES))) +LDELFFLAGS = -r -e _start -Bstatic +LDELFFLAGS += $(addprefix -T, $(ELFLDNAME)) diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index a489381b4983..5f2a2a528e05 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit a489381b49835ecba6f3b873b5071d882a18152f +Subproject commit 5f2a2a528e05c1cfb94bca26d1dc5a8511d11e72 diff --git a/platforms/nuttx/NuttX/extern/pf_crypto b/platforms/nuttx/NuttX/extern/pf_crypto new file mode 160000 index 000000000000..585520ffb534 --- /dev/null +++ b/platforms/nuttx/NuttX/extern/pf_crypto @@ -0,0 +1 @@ +Subproject commit 585520ffb534d04375f0787fb905a8d919a053ff diff --git a/platforms/nuttx/NuttX/include/cxx/cmath b/platforms/nuttx/NuttX/include/cxx/cmath index 3125ea762582..aab7195d8857 100644 --- a/platforms/nuttx/NuttX/include/cxx/cmath +++ b/platforms/nuttx/NuttX/include/cxx/cmath @@ -82,14 +82,14 @@ inline bool isinf(long double value) { return __builtin_isinf_sign(value); } * NuttX has no usable C++ math library, so we need to provide the needed definitions here manually. */ #define NUTTX_WRAP_MATH_FUN_UNARY(name) \ - inline float name(float x) { return ::name##f(x); } \ - inline double name(double x) { return ::name(x); } \ - inline long double name(long double x) { return ::name##l(x); } + inline float name(float x) { return ::__builtin_##name##f(x); } \ + inline double name(double x) { return ::__builtin_##name(x); } \ + inline long double name(long double x) { return ::__builtin_##name##l(x); } #define NUTTX_WRAP_MATH_FUN_BINARY(name) \ - inline float name(float x, float y) { return ::name##f(x, y); } \ - inline double name(double x, double y) { return ::name(x, y); } \ - inline long double name(long double x, long double y) { return ::name##l(x, y); } + inline float name(float x, float y) { return ::__builtin_##name##f(x, y); } \ + inline double name(double x, double y) { return ::__builtin_##name(x, y); } \ + inline long double name(long double x, long double y) { return ::__builtin_##name##l(x, y); } NUTTX_WRAP_MATH_FUN_UNARY(fabs) NUTTX_WRAP_MATH_FUN_UNARY(log) diff --git a/platforms/nuttx/NuttX/include/klibc_glue.h b/platforms/nuttx/NuttX/include/klibc_glue.h new file mode 100644 index 000000000000..ef2523cc6586 --- /dev/null +++ b/platforms/nuttx/NuttX/include/klibc_glue.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +/* This file forwards the user space memory API to the kernel versions */ + +#if !defined(CONFIG_BUILD_FLAT) && defined(__KERNEL__) + +#include + +#undef malloc +#undef realloc +#undef calloc +#undef free +#undef mallinfo + +#define malloc kmm_malloc +#define realloc kmm_realloc +#define calloc kmm_calloc +#define free kmm_free + +/* struct mallinfo has the same name as the function */ + +#define mallinfo() kmm_mallinfo() + +/* If exit() is used, forward the call to _exit() instead */ + +#undef exit +#define exit _exit + +#endif diff --git a/platforms/nuttx/NuttX/include/queue.h b/platforms/nuttx/NuttX/include/queue.h new file mode 100644 index 000000000000..1b8819873f45 --- /dev/null +++ b/platforms/nuttx/NuttX/include/queue.h @@ -0,0 +1,38 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +/* Wrapper for nuttx/queue.h */ + +#include diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index de41e7feaeff..480e85328eaf 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit de41e7feaeffaec3ce65327e9569e8fdb553ca3d +Subproject commit 480e85328eafcbe0ee0916375c9dd77439807d8b diff --git a/platforms/nuttx/NuttX/rc.sysinit.in b/platforms/nuttx/NuttX/rc.sysinit.in new file mode 100644 index 000000000000..75181cc282d9 --- /dev/null +++ b/platforms/nuttx/NuttX/rc.sysinit.in @@ -0,0 +1,8 @@ +# Create folder for the links +mkdir -p lnk + +# The link strings, these are generated +@kernel_symlinks_string@ + +# Start all modules as daemons into background +@daemon_app_strings@ diff --git a/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake index 94d37b084bea..a4c08874d5d4 100644 --- a/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake +++ b/platforms/nuttx/cmake/Platform/Generic-riscv64-unknown-elf-gcc-rv64gc.cmake @@ -1,9 +1,9 @@ if(CONFIG_ARCH_DPFPU) message(STATUS "Enabling double FP precision hardware instructions") - set(cpu_flags "-march=rv64gc -mabi=lp64d -mcmodel=medany") + set(cpu_flags "-march=rv64gc -mabi=lp64d -mcmodel=medany -Wl,--no-relax") else() - set(cpu_flags "-march=rv64imac -mabi=lp64 -mcmodel=medany") + set(cpu_flags "-march=rv64imac -mabi=lp64 -mcmodel=medany -Wl,--no-relax") endif() set(CMAKE_C_FLAGS "${cpu_flags}" CACHE STRING "" FORCE) diff --git a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake index 85e5caa00747..3a69862f97c5 100644 --- a/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake +++ b/platforms/nuttx/cmake/Toolchain-arm-none-eabi.cmake @@ -2,6 +2,7 @@ set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_VERSION 1) +set(PLATFORM_NAME "nuttx") set(triple arm-none-eabi) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) diff --git a/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake index 42f10bff332e..3d957a931757 100644 --- a/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake +++ b/platforms/nuttx/cmake/Toolchain-riscv64-unknown-elf.cmake @@ -2,6 +2,7 @@ set(CMAKE_SYSTEM_NAME Generic) set(CMAKE_SYSTEM_VERSION 1) +set(PLATFORM_NAME "nuttx") set(triple riscv64-unknown-elf) set(CMAKE_LIBRARY_ARCHITECTURE ${triple}) diff --git a/platforms/nuttx/cmake/bin_romfs.cmake b/platforms/nuttx/cmake/bin_romfs.cmake new file mode 100644 index 000000000000..51df9233d4eb --- /dev/null +++ b/platforms/nuttx/cmake/bin_romfs.cmake @@ -0,0 +1,135 @@ +############################################################################ +# +# Copyright (c) 2022 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +#============================================================================= +# +# make_bin_romfs +# +# This function creates a ROMFS from the input binary directory and places +# the generated C-style header into the specified output directory. To +# reduce the ROMFS size, an option is given to strip the input files, which +# reduces their size considerably +# +# Usage: +# make_bin_romfs( +# BINDIR +# OUTDIR +# OUTPREFIX +# LABEL +# STRIPPED +# DEPS +# ) +# +# Input: +# BINDIR : Input binary directory (source for ROMFS) +# OUTDIR : Output directory (${OUTPREFIX}_romfs.h is placed here) +# OUTPREFIX : Prefix for the output file +# LABEL : Volume label +# STRIPPED : Set to 'y' to strip the input binaries +# DEPS : Dependencies for the ROMFS +# +# Output: +# ${OUTPREFIX}_romfs.h file, that can be added to any C-source file. +# Mounting as a ROM-disk will be trivial once this is done. +# +function(make_bin_romfs) + + px4_parse_function_args( + NAME make_bin_romfs + ONE_VALUE BINDIR OUTDIR OUTPREFIX LABEL STRIPPED + MULTI_VALUE DEPS + REQUIRED BINDIR OUTDIR OUTPREFIX DEPS + ARGN ${ARGN} + ) + + if (NOT STRIPPED) + set(STRIPPED "n") + endif() + + if (NOT LABEL) + set(LABEL "NuttXRomfsVol") + endif() + + # Strip the elf files to reduce romfs image size + + if (${STRIPPED} STREQUAL "y") + # Preserve the files with debug symbols + + add_custom_target(debug_${OUTPREFIX} + COMMAND cp -r ${BINDIR} ${BINDIR}_debug + DEPENDS ${DEPS} + ) + + # Then strip the binaries in place + + add_custom_command(OUTPUT ${OUTPREFIX}_stripped_bins + COMMAND for f in * \; do if [ -f "$$f" ]; then ${CMAKE_STRIP} $$f --strip-unneeded \; fi \; done + DEPENDS ${DEPS} debug_${OUTPREFIX} + WORKING_DIRECTORY ${BINDIR} + ) + else() + add_custom_command(OUTPUT ${OUTPREFIX}_stripped_bins + COMMAND touch ${BINDIR} + ) + endif() + + # Make sure we have what we need + + find_program(GENROMFS genromfs) + if(NOT GENROMFS) + message(FATAL_ERROR "genromfs not found") + endif() + + find_program(XXD xxd) + if(NOT XXD) + message(FATAL_ERROR "xxd not found") + endif() + + find_program(SED sed) + if(NOT SED) + message(FATAL_ERROR "sed not found") + endif() + + # Generate the ROM file system + + add_custom_command(OUTPUT ${OUTDIR}/${OUTPREFIX}_romfsimg.h + COMMAND ${GENROMFS} -f ${OUTPREFIX}_romfs.img -d ${BINDIR} -V "${LABEL}" + COMMAND ${XXD} -i ${OUTPREFIX}_romfs.img | + ${SED} 's/^unsigned char/const unsigned char/g' >${OUTPREFIX}_romfsimg.h + COMMAND mv ${OUTPREFIX}_romfsimg.h ${OUTDIR}/${OUTPREFIX}_romfsimg.h + COMMAND rm -f ${OUTPREFIX}_romfs.img + DEPENDS ${OUTDIR} ${DEPS} ${OUTPREFIX}_stripped_bins + ) + add_custom_target(${OUTPREFIX}_romfsimg DEPENDS ${OUTDIR}/${OUTPREFIX}_romfsimg.h) + +endfunction() diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 4c5cbab4ef6c..b2b067cfc9e0 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -62,6 +62,7 @@ function(px4_os_add_flags) ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/nuttx/arch/${CONFIG_ARCH}/src/common ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/apps/include + ${PX4_SOURCE_DIR}/platforms/nuttx/NuttX/include ) @@ -148,6 +149,9 @@ function(px4_os_determine_build_chip) elseif(CONFIG_ARCH_CHIP_RP2040) set(CHIP_MANUFACTURER "rpi") set(CHIP "rp2040") + elseif(CONFIG_ARCH_CHIP_MPFS) + set(CHIP_MANUFACTURER "microchip") + set(CHIP "mpfs") else() message(FATAL_ERROR "Could not determine chip architecture from NuttX config. You may have to add it.") endif() diff --git a/platforms/nuttx/cmake/upload.cmake b/platforms/nuttx/cmake/upload.cmake index ce776b8a6247..d2ab7ba0d5da 100644 --- a/platforms/nuttx/cmake/upload.cmake +++ b/platforms/nuttx/cmake/upload.cmake @@ -34,8 +34,14 @@ # NuttX CDCACM vendor and product strings set(vendorstr_underscore) set(productstr_underscore) -string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) -string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) + +if (CONFIG_COMPOSITE_VENDORSTR) + string(REPLACE " " "_" vendorstr_underscore ${CONFIG_COMPOSITE_VENDORSTR}) + string(REPLACE " " "_" productstr_underscore ${CONFIG_COMPOSITE_PRODUCTSTR}) +else() + string(REPLACE " " "_" vendorstr_underscore ${CONFIG_CDCACM_VENDORSTR}) + string(REPLACE " " "_" productstr_underscore ${CONFIG_CDCACM_PRODUCTSTR}) +endif() set(serial_ports) if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Linux") diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index c0c68bff1c8f..6b9bb43b8e38 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -309,7 +309,6 @@ jump_to_app() #ifdef BOOTLOADER_USE_SECURITY crypto_init(); #endif - const image_toc_entry_t *toc_entries; uint8_t len; uint8_t i = 0; diff --git a/platforms/nuttx/src/bootloader/common/crypto.c b/platforms/nuttx/src/bootloader/common/crypto.c index 12bf011dd985..8dd1e3f20d65 100644 --- a/platforms/nuttx/src/bootloader/common/crypto.c +++ b/platforms/nuttx/src/bootloader/common/crypto.c @@ -32,8 +32,10 @@ ****************************************************************************/ #include + #include "image_toc.h" #include "hw_config.h" +#include "crypto.h" #ifdef BOOTLOADER_USE_SECURITY diff --git a/platforms/nuttx/src/bootloader/common/image_toc.c b/platforms/nuttx/src/bootloader/common/image_toc.c index 3d8494f6daf6..01c9cf515f9c 100644 --- a/platforms/nuttx/src/bootloader/common/image_toc.c +++ b/platforms/nuttx/src/bootloader/common/image_toc.c @@ -31,13 +31,12 @@ * ****************************************************************************/ -#include "hw_config.h" - #include #include #include #include +#include "hw_config.h" #include "image_toc.h" #include "bl.h" @@ -45,14 +44,29 @@ /* Helper macros to define flash start and end addresses, based on info from * hw_config.h */ +#ifndef FLASH_START_ADDRESS #define FLASH_START_ADDRESS (APP_LOAD_ADDRESS & (~(BOARD_FLASH_SIZE - 1))) +#endif #define FLASH_END_ADDRESS (FLASH_START_ADDRESS + BOARD_FLASH_SIZE) bool find_toc(const image_toc_entry_t **toc_entries, uint8_t *len) { - const uintptr_t toc_start_u32 = APP_LOAD_ADDRESS + BOOT_DELAY_ADDRESS + 8; - const image_toc_start_t *toc_start = (const image_toc_start_t *)toc_start_u32; - const image_toc_entry_t *entry = (const image_toc_entry_t *)(toc_start_u32 + sizeof(image_toc_start_t)); + uintptr_t toc_start_u32 = TOC_ADDRESS; + const image_toc_start_t *toc_start; + const image_toc_entry_t *entry; + +#ifdef TOC_ADDRESS_ALT + const uint32_t toc_start_u32_alt = TOC_ADDRESS_ALT; + const image_toc_start_t *toc_start_alt = (const image_toc_start_t *)toc_start_u32_alt; + + if (toc_start_alt->magic == TOC_START_MAGIC) { + toc_start_u32 = TOC_ADDRESS_ALT; + } + +#endif + + toc_start = (const image_toc_start_t *)toc_start_u32; + entry = (const image_toc_entry_t *)(toc_start_u32 + sizeof(image_toc_start_t)); int i = 0; uint8_t sig_idx; @@ -72,22 +86,22 @@ bool find_toc(const image_toc_entry_t **toc_entries, uint8_t *len) toc_end_u32 = (uintptr_t)&entry[i] + sizeof(toc_end_magic); /* The number of ToC entries found must be within bounds, and the - * ToC has to lie within the flashable area. Also ensure that - * end > start. + * first entry, containing the ToC, has to lie within the flashable area. + * Also ensure that end > start. */ - if (i <= MAX_TOC_ENTRIES && i > 0 && toc_start_u32 >= (uintptr_t)entry[0].start && toc_end_u32 < (uintptr_t)entry[0].end && - (uintptr_t)entry[0].start == APP_LOAD_ADDRESS && + (uintptr_t)entry[0].start >= FLASH_START_ADDRESS && (uintptr_t)entry[0].end <= (FLASH_END_ADDRESS - sizeof(uintptr_t)) && (uintptr_t)entry[0].end > (uintptr_t)entry[0].start) { + sig_idx = entry[0].signature_idx; /* The signature idx for the first app must be within the TOC, and - * the signature must be within the flash area, not overlapping the - * app. Also ensure that end > start. - */ + * the signature must be within the flash area, not overlapping the + * first entry containing the TOC. Also ensure that end > start. + */ if (sig_idx > 0 && sig_idx < MAX_TOC_ENTRIES && diff --git a/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt b/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt new file mode 100644 index 000000000000..36843c5694ef --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt b/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt new file mode 100644 index 000000000000..97f45e3fb332 --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/CMakeLists.txt @@ -0,0 +1,56 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_bootloader + main.c + systick.c +) + +execute_process( + COMMAND git describe --dirty --all --long + WORKING_DIRECTORY ${CMAKE_CURRENT_LIST_DIR} + OUTPUT_VARIABLE VERSION + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + +add_definitions(-DVERSION=\"${VERSION}\") + +target_link_libraries(arch_bootloader + PRIVATE + bootloader_lib + nuttx_arch +) + +target_include_directories(arch_bootloader PRIVATE ${PX4_BOARD_DIR}) +target_link_libraries(nuttx_fs INTERFACE nuttx_c) +target_link_libraries(nuttx_drivers INTERFACE nuttx_fs) diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/main.c b/platforms/nuttx/src/bootloader/microchip/mpfs/main.c new file mode 100644 index 000000000000..c77e9cb205e2 --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/main.c @@ -0,0 +1,1011 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/* + * MPFS board support for the bootloader. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hw_config.h" +#include "board_type.h" + +#include "bl.h" +#include "uart.h" +#include "lib/flash_cache.h" +#include "riscv_internal.h" + +#include +#include + +#include + +#include + +#include "image_toc.h" +#include "crypto.h" + +extern int sercon_main(int c, char **argv); + +#if defined(CONFIG_OPENSBI) +extern void mpfs_opensbi_relocate_from_envm(void); +#endif + +#define MK_GPIO_INPUT(def) (((def) & (~(GPIO_OUTPUT))) | GPIO_INPUT) + +#define APP_SIZE_MAX BOARD_FLASH_SIZE + +// Reads/writes to flash are done in this size chunks +#define FLASH_RW_BLOCK 4096 + +/* context passed to cinit */ +#if INTERFACE_USART +# define BOARD_INTERFACE_CONFIG_USART INTERFACE_USART_CONFIG +#endif +#if INTERFACE_USB +# define BOARD_INTERFACE_CONFIG_USB INTERFACE_USB_CONFIG +#endif + +/* Reset reasons */ + +/* Reset was caused by the SCB periphery reset signal*/ +#define RESET_SR_SCB_PERIPH_RESET_MASK (0x01 << 0x0) + +/* Reset was caused by the SCB MSS reset register*/ +#define RESET_SR_SCB_MSS_RESET_MASK (0x01 << 0x1) + +/* Reset was caused by the SCB CPU reset register*/ +#define RESET_SR_SCB_CPU_RESET_MASK (0x01 << 0x2) + +/* Reset was caused by the Risc-V Debugger*/ +#define RESET_SR_DEBUGGER_RESET_MASK (0x01 << 0x3) + +/* Reset was caused by the fabric*/ +#define RESET_SR_FABRIC_RESET_MASK (0x01 << 0x4) + +/* Reset was caused by the watchdog*/ +#define RESET_SR_WDOG_RESET_MASK (0x01 << 0x5) + +/* Indicates that fabric asserted the GPIO reset inputs*/ +#define RESET_SR_GPIO_RESET_MASK (0x01 << 0x6) + +/* Indicates that SCB bus reset occurred (which causes warm reset of MS + S)*/ +#define RESET_SR_SCB_BUS_RESET_MASK (0x01 << 0x7) + +/* Indicates that CPU soft reset occured */ +#define RESET_SR_CPU_SOFT_RESET_MASK (0x01 << 0x8) + +#ifdef CONFIG_MMCSD +static int px4_fd = -1; +static bool sdcard_mounted; +#endif + + +#if BOOTLOADER_VERIFY_UBOOT + +#define UBOOT_BINARY "u-boot_signed.bin" +#define UBOOT_SIGNATURE_SIZE 64 +#else + +#define UBOOT_BINARY "u-boot.bin" +#endif + +static struct mtd_dev_s *mtd = 0; +static struct mtd_geometry_s geo; + +#if defined(CONFIG_MTD_M25P) +static struct spi_dev_s *spinor = 0; +#endif + +static int loader_task = -1; +typedef enum { + UNINITIALIZED = -1, + IN_PROGRESS, + DONE, + SW_UPDATED, + INTERRUPTED, + LOAD_FAIL +} image_loading_status_t; + +static image_loading_status_t loading_status; +static bool u_boot_loaded = false; + +/* board definition */ +struct boardinfo board_info = { + .board_type = BOARD_TYPE, + .board_rev = 0, + .fw_size = 0, + .systick_mhz = 600, +}; + +static void board_init(void); + +/* LED_ACTIVITY == 1, LED_BOOTLOADER == 2 */ +static bool g_led_state[3]; + +/* State of an inserted USB cable */ +static bool usb_connected = false; + +devinfo_t device_info __attribute__((section(".deviceinfo"))); + +/* PX4 image TOC 'reserved' field for vendor specific info_bits + * Bit 0 marks for whether SBI should be used or not +*/ + +static uint8_t info_bits; + +static uint32_t board_get_reset_reason(void) +{ + return getreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_RESET_SR_OFFSET); +} + +static void board_set_reset_reason(uint32_t reason) +{ + putreg32(reason, MPFS_SYSREG_BASE + MPFS_SYSREG_RESET_SR_OFFSET); +} + +static bool board_test_force_pin(void) +{ +#ifdef BOARD_FORCE_BL_PIN + + if (px4_arch_gpioread(BOARD_FORCE_BL_PIN)) { + return true; + } + +#endif + return false; +} + +uint32_t +board_get_devices(void) +{ + uint32_t devices = BOOT_DEVICES_SELECTION; + + if (usb_connected) { + devices &= BOOT_DEVICES_FILTER_ONUSB; + } + + return devices; +} + +#if defined(CONFIG_MPFS_SPI0) +#define SPI_NOR_CS_FUNC mpfs_spi0_select +#elif defined(CONFIG_MPFS_SPI1) +#define SPI_NOR_CS_FUNC mpfs_spi1_select +#endif + +#if defined(BOARD_PIN_CS_SPINOR) +void +SPI_NOR_CS_FUNC(FAR struct spi_dev_s *dev, uint32_t devid, bool selected) +{ + px4_arch_gpiowrite(BOARD_PIN_CS_SPINOR, !selected); +} +#endif + +#ifdef CONFIG_MMCSD + +static ssize_t load_sdcard_images(const char *name, uint64_t loadaddr) +{ + struct stat file_stat; + + int mmcsd_fd = open(name, O_RDONLY); + + if (mmcsd_fd > 0) { + fstat(mmcsd_fd, &file_stat); + size_t got = read(mmcsd_fd, (void *)loadaddr, file_stat.st_size); + + if (got > 0 && got == (size_t)file_stat.st_size) { + _alert("Loading %s OK\n", name); + close(mmcsd_fd); + return got; + } + } + + _alert("Loading %s failed\n", name); + close(mmcsd_fd); + return -1; +} +#endif + +static void +board_init(void) +{ + /* fix up the max firmware size, we have to read memory to get this */ + board_info.fw_size = APP_SIZE_MAX; + +#if defined(BOARD_FORCE_BL_PIN) + /* configure the force BL pins */ + px4_arch_configgpio(BOARD_FORCE_BL_PIN); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_ACTIVITY); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(BOARD_PIN_LED_BOOTLOADER); +#endif + +#if defined(CONFIG_MTD_M25P) + px4_arch_configgpio(BOARD_PIN_CS_SPINOR); + spinor = mpfs_spibus_initialize(1); + mtd = m25p_initialize(spinor); + + if (mtd) { + if (mtd->ioctl(mtd, MTDIOC_GEOMETRY, + (unsigned long)((uintptr_t)&geo))) { + } + } else { + _alert("ERROR: MTD initialization failure!\n"); + } + +#endif + +#if defined(CONFIG_USBDEV) + mpfs_usbinitialize(); +# if defined(CONFIG_SYSTEM_CDCACM) + sercon_main(0, NULL); +# endif +#endif + +#if defined(CONFIG_MMCSD) + + if (mpfs_board_emmcsd_init() == OK) { + +# if defined(CONFIG_USBMSC_COMPOSITE) + + if (board_composite_initialize(0) == OK) { + if (board_composite_connect(0, 0) == NULL) { + _alert("Failed to connect composite\n"); + } + + } else { + _alert("Failed to initialize composite\n"); + } + +# endif + + if (mpfs_board_register_partition(0) == OK) { + + /* Mount the sdcard/eMMC */ + sdcard_mounted = mount("/dev/mmcsd0p0", "/sdcard/", "vfat", 0, NULL) == 0 ? true : false; + + if (!sdcard_mounted) { + _alert("SD/eMMC mount failed\n"); + + } else { + _alert("SD/eMMC mounted\n"); + } + } + + } else { + _alert("ERROR: Failed to initialize SD card"); + } + +#endif + +} + +void +board_deinit(void) +{ + +#if INTERFACE_USART +#ifdef CONFIG_MPFS_FPGA_UART + up_disable_irq(MPFS_IRQ_FABRIC_F2H_12); + up_disable_irq(MPFS_IRQ_FABRIC_F2H_13); +#else + up_disable_irq(MPFS_IRQ_MMUART0); + up_disable_irq(MPFS_IRQ_MMUART1); +#endif +#endif + +#ifdef CONFIG_MMCSD + + /* Umount the sdcard now if mounted */ + if (sdcard_mounted) { + umount("/sdcard"); + } + + /* If this is left open from flashing, close it now */ + close(px4_fd); + + /* deinitialise the MMC/SD interrupt */ + up_disable_irq(MPFS_IRQ_MMC_MAIN); +#endif + +#ifdef CONFIG_MTD_M25P + mpfs_spibus_uninitialize(spinor); +#endif + +#if defined(BOARD_FORCE_BL_PIN) + /* deinitialise the force BL pin */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_FORCE_BL_PIN)); +#endif + +#if defined(BOARD_PIN_LED_ACTIVITY) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_ACTIVITY)); +#endif +#if defined(BOARD_PIN_LED_BOOTLOADER) + /* Initialize LEDs */ + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_LED_BOOTLOADER)); +#endif + +#if defined(BOARD_PIN_CS_SPINOR) + px4_arch_configgpio(MK_GPIO_INPUT(BOARD_PIN_CS_SPINOR)); +#endif + +#if defined(CONFIG_USBDEV) + up_disable_irq(MPFS_IRQ_USB_MC); +#endif +} + +static inline void +clock_init(void) +{ + // Done by Nuttx +} + +void +clock_deinit(void) +{ + up_disable_irq(RISCV_IRQ_MTIMER); +} + +void arch_flash_lock(void) +{ +} + +void arch_flash_unlock(void) +{ +} + +inline void arch_setvtor(const uint32_t *address) +{ +} + +inline static uint32_t +flash_func_block_size(void) +{ + return mtd ? geo.blocksize : 4096; +} + +uint32_t +flash_func_sector_size(unsigned sector) +{ + size_t erasesize = mtd ? geo.erasesize : 4096; + + if (sector * erasesize < BOARD_FLASH_SIZE) { + return erasesize; + + } else { + return 0; + } +} + +#ifdef CONFIG_MMCSD +static void create_px4_file(void) +{ + if (sdcard_mounted) { + px4_fd = open("/sdcard/boot/" IMAGE_FN, O_RDWR | O_CREAT | O_TRUNC); + + /* Couldn't open the file, make sure that the directory exists and try to re-open */ + + if (px4_fd < 0) { + int ret = mkdir("/sdcard/boot", S_IRWXU | S_IRWXG | S_IRWXO); + + if (ret < 0) { + _alert("boot directory creation failed %d\n", ret); + } + + px4_fd = open("/sdcard/boot/" IMAGE_FN, O_RDWR | O_CREAT | O_TRUNC, 0644); + } + + if (px4_fd < 0) { + _alert("FATAL: Not able to create px4 fw image!\n"); + + } + } +} +#endif + +void +flash_func_erase_sector(unsigned sector) +{ + + unsigned ss = flash_func_sector_size(sector); + uint64_t *addr = (uint64_t *)((uint64_t)APP_LOAD_ADDRESS + sector * ss); + + /** + * Break any loading process + * + * Wait for loading to stop. Loading always ends up with either + * LOAD_FAIL or DONE, and any other values are intermediate + */ + + irqstate_t flags = px4_enter_critical_section(); + + if (loading_status != LOAD_FAIL && loading_status != DONE) { + loading_status = INTERRUPTED; + } + + px4_leave_critical_section(flags); + + while (loading_status != LOAD_FAIL && loading_status != DONE) { + usleep(1000); + } + +#ifdef CONFIG_MTD_M25P + /* Flash into NOR flash */ + + int ret = MTD_ERASE(mtd, sector, 1); + + if (ret < 0) { + ferr("ERROR: Erase block=%u failed: %d\n", + sector, ret); + } + +#endif + +#if defined(CONFIG_MMCSD) + /* Flashing into eMMC/SD */ + + /* Create the file when erasing the first sector */ + if (sector == 0) { + create_px4_file(); + } + +#endif + + /* Erase the RAM contents */ + + memset(addr, 0xFFFFFFFF, ss); + +} + +#if defined(CONFIG_MTD_M25P) || defined(CONFIG_MMCSD) +static ssize_t flash_write_pages(off_t start, unsigned n_pages, uint8_t *src) +{ + ssize_t ret = 0; +#ifdef CONFIG_MTD_M25P + + ret = MTD_BWRITE(mtd, start, n_pages, src); + + if (ret != n_pages) { + _alert("SPI NOR write error in pages %d-%d\n", start, start + n_pages); + ret = -errno; + } + +#elif defined(CONFIG_MMCSD) + + if (!sdcard_mounted) { + return -EBADF; + } + + /* Write to file, from the app_load_address */ + ret = (ssize_t)lseek(px4_fd, start * flash_func_block_size(), SEEK_SET); + + if (ret >= 0) { + ssize_t bytes = n_pages * flash_func_block_size(); + + ret = write(px4_fd, (void *)src, bytes); + + if (ret != bytes) { + ret = -errno; + _alert("eMMC write error at 0x%lx-0x%lx\n", start * flash_func_block_size(), + (start + n_pages) * flash_func_block_size()); + } + + } else { + _alert("File lseek fail\n"); + ret = -errno; + } + +#endif + return ret; +} +#endif + +void +flash_func_write_word(uintptr_t address, uint32_t word) +{ + /* Also copy it directly to load address for booting */ + uint32_t *app_load_addr = (uint32_t *)(address + APP_LOAD_ADDRESS); + + *app_load_addr = word; + +#if defined(CONFIG_MTD_M25P) || defined(CONFIG_MMCSD) + + static uintptr_t end_address = 0; + static uintptr_t first_unwritten = 0; + + // start of this block in memory + uint8_t *block_start; + + // total bytes to be written + unsigned bytes; + + int ret = 0; + + if (address > 0 && + ((address + sizeof(uint32_t)) % FLASH_RW_BLOCK) == 0) { + // Every time a full FLASH_RW_BLOCK is received, store it to disk + + block_start = ((uint8_t *)app_load_addr + sizeof(uint32_t)) - FLASH_RW_BLOCK; + bytes = FLASH_RW_BLOCK; + + // first page to be written + off_t write_page = address / flash_func_block_size(); + // total pages to be written + unsigned n_pages = (FLASH_RW_BLOCK / flash_func_block_size()); + // write pages + ret = flash_write_pages(write_page, n_pages, (uint8_t *)block_start); + // store the first unwritten address for the end of image handling + first_unwritten = address + sizeof(uint32_t); + } + + // the address 0 is written last, in the end of flashing + if (address == 0 && word != 0xffffffffu) { + // Write the last incomplete block + bytes = end_address - first_unwritten; + unsigned n_pages = bytes / flash_func_block_size(); + + if (bytes % flash_func_block_size()) { + n_pages += 1; + } + + if (n_pages) { + block_start = (uint8_t *)(APP_LOAD_ADDRESS + first_unwritten); + ret = flash_write_pages(first_unwritten / flash_func_block_size(), n_pages, + block_start); + } + + // re-write the first page + if (ret >= 0) { + block_start = (uint8_t *)APP_LOAD_ADDRESS; + bytes = flash_func_block_size(); + ret = flash_write_pages(0, 1, block_start); + } + } + + // if the writing failed, erase the written data in DRAM, and also + // the first word, in case this was the last write + if (ret < 0) { + memset((uint8_t *)block_start, 0xff, bytes); + *(uint32_t *)APP_LOAD_ADDRESS = 0xffffffffu; + } + + end_address = address + sizeof(uint32_t); +#endif + + /* After the last word has been written, update the loading_status */ + if (address == 0 && word != 0xffffffffu) { +#ifdef CONFIG_MMCSD + close(px4_fd); + px4_fd = -1; +#endif + loading_status = SW_UPDATED; + } +} + +uint32_t flash_func_read_word(uintptr_t address) +{ + uint32_t word; + + address += APP_LOAD_ADDRESS; + word = *(uint32_t *)((uintptr_t)address); + + return word; +} + + +uint32_t +flash_func_read_otp(uintptr_t address) +{ + return 0; +} + +uint32_t get_mcu_id(void) +{ + return 0; +} + +int get_mcu_desc(int max, uint8_t *revstr) +{ + revstr[0] = 1; + revstr[1] = ','; + revstr[2] = 0; + return 3; +} + + +int check_silicon(void) +{ + return 0; +} + +uint32_t +flash_func_read_sn(uintptr_t address) +{ + return 0 ; +} + +void +led_on(unsigned led) +{ + g_led_state[led] = true; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_ON); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_ON); +#endif + break; + } +} + +void +led_off(unsigned led) +{ + g_led_state[led] = false; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, BOARD_LED_OFF); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, BOARD_LED_OFF); +#endif + break; + } +} + +void +led_toggle(unsigned led) +{ + g_led_state[led] ^= 1; + + switch (led) { + case LED_ACTIVITY: +#if defined(BOARD_PIN_LED_ACTIVITY) + + px4_arch_gpiowrite(BOARD_PIN_LED_ACTIVITY, g_led_state[led]); +#endif + break; + + case LED_BOOTLOADER: +#if defined(BOARD_PIN_LED_BOOTLOADER) + px4_arch_gpiowrite(BOARD_PIN_LED_BOOTLOADER, g_led_state[led]); +#endif + break; + } +} + +/* Make the actual jump to app */ +void +arch_do_jump(const uint32_t *app_base) +{ + + /* PX4 on hart 2 */ +#if CONFIG_MPFS_HART2_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + bool use_sbi = info_bits & INFO_BIT_USE_SBI ? true : false; + mpfs_set_use_sbi(2, use_sbi); + mpfs_set_entrypt(2, (uintptr_t)app_base); + *(volatile uint32_t *)MPFS_CLINT_MSIP2 = 0x01U; +#endif + + /* Linux on harts 1, 3 and 4 */ + if (u_boot_loaded) { +#if CONFIG_MPFS_HART3_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + *(volatile uint32_t *)MPFS_CLINT_MSIP3 = 0x01U; +#endif + +// If SMP is used on Linux, the primary core (Hart 3) will boot the secondary +// cores (Hart 1 and 4), so they should not be booted here +#if BOOTLOADER_BOOT_HART_1 +#if CONFIG_MPFS_HART1_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + *(volatile uint32_t *)MPFS_CLINT_MSIP1 = 0x01U; +#endif +#endif + +#if BOOTLOADER_BOOT_HART_4 +#if CONFIG_MPFS_HART4_ENTRYPOINT != 0xFFFFFFFFFFFFFFFF + *(volatile uint32_t *)MPFS_CLINT_MSIP4 = 0x01U; +#endif +#endif + + } + + // TODO. monitor? + while (1) { + usleep(1000000); + } +} + +#if defined(CONFIG_MMCSD) || defined(CONFIG_MTD_M25P) +static size_t get_image_size(void) +{ + const image_toc_entry_t *toc_entries; + const void *end = (const void *)APP_LOAD_ADDRESS; + uint8_t len; + + if (find_toc(&toc_entries, &len)) { + _alert("Found TOC\n"); + + // find the boot image vendor flags and the largest end address + const uint32_t sig = ('B' << 0) | ('O' << 8) | ('O' << 16) | ('T' << 24); + + for (int i = 0; i < len; i++) { + if (*(uint32_t *)(void *)toc_entries[i].name == sig) { + info_bits = toc_entries[i].flags2; + } + + if (toc_entries[i].end > end) { + end = toc_entries[i].end; + } + } + } + + // image size is end address - app start + return (uintptr_t)end - APP_LOAD_ADDRESS; +} +#endif + +#if BOOTLOADER_VERIFY_UBOOT +bool verify_image(void *image_start, size_t image_size, size_t signature_size) +{ + uint8_t signature_idx = 1; + uint16_t index = 0; + + uint8_t *image_end = (uint8_t *)image_start + image_size - signature_size; + uint8_t *signature_start = (uint8_t *)image_start + image_size - signature_size; + uint8_t *signature_end = (uint8_t *)image_start + image_size; + + image_toc_entry_t toc_entries[2] = { + {"IMG ", image_start, image_end, 0, signature_idx, 0, 0, 0}, + {"SIG ", signature_start, signature_end, 0, 0, 0, 0, 0} + }; + + return verify_app(index, toc_entries); +} +#endif + +static int loader_main(int argc, char *argv[]) +{ + ssize_t image_sz = 0; + loading_status = IN_PROGRESS; + +#if defined(CONFIG_MMCSD) + ssize_t ret = 0; + + if (sdcard_mounted) { + ret = load_sdcard_images("/sdcard/boot/" IMAGE_FN, APP_LOAD_ADDRESS); + + if (ret > 0) { + image_sz = get_image_size(); + + if (image_sz > 0) { + _alert("PX4 load success\n"); + } + } + } + +#endif + +#if defined(CONFIG_MTD_M25P) + /* If loading from sdcard didn't succeed, use SPI-NOR */ + + // Read first FLASH_RW_BLOCK size data, search if TOC exists + + if (image_sz == 0) { + const unsigned pgs_per_block = FLASH_RW_BLOCK / flash_func_block_size(); + size_t pages = MTD_BREAD(mtd, 0, pgs_per_block, (uint8_t *)APP_LOAD_ADDRESS); + + if (pages == pgs_per_block) { + image_sz = get_image_size(); + + if (image_sz > 0) { + _alert("Loading from NOR flash\n"); + unsigned reads_left = image_sz / FLASH_RW_BLOCK - 1; + + if (image_sz % FLASH_RW_BLOCK) { + reads_left += 1; + } + + // read the rest in FLASH_RW_BLOCK blocks + for (unsigned i = 1; i < reads_left + 1; i++) { + if (loading_status == INTERRUPTED) { + image_sz = -1; + break; + } + + MTD_BREAD(mtd, i * pgs_per_block, pgs_per_block, ((uint8_t *)APP_LOAD_ADDRESS) + (i * FLASH_RW_BLOCK)); + } + } + } + } + +#endif + +#if defined(CONFIG_OPENSBI) && defined(CONFIG_MMCSD) + ssize_t uboot_size = 0; + + /* Relocate code from eNVM into L2 zero device */ + + mpfs_opensbi_relocate_from_envm(); + + if (sdcard_mounted) { + uboot_size = load_sdcard_images("/sdcard/boot/"UBOOT_BINARY, CONFIG_MPFS_HART3_ENTRYPOINT); + + if (uboot_size > 0) { + u_boot_loaded = true; +#if BOOTLOADER_VERIFY_UBOOT + + if (!verify_image((void *)CONFIG_MPFS_HART3_ENTRYPOINT, uboot_size, UBOOT_SIGNATURE_SIZE)) { + u_boot_loaded = false; + /* Wipe the memory */ + memset((void *)CONFIG_MPFS_HART3_ENTRYPOINT, 0, uboot_size); + _alert("u-boot Authentication Failed\n"); + } + +#endif + + } else { + _alert("u-boot loading failed\n"); + u_boot_loaded = false; + } + } + +#endif + + /* image_sz < 0 means that the load was interrupted due to flashing in progress */ + if (image_sz == 0) { + _alert("No boot image found\n"); + loading_status = LOAD_FAIL; + + } else if (loading_status == IN_PROGRESS) { + _alert("Image loaded succesfully, size %ld\n", image_sz); + loading_status = DONE; + + } else { + _alert("Image loading interrupted\n"); + loading_status = LOAD_FAIL; + } + + return 0; +} + +static void start_image_loading(void) +{ + /* Mark that we are waiting for the task to start */ + loading_status = UNINITIALIZED; + + /* create the task */ + loader_task = task_create("loader", SCHED_PRIORITY_MAX - 6, 8192, loader_main, (char *const *)0); + + /* wait for the task to start */ + while (loading_status == UNINITIALIZED) { + usleep(1000); + } +} + +int +bootloader_main(int argc, char *argv[]) +{ + unsigned timeout = BOOTLOADER_DELAY; /* if nonzero, drop out of the bootloader after this time */ + bool try_boot; + snprintf(device_info.bl_version, sizeof(device_info.bl_version), VERSION); + _alert("Version: %s\n", device_info.bl_version); + + /* do board-specific initialisation */ + board_init(); + + /* configure the clock for bootloader activity */ + clock_init(); + + /* check the bootloader force pin status */ + try_boot = !board_test_force_pin(); + + start_image_loading(); + + /* + * Check the boot reason. In case we came here with SW system reset, + * we stay in bootloader. + * SW system reset is issued in PX4 with "reboot -b" + */ + + uint32_t reset_reason = board_get_reset_reason(); + + /* Is not FABRIC reset and not caused by WDOG? FABRIC reset bit is only + * set in POR, since it is not even connected in FPGA. + * If also CPU_SOFT_RESET is set, we know that this is soft reset from PX4 (reboot -b) + */ + if ((reset_reason & RESET_SR_FABRIC_RESET_MASK) == 0 && + (reset_reason & RESET_SR_WDOG_RESET_MASK) == 0 && + (reset_reason & RESET_SR_CPU_SOFT_RESET_MASK) == RESET_SR_CPU_SOFT_RESET_MASK) { + + /* Don't drop out of the bootloader until something has been uploaded */ + timeout = 0; + } + + /* Clear the reset reason */ + board_set_reset_reason(0); + + /* start the interface */ +#if INTERFACE_USART + cinit(BOARD_INTERFACE_CONFIG_USART, USART); +#endif + +#if INTERFACE_USB + cinit(BOARD_INTERFACE_CONFIG_USB, USB); +#endif + + while (1) { + /* run the bootloader, come back after an app is uploaded or we time out */ + bootloader(timeout); + + /* if the sw was just re-flashed, load the image again */ + if (loading_status == SW_UPDATED) { + timeout = BOOTLOADER_DELAY; + start_image_loading(); + } + + /* look to see if we can boot the app */ + + if (try_boot && loading_status == DONE) { + jump_to_app(); + } + } +} diff --git a/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c b/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c new file mode 100644 index 000000000000..6bb775fdb3ee --- /dev/null +++ b/platforms/nuttx/src/bootloader/microchip/mpfs/systick.c @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 + +void arch_systic_init(void) +{ +} + +void arch_systic_deinit(void) +{ +} + +uint8_t systick_get_countflag(void) +{ + return 0; +} + +void systick_set_reload(uint32_t value) +{ +} + + +void systick_set_clocksource(uint8_t clocksource) +{ +} + +void systick_counter_enable(void) +{ +} + +void systick_counter_disable(void) +{ +} + +void systick_interrupt_enable(void) +{ +} + +void systick_interrupt_disable(void) +{ +} diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index 894834f078fa..d5f575364050 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -634,7 +634,7 @@ arch_do_jump(const uint32_t *app_base) } int -bootloader_main(void) +bootloader_main(int argc, char *argv[]) { bool try_boot = true; /* try booting before we drop to the bootloader */ unsigned timeout = BOOTLOADER_DELAY; /* if nonzero, drop out of the bootloader after this time */ diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index ab16e77a0e30..263ca0c5c0ce 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -38,12 +38,12 @@ if(NOT PX4_BOARD MATCHES "io-v2") board_crashdump.c board_dma_alloc.c board_fat_dma_alloc.c - cdc_acm_check.cpp console_buffer.cpp cpuload.cpp gpio.c print_load.cpp tasks.cpp + px4_atomic.cpp px4_nuttx_impl.cpp px4_init.cpp px4_manifest.cpp @@ -59,6 +59,7 @@ if(NOT PX4_BOARD MATCHES "io-v2") arch_board_critmon arch_version nuttx_sched + crc ) if (NOT DEFINED CONFIG_BUILD_FLAT AND "${PX4_PLATFORM}" MATCHES "nuttx") diff --git a/platforms/nuttx/src/px4/common/board_crashdump.c b/platforms/nuttx/src/px4/common/board_crashdump.c index 4a5f50f7cef7..7a232557ce69 100644 --- a/platforms/nuttx/src/px4/common/board_crashdump.c +++ b/platforms/nuttx/src/px4/common/board_crashdump.c @@ -250,13 +250,14 @@ static uint32_t *__attribute__((noinline)) __ebss_addr(void) static uint32_t *__attribute__((noinline)) __sdata_addr(void) { - return &_sdata; + return (uint32_t *)(uint32_t)_sdata; } #endif - -__EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const char *filename, int lineno) +__EXPORT void board_crashdump(uintptr_t currentsp, FAR struct tcb_s *rtcb, + FAR const char *filename, int lineno, + FAR const char *msg, FAR void *regs) { #ifndef BOARD_CRASHDUMP_RESET_ONLY /* We need a chunk of ram to save the complete context in. @@ -274,8 +275,6 @@ __EXPORT void board_crashdump(uintptr_t currentsp, FAR void *tcb, FAR const char (void)enter_critical_section(); - struct tcb_s *rtcb = (struct tcb_s *)tcb; - /* Zero out everything */ memset(pdump, 0, sizeof(fullcontext_s)); diff --git a/platforms/nuttx/src/px4/common/board_ioctl.c b/platforms/nuttx/src/px4/common/board_ioctl.c index 9db26b15a09a..d510b743488a 100644 --- a/platforms/nuttx/src/px4/common/board_ioctl.c +++ b/platforms/nuttx/src/px4/common/board_ioctl.c @@ -42,14 +42,22 @@ #include #include "board_config.h" -#include +#include + #include -FAR const struct builtin_s g_kernel_builtins[] = { +struct kernel_builtin_s { + const char *name; /* Invocation name and as seen under /sbin/ */ + int priority; /* Use: SCHED_PRIORITY_DEFAULT */ + int stacksize; /* Desired stack size */ + main_t main; /* Entry point: main(int argc, char *argv[]) */ +}; + +const struct kernel_builtin_s g_kernel_builtins[] = { #include }; -const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(struct builtin_s); +const int g_n_kernel_builtins = sizeof(g_kernel_builtins) / sizeof(g_kernel_builtins[0]); static struct { ioctl_ptr_t ioctl_func; @@ -107,10 +115,11 @@ int board_ioctl(unsigned int cmd, uintptr_t arg) static int launch_kernel_builtin(int argc, char **argv) { int i; - FAR const struct builtin_s *builtin = NULL; + const struct kernel_builtin_s *builtin = NULL; + char *name = basename(argv[0]); for (i = 0; i < g_n_kernel_builtins; i++) { - if (!strcmp(g_kernel_builtins[i].name, argv[0])) { + if (!strcmp(g_kernel_builtins[i].name, name)) { builtin = &g_kernel_builtins[i]; break; } diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp index d86556894d32..c9bcbcf49afd 100644 --- a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp +++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp @@ -38,18 +38,18 @@ __BEGIN_DECLS #include #include #include +#include #include -#include #include #include +#include #include -extern int sercon_main(int c, char **argv); -extern int serdis_main(int c, char **argv); __END_DECLS #include +#include #include #include @@ -77,6 +77,7 @@ __END_DECLS static struct work_s usb_serial_work; static bool vbus_present_prev = false; static int ttyacm_fd = -1; +static void *usb_handle; enum class UsbAutoStartState { disconnected, @@ -85,6 +86,34 @@ enum class UsbAutoStartState { disconnecting, } usb_auto_start_state{UsbAutoStartState::disconnected}; +// Forklifted from apps/system/cdcacm/cdcacm_main.c +static int serial_connect(void) +{ + struct boardioc_usbdev_ctrl_s ctrl = { + .usbdev = BOARDIOC_USBDEV_CDCACM, + .action = BOARDIOC_USBDEV_CONNECT, + .instance = CONFIG_SYSTEM_CDCACM_DEVMINOR, + .handle = &usb_handle, + }; + + return boardctl(BOARDIOC_USBDEV_CONTROL, (uintptr_t)&ctrl); +} + +static int serial_disconnect(void) +{ + struct boardioc_usbdev_ctrl_s ctrl = { + .usbdev = BOARDIOC_USBDEV_CDCACM, + .action = BOARDIOC_USBDEV_DISCONNECT, + .instance = CONFIG_SYSTEM_CDCACM_DEVMINOR, + .handle = &usb_handle, + }; + + boardctl(BOARDIOC_USBDEV_CONTROL, (uintptr_t)&ctrl); + usb_handle = NULL; + + return 0; +} + static void mavlink_usb_check(void *arg) { @@ -116,7 +145,7 @@ static void mavlink_usb_check(void *arg) switch (usb_auto_start_state) { case UsbAutoStartState::disconnected: if (vbus_present && vbus_present_prev) { - if (sercon_main(0, nullptr) == EXIT_SUCCESS) { + if (serial_connect() == 0) { usb_auto_start_state = UsbAutoStartState::connecting; rescheduled = work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, USEC2TICK(100000)); } @@ -305,7 +334,7 @@ static void mavlink_usb_check(void *arg) else if (launch_passthru) { sched_lock(); exec_argv = (char **)gps_argv; - exec_builtin(exec_argv[0], exec_argv, nullptr, 0); + px4_exec(exec_argv[0], exec_argv, nullptr, 0); sched_unlock(); exec_argv = (char **)passthru_argv; } @@ -314,7 +343,7 @@ static void mavlink_usb_check(void *arg) sched_lock(); - if (exec_builtin(exec_argv[0], exec_argv, nullptr, 0) > 0) { + if (px4_exec(exec_argv[0], exec_argv, nullptr, 0) > 0) { usb_auto_start_state = UsbAutoStartState::connected; } else { @@ -342,9 +371,8 @@ static void mavlink_usb_check(void *arg) case UsbAutoStartState::connected: if (!vbus_present && !vbus_present_prev) { sched_lock(); - static const char app[] {"mavlink"}; static const char *stop_argv[] {"mavlink", "stop", "-d", USB_DEVICE_PATH, NULL}; - exec_builtin(app, (char **)stop_argv, NULL, 0); + px4_exec(stop_argv[0], (char **)stop_argv, NULL, 0); sched_unlock(); usb_auto_start_state = UsbAutoStartState::disconnecting; @@ -354,7 +382,7 @@ static void mavlink_usb_check(void *arg) case UsbAutoStartState::disconnecting: // serial disconnect if unused - serdis_main(0, NULL); + serial_disconnect(); usb_auto_start_state = UsbAutoStartState::disconnected; break; } @@ -367,9 +395,12 @@ static void mavlink_usb_check(void *arg) } } - void cdcacm_init(void) { +#ifdef CONFIG_BUILD_KERNEL + // Must start the worker as it is not automatically started by the system + work_usrstart(); +#endif work_queue(LPWORK, &usb_serial_work, mavlink_usb_check, nullptr, 0); } diff --git a/platforms/nuttx/src/px4/common/console_buffer.cpp b/platforms/nuttx/src/px4/common/console_buffer.cpp index c949421d2670..6a35a2bdd9c5 100644 --- a/platforms/nuttx/src/px4/common/console_buffer.cpp +++ b/platforms/nuttx/src/px4/common/console_buffer.cpp @@ -206,13 +206,57 @@ ssize_t console_buffer_write(struct file *filep, const char *buffer, size_t len) return len; } +ssize_t console_buffer_read(struct file *filep, char *buffer, size_t buflen) +{ + return g_console_buffer.read(buffer, buflen, (int *)&filep->f_pos); +} + +off_t console_buffer_seek(struct file *filep, off_t offset, int whence) +{ + switch (whence) { + case SEEK_SET: + filep->f_pos = offset; + break; + + case SEEK_CUR: + filep->f_pos += offset; + break; + + case SEEK_END: + filep->f_pos = (g_console_buffer.size() + offset) % BOARD_CONSOLE_BUFFER_SIZE; + break; + + default: + return -1; + } + + return filep->f_pos; +} + +int console_buffer_ioctl(struct file *filep, int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case FIONSPACE: + *(int *)arg = g_console_buffer.size(); + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + static const struct file_operations g_console_buffer_fops = { NULL, /* open */ NULL, /* close */ - NULL, /* read */ + console_buffer_read, /* read */ console_buffer_write, /* write */ - NULL, /* seek */ - NULL /* ioctl */ + console_buffer_seek, /* seek */ + console_buffer_ioctl /* ioctl */ #ifndef CONFIG_DISABLE_POLL , NULL /* poll */ #endif diff --git a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp index 83023f934d0e..643fbdcf7d2e 100644 --- a/platforms/nuttx/src/px4/common/console_buffer_usr.cpp +++ b/platforms/nuttx/src/px4/common/console_buffer_usr.cpp @@ -38,28 +38,56 @@ #include #include #include +#include +#include #ifdef BOARD_ENABLE_CONSOLE_BUFFER -#ifndef BOARD_CONSOLE_BUFFER_SIZE -# define BOARD_CONSOLE_BUFFER_SIZE (1024*4) // default buffer size -#endif - -// TODO: User side implementation of px4_console_buffer +static int console_fd = -1; int px4_console_buffer_init() { - return 0; + console_fd = open(CONSOLE_BUFFER_DEVICE, O_RDWR); + + if (console_fd < 0) { + return ERROR; + } + + return OK; } int px4_console_buffer_size() { - return 0; + int size; + + if (ioctl(console_fd, FIONSPACE, &size) < 0) { + return 0; + } + + return size; } int px4_console_buffer_read(char *buffer, int buffer_length, int *offset) { - return 0; + FILE *fp; + ssize_t nread; + + /* Open a file stream to keep track of offset */ + fp = fdopen(dup(console_fd), "r"); + + if (fp == NULL) { + return -1; + } + + /* The driver does not utilize file position, we have to do it for it */ + fseek(fp, *offset, SEEK_SET); + nread = read(console_fd, buffer, buffer_length); + *offset = fseek(fp, 0, SEEK_CUR); + + /* Now we can close the file */ + fclose(fp); + + return (int)nread; } #endif /* BOARD_ENABLE_CONSOLE_BUFFER */ diff --git a/platforms/nuttx/src/px4/common/hrt_ioctl.c b/platforms/nuttx/src/px4/common/hrt_ioctl.c index e9b6e82efd81..4cba925a47a0 100644 --- a/platforms/nuttx/src/px4/common/hrt_ioctl.c +++ b/platforms/nuttx/src/px4/common/hrt_ioctl.c @@ -42,40 +42,151 @@ #include #include +#include +#include #ifndef MODULE_NAME # define MODULE_NAME "hrt_ioctl" #endif -#define HRT_ENTRY_QUEUE_MAX_SIZE 3 -static px4_sem_t g_wait_sem; -static struct hrt_call *next_hrt_entry[HRT_ENTRY_QUEUE_MAX_SIZE]; -static int hrt_entry_queued = 0; -static bool suppress_entry_queue_error = false; -static bool hrt_entry_queue_error = false; +struct usr_hrt_call { + struct sq_entry_s list_item; /* List head for local sl_list */ + struct hrt_call entry; /* Kernel side entry for HRT driver */ + struct hrt_call *usr_entry; /* Reference to user side entry */ +}; -void hrt_usr_call(void *arg) +static sq_queue_t callout_queue; +static sq_queue_t callout_freelist; +static sq_queue_t callout_inflight; + +/* Check if entry is in list */ + +static bool entry_inlist(sq_queue_t *queue, sq_entry_t *item) { - // This is called from hrt interrupt - if (hrt_entry_queued < HRT_ENTRY_QUEUE_MAX_SIZE) { - next_hrt_entry[hrt_entry_queued++] = (struct hrt_call *)arg; + sq_entry_t *queued; + + sq_for_every(queue, queued) { + if (queued == item) { + return true; + } + } + + return false; +} + +/* Find (pop) first entry for user from queue, the queue must be locked prior */ + +static struct usr_hrt_call *pop_user(sq_queue_t *queue, const px4_hrt_handle_t handle) +{ + sq_entry_t *queued; + + sq_for_every(queue, queued) { + struct usr_hrt_call *e = (void *)queued; + + if (e->entry.callout_sem == handle) { + sq_rem(queued, queue); + return e; + } + } + + return NULL; +} + +/* Find (pop) entry from queue, the queue must be locked prior */ + +static struct usr_hrt_call *pop_entry(sq_queue_t *queue, const px4_hrt_handle_t handle, struct hrt_call *entry) +{ + sq_entry_t *queued; + + sq_for_every(queue, queued) { + struct usr_hrt_call *e = (void *)queued; + + if (e->usr_entry == entry && e->entry.callout_sem == handle) { + sq_rem(queued, queue); + return e; + } + } + + return NULL; +} + +/** + * Copy user entry to kernel space. Either re-uses existing one or if none can + * be found, creates a new. + * + * handle : user space handle to identify who is behind the HRT request + * entry : user space HRT entry + * callout : user callback + * arg : user argument passed in callback + */ +static struct usr_hrt_call *dup_entry(const px4_hrt_handle_t handle, struct hrt_call *entry, hrt_callout callout, + void *arg) +{ + struct usr_hrt_call *e = NULL; + + irqstate_t flags = px4_enter_critical_section(); + + /* check if this is already queued */ + e = pop_entry(&callout_queue, handle, entry); + + /* it was not already queued, get from freelist */ + if (!e) { + e = (void *)sq_remfirst(&callout_freelist); + } + + px4_leave_critical_section(flags); + + if (!e) { + /* Allocate a new kernel side item for the user call */ + + e = kmm_malloc(sizeof(struct usr_hrt_call)); + } + + if (e) { + + /* Store the user side callout function and argument to the user's handle */ + entry->callout = callout; + entry->arg = arg; + + /* Store reference to the kernel side entry to the user side struct and + * references to the semaphore and user side entry to the kernel side item + */ + + e->entry.callout_sem = handle; + e->usr_entry = entry; + + /* Add this to the callout_queue list */ + flags = px4_enter_critical_section(); + sq_addfirst(&e->list_item, &callout_queue); + px4_leave_critical_section(flags); } else { - hrt_entry_queue_error = true; + PX4_ERR("out of memory"); + } - px4_sem_post(&g_wait_sem); + return e; +} + +void hrt_usr_call(void *arg) +{ + // This is called from hrt interrupt + struct usr_hrt_call *e = (struct usr_hrt_call *)arg; + + // Make sure the event is not already in flight + if (!entry_inlist(&callout_inflight, (sq_entry_t *)e)) { + sq_addfirst(&e->list_item, &callout_inflight); + px4_sem_post(e->entry.callout_sem); + } } int hrt_ioctl(unsigned int cmd, unsigned long arg); void hrt_ioctl_init(void) { - /* Create a semaphore for handling hrt driver callbacks */ - px4_sem_init(&g_wait_sem, 0, 0); - - /* this is a signalling semaphore */ - px4_sem_setprotocol(&g_wait_sem, SEM_PRIO_NONE); + sq_init(&callout_queue); + sq_init(&callout_freelist); + sq_init(&callout_inflight); /* register ioctl callbacks */ px4_register_boardct_ioctl(_HRTIOCBASE, hrt_ioctl); @@ -105,26 +216,32 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) switch (cmd) { case HRT_WAITEVENT: { irqstate_t flags; - px4_sem_wait(&g_wait_sem); + struct hrt_boardctl *ioc_parm = (struct hrt_boardctl *)arg; + px4_sem_t *callout_sem = (px4_sem_t *)ioc_parm->handle; + struct usr_hrt_call *e; + px4_sem_wait(callout_sem); + /* Atomically update the pointer to user side hrt entry */ flags = px4_enter_critical_section(); + e = pop_user(&callout_inflight, callout_sem); + + if (e) { + ioc_parm->callout = e->usr_entry->callout; + ioc_parm->arg = e->usr_entry->arg; + + // If the period is 0, the callout is no longer queued by hrt driver + // move it back to freelist + if (e->entry.period == 0) { + sq_rem((sq_entry_t *)e, &callout_queue); + sq_addfirst((sq_entry_t *)e, &callout_freelist); - /* This should be always true, but check it anyway */ - if (hrt_entry_queued > 0) { - *(struct hrt_call **)arg = next_hrt_entry[--hrt_entry_queued]; - next_hrt_entry[hrt_entry_queued] = NULL; + } } else { - hrt_entry_queue_error = true; + PX4_ERR("HRT_WAITEVENT error no entry"); } px4_leave_critical_section(flags); - - /* Warn once for entry queue being full */ - if (hrt_entry_queue_error && !suppress_entry_queue_error) { - PX4_ERR("HRT entry error, queue size now %d", hrt_entry_queued); - suppress_entry_queue_error = true; - } } break; @@ -132,21 +249,46 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) *(hrt_abstime *)arg = hrt_absolute_time(); break; - case HRT_CALL_AFTER: - hrt_call_after(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_AFTER: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_after(&e->entry, h->time, (hrt_callout)hrt_usr_call, e); + } + } break; - case HRT_CALL_AT: - hrt_call_at(h->entry, h->time, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_AT: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_at(&e->entry, h->time, (hrt_callout)hrt_usr_call, e); + } + } break; - case HRT_CALL_EVERY: - hrt_call_every(h->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, h->entry); + case HRT_CALL_EVERY: { + struct usr_hrt_call *e = dup_entry(h->handle, h->entry, h->callout, h->arg); + + if (e) { + hrt_call_every(&e->entry, h->time, h->interval, (hrt_callout)hrt_usr_call, e); + } + } break; case HRT_CANCEL: if (h && h->entry) { - hrt_cancel(h->entry); + /* Find the user entry */ + irqstate_t flags = px4_enter_critical_section(); + struct usr_hrt_call *e = pop_entry(&callout_queue, h->handle, h->entry); + + if (e) { + hrt_cancel(&e->entry); + sq_addfirst((sq_entry_t *)e, &callout_freelist); + + } + + px4_leave_critical_section(flags); } else { PX4_ERR("HRT_CANCEL called with NULL entry"); @@ -164,6 +306,59 @@ hrt_ioctl(unsigned int cmd, unsigned long arg) reset_latency_counters(); break; + case HRT_REGISTER: { + px4_sem_t *callback_sem = kmm_malloc(sizeof(px4_sem_t)); + + /* Create a semaphore for handling hrt driver callbacks */ + if (px4_sem_init(callback_sem, 0, 0) == 0) { + + /* this is a signalling semaphore */ + px4_sem_setprotocol(callback_sem, SEM_PRIO_NONE); + *(px4_sem_t **)arg = callback_sem; + + } else { + *(px4_sem_t **)arg = NULL; + return -ENOMEM; + } + + } + + break; + + case HRT_UNREGISTER: { + px4_sem_t *callback_sem = *(px4_sem_t **)arg; + struct usr_hrt_call *e; + irqstate_t flags; + + flags = px4_enter_critical_section(); + + while ((e = (void *)sq_remfirst(&callout_queue))) { + if (callback_sem == e->entry.callout_sem) { + hrt_cancel(&e->entry); + /* Remove potential inflight entry as well */ + sq_rem(&e->list_item, &callout_inflight); + kmm_free(e); + } + } + + px4_sem_destroy(callback_sem); + + px4_leave_critical_section(flags); + + *(px4_sem_t **)arg = NULL; + kmm_free(callback_sem); + } + break; + + case HRT_ABSTIME_BASE: { +#ifdef PX4_USERSPACE_HRT + *(uintptr_t *)arg = hrt_absolute_time_usr_base(); +#else + *(uintptr_t *)arg = NULL; +#endif + } + break; + default: return -EINVAL; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h b/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h new file mode 100644 index 000000000000..f95d08ed858c --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/atomic_block.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include + +namespace px4 +{ + +class atomic_block +{ +public: + atomic_block(); + ~atomic_block(); + void start(); + void finish(); +private: + union { + px4_sem_t _lock; + irqstate_t _irqlock; + }; +}; + +} diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h index 9e1654451c55..b85da67e0744 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h @@ -53,7 +53,8 @@ #define _CRYPTOIOCBASE IOCTL_IDX_TO_BASE(2) #define _PARAMIOCBASE IOCTL_IDX_TO_BASE(3) #define _PLATFORMIOCBASE IOCTL_IDX_TO_BASE(4) -#define MAX_IOCTL_PTRS 5 +#define _EVENTSIOCBASE IOCTL_IDX_TO_BASE(5) +#define MAX_IOCTL_PTRS 6 /* The PLATFORMIOCLAUNCH IOCTL is used to launch kernel side modules * from the user side code diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/task.h b/platforms/nuttx/src/px4/common/include/px4_platform/task.h new file mode 100644 index 000000000000..f41fc96a998e --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/px4_platform/task.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * Copyright (C) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include + +__BEGIN_DECLS + +#ifdef CONFIG_BUILD_KERNEL +int task_create(FAR const char *name, int priority, + int stack_size, main_t entry, FAR char *const argv[]); +int task_delete(int pid); +#endif + +__END_DECLS diff --git a/platforms/nuttx/src/px4/common/main.cpp b/platforms/nuttx/src/px4/common/main.cpp new file mode 100644 index 000000000000..6b8d87764dd3 --- /dev/null +++ b/platforms/nuttx/src/px4/common/main.cpp @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +extern "C" +{ + void px4_userspace_init(void); + int entry(int argc, char *argv[]); +} + +int main(int argc, char *argv[]) +{ + px4_userspace_init(); + return entry(argc, argv); +} diff --git a/platforms/nuttx/src/px4/common/nuttx_random.c b/platforms/nuttx/src/px4/common/nuttx_random.c index 1339aa916904..e0ef28a6f527 100644 --- a/platforms/nuttx/src/px4/common/nuttx_random.c +++ b/platforms/nuttx/src/px4/common/nuttx_random.c @@ -33,12 +33,14 @@ #include -#if defined(CONFIG_CRYPTO_RANDOM_POOL) size_t px4_get_secure_random(uint8_t *out, size_t outlen) { +#if defined(CONFIG_CRYPTO_RANDOM_POOL) /* TODO: can getrandom fail?? */ arc4random_buf(out, outlen); return outlen; -} +#else + return 0; #endif +} diff --git a/platforms/nuttx/src/px4/common/print_load.cpp b/platforms/nuttx/src/px4/common/print_load.cpp index 8c6486453ade..230b23acd59c 100644 --- a/platforms/nuttx/src/px4/common/print_load.cpp +++ b/platforms/nuttx/src/px4/common/print_load.cpp @@ -202,14 +202,14 @@ void print_load_buffer(char *buffer, int buffer_length, print_load_callback_f cb if (system_load.tasks[i].tcb->pid == 0) { stack_size = (CONFIG_ARCH_INTERRUPTSTACK & ~3); - stack_free = up_check_intstack_remain(); + stack_free = (CONFIG_ARCH_INTERRUPTSTACK & ~3) - up_check_intstack(); } else { - stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb); + stack_free = system_load.tasks[i].tcb->adj_stack_size - up_check_tcbstack(system_load.tasks[i].tcb); } #else - stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb); + stack_free = system_load.tasks[i].tcb->adj_stack_size - up_check_tcbstack(system_load.tasks[i].tcb); #endif #if CONFIG_ARCH_BOARD_SIM || !defined(CONFIG_PRIORITY_INHERITANCE) diff --git a/platforms/nuttx/src/px4/common/process b/platforms/nuttx/src/px4/common/process new file mode 160000 index 000000000000..8203ab425ce8 --- /dev/null +++ b/platforms/nuttx/src/px4/common/process @@ -0,0 +1 @@ +Subproject commit 8203ab425ce8c2855a1be2800f2ca9a72c258ceb diff --git a/platforms/nuttx/src/px4/common/progmem_dump.c b/platforms/nuttx/src/px4/common/progmem_dump.c index e37a6dd697e6..f360f9b68719 100644 --- a/platforms/nuttx/src/px4/common/progmem_dump.c +++ b/platforms/nuttx/src/px4/common/progmem_dump.c @@ -51,7 +51,7 @@ #include #include -#include +#include #ifdef CONFIG_BOARD_CRASHDUMP diff --git a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c index 65e5794a962c..3617a802bd13 100644 --- a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c +++ b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c @@ -71,6 +71,7 @@ #include #include +#include /************************************************************************************ * Pre-processor Definitions @@ -155,6 +156,9 @@ # define CONFIG_AT24XX_WRITE_TIMEOUT_MS 20 #endif +/* Length of the CRC code of each block, in bytes */ +#define CRC_LEN 2 + /************************************************************************************ * Private Types ************************************************************************************/ @@ -220,10 +224,14 @@ static int at24c_eraseall(FAR struct at24c_dev_s *priv) memset(&buf[2], 0xff, priv->pagesize); + uint16_t crc = crc16_signature(0xffff, priv->pagesize, &buf[2]); + buf[2 + priv->pagesize] = crc & 0xff; + buf[2 + priv->pagesize + 1] = crc >> 8; + BOARD_EEPROM_WP_CTRL(false); for (startblock = 0; startblock < priv->npages; startblock++) { - uint16_t offset = startblock * priv->pagesize; + uint16_t offset = startblock * (priv->pagesize + CRC_LEN); buf[1] = offset & 0xff; buf[0] = (offset >> 8) & 0xff; @@ -288,7 +296,10 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, FAR struct at24c_dev_s *priv = (FAR struct at24c_dev_s *)dev; size_t blocksleft; uint8_t addr[2]; + uint8_t buf[AT24XX_PAGESIZE]; int ret; + FAR uint8_t *buffer_start = buffer; + bool crc_error = false; struct i2c_msg_s msgv[2] = { { @@ -302,8 +313,8 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, .frequency = 400000, .addr = priv->addr, .flags = I2C_M_READ, - .buffer = 0, - .length = priv->pagesize, + .buffer = buf, + .length = priv->pagesize + CRC_LEN, } }; @@ -324,21 +335,27 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, } while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; + uint16_t offset = startblock * (priv->pagesize + CRC_LEN); unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; - addr[1] = offset & 0xff; addr[0] = (offset >> 8) & 0xff; - msgv[1].buffer = buffer; for (;;) { - perf_begin(priv->perf_transfers); ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); perf_end(priv->perf_transfers); if (ret >= 0) { - break; + uint16_t crc_expected = ((uint16_t)buf[priv->pagesize + 1]) << 8 | buf[priv->pagesize]; + + if (crc_expected == crc16_signature(0xffff, priv->pagesize, buf)) { + memcpy(buffer, buf, priv->pagesize); + break; + + } else { + crc_error = true; + ferr("read error, retrying"); + } } finfo("read stall"); @@ -354,6 +371,15 @@ static ssize_t at24c_bread(FAR struct mtd_dev_s *dev, off_t startblock, if (--tries == 0) { perf_count(priv->perf_errors); + memset(buffer_start, 0xff, priv->pagesize * nblocks); + + if (crc_error) { + // The data in this block is permanently corrupt. Try to reset the block so that + // it becomes usable again + ferr("CRC error, resetting block %jd\n", (intmax_t)startblock); + at24c_bwrite(dev, startblock, 1, buffer); + } + return ERROR; } } @@ -413,13 +439,17 @@ static ssize_t at24c_bwrite(FAR struct mtd_dev_s *dev, off_t startblock, size_t BOARD_EEPROM_WP_CTRL(false); while (blocksleft-- > 0) { - uint16_t offset = startblock * priv->pagesize; + uint16_t offset = startblock * (priv->pagesize + CRC_LEN); unsigned tries = CONFIG_AT24XX_WRITE_TIMEOUT_MS; buf[1] = offset & 0xff; buf[0] = (offset >> 8) & 0xff; memcpy(&buf[2], buffer, priv->pagesize); + uint16_t crc = crc16_signature(0xffff, priv->pagesize, &buf[2]); + buf[2 + priv->pagesize] = crc & 0xff; + buf[2 + priv->pagesize + 1] = crc >> 8; + for (;;) { perf_begin(priv->perf_transfers); @@ -494,8 +524,8 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) */ #if CONFIG_AT24XX_MTD_BLOCKSIZE > AT24XX_PAGESIZE - geo->blocksize = CONFIG_AT24XX_MTD_BLOCKSIZE; - geo->erasesize = CONFIG_AT24XX_MTD_BLOCKSIZE; + geo->blocksize = priv->pagesize * (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); + geo->erasesize = priv->pagesize * (CONFIG_AT24XX_MTD_BLOCKSIZE / AT24XX_PAGESIZE); geo->neraseblocks = (CONFIG_AT24XX_SIZE * 1024 / 8) / CONFIG_AT24XX_MTD_BLOCKSIZE; #else geo->blocksize = priv->pagesize; @@ -559,7 +589,7 @@ int px4_at24c_initialize(FAR struct i2c_master_s *dev, if (priv) { /* Initialize the allocated structure */ priv->addr = address; - priv->pagesize = AT24XX_PAGESIZE; + priv->pagesize = AT24XX_PAGESIZE - CRC_LEN; priv->npages = AT24XX_NPAGES; priv->mtd.erase = at24c_erase; diff --git a/platforms/nuttx/src/px4/common/px4_atomic.cpp b/platforms/nuttx/src/px4/common/px4_atomic.cpp new file mode 100644 index 000000000000..819dbc6ecc8b --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_atomic.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 + +namespace px4 +{ + +atomic_block::atomic_block(void) +{ + _irqlock = 0; +} + +atomic_block::~atomic_block(void) +{ + +} + +void atomic_block::start(void) +{ + _irqlock = enter_critical_section(); +} + +void atomic_block::finish(void) +{ + leave_critical_section(_irqlock); +} + +} diff --git a/platforms/nuttx/src/px4/common/px4_crypto.cpp b/platforms/nuttx/src/px4/common/px4_crypto.cpp index b14c5782b669..6cafd9b250f3 100644 --- a/platforms/nuttx/src/px4/common/px4_crypto.cpp +++ b/platforms/nuttx/src/px4/common/px4_crypto.cpp @@ -117,33 +117,59 @@ void PX4Crypto::close() unlock(); } +bool PX4Crypto::signature_check(uint8_t key_index, + const uint8_t *signature, + const uint8_t *message, + size_t message_size) +{ + return crypto_signature_check(_crypto_handle, key_index, signature, message, message_size); +} + bool PX4Crypto::encrypt_data(uint8_t key_index, const uint8_t *message, size_t message_size, uint8_t *cipher, - size_t *cipher_size) + size_t *cipher_size, + uint8_t *mac, + size_t *mac_size) { - return crypto_encrypt_data(_crypto_handle, key_index, message, message_size, cipher, cipher_size); + return crypto_encrypt_data(_crypto_handle, key_index, message, message_size, cipher, cipher_size, mac, mac_size); } -bool PX4Crypto::generate_key(uint8_t idx, - bool persistent) +bool PX4Crypto::decrypt_data(uint8_t key_index, + const uint8_t *cipher, + size_t cipher_size, + const uint8_t *mac, + size_t mac_size, + uint8_t *message, + size_t *message_size) +{ + return crypto_decrypt_data(_crypto_handle, key_index, cipher, cipher_size, mac, mac_size, message, + message_size); +} + +bool PX4Crypto::generate_key(uint8_t idx, + bool persistent) { return crypto_generate_key(_crypto_handle, idx, persistent); } +bool PX4Crypto::renew_nonce(const uint8_t *nonce, + size_t nonce_size) +{ + return crypto_renew_nonce(_crypto_handle, nonce, nonce_size); +} -bool PX4Crypto::get_nonce(uint8_t *nonce, - size_t *nonce_len) +bool PX4Crypto::get_nonce(uint8_t *nonce, + size_t *nonce_len) { return crypto_get_nonce(_crypto_handle, nonce, nonce_len); } - -bool PX4Crypto::get_encrypted_key(uint8_t key_idx, - uint8_t *key, - size_t *key_len, - uint8_t encryption_key_idx) +bool PX4Crypto::get_encrypted_key(uint8_t key_idx, + uint8_t *key, + size_t *key_len, + uint8_t encryption_key_idx) { return crypto_get_encrypted_key(_crypto_handle, key_idx, key, key_len, encryption_key_idx); } @@ -173,7 +199,7 @@ int PX4Crypto::crypto_ioctl(unsigned int cmd, unsigned long arg) case CRYPTOIOCENCRYPT: { cryptoiocencrypt_t *data = (cryptoiocencrypt_t *)arg; data->ret = crypto_encrypt_data(*(data->handle), data->key_index, data->message, data->message_size, data->cipher, - data->cipher_size); + data->cipher_size, data->mac, data->mac_size); } break; @@ -183,6 +209,12 @@ int PX4Crypto::crypto_ioctl(unsigned int cmd, unsigned long arg) } break; + case CRYPTOIOCRENEWNONCE: { + cryptoiocrenewnonce_t *data = (cryptoiocrenewnonce_t *)arg; + data->ret = crypto_renew_nonce(*(data->handle), data->nonce, data->nonce_size); + } + break; + case CRYPTOIOCGETNONCE: { cryptoiocgetnonce_t *data = (cryptoiocgetnonce_t *)arg; data->ret = crypto_get_nonce(*(data->handle), data->nonce, data->nonce_len); @@ -197,12 +229,26 @@ int PX4Crypto::crypto_ioctl(unsigned int cmd, unsigned long arg) } break; + case CRYPTOIOCSIGNATURECHECK: { + cryptoiocsignaturecheck_t *data = (cryptoiocsignaturecheck_t *)arg; + data->ret = crypto_signature_check(*(data->handle), data->key_index, data->signature, data->message, + data->message_size); + } + break; + case CRYPTOIOCGETBLOCKSZ: { cryptoiocgetblocksz_t *data = (cryptoiocgetblocksz_t *)arg; data->ret = crypto_get_min_blocksize(*(data->handle), data->key_idx); } break; + case CRYPTOIOCDECRYPTDATA: { + cryptoiocdecryptdata_t *data = (cryptoiocdecryptdata_t *)arg; + data->ret = crypto_decrypt_data(*(data->handle), data->key_index, data->cipher, data->cipher_size, + data->mac, data->mac_size, data->message, data->message_size); + } + break; + default: ret = PX4_ERROR; break; diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093a9..ae518dca20bc 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -61,14 +62,21 @@ #include #endif +#ifndef I2C_RESET_SPEED +#define I2C_RESET_SPEED I2C_SPEED_STANDARD +#endif + extern void cdcacm_init(void); #if !defined(CONFIG_BUILD_FLAT) typedef CODE void (*initializer_t)(void); -extern initializer_t _sinit; -extern initializer_t _einit; -extern uint32_t _stext; -extern uint32_t _etext; +extern initializer_t _sinit[]; +extern initializer_t _einit[]; +extern uint8_t _stext[]; +extern uint8_t _etext[]; + +extern FAR void *__dso_handle weak_data; +FAR void *__dso_handle = &__dso_handle; static void cxx_initialize(void) { @@ -76,7 +84,7 @@ static void cxx_initialize(void) /* Visit each entry in the initialization table */ - for (initp = &_sinit; initp != &_einit; initp++) { + for (initp = _sinit; initp != _einit; initp++) { initializer_t initializer = *initp; /* Make sure that the address is non-NULL and lies in the text @@ -84,12 +92,18 @@ static void cxx_initialize(void) * NULL values or counts in the initialization table. */ - if ((FAR void *)initializer >= (FAR void *)&_stext && - (FAR void *)initializer < (FAR void *)&_etext) { + if ((FAR void *)initializer >= (FAR void *)_stext && + (FAR void *)initializer < (FAR void *)_etext) { initializer(); } } } + +int __cxa_atexit(CODE void (*func)(FAR void *), FAR void *arg, + FAR void *dso_handle) +{ + return -ENOTSUP; +} #endif int px4_platform_init() @@ -125,9 +139,12 @@ int px4_platform_init() #if !defined(CONFIG_BUILD_FLAT) hrt_ioctl_init(); + events_ioctl_init(); #endif +#ifdef CONFIG_SYSTEMCMDS_PARAM param_init(); +#endif /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION @@ -150,7 +167,7 @@ int px4_platform_init() buf[0] = 0x06; // software reset i2c_msg_s msg{}; - msg.frequency = I2C_SPEED_STANDARD; + msg.frequency = I2C_RESET_SPEED; msg.addr = 0x00; // general call address msg.buffer = &buf[0]; msg.length = 1; @@ -165,7 +182,7 @@ int px4_platform_init() #if defined(CONFIG_FS_PROCFS) int ret_mount_procfs = mount(nullptr, "/proc", "procfs", 0, nullptr); - if (ret < 0) { + if (ret_mount_procfs < 0) { syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret_mount_procfs); } diff --git a/platforms/nuttx/src/px4/common/px4_kmmap.c b/platforms/nuttx/src/px4/common/px4_kmmap.c new file mode 100644 index 000000000000..8715a2bf01b1 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_kmmap.c @@ -0,0 +1,99 @@ +/**************************************************************************** + * px4_kmmap.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +#include + +#include +#include +#include + +#include +#include +#include + +/* This is naughty, but only option as these are in NuttX private headers */ + +extern int inode_lock(void); +extern void inode_unlock(void); + +struct shmfs_object_s { + size_t length; + void *paddr; +}; + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset) +{ + struct file *filep; + struct shmfs_object_s *object; + void **pages; + void *vaddr; + unsigned int npages; + int ret; + + if (fs_getfilep(fd, &filep) < 0) { + ret = -EBADF; + goto errout; + } + + ret = inode_lock(); + + if (ret < 0) { + goto errout; + } + + /* Return the physical address */ + + object = (struct shmfs_object_s *)filep->f_inode->i_private; + + if (!object) { + ret = -EINVAL; + goto errout_with_lock; + } + + /* Map the object to kernel */ + + pages = &object->paddr; + npages = MM_NPAGES(object->length); + + /* Do the mapping */ + + vaddr = kmm_map(pages, npages, PROT_READ | PROT_WRITE); + + if (!vaddr) { + ret = -ENOMEM; + goto errout_with_lock; + } + + filep->f_inode->i_crefs++; + inode_unlock(); + return vaddr; + +errout_with_lock: + inode_unlock(); +errout: + set_errno(-ret); + return MAP_FAILED; +} + +int px4_munmap(void *start, size_t length) +{ + kmm_unmap(start); + return OK; +} diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7f76502e6842..7f54024709d6 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -11,15 +11,16 @@ add_library(px4_layer ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp px4_userspace_init.cpp px4_usr_crypto.cpp - px4_mtd.cpp + usr_atomic.cpp usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp ) target_link_libraries(px4_layer + PUBLIC + board_bus_info PRIVATE - m nuttx_c nuttx_xx nuttx_mm @@ -35,11 +36,6 @@ add_library(px4_board_ctrl add_dependencies(px4_board_ctrl nuttx_context px4_kernel_builtin_list_target) target_compile_options(px4_board_ctrl PRIVATE -D__KERNEL__) -target_link_libraries(px4_layer - PUBLIC - board_bus_info -) - # Build the kernel side px4_kernel_layer add_library(px4_kernel_layer @@ -47,16 +43,17 @@ add_library(px4_kernel_layer ) target_link_libraries(px4_kernel_layer + PUBLIC + px4_board_ctrl + board_bus_info PRIVATE ${KERNEL_LIBS} nuttx_kc + nuttx_kxx nuttx_karch nuttx_kmm -) - -target_link_libraries(px4_kernel_layer - PUBLIC - board_bus_info + PRIVATE + kernel_events_interface # events_ioctl_init ) if (DEFINED PX4_CRYPTO) @@ -66,4 +63,8 @@ endif() add_dependencies(px4_kernel_layer prebuild_targets) target_compile_options(px4_kernel_layer PRIVATE -D__KERNEL__) -target_link_libraries(px4_kernel_layer PUBLIC px4_board_ctrl) + +if (CONFIG_BUILD_KERNEL) + target_sources(px4_layer PRIVATE usr_mmap.c process/task.c) + target_sources(px4_kernel_layer PRIVATE px4_kmmap.c) +endif() diff --git a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp index f5d9882ca23a..a2a339524467 100644 --- a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp @@ -38,11 +38,11 @@ */ #include -#include +#include #include #include -#include -#include +#include +#include extern void cdcacm_init(void); @@ -52,11 +52,19 @@ extern "C" void px4_userspace_init(void) px4_set_spi_buses_from_hw_version(); + board_determine_hw_info(); + + // Do lazy init of wq:manager for kernel. This saves a considerable amount + // of scheduling time due to unnecessary threads. +#ifndef CONFIG_BUILD_KERNEL px4::WorkQueueManagerStart(); +#endif + + px4_log_initialize(); - uorb_start(); + px4_console_buffer_init(); -#if defined(CONFIG_SYSTEM_CDCACM) +#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_PROTECTED) cdcacm_init(); #endif } diff --git a/platforms/nuttx/src/px4/common/px4_usr_crypto.cpp b/platforms/nuttx/src/px4/common/px4_usr_crypto.cpp index 4a12a3068367..22f4bb113df5 100644 --- a/platforms/nuttx/src/px4/common/px4_usr_crypto.cpp +++ b/platforms/nuttx/src/px4/common/px4_usr_crypto.cpp @@ -77,37 +77,70 @@ void PX4Crypto::close() boardctl(CRYPTOIOCCLOSE, reinterpret_cast(&_crypto_handle)); } +bool PX4Crypto::signature_check(uint8_t key_index, + const uint8_t *signature, + const uint8_t *message, + size_t message_size) +{ + cryptoiocsignaturecheck_t data = {&_crypto_handle, key_index, signature, message, message_size, false}; + boardctl(CRYPTOIOCSIGNATURECHECK, reinterpret_cast(&data)); + return data.ret; +} + bool PX4Crypto::encrypt_data(uint8_t key_index, const uint8_t *message, size_t message_size, uint8_t *cipher, - size_t *cipher_size) + size_t *cipher_size, + uint8_t *mac, + size_t *mac_size) { - cryptoiocencrypt_t data = {&_crypto_handle, key_index, message, message_size, cipher, cipher_size, false}; + cryptoiocencrypt_t data = {&_crypto_handle, key_index, message, message_size, cipher, cipher_size, mac, mac_size, false}; boardctl(CRYPTOIOCENCRYPT, reinterpret_cast(&data)); return data.ret; } -bool PX4Crypto::generate_key(uint8_t idx, - bool persistent) +bool PX4Crypto::decrypt_data(uint8_t key_index, + const uint8_t *cipher, + size_t cipher_size, + const uint8_t *mac, + size_t mac_size, + uint8_t *message, + size_t *message_size) +{ + cryptoiocdecryptdata_t data = {&_crypto_handle, key_index, cipher, cipher_size, mac, mac_size, message, message_size, false}; + boardctl(CRYPTOIOCDECRYPTDATA, reinterpret_cast(&data)); + return data.ret; +} + +bool PX4Crypto::generate_key(uint8_t idx, + bool persistent) { cryptoiocgenkey_t data = {&_crypto_handle, idx, persistent, false}; boardctl(CRYPTOIOCGENKEY, reinterpret_cast(&data)); return data.ret; } -bool PX4Crypto::get_nonce(uint8_t *nonce, - size_t *nonce_len) +bool PX4Crypto::renew_nonce(const uint8_t *nonce, + size_t nonce_size) +{ + cryptoiocrenewnonce_t data = {&_crypto_handle, nonce, nonce_size, false}; + boardctl(CRYPTOIOCRENEWNONCE, reinterpret_cast(&data)); + return data.ret; +} + +bool PX4Crypto::get_nonce(uint8_t *nonce, + size_t *nonce_len) { cryptoiocgetnonce_t data = {&_crypto_handle, nonce, nonce_len, false}; boardctl(CRYPTOIOCGETNONCE, reinterpret_cast(&data)); return data.ret; } -bool PX4Crypto::get_encrypted_key(uint8_t key_idx, - uint8_t *key, - size_t *key_len, - uint8_t encryption_key_idx) +bool PX4Crypto::get_encrypted_key(uint8_t key_idx, + uint8_t *key, + size_t *key_len, + uint8_t encryption_key_idx) { cryptoiocgetkey_t data = {&_crypto_handle, key_idx, key, key_len, encryption_key_idx, false}; boardctl(CRYPTOIOCGETKEY, reinterpret_cast(&data)); @@ -121,4 +154,7 @@ size_t PX4Crypto::get_min_blocksize(uint8_t key_idx) return data.ret; } + + + #endif diff --git a/platforms/nuttx/src/px4/common/tasks.cpp b/platforms/nuttx/src/px4/common/tasks.cpp index 9ad860dc182f..7d3b65f84ba5 100644 --- a/platforms/nuttx/src/px4/common/tasks.cpp +++ b/platforms/nuttx/src/px4/common/tasks.cpp @@ -41,6 +41,9 @@ #include #include +#include + +#include #include #include @@ -54,12 +57,16 @@ #include #include #include +#include +#include + +#include int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char *const argv[]) { sched_lock(); -#if !defined(CONFIG_DISABLE_ENVIRON) +#if !defined(CONFIG_DISABLE_ENVIRON) && !defined(CONFIG_BUILD_KERNEL) /* None of the modules access the environment variables (via getenv() for instance), so delete them * all. They are only used within the startup script, and NuttX automatically exports them to the children * tasks. @@ -88,16 +95,79 @@ int px4_task_spawn_cmd(const char *name, int scheduler, int priority, int stack_ int px4_task_delete(int pid) { +#if !defined(__KERNEL__) return task_delete(pid); +#else + return kthread_delete(pid); +#endif } const char *px4_get_taskname(void) { -#if CONFIG_TASK_NAME_SIZE > 0 && (defined(__KERNEL__) || defined(CONFIG_BUILD_FLAT)) - FAR struct tcb_s *thisproc = nxsched_self(); + return getprogname(); +} - return thisproc->name; +int px4_exec(const char *appname, char *const *argv, const char *redirfile, int oflags) +{ +#ifdef CONFIG_BUILTIN + return exec_builtin(appname, argv, redirfile, oflags); #else - return "app"; + char path[CONFIG_PATH_MAX]; + posix_spawn_file_actions_t file_actions; + posix_spawnattr_t attr; + pid_t pid; + int ret; + + /* We launch processes from the /bin/ folder only */ + + sprintf(path, "/bin/"); + strcat(path, basename((char *)appname)); + + /* Initialize the attributes */ + + ret = posix_spawnattr_init(&attr); + + if (ret != 0) { + goto errout; + } + + /* Initialize the file actions structure */ + + ret = posix_spawn_file_actions_init(&file_actions); + + if (ret != 0) { + goto errout_with_attrs; + } + + /* Redirect output if instructed to do so */ + + if (redirfile) { + ret = posix_spawn_file_actions_addopen(&file_actions, 1, redirfile, oflags, 0644); + + if (ret != 0) { + goto errout_with_actions; + } + } + + /* Attempt to load the executable */ + + ret = posix_spawnp(&pid, path, &file_actions, &attr, argv, environ); + + if (ret != 0) { + goto errout_with_actions; + } + + posix_spawn_file_actions_destroy(&file_actions); + posix_spawnattr_destroy(&attr); + return pid; + +errout_with_actions: + posix_spawn_file_actions_destroy(&file_actions); + +errout_with_attrs: + posix_spawnattr_destroy(&attr); + +errout: + return ERROR; #endif } diff --git a/platforms/nuttx/src/px4/common/usr_atomic.cpp b/platforms/nuttx/src/px4/common/usr_atomic.cpp new file mode 100644 index 000000000000..b1221e4e1132 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_atomic.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 + +namespace px4 +{ + +atomic_block::atomic_block(void) +{ + px4_sem_init(&_lock, 0, 1); +} + +atomic_block::~atomic_block(void) +{ + px4_sem_destroy(&_lock); +} + +void atomic_block::start(void) +{ + do {} while (px4_sem_wait(&_lock) != 0); +} + +void atomic_block::finish(void) +{ + px4_sem_post(&_lock); +} + +} diff --git a/platforms/nuttx/src/px4/common/usr_hrt.cpp b/platforms/nuttx/src/px4/common/usr_hrt.cpp index d57712a32d45..2e2264bb972e 100644 --- a/platforms/nuttx/src/px4/common/usr_hrt.cpp +++ b/platforms/nuttx/src/px4/common/usr_hrt.cpp @@ -40,8 +40,13 @@ * */ +#ifndef MODULE_NAME +#define MODULE_NAME "usr_hrt" +#endif + #include #include +#include #include #include @@ -61,57 +66,101 @@ #include static px4_task_t g_usr_hrt_task = -1; +static px4_hrt_handle_t g_hrt_client_handle; +static px4_sem_t g_worker_lock; -/** - * Fetch a never-wrapping absolute time value in microseconds from - * some arbitrary epoch shortly after system start. - */ -hrt_abstime -hrt_absolute_time(void) -{ - hrt_abstime abstime = 0; - boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime); - return abstime; -} +#ifdef PX4_USERSPACE_HRT +static uintptr_t g_abstime_base; +#endif /** - * Store the absolute time in an interrupt-safe fashion + * Wrapper for atexit() */ -void -hrt_store_absolute_time(volatile hrt_abstime *t) +static void hrt_stop(void) { - irqstate_t flags = px4_enter_critical_section(); - *t = hrt_absolute_time(); - px4_leave_critical_section(flags); + if (g_usr_hrt_task >= 0) { + px4_task_delete(g_usr_hrt_task); + } + + px4_sem_destroy(&g_worker_lock); + boardctl(HRT_UNREGISTER, (uintptr_t)&g_hrt_client_handle); } /** * Event dispatcher thread */ -int -event_thread(int argc, char *argv[]) +static int event_thread(int argc, char *argv[]) { - struct hrt_call *entry = NULL; + struct hrt_boardctl ioc_parm { + .handle = g_hrt_client_handle, + .entry = nullptr, + .time = 0, + .interval = 0, + .callout = nullptr, + .arg = nullptr + }; while (1) { /* Wait for hrt tick */ - boardctl(HRT_WAITEVENT, (uintptr_t)&entry); + boardctl(HRT_WAITEVENT, (uintptr_t)&ioc_parm); /* HRT event received, dispatch */ - if (entry) { - entry->usr_callout(entry->usr_arg); + if (ioc_parm.callout) { + ioc_parm.callout(ioc_parm.arg); } } return 0; } +static void start_worker(void) +{ + if (g_usr_hrt_task >= 0) { + // Worker is already (for sure) running, get out + return; + } + + // Ensure only a single thread gets to create the worker, the rest will wait + px4_sem_wait(&g_worker_lock); + + if (g_usr_hrt_task < 0) { + g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, + PX4_STACK_ADJUSTED(1024), event_thread, NULL); + } + + px4_sem_post(&g_worker_lock); +} + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ +#ifndef PX4_USERSPACE_HRT + hrt_abstime abstime = 0; + boardctl(HRT_ABSOLUTE_TIME, (uintptr_t)&abstime); + return abstime; +#else + + if (g_abstime_base) { + return getreg64(g_abstime_base); + + } else { + PX4_ERR("g_abstime_base is NULL\n"); + return 0; + } + +#endif +} + /** * Request stop. */ bool hrt_request_stop() { - px4_task_delete(g_usr_hrt_task); + hrt_stop(); return true; } @@ -121,8 +170,15 @@ bool hrt_request_stop() void hrt_init(void) { - px4_register_shutdown_hook(hrt_request_stop); - g_usr_hrt_task = px4_task_spawn_cmd("usr_hrt", SCHED_DEFAULT, SCHED_PRIORITY_MAX, 1000, event_thread, NULL); + boardctl(HRT_REGISTER, (uintptr_t)&g_hrt_client_handle); +#ifdef PX4_USERSPACE_HRT + boardctl(HRT_ABSTIME_BASE, (uintptr_t)&g_abstime_base); +#endif + + if (g_hrt_client_handle) { + atexit(hrt_stop); + px4_sem_init(&g_worker_lock, 0, 1); + } } /** @@ -131,14 +187,16 @@ hrt_init(void) void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = delay; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + struct hrt_boardctl ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + .time = delay, + .interval = 0, + .callout = callout, + .arg = arg + }; + start_worker(); boardctl(HRT_CALL_AFTER, (uintptr_t)&ioc_parm); } @@ -148,15 +206,16 @@ hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, v void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = calltime; - ioc_parm.interval = 0; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + hrt_boardctl_t ioc_parm{ + .handle = g_hrt_client_handle, + .entry = entry, + .time = calltime, + .interval = 0, + .callout = callout, + .arg = arg + }; + start_worker(); boardctl(HRT_CALL_AT, (uintptr_t)&ioc_parm); } @@ -166,15 +225,16 @@ hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, v void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) { - hrt_boardctl_t ioc_parm; - ioc_parm.entry = entry; - ioc_parm.time = delay; - ioc_parm.interval = interval; - ioc_parm.callout = callout; - ioc_parm.arg = arg; - entry->usr_callout = callout; - entry->usr_arg = arg; + hrt_boardctl_t ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + .time = delay, + .interval = interval, + .callout = callout, + .arg = arg, + }; + start_worker(); boardctl(HRT_CALL_EVERY, (uintptr_t)&ioc_parm); } @@ -184,7 +244,13 @@ hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, void hrt_cancel(struct hrt_call *entry) { - boardctl(HRT_CANCEL, (uintptr_t)entry); + hrt_boardctl_t ioc_parm { + .handle = g_hrt_client_handle, + .entry = entry, + }; + + start_worker(); + boardctl(HRT_CANCEL, (uintptr_t)&ioc_parm); } void diff --git a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp index 1fc3f57b2dd7..a641ceb7a3d7 100644 --- a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp +++ b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp @@ -37,17 +37,40 @@ * Implementation of generic user-space version API */ +#ifndef MODULE_NAME +#define MODULE_NAME "usr_mcu_version" +#endif + #include #include #include #include #include +#include + +#include +#include +#include +#include +#include + #include "board_config.h" -static int hw_version = 0; -static int hw_revision = 0; -static char hw_info[] = HW_INFO_INIT_PREFIX HW_INFO_SUFFIX; +#define HW_INFO_SIZE 64 + +#ifndef max +# define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef min +# define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +static unsigned hw_version = 0; +static unsigned hw_revision = 0; +static char hw_info[HW_INFO_SIZE] = { 0 }; +static uint16_t soc_arch_id = 0; +static mfguid_t device_serial_number = { 0 }; __EXPORT const char *board_get_hw_type_name(void) { @@ -66,9 +89,9 @@ __EXPORT int board_get_hw_revision() __EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) { - /* TODO: This is a stub for userspace build. Use some proper interface - * to fetch the uuid32 from the kernel - */ + + /* DEPRECATED. Use board_get_px4_guid() */ + uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH]; memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4); @@ -79,19 +102,36 @@ __EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) int board_mcu_version(char *rev, const char **revstr, const char **errata) { - /* TODO: This is a stub for userspace build. Use some proper interface - * to fetch the version from the kernel - */ - return -1; + const struct { + const char *revstr; + char rev; + const char *errata; + } hw_version_table[] = BOARD_REVISIONS; + + unsigned len = sizeof(hw_version_table) / sizeof(hw_version_table[0]); + + if (hw_version < len) { + *rev = hw_version_table[hw_version].rev; + *revstr = hw_version_table[hw_version].revstr; + *errata = hw_version_table[hw_version].errata; + } + + return hw_version; } int board_get_px4_guid(px4_guid_t px4_guid) { - /* TODO: This is a stub for userspace build. Use some proper interface - * to fetch the guid from the kernel - */ - uint8_t *pb = (uint8_t *) &px4_guid[0]; - memset(pb, 0, PX4_GUID_BYTE_LENGTH); + uint8_t *pb = (uint8_t *) &px4_guid[0]; + + memset(pb, 0, sizeof(px4_guid_t)); + + static_assert(sizeof(device_serial_number) == 16); + static_assert(sizeof(px4_guid_t) >= sizeof(device_serial_number) + 2); + + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + memcpy(pb, device_serial_number, sizeof(device_serial_number)); return PX4_GUID_BYTE_LENGTH; } @@ -115,3 +155,55 @@ int board_get_px4_guid_formated(char *format_buffer, int size) return offset; } +int board_determine_hw_info(void) +{ + orb_sub_t hwinfo_sub; + orb_sub_t guid_sub; + orb_sub_t system_version_sub; + struct system_version_s ver; + struct guid_s guid; + struct hw_info_s hwinfo; + + /* System hw_version, hw_revision, soc_arch_id */ + system_version_sub = orb_subscribe(ORB_ID(system_version)); + + if (!orb_sub_valid(system_version_sub)) { + PX4_ERR("Failed to subscribe to system version (%i)", errno); + return -1; + } + + orb_copy(ORB_ID(system_version), system_version_sub, &ver); + orb_unsubscribe(system_version_sub); + + hw_version = ver.hw_version; + hw_revision = ver.hw_revision; + soc_arch_id = ver.soc_arch_id; + + /* Hardware information string */ + hwinfo_sub = orb_subscribe(ORB_ID(hw_info)); + + if (!orb_sub_valid(hwinfo_sub)) { + PX4_ERR("Failed to subscribe to hwinfo_sub (%i)", errno); + return -1; + } + + orb_copy(ORB_ID(hw_info), hwinfo_sub, &hwinfo); + orb_unsubscribe(hwinfo_sub); + + memcpy(hw_info, &hwinfo, min(sizeof(hwinfo), sizeof(hw_info))); + + /* Drone guid */ + guid_sub = orb_subscribe(ORB_ID(guid)); + + if (!orb_sub_valid(guid_sub)) { + PX4_ERR("Failed to subscribe to guid (%i)", errno); + return -1; + } + + orb_copy(ORB_ID(guid), guid_sub, &guid); + orb_unsubscribe(guid_sub); + + memcpy(device_serial_number, &guid, min(sizeof(device_serial_number), sizeof(guid))); + + return 0; +} diff --git a/platforms/nuttx/src/px4/common/usr_mmap.c b/platforms/nuttx/src/px4/common/usr_mmap.c new file mode 100644 index 000000000000..496a588fbd48 --- /dev/null +++ b/platforms/nuttx/src/px4/common/usr_mmap.c @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * Copyright (c) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +#include + +void *px4_mmap(void *start, size_t length, int prot, int flags, int fd, off_t offset) +{ + return mmap(start, length, prot, flags, fd, offset); +} + +int px4_munmap(void *start, size_t length) +{ + return munmap(start, length); +} diff --git a/platforms/nuttx/src/px4/microchip/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/CMakeLists.txt new file mode 100644 index 000000000000..36843c5694ef --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +add_subdirectory(${PX4_CHIP}) diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt new file mode 100644 index 000000000000..a78db53e8b5a --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/adc/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2022 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_adc + adc.cpp +) diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp b/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp new file mode 100644 index 000000000000..dd0984ba0ef6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/adc/adc.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 +#include +#include + +int px4_arch_adc_init(uint32_t base_address) +{ + return 0; +} + +void px4_arch_adc_uninit(uint32_t base_address) +{ + +} + +uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) +{ + return 0xffffffffu; +} + +float px4_arch_adc_reference_v() +{ + return 0.0f; +} + +uint32_t px4_arch_adc_temp_sensor_mask() +{ + + return 0; + +} + +uint32_t px4_arch_adc_dn_fullcount() +{ + return 0; +} diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/device_info.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/device_info.h new file mode 100644 index 000000000000..5e73e1d8f36e --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/device_info.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include + +typedef struct __attribute__((__packed__)) +{ + /* Magic, matches characters "DNFO" when valid */ + + uint32_t magic; + + /* Anti-rollback counters for different payloads */ + + uint16_t arb[IMAGE_NUM_TYPES]; + + /* Feature flags. RD certificate feature bits are ORd to this. + * Passed to payload OSs for enabling / disabling features + */ + + uint32_t features; + + /* Device MAC addresses */ + + uint8_t mac[4][6]; + + /* Bootloader version info */ + + char bl_version[32]; + + /* FPGA version info */ + + uint32_t fpga_version; + + /* Reserved for future use */ + + uint8_t reserved[424]; +} devinfo_t; diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..58f666db1d07 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + + +static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = false; + return ret; +} + +static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus) +{ + px4_i2c_bus_t ret{}; + ret.bus = bus; + ret.is_external = true; + return ret; +} diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..afa84bf34e6b --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/micro_hal.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Techonology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + +#include diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/px4io_serial.h new file mode 100644 index 000000000000..56939020506c --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/px4io_serial.h @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file px4io_serial.h + * + * Serial Interface definition for PX4IO + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include + +class PX4IO_serial : public device::Device +{ +public: + PX4IO_serial(); + virtual ~PX4IO_serial(); + + virtual int init() = 0; + virtual int read(unsigned offset, void *data, unsigned count = 1); + virtual int write(unsigned address, void *data, unsigned count = 1); + +protected: + /** + * Does the PX4IO_serial instance initialization. + * @param io_buffer The IO buffer that should be used for transfers. + * @return 0 on success. + */ + int init(IOPacket *io_buffer); + + /** + * Start the transaction with IO and wait for it to complete. + */ + virtual int _bus_exchange(IOPacket *_packet) = 0; + + /** + * Performance counters. + */ + perf_counter_t _pc_txns; + perf_counter_t _pc_retries; + perf_counter_t _pc_timeouts; + perf_counter_t _pc_crcerrs; + perf_counter_t _pc_protoerrs; + perf_counter_t _pc_uerrs; + perf_counter_t _pc_idle; + perf_counter_t _pc_badidle; +private: + + /** + * Pointer to the io buffer + */ + IOPacket *_io_buffer_ptr; + + /** + * bus-ownership mutex + */ + px4_sem_t _bus_semaphore; + + /* do not allow top copying this class */ + PX4IO_serial(PX4IO_serial &); + PX4IO_serial &operator = (const PX4IO_serial &); +}; + +class ArchPX4IOSerial : public PX4IO_serial +{ +public: + ArchPX4IOSerial(); + ArchPX4IOSerial(ArchPX4IOSerial &) = delete; + ArchPX4IOSerial &operator = (const ArchPX4IOSerial &) = delete; + virtual ~ArchPX4IOSerial(); + + virtual int init(); + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + /** + * Start the transaction with IO and wait for it to complete. + */ + int _bus_exchange(IOPacket *_packet); + +private: + + /** packet pointer and counters */ + IOPacket *_current_packet; + uint8_t _packet_cnt; + uint8_t _expected_size; + + int _fd{-1}; + + /** client-waiting lock/signal */ + px4_sem_t _completion_semaphore; + + /** + * Uart initialization + */ + + int init_uart(); + + /** + * Serial interrupt handler. + */ + static int _interrupt(int vector, void *context, void *arg); + void _do_interrupt(); + + /* + * IO Buffer storage + */ + static uint8_t _io_buffer_storage[] px4_cache_aligned_data(); +}; diff --git a/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..cb9a86e22155 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/microchip_common/include/px4_arch/spi_hw_description.h @@ -0,0 +1,149 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +#include + +#if defined(CONFIG_SPI) + +static inline constexpr px4_spi_bus_device_t initSPIDevice(uint32_t devid, SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + px4_spi_bus_device_t ret{}; + + ret.cs_gpio = getGPIOBank(cs_gpio.bank) | getGPIOPin(cs_gpio.pin) | (GPIO_OUTPUT | GPIO_BUFFER_ENABLE); + + if (drdy_gpio.bank != GPIO::BankInvalid) { + ret.drdy_gpio = getGPIOBank(drdy_gpio.bank) | getGPIOPin(drdy_gpio.pin) | GPIO_INPUT; + } + + if (PX4_SPIDEVID_TYPE(devid) == 0) { // it's a PX4 device (internal or external) + // Construct device id including cs_gpio pin information into address field + ret.devid = PX4_SPIDEV_ID(PX4_SPI_DEVICE_ID, devid) | (cs_gpio.pin << 8); + + } else { // it's a NuttX device (e.g. SPIDEV_FLASH(0)) + ret.devid = devid; + } + + ret.devtype_driver = PX4_SPI_DEV_ID(devid); + + return ret; +} + +static inline constexpr px4_spi_bus_t initSPIBusInternal(SPI::Bus bus, const px4_spi_bus_devices_t &devices, + GPIO::GPIOPin power_enable = {}) +{ + px4_spi_bus_t ret{}; + + ret.requires_locking = false; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + ret.devices[i] = devices.devices[i]; + + // check that the same device is configured only once (the chip-select code depends on that) + for (int j = i + 1; j < SPI_BUS_MAX_DEVICES; ++j) { + if (ret.devices[j].cs_gpio != 0) { + constexpr_assert(ret.devices[i].devid != ret.devices[j].devid, "Same device configured multiple times"); + } + } + + if (ret.devices[i].cs_gpio != 0) { + // A bus potentially requires locking if it is accessed by non-PX4 devices (i.e. NuttX drivers) + if (PX4_SPI_DEVICE_ID != PX4_SPIDEVID_TYPE(ret.devices[i].devid)) { + ret.requires_locking = true; + } + } + } + + ret.bus = (int)bus; + ret.is_external = false; + + if (power_enable.bank != GPIO::BankInvalid) { + ret.power_enable_gpio = getGPIOBank(power_enable.bank) | getGPIOPin(power_enable.pin) | GPIO_OUTPUT; + } + + return ret; +} + +// just a wrapper since we cannot pass brace-enclosed initialized arrays directly as arguments +struct bus_device_external_cfg_array_t { + SPI::bus_device_external_cfg_t devices[SPI_BUS_MAX_DEVICES]; +}; + +static inline constexpr px4_spi_bus_t initSPIBusExternal(SPI::Bus bus, const bus_device_external_cfg_array_t &devices) +{ + px4_spi_bus_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_DEVICES; ++i) { + if (devices.devices[i].cs_gpio.bank == GPIO::BankInvalid) { + break; + } + + ret.devices[i] = initSPIDevice(i, devices.devices[i].cs_gpio, devices.devices[i].drdy_gpio); + } + + ret.bus = (int)bus; + ret.is_external = true; + ret.requires_locking = false; // external buses are never accessed by NuttX drivers + return ret; + + return ret; +} + +static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI::CS cs_gpio, SPI::DRDY drdy_gpio = {}) +{ + SPI::bus_device_external_cfg_t ret{}; + ret.cs_gpio = cs_gpio; + ret.drdy_gpio = drdy_gpio; + return ret; +} + +struct px4_spi_bus_array_t { + px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS]; +}; +static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision, + const px4_spi_bus_array_t &bus_items) +{ + px4_spi_bus_all_hw_t ret{}; + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + ret.buses[i] = bus_items.item[i]; + } + + ret.board_hw_version_revision = hw_version_revision; + return ret; +} +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt new file mode 100644 index 000000000000..aae38f8c8234 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + + +add_subdirectory(board_critmon) +add_subdirectory(board_reset board_reset) +add_subdirectory(hrt hrt) +add_subdirectory(version version) +add_subdirectory(tone_alarm tone_alarm) +add_subdirectory(../microchip_common/adc adc) +add_subdirectory(px4io_serial) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt new file mode 100644 index 000000000000..c66b17e5bce6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_board_critmon + board_critmon.c +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c new file mode 100644 index 000000000000..0d115e641ce1 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_critmon/board_critmon.c @@ -0,0 +1,67 @@ +/************************************************************************************ + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * Author: Jukka Laitinen + * + * 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. + * 3. Neither the name NuttX nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ************************************************************************************/ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#include + +#if defined(CONFIG_SCHED_CRITMONITOR) || defined(CONFIG_SCHED_IRQMONITOR) + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +#error "critmon not supported yet" + +/************************************************************************************ + * Name: up_critmon_gettime + ************************************************************************************/ + +uint32_t up_critmon_gettime(void) +{ + return 0; +} + +/************************************************************************************ + * Name: up_critmon_convert + ************************************************************************************/ + +void up_critmon_convert(uint32_t elapsed, FAR struct timespec *ts) +{ +} + +#endif /* CONFIG_SCHED_CRITMONITOR || CONFIG_SCHED_IRQMONITOR */ diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt new file mode 100644 index 000000000000..ef783ad14199 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_board_reset + board_reset.cpp +) + +if (NOT DEFINED CONFIG_BUILD_FLAT) + target_link_libraries(arch_board_reset PRIVATE nuttx_karch) +else() + target_link_libraries(arch_board_reset PRIVATE nuttx_arch) +endif() diff --git a/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp new file mode 100644 index 000000000000..a532977f9556 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/board_reset/board_reset.cpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * Author: @author Jukka Laitinen + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file board_reset.cpp + * Implementation of Microchip PolarFire based Board RESET API + */ + +#include +#include +#include +#include + +#include "riscv_internal.h" + +extern "C" void __start(void); + +static void board_reset_enter_bootloader() +{ + /* Reset the whole SoC */ + + up_systemreset(); +} + +static void board_reset_enter_bootloader_and_continue_boot() +{ + /* Reset by triggering WDOG */ + + putreg32(WDOG_FORCE_IMMEDIATE_RESET, MPFS_WDOG0_LO_BASE + MPFS_WDOG_FORCE_OFFSET); + + /* Wait for the reset */ + + for (; ;); +} + +static void board_reset_enter_app(uintptr_t hartid) +{ + register long r0 asm("a0") = (long)(hartid); + + asm volatile + ( + "tail __start\n" :: "r"(r0) : "memory" + ); +} + +int board_reset(int status) +{ + uintptr_t hartid = riscv_mhartid(); +#if defined(BOARD_HAS_ON_RESET) + board_on_reset(status); +#endif + + if (status == 1) { + board_reset_enter_bootloader(); + + } else if (status == 2) { + board_reset_enter_bootloader_and_continue_boot(); + } + + /* Just reboot via reset vector */ + + board_reset_enter_app(hartid); + + return 0; +} diff --git a/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt new file mode 100644 index 000000000000..2989ba02d5f9 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/hrt/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_hrt + hrt.c +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c b/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c new file mode 100644 index 000000000000..21dd8bc7591f --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/hrt/hrt.c @@ -0,0 +1,471 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file drv_hrt.c + * Author: Jani Paalijärvi + * + * High-resolution timer callouts and timekeeping. + * The implementation is based on 32-bit decrementing counter (MSTIMER) + * which is used in periodic mode and generates an interrupt + * when timer reaches zero. + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "mpfs_memorymap.h" +#include "hardware/mpfs_timer.h" + +#define getreg32(a) (*(volatile uint32_t *)(a)) +#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) + +#ifdef CONFIG_DEBUG_HRT +# define hrtinfo _info +#else +# define hrtinfo(x...) +#endif + +/* This assumes MTIMER count at 1MHz in MPFS */ + +#if MPFS_MSS_RTC_TOGGLE_CLK != 1000000 +# error This driver currently assumes that MTIMER runs at 1MHz +#endif + +#define CLOCK_RATE_MHZ (MPFS_MSS_APB_AHB_CLK / 1000000) + +/* + * Scaling factor(s) for the free-running counter; convert an input + * in counts to a time in microseconds. + */ + +#define HRT_COUNTS_TO_TIME(_c) ((_c) / CLOCK_RATE_MHZ) +#define HRT_TIME_TO_COUNTS(_a) ((_a) * CLOCK_RATE_MHZ) + +#define HRT_INTERVAL_MIN 50UL // 50 microseconds +#define HRT_INTERVAL_MAX HRT_COUNTS_TO_TIME(0xFFFFFFFF) // ~28.6s at 150MHz timer + +/* + * Queue of callout entries. + */ +static struct sq_queue_s callout_queue; + +/* last loaded value for irq latency calculation */ +static hrt_abstime loadval; + +/* latency histogram */ +const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + +/* timer-specific functions */ +static void hrt_tim_init(void); +static int hrt_tim_isr(int irq, void *context, void *args); +static void hrt_latency_update(void); + +/* callout list manipulation */ +static void hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, + void *arg); +static void hrt_call_enter(struct hrt_call *entry); +static void hrt_call_reschedule(void); +static void hrt_call_invoke(void); + +/** + * function to set new time to the the next interrupt. + * + * Always call with interrupts disabled. + */ + +inline static void hrt_set_new_deadline(uint32_t deadline) +{ + /* load the new deadline into register and store it locally */ + putreg32(HRT_TIME_TO_COUNTS(deadline), MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1LOADVAL_OFFSET); + loadval = hrt_absolute_time() + deadline; +} + +/** + * Initialize the timer we are going to use. + */ +static void +hrt_tim_init(void) +{ + /* e.g. connect hrt_tim_isr to a timer vector, initialize the timer */ + + /* attach irq */ + int ret; + ret = irq_attach(MPFS_IRQ_TIMER1, hrt_tim_isr, NULL); + + if (ret == OK) { + + /* Assumes that the clock for timer is enabled and not in reset */ + + /* set an initial timeout to 1 ms*/ + hrt_set_new_deadline(1000); + + /* enable interrupt for timer, set periodic mode and enable timer */ + putreg32((MPFS_MSTIMER_INTEN_MASK | MPFS_MSTIMER_ENABLE_MASK) & ~(MPFS_MSTIMER_MODE_MASK), + MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1CONTROL_OFFSET); + + /* enable interrupts */ + up_enable_irq(MPFS_IRQ_TIMER1); + } +} + +/** + * Handle the timer interrupt by calling the callout dispatcher + * and then re-scheduling the next deadline. + */ +static int +hrt_tim_isr(int irq, void *context, void *arg) +{ + uint32_t status; + + status = getreg32(MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1RIS_OFFSET); + + /* was this a timer tick? */ + if (status & MPFS_MSTIMER_RIS_MASK) { + + /* do latency calculations */ + hrt_latency_update(); + + /* run any callouts that have met their deadline */ + hrt_call_invoke(); + + /* and schedule the next interrupt */ + hrt_call_reschedule(); + + /* clear the interrupt */ + putreg32((MPFS_MSTIMER_RIS_MASK), MPFS_MSTIMER_LO_BASE + MPFS_MSTIMER_TIM1RIS_OFFSET); + } + + return OK; +} + +/** + * Fetch a never-wrapping absolute time value in microseconds from + * some arbitrary epoch shortly after system start. + */ +hrt_abstime +hrt_absolute_time(void) +{ + return getreg64(MPFS_CLINT_MTIME); +} + +/** + * Compare a time value with the current time as atomic operation + */ +hrt_abstime +hrt_elapsed_time_atomic(const volatile hrt_abstime *then) +{ + irqstate_t flags = px4_enter_critical_section(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + px4_leave_critical_section(flags); + + return delta; +} + +/** + * Store the absolute time in an interrupt-safe fashion + */ +void +hrt_store_absolute_time(volatile hrt_abstime *t) +{ + irqstate_t flags = px4_enter_critical_section(); + *t = hrt_absolute_time(); + px4_leave_critical_section(flags); +} + +/** + * Initialise the high-resolution timing module. + */ +void +hrt_init(void) +{ + sq_init(&callout_queue); + hrt_tim_init(); +} + +/** + * Call callout(arg) after interval has elapsed. + */ +void +hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + 0, + callout, + arg); +} + +/** + * Call callout(arg) at calltime. + */ +void +hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, calltime, 0, callout, arg); +} + +/** + * Call callout(arg) every period. + */ +void +hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) +{ + hrt_call_internal(entry, + hrt_absolute_time() + delay, + interval, + callout, + arg); +} + +static void +hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg) +{ + irqstate_t flags = px4_enter_critical_section(); + + /* if the entry is currently queued, remove it */ + /* note that we are using a potentially uninitialised + entry->link here, but it is safe as sq_rem() doesn't + dereference the passed node unless it is found in the + list. So we potentially waste a bit of time searching the + queue for the uninitialised entry->link but we don't do + anything actually unsafe. + */ + if (entry->deadline != 0) { + sq_rem(&entry->link, &callout_queue); + } + + entry->deadline = deadline; + entry->period = interval; + entry->callout = callout; + entry->arg = arg; + + hrt_call_enter(entry); + + px4_leave_critical_section(flags); +} + +/** + * If this returns true, the call has been invoked and removed from the callout list. + * + * Always returns false for repeating callouts. + */ +bool +hrt_called(struct hrt_call *entry) +{ + return (entry->deadline == 0); +} + +/** + * Remove the entry from the callout list. + */ +void +hrt_cancel(struct hrt_call *entry) +{ + irqstate_t flags = px4_enter_critical_section(); + + sq_rem(&entry->link, &callout_queue); + entry->deadline = 0; + + /* if this is a periodic call being removed by the callout, prevent it from + * being re-entered when the callout returns. + */ + entry->period = 0; + + px4_leave_critical_section(flags); +} + +static void +hrt_call_enter(struct hrt_call *entry) +{ + struct hrt_call *call, *next; + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if ((call == NULL) || (entry->deadline < call->deadline)) { + sq_addfirst(&entry->link, &callout_queue); + hrtinfo("call enter at head, reschedule\n"); + /* we changed the next deadline, reschedule the timer event */ + hrt_call_reschedule(); + + } else { + do { + next = (struct hrt_call *)sq_next(&call->link); + + if ((next == NULL) || (entry->deadline < next->deadline)) { + hrtinfo("call enter after head\n"); + sq_addafter(&call->link, &entry->link, &callout_queue); + break; + } + } while ((call = next) != NULL); + } + + hrtinfo("scheduled\n"); +} + +static void +hrt_call_invoke(void) +{ + struct hrt_call *call; + hrt_abstime deadline; + + while (true) { + /* get the current time */ + hrt_abstime now = hrt_absolute_time(); + + call = (struct hrt_call *)sq_peek(&callout_queue); + + if (call == NULL) { + break; + } + + if (call->deadline > now) { + break; + } + + sq_rem(&call->link, &callout_queue); + hrtinfo("call pop\n"); + + /* save the intended deadline for periodic calls */ + deadline = call->deadline; + + /* zero the deadline, as the call has occurred */ + call->deadline = 0; + + /* invoke the callout (if there is one) */ + if (call->callout) { + hrtinfo("call %p: %p(%p)\n", call, call->callout, call->arg); + call->callout(call->arg); + } + + /* if the callout has a non-zero period, it has to be re-entered */ + if (call->period != 0) { + // re-check call->deadline to allow for + // callouts to re-schedule themselves + // using hrt_call_delay() + if (call->deadline <= now) { + call->deadline = deadline + call->period; + } + + hrt_call_enter(call); + } + } +} + +/** + * Reschedule the next timer interrupt. + * + * This routine must be called with interrupts disabled. + */ +static void +hrt_call_reschedule() +{ + hrt_abstime now = hrt_absolute_time(); + struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); + uint32_t deadline = HRT_INTERVAL_MAX; + + /* + * Determine what the next deadline will be. + * + */ + if (next != NULL) { + hrtinfo("entry in queue\n"); + + if (next->deadline <= (now + HRT_INTERVAL_MIN)) { + hrtinfo("pre-expired\n"); + /* set a minimal deadline so that we call ASAP */ + deadline = HRT_INTERVAL_MIN; + + } else if (next->deadline < now + HRT_INTERVAL_MAX) { + hrtinfo("due soon\n"); + /* calculate how much time we have to the next deadline */ + deadline = (next->deadline - now); + } + } + + hrtinfo("schedule for %"PRIu64" at %"PRIu64"\n", deadline, now); + + /* set next deadline */ + hrt_set_new_deadline(deadline); +} + +static void +hrt_latency_update(void) +{ + uint32_t latency = hrt_absolute_time() - loadval; + unsigned index; + + /* bounded buckets */ + for (index = 0; index < LATENCY_BUCKET_COUNT; index++) { + if (latency <= latency_buckets[index]) { + latency_counters[index]++; + return; + } + } + + /* catch-all at the end */ + latency_counters[index]++; +} + +void +hrt_call_init(struct hrt_call *entry) +{ + memset(entry, 0, sizeof(*entry)); +} + +void +hrt_call_delay(struct hrt_call *entry, hrt_abstime delay) +{ + entry->deadline = hrt_absolute_time() + delay; +} diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h new file mode 100644 index 000000000000..a08361bbf0be --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/adc.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + +#define SYSTEM_ADC_BASE 0 // not used diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/device_info.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/device_info.h new file mode 100644 index 000000000000..52fd752cbef2 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/device_info.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include "../../../microchip_common/include/px4_arch/device_info.h" diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h new file mode 100644 index 000000000000..dfcc152d28bf --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/hw_description.h @@ -0,0 +1,219 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + + +#include + +#include + +#include + +/* + * Timers + */ + +namespace Timer +{ +enum Timer { + FTM0 = 0, +}; +enum Channel { + Channel0 = 0, + Channel1, + Channel2, + Channel3, + Channel4, + Channel5, +}; +struct TimerChannel { + Timer timer; + Channel channel; +}; +} + +namespace GPIO +{ +enum Bank { + BankInvalid = 0, + Bank0, + Bank1, + Bank2, +}; +enum Pin { + Pin0 = 0, + Pin1, + Pin2, + Pin3, + Pin4, + Pin5, + Pin6, + Pin7, + Pin8, + Pin9, + Pin10, + Pin11, + Pin12, + Pin13, + Pin14, + Pin15, + Pin16, + Pin17, + Pin18, + Pin19, + Pin20, + Pin21, + Pin22, + Pin23, + Pin24, + Pin25, + Pin26, + Pin27, + Pin28, + Pin29, + Pin30, + Pin31, +}; + +struct GPIOPin { + Bank bank; + Pin pin; +}; +} + +static inline constexpr uint32_t getGPIOBank(GPIO::Bank bank) +{ + switch (bank) { + case GPIO::Bank0: return GPIO_BANK0; + + case GPIO::Bank1: return GPIO_BANK1; + + case GPIO::Bank2: return GPIO_BANK2; + + default: break; + } + + return 0; +} + +static inline constexpr uint32_t getGPIOPin(GPIO::Pin pin) +{ + switch (pin) { + case GPIO::Pin0: return GPIO_PIN0; + + case GPIO::Pin1: return GPIO_PIN1; + + case GPIO::Pin2: return GPIO_PIN2; + + case GPIO::Pin3: return GPIO_PIN3; + + case GPIO::Pin4: return GPIO_PIN4; + + case GPIO::Pin5: return GPIO_PIN5; + + case GPIO::Pin6: return GPIO_PIN6; + + case GPIO::Pin7: return GPIO_PIN7; + + case GPIO::Pin8: return GPIO_PIN8; + + case GPIO::Pin9: return GPIO_PIN9; + + case GPIO::Pin10: return GPIO_PIN10; + + case GPIO::Pin11: return GPIO_PIN11; + + case GPIO::Pin12: return GPIO_PIN12; + + case GPIO::Pin13: return GPIO_PIN13; + + case GPIO::Pin14: return GPIO_PIN14; + + case GPIO::Pin15: return GPIO_PIN15; + + case GPIO::Pin16: return GPIO_PIN16; + + case GPIO::Pin17: return GPIO_PIN17; + + case GPIO::Pin18: return GPIO_PIN18; + + case GPIO::Pin19: return GPIO_PIN19; + + case GPIO::Pin20: return GPIO_PIN20; + + case GPIO::Pin21: return GPIO_PIN21; + + case GPIO::Pin22: return GPIO_PIN22; + + case GPIO::Pin23: return GPIO_PIN23; + + case GPIO::Pin24: return GPIO_PIN24; + + case GPIO::Pin25: return GPIO_PIN25; + + case GPIO::Pin26: return GPIO_PIN26; + + case GPIO::Pin27: return GPIO_PIN27; + + case GPIO::Pin28: return GPIO_PIN28; + + case GPIO::Pin29: return GPIO_PIN29; + + case GPIO::Pin30: return GPIO_PIN30; + + case GPIO::Pin31: return GPIO_PIN31; + } + + return 0; +} + +namespace SPI +{ +enum class Bus { + SPI0 = 1, + SPI1 = 2, + SPI2 = 3, + SPI3 = 4, + SPI4 = 5, +}; + +using CS = GPIO::GPIOPin; ///< chip-select pin +using DRDY = GPIO::GPIOPin; ///< data ready pin + +struct bus_device_external_cfg_t { + CS cs_gpio; + DRDY drdy_gpio; +}; + +} // namespace SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h new file mode 100644 index 000000000000..cc225c6ca4b4 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/i2c_hw_description.h @@ -0,0 +1,35 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../microchip_common/include/px4_arch/i2c_hw_description.h" diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h new file mode 100644 index 000000000000..6ae091cbd05a --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/micro_hal.h @@ -0,0 +1,146 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + + +#include "../../../microchip_common/include/px4_arch/micro_hal.h" + +__BEGIN_DECLS + +#define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_MPFS + +/* Some of the boards have 3+ I2C buses on FPGA. Others have always just 2 as SoC hard-ip blocks */ +#ifdef CONFIG_MPFS_COREI2C +# define PX4_NUMBER_I2C_BUSES CONFIG_MPFS_COREI2C_INSTANCES +#else +# if defined(CONFIG_MPFS_I2C0) && defined(CONFIG_MPFS_I2C1) +# define PX4_NUMBER_I2C_BUSES 2 +# else +# define PX4_NUMBER_I2C_BUSES 1 +# endif +#endif + +#define GPIO_OUTPUT_SET GPIO_OUTPUT_ONE +#define GPIO_OUTPUT_CLEAR GPIO_OUTPUT_ZERO + +#include +#include +#include +#include +#include + +#include "riscv_mmu.h" + +/* + * PX4 uses the words in bigendian order MSB to LSB + * word [0] [1] [2] [3] + * bits 127:96 95-64 63-32, 31-00, + */ +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* Battery backed up SRAM definitions. TODO: check what memory can actually be used */ +#define PX4_BBSRAM_SIZE 2048 + + +// TODO: Use some proper UUID which can be obtained from the HW +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#define PX4_CPU_UUID_WORD32_LENGTH (PX4_CPU_UUID_BYTE_LENGTH/sizeof(uint32_t)) + +/* The mfguid will be an array of bytes with + * MSD @ index 0 - LSD @ index PX4_CPU_MFGUID_BYTE_LENGTH-1 + * + * It will be converted to a string with the MSD on left and LSD on the right most position. + */ +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH + +/* define common formating across all commands */ + +#define PX4_CPU_UUID_WORD32_FORMAT "%08x" +#define PX4_CPU_UUID_WORD32_SEPARATOR ":" + +#define PX4_CPU_UUID_WORD32_UNIQUE_H 3 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 2 /* Middle High significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 1 /* Middle Low significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_N 0 /* Most significant digits change the least */ + +static inline struct spi_dev_s *mpfs_spibus_initialize_wrap(int port) +{ +#if defined(CONFIG_MPFS_CORESPI) + return mpfs_corespibus_initialize(port); +#else + return mpfs_spibus_initialize(port); +#endif +} + +#define PX4_BUS_OFFSET 1 /* MPFS buses are 0 based, so adjustment needed */ +#define px4_savepanic(fileno, context, length) mpfs_bbsram_savepanic(fileno, context, length) +#define px4_spibus_initialize(bus_num_1based) mpfs_spibus_initialize_wrap(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) + +#define px4_i2cbus_initialize(bus_num_1based) mpfs_i2cbus_initialize(PX4_BUS_NUMBER_FROM_PX4(bus_num_1based)) +#define px4_i2cbus_uninitialize(pdev) mpfs_i2cbus_uninitialize(pdev) + +#define px4_arch_configgpio(pinset) mpfs_configgpio(pinset) +#define px4_arch_unconfiggpio(pinset) mpfs_unconfiggpio(pinset) +#define px4_arch_gpioread(pinset) mpfs_gpioread(pinset) +#define px4_arch_gpiowrite(pinset, value) mpfs_gpiowrite(pinset, value) +#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) mpfs_gpiosetevent(pinset,r,f,false,false,e,fp,a) + +#define PX4_MAKE_GPIO_INPUT(gpio) (((gpio) & (~GPIO_OUTPUT | GPIO_BANK_MASK | GPIO_PIN_MASK | GPIO_BUFFER_MASK)) | GPIO_INPUT) + +#define PX4_MAKE_GPIO_OUTPUT(gpio) (((gpio) & (~GPIO_INPUT | GPIO_BANK_MASK | GPIO_PIN_MASK | GPIO_BUFFER_MASK)) | GPIO_OUTPUT) + +#define PX4_GPIO_PIN_OFF(gpio) (gpio & (GPIO_BANK_MASK | GPIO_PIN_MASK)) + +/* MPFS can access HRT timer counter directly from user space in + * CONFIG_BUILD_PROTECTED and CONFIG_BUILD_KERNEL + */ + +#define PX4_USERSPACE_HRT +static inline uintptr_t hrt_absolute_time_usr_base(void) +{ + return USRIO_START + (MPFS_CLINT_MTIME & RV_MMU_PAGE_MASK); +} + +// TODO! +# define px4_cache_aligned_data() +# define px4_cache_aligned_alloc malloc + +__END_DECLS diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/px4io_serial.h new file mode 100644 index 000000000000..dec76dafb9a6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/px4io_serial.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include "../../../microchip_common/include/px4_arch/px4io_serial.h" diff --git a/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h new file mode 100644 index 000000000000..f559190ff548 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/include/px4_arch/spi_hw_description.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + +#include "../../../microchip_common/include/px4_arch/spi_hw_description.h" + +#if defined(CONFIG_SPI) + +constexpr bool validateSPIConfig(const px4_spi_bus_t spi_busses_conf[SPI_BUS_MAX_BUS_ITEMS]) +{ + const bool nuttx_enabled_spi_buses[] = { + +#ifdef CONFIG_MPFS_CORESPI + true, +#if MPFS_CORESPI_INSTANCES >= 2 + true, +#endif +#if MPFS_CORESPI_INSTANCES >= 3 + true, +#endif +#if MPFS_CORESPI_INSTANCES >= 4 + true, +#endif +#if MPFS_CORESPI_INSTANCES >= 5 + true, +#endif +#else /* !CONFIG_MPFS_CORESPI */ +#ifdef CONFIG_MPFS_SPI0 + true, +#else + false, +#endif +#ifdef CONFIG_MPFS_SPI1 + true, +#else + false, +#endif +#endif /* CONFIG_MPFS_CORESPI */ + + }; + + for (unsigned i = 0; i < sizeof(nuttx_enabled_spi_buses) / sizeof(nuttx_enabled_spi_buses[0]); ++i) { + bool found_bus = false; + + for (int j = 0; j < SPI_BUS_MAX_BUS_ITEMS; ++j) { + if (spi_busses_conf[j].bus == (int)i + 1) { + found_bus = true; + } + } + + // Either the bus is enabled in NuttX and configured in spi_busses_conf, or disabled and not configured + constexpr_assert(found_bus == nuttx_enabled_spi_buses[i], "SPI bus config mismatch (CONFIG_MPFS_SPIx)"); + } + + return false; +} + +constexpr bool validateSPIConfig(const px4_spi_bus_all_hw_t spi_buses_conf[BOARD_NUM_SPI_CFG_HW_VERSIONS]) +{ + for (int ver = 0; ver < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++ver) { + validateSPIConfig(spi_buses_conf[ver].buses); + } + + for (int ver = 1; ver < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++ver) { + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + const bool equal_power_enable_gpio = spi_buses_conf[ver].buses[i].power_enable_gpio == spi_buses_conf[ver - + 1].buses[i].power_enable_gpio; + // currently board_control_spi_sensors_power_configgpio() depends on that - this restriction can be removed + // by ensuring board_control_spi_sensors_power_configgpio() is called after the hw version is determined + // and SPI config is initialized. + constexpr_assert(equal_power_enable_gpio, "All HW versions must define the same power enable GPIO"); + } + } + + return false; +} +#endif // CONFIG_SPI diff --git a/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/CMakeLists.txt new file mode 100644 index 000000000000..8f3c837d65db --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2023 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_px4io_serial + px4io_serial.cpp +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/px4io_serial.cpp new file mode 100644 index 000000000000..5a67bd12a6ad --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/px4io_serial/px4io_serial.cpp @@ -0,0 +1,378 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file px4io_serial.cpp + * + * Serial interface for PX4IO on MPFS + */ + +#include + +#include +#include +#include + +/* Nb. this register only exists in FPGA UART implementation. For MSS uart + * this is a scratch register, and the code works by IRQ on each byte + */ + +#define MPFS_UART_RFT_OFFSET 0x1C + +uint8_t ArchPX4IOSerial::_io_buffer_storage[sizeof(IOPacket)]; + +ArchPX4IOSerial::ArchPX4IOSerial() : + _current_packet(nullptr), + _completion_semaphore(SEM_INITIALIZER(0)) +{ +} + +ArchPX4IOSerial::~ArchPX4IOSerial() +{ + + /* detach our interrupt handler */ + up_disable_irq(PX4IO_SERIAL_VECTOR); + irq_detach(PX4IO_SERIAL_VECTOR); + + /* and kill our semaphores */ + px4_sem_destroy(&_completion_semaphore); +} + + +int ArchPX4IOSerial::init_uart() +{ + /* Reset off */ + + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SOFT_RESET_CR_OFFSET, + SYSREG_SOFT_RESET_CR_FIC3 | SYSREG_SOFT_RESET_CR_FPGA, 0); + + /* Clock on */ + + modifyreg32(MPFS_SYSREG_BASE + MPFS_SYSREG_SUBBLK_CLOCK_CR_OFFSET, + 0, SYSREG_SUBBLK_CLOCK_CR_FIC3); + + + /* Disable interrupts */ + + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_IER_OFFSET); + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_IEM_OFFSET); + + /* Clear fifos */ + + putreg32((UART_FCR_RFIFOR | UART_FCR_XFIFOR), PX4IO_SERIAL_BASE + MPFS_UART_FCR_OFFSET); + + /* Set filter to minimum value */ + + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_GFR_OFFSET); + + /* Set default TX time guard */ + + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_TTG_OFFSET); + + /* 8 bits, no parity 1 stop */ + + uint32_t lcr = UART_LCR_DLS_8BITS; + + /* Set bits, enter dlab */ + + putreg32(lcr | UART_LCR_DLAB, PX4IO_SERIAL_BASE + MPFS_UART_LCR_OFFSET); + + /* Baud_value = PCLK_Frequency / (baud_rate * 16) + * + * Calculate in fixed point 26.6 format (fraction is presented by 6 bits in PF) + */ + + uint32_t baud26_6 = ((uint32_t)MPFS_FPGA_PERIPHERAL_CLK << 2) / PX4IO_SERIAL_BITRATE; + uint16_t baud = baud26_6 >> 6; + uint8_t fraction = baud26_6 & 0x3F; + + putreg32(baud >> 8, PX4IO_SERIAL_BASE + MPFS_UART_DLH_OFFSET); + putreg32(baud & 0xff, PX4IO_SERIAL_BASE + MPFS_UART_DLL_OFFSET); + + if (baud > 1) { + /* Enable Fractional baud rate */ + + uint8_t mm0 = getreg32(PX4IO_SERIAL_BASE + MPFS_UART_MM0_OFFSET); + mm0 |= UART_MM0_EFBR; + putreg32(mm0, PX4IO_SERIAL_BASE + MPFS_UART_MM0_OFFSET); + putreg32(fraction, PX4IO_SERIAL_BASE + MPFS_UART_DFR_OFFSET); + } + + /* Clear dlab */ + + putreg32(lcr, PX4IO_SERIAL_BASE + MPFS_UART_LCR_OFFSET); + + /* Configure the FIFO's */ + + putreg32((UART_FCR_XFIFOR | UART_FCR_RFIFOR | + UART_FCR_FIFOE), PX4IO_SERIAL_BASE + MPFS_UART_FCR_OFFSET); + + /* Attach serial interrupt handler */ + + irq_attach(PX4IO_SERIAL_VECTOR, _interrupt, this); + up_enable_irq(PX4IO_SERIAL_VECTOR); + + /* Create semaphores */ + + px4_sem_init(&_completion_semaphore, 0, 0); + + /* _completion_semaphore use case is a signal */ + + px4_sem_setprotocol(&_completion_semaphore, SEM_PRIO_NONE); + + return 0; +} + +int +ArchPX4IOSerial::init() +{ + /* initialize base implementation */ + int r = PX4IO_serial::init((IOPacket *)&_io_buffer_storage[0]); + + if (r != 0) { + return r; + } + + r = init_uart(); + + return r; +} + +int +ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg) +{ + int ret = 0; + + switch (operation) { + + case 1: /* XXX magic number - test operation */ + switch (arg) { + case 0: + syslog(LOG_INFO, "test 0\n"); + + while ((getreg32(PX4IO_SERIAL_BASE + MPFS_UART_LSR_OFFSET) + & UART_LSR_THRE) == 0); + + putreg32(0x55, PX4IO_SERIAL_BASE + MPFS_UART_THR_OFFSET); + + break; + + case 1: { + unsigned fails = 0; + + for (unsigned count = 0;; count++) { + uint16_t value = count & 0xffff; + + if (write((PX4IO_PAGE_TEST << 8) | PX4IO_P_TEST_LED, &value, 1) != 0) { + fails++; + } + + if (count >= 5000) { + syslog(LOG_INFO, "==== test 1 : %u failures ====\n", fails); + perf_print_counter(_pc_txns); + perf_print_counter(_pc_retries); + perf_print_counter(_pc_timeouts); + perf_print_counter(_pc_crcerrs); + perf_print_counter(_pc_protoerrs); + perf_print_counter(_pc_uerrs); + count = 0; + } + } + } + break; + + case 2: + syslog(LOG_INFO, "test 2\n"); + break; + + default: + break; + } + + break; + + default: + ret = -1; + break; + } + + return ret; +} + +int +ArchPX4IOSerial::_bus_exchange(IOPacket *_packet) +{ + int ret = OK; + + uint8_t *packet = (uint8_t *)_packet; + size_t send_size = PKT_SIZE(*_packet); + + /* Protect from concurrent access */ + + _current_packet = _packet; + _packet_cnt = 0; + + /* Incoming packet size too large ? */ + + if (send_size > sizeof(*_current_packet)) { + perf_count(_pc_protoerrs); + return -EINVAL; + } + + /* Measure exchange time */ + + perf_begin(_pc_txns); + + /* Clear fifos */ + + putreg32((UART_FCR_RFIFOR | UART_FCR_XFIFOR), PX4IO_SERIAL_BASE + MPFS_UART_FCR_OFFSET); + + /* Set first interrupt to occur when at least one byte is available */ + + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_RFT_OFFSET); + + /* Enable received data available interrupt */ + + putreg32(UART_IER_ERBFI, PX4IO_SERIAL_BASE + MPFS_UART_IER_OFFSET); + + /* Write package to tx fifo */ + + /* Don't allow interrupts during the UART fifo fill; any delay here could timeout the px4io */ + + irqstate_t flags = px4_enter_critical_section(); + + for (size_t i = 0; i < send_size; i++) { + putreg32(packet[i], PX4IO_SERIAL_BASE + MPFS_UART_THR_OFFSET); + } + + px4_leave_critical_section(flags); + + /* Wait for response, max 10 ms */ + + ret = nxsem_tickwait_uninterruptible(&_completion_semaphore, MSEC2TICK(10)); + + if (ret == -ETIMEDOUT) { + perf_count(_pc_timeouts); + perf_cancel(_pc_txns); /* don't count this as a transaction */ + } + + if (ret == OK) { + /* Check packet CRC - corrupt packet errors mean IO receive CRC error */ + uint8_t crc = _current_packet->crc; + _current_packet->crc = 0; + + if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) { + perf_count(_pc_crcerrs); + ret = -EIO; + } + } + + /* Update counters */ + + perf_end(_pc_txns); + + return ret; +} + +int +ArchPX4IOSerial::_interrupt(int irq, void *context, void *arg) +{ + if (arg != nullptr) { + ArchPX4IOSerial *instance = reinterpret_cast(arg); + + instance->_do_interrupt(); + } + + return 0; +} + +void +ArchPX4IOSerial::_do_interrupt() +{ + /* Get the current status */ + + uint32_t status = getreg32(PX4IO_SERIAL_BASE + MPFS_UART_IIR_OFFSET); + uint8_t *packet = (uint8_t *)_current_packet; + bool first_rcv = _packet_cnt == 0 ? true : false; + int ret = OK; + + switch (status & UART_IIR_IID_MASK) { + + case UART_IIR_IID_RECV: + + /* Get all the available characters from the RX fifo */ + + while ((getreg32(PX4IO_SERIAL_BASE + MPFS_UART_LSR_OFFSET) & UART_LSR_DR) != 0 + && _packet_cnt < sizeof(*_current_packet)) { + packet[_packet_cnt++] = getreg32(PX4IO_SERIAL_BASE + MPFS_UART_RBR_OFFSET); + } + + /* If first bytes of the package were received, extract the packet size */ + + if (first_rcv) { + _expected_size = PKT_SIZE(*_current_packet); + + if (_expected_size > sizeof(*_current_packet)) { + perf_count(_pc_uerrs); + ret = ERROR; + break; + } + } + + if (_packet_cnt >= _expected_size) { + /* Receive complete, + * Disable all interrupts, post the completion semaphore + */ + + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_IER_OFFSET); + sem_post(&_completion_semaphore); + + } else { + /* Set next interrupt to occur when the rest of the bytes are available */ + + putreg32(_expected_size - _packet_cnt - 1, PX4IO_SERIAL_BASE + MPFS_UART_RFT_OFFSET); + } + + break; + + default: + /* Unexpected interrupt */ + ret = ERROR; + break; + } + + if (ret != OK) { + /* For any errors, disable all interrupts, exchange will timeout */ + putreg32(0, PX4IO_SERIAL_BASE + MPFS_UART_IER_OFFSET); + } +} diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt new file mode 100644 index 000000000000..74be8ec1bb43 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/CMakeLists.txt @@ -0,0 +1,36 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_tone_alarm + ToneAlarmInterface.cpp +) diff --git a/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp new file mode 100644 index 000000000000..7b14e1cecbed --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/tone_alarm/ToneAlarmInterface.cpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file ToneAlarmInterface.cpp + */ + +#include +#include +#include +#include +#include +#include + +#ifndef TONE_ALARM_PWM_OUT_PATH +#define TONE_ALARM_PWM_OUT_PATH "/dev/pwmX"; +#endif + +#define TONE_ALARM_PWM_DUTY 50 + +namespace ToneAlarmInterface +{ + +int pwm_fd = 0; + +void init() +{ + /* Open the PWM devnode */ + pwm_fd = open(TONE_ALARM_PWM_OUT_PATH, O_RDONLY); + + if (pwm_fd < 0) { + PX4_ERR("failed to open file %s\n", TONE_ALARM_PWM_OUT_PATH); + } +} + +hrt_abstime start_note(unsigned frequency) +{ + hrt_abstime time_started = 0; + + struct pwm_info_s pwm; + memset(&pwm, 0, sizeof(struct pwm_info_s)); + + pwm.frequency = frequency; + pwm.channels[0].channel = 1; + pwm.channels[0].duty = ((uint32_t)(TONE_ALARM_PWM_DUTY << 16) / 100); + + irqstate_t flags = enter_critical_section(); + + // Set frequency + if (ioctl(pwm_fd, PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n", errno); + } + + time_started = hrt_absolute_time(); + + // Start + if (ioctl(pwm_fd, PWMIOC_START, 0) < 0) { + PX4_ERR("PWMIOC_START failed: %d\n", errno); + } + + leave_critical_section(flags); + + return time_started; +} + +void stop_note() +{ + if (ioctl(pwm_fd, PWMIOC_STOP, 0) < 0) { + PX4_ERR("PWMIOC_STOP failed: %d\n", errno); + } +} + +} /* namespace ToneAlarmInterface */ diff --git a/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt b/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt new file mode 100644 index 000000000000..3c25950736b6 --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/version/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(arch_version + board_mcu_version.c +) + +if (CONFIG_BUILD_FLAT) + target_link_libraries(arch_version PRIVATE nuttx_arch) +else() + target_link_libraries(arch_version PRIVATE nuttx_karch) +endif() diff --git a/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c b/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c new file mode 100644 index 000000000000..a2fd4df5c5de --- /dev/null +++ b/platforms/nuttx/src/px4/microchip/mpfs/version/board_mcu_version.c @@ -0,0 +1,364 @@ +/**************************************************************************** + * + * Copyright (C) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file usr_mcu_version.c + * Implementation of MPFS version API + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#ifndef arraySize +#define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) +#endif + +#define HW_INFO_FPGA_PREFIX "FPGA: " +#define HW_INFO_FPGA_SUFFIX "%u.%u" +#define HW_INFO_FPGA_VER_DIGITS 3 +#define HW_INFO_FPGA_REV_DIGITS 5 + +#define HW_INFO_SIZE (int) arraySize(HW_INFO_INIT_PREFIX) + HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS + sizeof(HW_INFO_FPGA_PREFIX) + HW_INFO_FPGA_VER_DIGITS + HW_INFO_FPGA_REV_DIGITS + 1 +#define FPGA_VER_REGISTER 0x42000000 +#define MPFS_SYS_SERVICE_CR 0x37020050 +#define MPFS_SYS_SERVICE_SR 0x37020054 +#define MPFS_SYS_SERVICE_MAILBOX 0x37020800 +#define SERVICE_CR_REQ (1 << 0) +#define SERVICE_SR_BUSY (1 << 1) + +#define getreg8(a) (*(volatile uint8_t *)(a)) +#define getreg32(a) (*(volatile uint32_t *)(a)) +#define putreg32(v,a) (*(volatile uint32_t *)(a) = (v)) + +#ifndef max +# define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif +#ifndef min +# define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif + +static unsigned hw_version = 0; +static unsigned hw_revision = 0; +static unsigned fpga_version_major; +static unsigned fpga_version_minor; +static char hw_info[HW_INFO_SIZE] = {0}; + +static mfguid_t device_serial_number = { 0 }; + +devinfo_t device_boot_info __attribute__((section(".deviceinfo"))); + +static void determine_hw(uint32_t fpga_version); + +static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID_MPFS; + +__EXPORT const char *board_get_hw_type_name(void) +{ + return (const char *) hw_info; +} + +__EXPORT int board_get_hw_version(void) +{ + return hw_version; +} + +__EXPORT int board_get_hw_revision() +{ + return hw_revision; +} + +__EXPORT void board_get_uuid32(uuid_uint32_t uuid_words) +{ + + /* DEPRECATED. Use board_get_px4_guid() */ + + uint32_t chip_uuid[PX4_CPU_UUID_WORD32_LENGTH]; + memset((uint8_t *)chip_uuid, 0, PX4_CPU_UUID_WORD32_LENGTH * 4); + + for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) { + uuid_words[i] = chip_uuid[i]; + } +} + +int board_mcu_version(char *rev, const char **revstr, const char **errata) +{ + const struct { + const char *revstr; + char rev; + const char *errata; + } hw_version_table[] = BOARD_REVISIONS; + + unsigned len = sizeof(hw_version_table) / sizeof(hw_version_table[0]); + + if (hw_version < len) { + *rev = hw_version_table[hw_version].rev; + *revstr = hw_version_table[hw_version].revstr; + *errata = hw_version_table[hw_version].errata; + } + + return hw_version; +} + +const char *board_bl_version_string(void) +{ + return device_boot_info.bl_version; +} + +int board_get_px4_guid(px4_guid_t px4_guid) +{ + uint8_t *pb = (uint8_t *) &px4_guid[0]; + + memset(pb, 0, sizeof(px4_guid_t)); + + static_assert(sizeof(device_serial_number) == MPFS_DSN_LENGTH); + static_assert(sizeof(px4_guid_t) >= sizeof(device_serial_number) + 2); + + *pb++ = (soc_arch_id >> 8) & 0xff; + *pb++ = (soc_arch_id & 0xff); + + memcpy(pb, device_serial_number, sizeof(device_serial_number)); + + return PX4_GUID_BYTE_LENGTH; +} + +int board_get_px4_guid_formated(char *format_buffer, int size) +{ + px4_guid_t px4_guid; + board_get_px4_guid(px4_guid); + int offset = 0; + + /* size should be 2 per byte + 1 for termination + * So it needs to be odd + */ + size = size & 1 ? size : size - 1; + + /* Discard from MSD */ + for (unsigned i = PX4_GUID_BYTE_LENGTH - size / 2; offset < size && i < PX4_GUID_BYTE_LENGTH; i++) { + offset += snprintf(&format_buffer[offset], size - offset, "%02x", px4_guid[i]); + } + + return offset; +} + +/* Parse git tag string to a 64-bit hex as follows: + + * 0xvvmmrrpphhhhhhhd , where + * v = major version + * m = minor version + * r = revision + * p = patch version + * h = git hash + * d = dirty (0 if clean) + */ + +static uint64_t parse_tag_to_version(const char *ver_str) +{ + uint64_t out = 0; + unsigned len = strlen(ver_str); + unsigned g_count = 0; + unsigned dot_count = 0; + unsigned dash_count = 0; + uint64_t ver = 0; + unsigned i; + char c; + + for (i = 0; i < len; i++) { + c = ver_str[i]; + + if (g_count == 0 && + c >= '0' && c <= '9') { + ver = ver * 10 + c - '0'; + } + + /* Bits 63-32 are version numbers, 8 bits each (major minor revision patch) */ + + if (c == '.' || (dash_count == 0 && c == '-')) { + dot_count++; + out |= ((ver & 0xff) << (64 - 8 * dot_count)); + ver = 0; + } + + if (c == '-') { + dash_count++; + } + + /* Bits 31-4 are the 7 digits of git hash */ + + if (g_count > 0 && g_count < 8) { + if (c >= '0' && c <= '9') { + out |= (uint64_t)(c - '0') << (32 - 4 * g_count++); + + } else if (c >= 'a' && c <= 'f') { + out |= (uint64_t)(c - 'a' + 10) << (32 - 4 * g_count++); + + } else { + g_count = 8; + } + } + + if (c == 'g') { + g_count++; + } + + /* If d(i)rty, set bits 3-0 */ + + if (g_count > 7 && c == 'i') { + out |= 0xf; + break; + } + } + + return out; +} + +/************************************************************************************ + * Name: board_determine_hw_info + * + * Description: + * Uses the HW revision and version detection + * + * Input Parameters: + * None + * + * Returned Value: + * 0 - on success or negated errono + * 1) The values for integer value of this boards hardware revision is set + * 2) The integer value of this boards hardware version is set. + * 3) hw_info is populated + * + ************************************************************************************/ + +int board_determine_hw_info(void) +{ + struct system_version_string_s ver_str; + struct system_version_s ver; + struct guid_s guid; + struct hw_info_s hwinfo; + orb_advert_t ver_str_pub = orb_advertise(ORB_ID(system_version_string), NULL); + orb_advert_t ver_pub = orb_advertise(ORB_ID(system_version), NULL); + orb_advert_t mfguid_pub = orb_advertise(ORB_ID(guid), NULL); + orb_advert_t hw_info_pub = orb_advertise(ORB_ID(hw_info), NULL); + + uint32_t fpga_version = getreg32(FPGA_VER_REGISTER); // todo: replace eventually with device_boot_info + + memset(&ver_str, 0, sizeof(ver_str)); + memset(&ver, 0, sizeof(ver)); + + determine_hw(fpga_version); + + snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX " " HW_INFO_FPGA_PREFIX HW_INFO_FPGA_SUFFIX, + hw_version, hw_revision, fpga_version_major, fpga_version_minor); + + /* HW version */ + + snprintf(ver_str.hw_version, sizeof(ver_str.hw_version), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision); + ver.hw_version = hw_version; + + /* HW revision */ + + ver.hw_revision = hw_revision; + + /* SoC architecture ID */ + + ver.soc_arch_id = soc_arch_id; + + /* PX4 version */ + + strncpy(ver_str.sw_version, PX4_GIT_TAG_STR, sizeof(ver_str.sw_version)); + ver.sw_version = parse_tag_to_version(PX4_GIT_TAG_STR); + + /* NuttX version */ + + snprintf(ver_str.os_version, sizeof(ver_str.os_version), NUTTX_GIT_TAG_STR "-g" NUTTX_GIT_VERSION_STR); + ver.os_version = parse_tag_to_version(ver_str.os_version); + + /* Bootloader version */ + + strncpy(ver_str.bl_version, device_boot_info.bl_version, sizeof(ver_str.bl_version)); + ver.bl_version = parse_tag_to_version(device_boot_info.bl_version); + + /* FPGA version */ + + snprintf(ver_str.component_version1, sizeof(ver_str.component_version1), + HW_INFO_FPGA_PREFIX HW_INFO_FPGA_SUFFIX " (0x%x)", fpga_version_major, fpga_version_minor, getreg32(FPGA_VER_REGISTER)); + ver.component_version1 = fpga_version; + + /* Make local copies of guid and hwinfo */ + + memcpy(&guid, device_serial_number, min(sizeof(device_serial_number), sizeof(guid))); + memcpy(&hwinfo, hw_info, min(sizeof(hwinfo), sizeof(hw_info))); + + /* Then publish the topics */ + + orb_publish(ORB_ID(system_version_string), &ver_str_pub, &ver_str); + orb_publish(ORB_ID(system_version), &ver_pub, &ver); + orb_publish(ORB_ID(guid), &mfguid_pub, &guid); + orb_publish(ORB_ID(hw_info), &hw_info_pub, &hwinfo); + + return OK; +} + +static void determine_hw(uint32_t fpga_version_reg) +{ + /* read device serial number */ + mpfs_read_dsn(device_serial_number, sizeof(device_serial_number)); + fpga_version_major = (fpga_version_reg >> 8) & 0xff; + fpga_version_minor = (fpga_version_reg >> 16) & 0xffff; + hw_version = fpga_version_reg & 0x3f; + +#ifdef BOARD_HAS_MULTIPURPOSE_VERSION_PINS + /* NOTE: utilizes same GPIOs as LEDs. Restore GPIO pin configuration */ + px4_arch_configgpio(GPIO_nLED_RED); + px4_arch_configgpio(GPIO_nLED_GREEN); + px4_arch_configgpio(GPIO_nLED_BLUE); +#endif +} + +int board_get_mfguid(mfguid_t mfgid) +{ + return mpfs_read_dsn(mfgid, sizeof(mfguid_t)); +} diff --git a/platforms/nuttx/toc/CMakeLists.txt b/platforms/nuttx/toc/CMakeLists.txt new file mode 100644 index 000000000000..60dc828490e5 --- /dev/null +++ b/platforms/nuttx/toc/CMakeLists.txt @@ -0,0 +1,78 @@ +############################################################################ +# +# Copyright (c) 2022 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +add_executable(toc + fw_image.c +) + +set(TOC_NAME ${PX4_BINARY_DIR}/toc.elf) +set_target_properties(toc PROPERTIES OUTPUT_NAME ${TOC_NAME}) +target_compile_options(toc PRIVATE -DPX4_UNSIGNED_FIRMWARE=${PX4_BINARY_OUTPUT}) +set_source_files_properties(fw_image.c PROPERTIES OBJECT_DEPENDS px4_binary) + +add_library(board_toc + ${PX4_BOARD_DIR}/src/toc.c # The board specific ToC file +) +add_dependencies(board_toc nuttx_context) + +target_link_libraries(toc PRIVATE + + -nostartfiles + -nodefaultlibs + -nostdlib + + -Wl,--script=${NUTTX_CONFIG_DIR}/scripts/${SCRIPT_PREFIX}toc.ld + -Wl,--warn-common + -Wl,-Map=${PX4_BINARY_DIR}/toc.map +) + +if(NOT USE_LD_GOLD) + target_link_libraries(toc PRIVATE -Wl,--print-memory-usage) +endif() + +set(toc_libs board_toc) + +if (TARGET image_cfg) + list(APPEND toc_libs image_cfg) +endif() + +target_link_libraries(toc PRIVATE ${toc_libs}) +target_link_libraries(drivers_board PRIVATE ${toc_libs}) + +set(TOC_BINARY_OUTPUT ${PX4_BINARY_DIR}/toc.bin) + +add_custom_command(OUTPUT ${TOC_BINARY_OUTPUT} + COMMAND ${CMAKE_OBJCOPY} -O binary ${TOC_NAME} ${TOC_BINARY_OUTPUT} + DEPENDS toc +) +add_custom_target(toc_bin DEPENDS ${TOC_BINARY_OUTPUT}) diff --git a/platforms/nuttx/toc/fw_image.c b/platforms/nuttx/toc/fw_image.c new file mode 100644 index 000000000000..2ea88b9d6652 --- /dev/null +++ b/platforms/nuttx/toc/fw_image.c @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * Copyright (C) 2022 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 PX4_UNSIGNED_FIRMWARE +# error "The path to the unsigned PX4 image is not set" +#endif + +/* The unsigned firmware is injected here + * + * The build process goes as follows: + * 1. The unsigned firmware is built + * - In case of protected mode the kernel + user bin files are concatenated + * together (with padding), resulting in a single, unsigned binary file + * 2. The Table-of-Contents (ToC) is built along with this file. This provides + * the ToC with the firmware boundaries and the location of the firmware + * signature. + * 3. The resulting ToC + firmware binary is signed, which is the final result + */ + +#define __STR(s) #s +#define __XSTR(s) __STR(s) + +__asm__ +( + ".section .firmware,\"ax\"\n" + ".incbin \"" __XSTR(PX4_UNSIGNED_FIRMWARE) "\"\n" +); diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index db6bfe850b41..9c2b78b56d08 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -79,6 +79,8 @@ #include "px4_daemon/server.h" #include "px4_daemon/pxh.h" +#include + #define MODULE_NAME "px4" static const char *LOCK_FILE_PATH = "/tmp/px4_lock"; diff --git a/src/drivers/adc/ads1115/ADS1115.cpp b/src/drivers/adc/ads1115/ADS1115.cpp index 5ce4ae350481..dc7b033e8067 100644 --- a/src/drivers/adc/ads1115/ADS1115.cpp +++ b/src/drivers/adc/ads1115/ADS1115.cpp @@ -55,7 +55,7 @@ int ADS1115::init() setChannel(ADS1115::A0); // prepare for the first measure. - ScheduleOnInterval(SAMPLE_INTERVAL / 4, SAMPLE_INTERVAL / 4); + ScheduleDelayed(SAMPLE_INTERVAL / 4); return PX4_OK; } diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 6f3c1e92be2d..79a31198c945 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -127,7 +127,7 @@ class ADS1115 : public device::I2C, public I2CSPIDriver private: - uORB::Publication _to_adc_report{ORB_ID(adc_report)}; + uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; static const hrt_abstime SAMPLE_INTERVAL{50_ms}; diff --git a/src/drivers/adc/ads1115/ads1115_main.cpp b/src/drivers/adc/ads1115/ads1115_main.cpp index 18aa4cee9ab9..ad80b46f11e5 100644 --- a/src/drivers/adc/ads1115/ads1115_main.cpp +++ b/src/drivers/adc/ads1115/ads1115_main.cpp @@ -129,6 +129,9 @@ void ADS1115::RunImpl() } } + // Schedule the next sample reading (regardless of isSampleReady()) + ScheduleDelayed(SAMPLE_INTERVAL / 4); + perf_end(_cycle_perf); } diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index c03e63aca45a..4fcbf0aa493f 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -852,7 +852,7 @@ CameraTrigger::engage(void *arg) trigger.feedback = false; trigger.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(camera_trigger), trig->_trigger_pub, &trigger); + orb_publish(ORB_ID(camera_trigger), &trig->_trigger_pub, &trigger); // increment frame count trig->_trigger_seq++; diff --git a/src/drivers/cyphal/legacy_data_types b/src/drivers/cyphal/legacy_data_types deleted file mode 160000 index 36a01e428b11..000000000000 --- a/src/drivers/cyphal/legacy_data_types +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 36a01e428b110ff84c8babe5b65667b5e3037d5e diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index 3144237ab899..db314c1f47ed 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/src/drivers/distance_sensor/mappydot/MappyDot.cpp @@ -196,7 +196,7 @@ class MappyDot : public device::I2C, public ModuleParams, public I2CSPIDriver +#include #endif __BEGIN_DECLS @@ -81,12 +82,10 @@ typedef struct hrt_call { hrt_callout callout; void *arg; #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) - hrt_callout usr_callout; - void *usr_arg; + px4_sem_t *callout_sem; #endif } *hrt_call_t; - #define LATENCY_BUCKET_COUNT 8 extern const uint16_t latency_bucket_count; extern const uint16_t latency_buckets[LATENCY_BUCKET_COUNT]; @@ -99,7 +98,10 @@ typedef struct latency_info { #if defined(__PX4_NUTTX) && !defined(CONFIG_BUILD_FLAT) +typedef void *px4_hrt_handle_t; + typedef struct hrt_boardctl { + const px4_hrt_handle_t handle; hrt_call_t entry; hrt_abstime time; /* delay or calltime */ hrt_abstime interval; @@ -123,6 +125,9 @@ typedef struct latency_boardctl { #define HRT_CANCEL _HRTIOC(6) #define HRT_GET_LATENCY _HRTIOC(7) #define HRT_RESET_LATENCY _HRTIOC(8) +#define HRT_REGISTER _HRTIOC(9) +#define HRT_UNREGISTER _HRTIOC(10) +#define HRT_ABSTIME_BASE _HRTIOC(11) #endif @@ -294,17 +299,17 @@ namespace time_literals // User-defined integer literals for different time units. // The base unit is hrt_abstime in microseconds -constexpr hrt_abstime operator "" _s(unsigned long long seconds) +constexpr hrt_abstime operator ""_s(unsigned long long seconds) { return hrt_abstime(seconds * 1000000ULL); } -constexpr hrt_abstime operator "" _ms(unsigned long long milliseconds) +constexpr hrt_abstime operator ""_ms(unsigned long long milliseconds) { return hrt_abstime(milliseconds * 1000ULL); } -constexpr hrt_abstime operator "" _us(unsigned long long microseconds) +constexpr hrt_abstime operator ""_us(unsigned long long microseconds) { return hrt_abstime(microseconds); } diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index a28f6f270654..0c17bdfd4d43 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16448; @@ -114,7 +114,7 @@ class ADIS16448 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _check_crc{false}; // CRC-16 not supported on earlier models (eg ADIS16448AMLZ) diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp index 40c6fab14f9f..a0a94056295f 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace Analog_Devices_ADIS16470; @@ -107,7 +107,7 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; bool _self_test_passed{false}; diff --git a/src/drivers/imu/bosch/bmi055/BMI055.hpp b/src/drivers/imu/bosch/bmi055/BMI055.hpp index f4fad2772ea2..6cc8617cb5b7 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055.hpp +++ b/src/drivers/imu/bosch/bmi055/BMI055.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -66,7 +67,7 @@ class BMI055 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088/BMI088.hpp b/src/drivers/imu/bosch/bmi088/BMI088.hpp index fa6cc223701a..7008f7cee243 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088/BMI088.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -66,7 +67,7 @@ class BMI088 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp index 6c1734a059db..25068c1c9245 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp +++ b/src/drivers/imu/bosch/bmi088_i2c/BMI088.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } @@ -72,7 +73,7 @@ class BMI088 : public device::I2C, public I2CSPIDriver int _total_failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index 3855ce12ea93..8838cd48c34e 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20602; @@ -140,7 +140,7 @@ class ICM20602 : public device::SPI, public I2CSPIDriver hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index 9608eeb816ac..bd11f45bcf4d 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20608G; @@ -138,7 +138,7 @@ class ICM20608G : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index 531f0be988ef..43aaa1c632c7 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20649; @@ -152,7 +152,7 @@ class ICM20649 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index df3b8a734e90..206b57ed5cff 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM20689; @@ -138,7 +138,7 @@ class ICM20689 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 103a03fe573e..c9e53deab785 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "ICM20948_AK09916.hpp" @@ -169,7 +169,7 @@ class ICM20948 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..f3f47677539e 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM40609D; @@ -146,7 +146,7 @@ class ICM40609D : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42605/ICM42605.hpp b/src/drivers/imu/invensense/icm42605/ICM42605.hpp index 98ad916d90dc..d4bee687721c 100644 --- a/src/drivers/imu/invensense/icm42605/ICM42605.hpp +++ b/src/drivers/imu/invensense/icm42605/ICM42605.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42605; @@ -146,7 +146,7 @@ class ICM42605 : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp index 362418f7aea3..ec497d67ea72 100644 --- a/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp +++ b/src/drivers/imu/invensense/icm42670p/ICM42670P.hpp @@ -47,7 +47,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42670P; @@ -150,7 +150,7 @@ class ICM42670P : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index a04e74e70784..4ad0d1beebce 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_ICM42688P; @@ -164,7 +164,7 @@ class ICM42688P : public device::SPI, public I2CSPIDriver enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; enum class STATE : uint8_t { @@ -179,11 +179,12 @@ class ICM42688P : public device::SPI, public I2CSPIDriver int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _checked_register_bank0{0}; - static constexpr uint8_t size_register_bank0_cfg{16}; + static constexpr uint8_t size_register_bank0_cfg{17}; register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { // Register | Set bits, Clear bits { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_ENDIAN | INTF_CONFIG0_BIT::SENSOR_DATA_ENDIAN | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_2000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp index 6c407216f044..79971a1bdd99 100644 --- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp +++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp @@ -159,6 +159,16 @@ enum SIGNAL_PATH_RESET_BIT : uint8_t { FIFO_FLUSH = Bit1, }; +// INTF_CONFIG0 +enum INTF_CONFIG0_BIT : uint8_t { + FIFO_HOLD_LAST_DATA_EN = Bit7, + FIFO_COUNT_REC = Bit6, + FIFO_COUNT_ENDIAN = Bit5, + SENSOR_DATA_ENDIAN = Bit4, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0, +}; + +// INTF_CONFIG1 enum INTF_CONFIG1_BIT : uint8_t { RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required CLKSEL = Bit0, diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index 2d5180013a22..ff8df4f409d7 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6000; @@ -140,7 +140,7 @@ class MPU6000 : public device::SPI, public I2CSPIDriver FIFO::DATA _fifo_sample_last_new_accel{}; uint32_t _fifo_accel_samples_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 7686db5114e5..712790ec3266 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU6500; @@ -138,7 +138,7 @@ class MPU6500 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 369d2ec6e7cb..c0d26521d21b 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include "MPU9250_AK8963.hpp" @@ -150,7 +150,7 @@ class MPU9250 : public device::SPI, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp index 7af10d0b6076..d6aa3bdde105 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include using namespace InvenSense_MPU9250; @@ -136,7 +136,7 @@ class MPU9250_I2C : public device::I2C, public I2CSPIDriver hrt_abstime _temperature_update_timestamp{0}; int _failure_count{0}; - px4::atomic _drdy_timestamp_sample{0}; + px4::atomic_from_isr _drdy_timestamp_sample{0}; int32_t _drdy_count{0}; bool _data_ready_interrupt_enabled{false}; diff --git a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp index 78279c60818b..5f3e90c56630 100755 --- a/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp +++ b/src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp @@ -248,7 +248,7 @@ extern "C" __EXPORT int rgbled_ncp5623c_main(int argc, char *argv[]) { using ThisDriver = RGBLED_NCP5623C; BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = 100000; + cli.default_i2c_frequency = 400000; cli.i2c_address = NCP5623C_ADDR; cli.custom1 = 123; int ch; diff --git a/src/drivers/pfsoc_crypto b/src/drivers/pfsoc_crypto new file mode 160000 index 000000000000..6444eeee668f --- /dev/null +++ b/src/drivers/pfsoc_crypto @@ -0,0 +1 @@ +Subproject commit 6444eeee668f2d49805ff4946347ea604c44ddaf diff --git a/src/drivers/pfsoc_keystore b/src/drivers/pfsoc_keystore new file mode 160000 index 000000000000..47b594070176 --- /dev/null +++ b/src/drivers/pfsoc_keystore @@ -0,0 +1 @@ +Subproject commit 47b59407017696c82b633349b28812162a010b57 diff --git a/src/drivers/pwm_esc/CMakeLists.txt b/src/drivers/pwm_esc/CMakeLists.txt new file mode 100644 index 000000000000..f6981328aef1 --- /dev/null +++ b/src/drivers/pwm_esc/CMakeLists.txt @@ -0,0 +1,51 @@ +############################################################################ +# +# Copyright (c) 2021 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +set(PARAM_PREFIX PWM_MAIN) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX PWM_AUX) +endif() + +px4_add_module( + MODULE drivers__pwm_esc + MAIN pwm_esc + COMPILE_FLAGS + -DPARAM_PREFIX="${PARAM_PREFIX}" + SRCS + pwm_esc.cpp + MODULE_CONFIG + module.yaml + DEPENDS + mixer_module + ) diff --git a/src/drivers/pwm_esc/Kconfig b/src/drivers/pwm_esc/Kconfig new file mode 100644 index 000000000000..04a3ff1b0f97 --- /dev/null +++ b/src/drivers/pwm_esc/Kconfig @@ -0,0 +1,12 @@ +menuconfig DRIVERS_PWM_ESC + bool "pwm_esc" + default n + ---help--- + Enable support for pwm_esc + +menuconfig USER_PWM_ESC + bool "pwm_esc running as userspace module" + default n + depends on BOARD_PROTECTED && DRIVERS_PWM_ESC + ---help--- + Put pwm_esc in userspace memory diff --git a/src/drivers/pwm_esc/module.yaml b/src/drivers/pwm_esc/module.yaml new file mode 100644 index 000000000000..70d213071b8e --- /dev/null +++ b/src/drivers/pwm_esc/module.yaml @@ -0,0 +1,12 @@ +module_name: PWMESC +actuator_output: + output_groups: + - param_prefix: '${PWM_MAIN_OR_AUX}' + channel_label: '${PWM_MAIN_OR_AUX}' + channel_label_module_name_prefix: false + num_channels: 10 + standard_params: + disarmed: { min: 800, max: 2200, default: 900 } + min: { min: 800, max: 1400, default: 1000 } + max: { min: 1600, max: 2200, default: 2000 } + failsafe: { min: 800, max: 2200 } diff --git a/src/drivers/pwm_esc/pwm_esc.cpp b/src/drivers/pwm_esc/pwm_esc.cpp new file mode 100644 index 000000000000..ce2bd28ec573 --- /dev/null +++ b/src/drivers/pwm_esc/pwm_esc.cpp @@ -0,0 +1,554 @@ +/**************************************************************************** + * + * Copyright (c) 2021 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file pwm_esc.cpp + * Driver for the NuttX PWM driver controleed escs + * + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#ifndef PWMESC_OUT_PATH +#define PWMESC_OUT_PATH "/dev/pwmX"; +#endif + +#ifndef PWMESC_MAX_DEVICES +#define PWMESC_MAX_DEVICES 2 +#endif + +#ifndef PWMESC_MAX_CHANNELS +#define PWMESC_MAX_CHANNELS CONFIG_PWM_NCHANNELS +#endif + +#ifndef PWM_DEFAULT_RATE +#define PWM_DEFAULT_RATE 400 +#endif + +//using namespace time_literals; + +/** + * The PWMESC class. + * + */ +class PWMESC : public OutputModuleInterface +{ +public: + /** + * Constructor. + * + * Initialize all class variables. + */ + PWMESC(); + + /** + * Destructor. + * + * Wait for worker thread to terminate. + */ + virtual ~PWMESC(); + + /** + * Initialize the PWMESC class. + * + * Retrieve relevant initial system parameters. Connect to PWM device + * + * @param hitl_mode set to suppress publication of actuator_outputs + */ + int init(bool hitl_mode); + + /** + * Start the PWMESC driver + */ + static int start(int argc, char *argv[]); + + /** + * Stop the PWMESC driver + */ + static int stop(); + + /** + * Return if the PWMESC driver is already running + */ + bool running() {return _initialized;}; + + /** + * updateOutputs + * + * Sets the actual PWM outputs. See OutputModuleInterface + * + */ + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; + + /** + * Don't allow more channels than MAX_ACTUATORS + */ + static_assert(PWMESC_MAX_CHANNELS <= MAX_ACTUATORS, "Increase MAX_ACTUATORS if this fails"); + +private: + + bool _initialized{false}; + + volatile int _task; ///< worker task id + volatile bool _task_should_exit; ///< worker terminate flag + + px4_sem_t _update_sem; + + perf_counter_t _perf_update; ///< local performance counter for PWM updates + + /* subscribed topics */ + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + + MixingOutput _mixing_output{PARAM_PREFIX, PWMESC_MAX_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, true}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1000000}; + + /* advertised topics */ + uORB::PublicationMulti _actuator_outputs_pub{ORB_ID(actuator_outputs)}; + + actuator_armed_s _actuator_armed; + + bool _hitl_mode; ///< Hardware-in-the-loop simulation mode - don't publish actuator_outputs + + int _pwm_fd[PWMESC_MAX_DEVICES]; + + int32_t _pwm_rate{PWM_DEFAULT_RATE}; + + int init_pwm_outputs(); + + /* Singleton pointer */ + static PWMESC *_instance; + + /** + * Trampoline to the worker task + */ + static int task_main_trampoline(int argc, char *argv[]); + + /** + * worker task + */ + void task_main(); + + /** + * Callback for mixer subscriptions + */ + void Run() override; + + void update_params(); + + /* No copy constructor */ + PWMESC(const PWMESC &); + PWMESC operator=(const PWMESC &); + + /** + * Get the singleton instance + */ + static inline PWMESC *getInstance() + { + if (_instance == nullptr) { + /* create the driver */ + _instance = new PWMESC(); + } + + return _instance; + } + + /** + * Set the PWMs on open device fd to 0 duty cycle + * and start or stop (request == PWMIOC_START / PWMIOC_STOP) + */ + int set_defaults(int fd, unsigned long request); +}; + +PWMESC *PWMESC::_instance = nullptr; + +PWMESC::PWMESC() : + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), + _task(-1), + _task_should_exit(false), + _perf_update(perf_alloc(PC_ELAPSED, "pwm update")), + _hitl_mode(false) +{ + + /* initialize tick semaphores */ + px4_sem_init(&_update_sem, 0, 0); + px4_sem_setprotocol(&_update_sem, SEM_PRIO_NONE); + + /* clear armed status */ + memset(&_actuator_armed, 0, sizeof(actuator_armed_s)); +} + +PWMESC::~PWMESC() +{ + /* tell the task we want it to go away */ + _task_should_exit = true; + + /* spin waiting for the task to stop */ + for (unsigned i = 0; (i < 10) && (_task != -1); i++) { + /* give it another 100ms */ + px4_usleep(100000); + } + + /* well, kill it anyway, though this will probably crash */ + if (_task != -1) { + PX4_ERR("Task exit fail\n"); + px4_task_delete(_task); + } + + /* deallocate perfs */ + perf_free(_perf_update); + + px4_sem_destroy(&_update_sem); +} + +int +PWMESC::init(bool hitl_mode) +{ + _hitl_mode = hitl_mode; + + /* start the main task */ + _task = px4_task_spawn_cmd("pwm_esc", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 3048, + (px4_main_t)&PWMESC::task_main_trampoline, + nullptr); + + if (_task < 0) { + PX4_ERR("task start failed: %d", errno); + return -errno; + } + + _initialized = true; + + /* schedule workqueue */ + ScheduleNow(); + + return OK; +} + +int +PWMESC::task_main_trampoline(int argc, char *argv[]) +{ + getInstance()->task_main(); + return 0; +} + +bool +PWMESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, + unsigned num_control_groups_updated) +{ + bool ret = true; + struct pwm_info_s pwm; + + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; + + for (unsigned i = 0; i < num_outputs; i++) { + // TODO: channel to proper pwm device map. + // this is now just quick hack for one pwm device, direct map of channels + + uint16_t pwm_val = outputs[i]; + pwm.channels[i].duty = ((((uint32_t)pwm_val) << 16) / (1000000 / _pwm_rate)); + pwm.channels[i].channel = i + 1; + } + + /* Only publish if not in hitl */ + + if (!_hitl_mode && + ::ioctl(_pwm_fd[0], PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)) < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS failed: %d\n", + errno); + ret = false; + } + + return ret; +} + +int +PWMESC::set_defaults(int fd, unsigned long request) +{ + /* Configure PWM to default rate, 1 as duty and start */ + + struct pwm_info_s pwm; + memset(&pwm, 0, sizeof(struct pwm_info_s)); + pwm.frequency = _pwm_rate; + + for (int j = 0; j < PWMESC_MAX_CHANNELS; j++) { + pwm.channels[j].duty = 1; /* 0 is not allowed duty cycle value */ + pwm.channels[j].channel = j + 1; + } + + /* Set the frequency and duty */ + + int ret = ::ioctl(fd, PWMIOC_SETCHARACTERISTICS, + (unsigned long)((uintptr_t)&pwm)); + + if (ret < 0) { + PX4_ERR("PWMIOC_SETCHARACTERISTICS) failed: %d\n", + errno); + + } else { + + /* Start / stop */ + + ret = ::ioctl(fd, request, 0); + + if (ret < 0) { + PX4_ERR("PWMIOC_START/STOP failed: %d\n", errno); + } + + } + + return ret; +} + +int +PWMESC::init_pwm_outputs() +{ + int ret = 0; + + _mixing_output.setIgnoreLockdown(_hitl_mode); + _mixing_output.setMaxNumOutputs(PWMESC_MAX_CHANNELS); + const int update_interval_in_us = math::constrain(1000000 / (_pwm_rate * 2), 500, 100000); + _mixing_output.setMaxTopicUpdateRate(update_interval_in_us); + + /* Open the PWM devnode */ + + // TODO: loop through the devices, open all fd:s + char pwm_device_name[] = PWMESC_OUT_PATH; + int n_pwm_devices = 1; + + for (int i = 0; i < 1 && i < n_pwm_devices; i++) { + pwm_device_name[sizeof(pwm_device_name) - 2] = '0' + i; + + _pwm_fd[i] = ::open(pwm_device_name, O_RDONLY); + + if (_pwm_fd[i] < 0) { + PX4_ERR("pwm_main: open %s failed: %d\n", pwm_device_name, errno); + ret = -1; + continue; + } + + /* Configure PWM to default rate, 0 pulse and start */ + + if (set_defaults(_pwm_fd[i], PWMIOC_START) != 0) { + ret = -1; + } + } + + return ret; +} + +void +PWMESC::Run() +{ + /* Just trigger the main task */ + px4_sem_post(&_update_sem); +} + +void +PWMESC::task_main() +{ + if (init_pwm_outputs() != 0) { + PX4_ERR("PWM initialization failed"); + _task_should_exit = true; + } + + while (!_task_should_exit) { + + /* Get the armed status */ + + _actuator_armed_sub.update(&_actuator_armed); + + struct timespec ts; + px4_clock_gettime(CLOCK_REALTIME, &ts); + /* Add 100 ms, this can't overflow */ + ts.tv_nsec += 100000000; + + if (ts.tv_nsec >= 1000000000) { + ts.tv_nsec -= 1000000000; + ts.tv_sec += 1; + } + + /* sleep waiting for mixer update */ + int ret = px4_sem_timedwait(&_update_sem, &ts); + + perf_begin(_perf_update); + + if (ret == 0) { + _mixing_output.update(); + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + update_params(); + } + + _mixing_output.updateSubscriptions(true); + + perf_end(_perf_update); + } + + PX4_DEBUG("exiting"); + + /* Configure PWM to default rate, 0 pulse and stop */ + + int n_pwm_devices = 1; + + for (int i = 0; i < 1 && i < n_pwm_devices; i++) { + set_defaults(_pwm_fd[i], PWMIOC_STOP); + } + + /* tell the dtor that we are exiting */ + _task = -1; +} + +void PWMESC::update_params() +{ + /* skip update when armed */ + if (_actuator_armed.armed) { + return; + } + + /* Call MixingOutput::updateParams */ + updateParams(); +} + +extern "C" __EXPORT int pwm_esc_main(int argc, char *argv[]); + +int +PWMESC::start(int argc, char *argv[]) +{ + int ret = 0; + + if (PWMESC::getInstance() == nullptr) { + PX4_ERR("Driver allocation failed"); + return -1; + } + + if (PWMESC::getInstance()->running()) { + PX4_ERR("Already running"); + return -1; + } + + bool hitl_mode = false; + + /* Check if started in hil mode */ + for (int extra_args = 1; extra_args < argc; extra_args++) { + if (!strcmp(argv[extra_args], "hil")) { + hitl_mode = true; + + } else if (argv[extra_args][0] != '\0') { + PX4_WARN("unknown argument: %s", argv[extra_args]); + } + } + + if (OK != PWMESC::getInstance()->init(hitl_mode)) { + delete PWMESC::getInstance(); + PX4_ERR("Driver init failed"); + return -1; + } + + return ret; +} + +int +PWMESC::stop() +{ + if (PWMESC::getInstance() == nullptr) { + PX4_ERR("Driver allocation failed"); + return -1; + } + + if (!PWMESC::getInstance()->running()) { + PX4_ERR("Not running"); + return -1; + } + + delete (PWMESC::getInstance()); + _instance = nullptr; + + return 0; +} + +int +pwm_esc_main(int argc, char *argv[]) +{ + /* check for sufficient number of arguments */ + if (argc < 2) { + PX4_ERR("Need a command, try 'start' / 'stop'"); + return -1; + } + + if (!strcmp(argv[1], "start")) { + return PWMESC::start(argc - 1, argv + 1); + } + + if (!strcmp(argv[1], "stop")) { + return PWMESC::stop(); + } + + return 0; +} diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 21c0a53b2bfc..7b9d29970a99 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -45,7 +45,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include @@ -706,7 +710,7 @@ void PX4IO::update_params() if (output_function >= (int)OutputFunction::Servo1 && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo int32_t val = 1500; - PX4_INFO("Setting channel %i disarmed to %i", (int) val, i); + PX4_INFO("Setting channel %i disarmed to %zu", (int) val, i); param_set(_mixing_output.disarmedParamHandle(i), &val); // If the whole timer group was not set previously, then set the pwm rate to 50 Hz @@ -738,10 +742,10 @@ void PX4IO::update_params() if (output_function >= (int)OutputFunction::Motor1 && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor int32_t val = 1100; - PX4_INFO("Setting channel %i minimum to %i", (int) val, i); + PX4_INFO("Setting channel %i minimum to %zu", (int) val, i); param_set(_mixing_output.minParamHandle(i), &val); val = 1900; - PX4_INFO("Setting channel %i maximum to %i", (int) val, i); + PX4_INFO("Setting channel %i maximum to %zu", (int) val, i); param_set(_mixing_output.maxParamHandle(i), &val); } } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 4010e0d5133e..17ae86a507f5 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -55,7 +55,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include "uploader.h" diff --git a/src/drivers/rover_interface/CMakeLists.txt b/src/drivers/rover_interface/CMakeLists.txt new file mode 100644 index 000000000000..55a9bf4852c8 --- /dev/null +++ b/src/drivers/rover_interface/CMakeLists.txt @@ -0,0 +1,16 @@ +# set(SCOUT_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/scout_sdk) +add_subdirectory(scout_sdk) + +px4_add_module( + MODULE drivers__rover_interface + MAIN rover_interface + STACK_MAIN 4096 + COMPILE_FLAGS + INCLUDES + ${SCOUT_SDK_DIR}/include + SRCS + RoverInterface.cpp + RoverInterface.hpp + DEPENDS + scout_sdk +) diff --git a/src/drivers/rover_interface/Kconfig b/src/drivers/rover_interface/Kconfig new file mode 100644 index 000000000000..806847ad87dd --- /dev/null +++ b/src/drivers/rover_interface/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ROVER_INTERFACE + bool "rover_interface" + default n + ---help--- + Enable support for rover_interface diff --git a/src/drivers/rover_interface/RoverInterface.cpp b/src/drivers/rover_interface/RoverInterface.cpp new file mode 100644 index 000000000000..bfa8d128db09 --- /dev/null +++ b/src/drivers/rover_interface/RoverInterface.cpp @@ -0,0 +1,450 @@ +#include "RoverInterface.hpp" +#include + +RoverInterface *RoverInterface::_instance; + +// CAN interface | default is can0 +const char *const RoverInterface::CAN_IFACE = "can0"; + +RoverInterface::RoverInterface(uint8_t rover_type, uint32_t bitrate, float vehicle_speed_max) + : ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rover_interface), + _rover_type(rover_type), + _bitrate(bitrate), + _vehicle_speed_max(vehicle_speed_max) +{ + pthread_mutex_init(&_node_mutex, nullptr); +} + + +RoverInterface::~RoverInterface() +{ + if (_instance) { + // Tell the task we want it to go away + _task_should_exit.store(true); + ScheduleNow(); + + unsigned i = 1000; + + do { + // Wait for it to exit or timeout + usleep(5000); + + if (--i == 0) { + PX4_ERR("Failed to Stop Task - reboot needed"); + break; + } + + } while (_instance); + } + + perf_free(_cycle_perf); + perf_free(_interval_perf); +} + + +int RoverInterface::start(uint8_t rover_type, uint32_t bitrate, float vehicle_speed_max) +{ + if (_instance != nullptr) { + PX4_ERR("Already started"); + return -1; + } + + _instance = new RoverInterface(rover_type, bitrate, vehicle_speed_max); + + if (_instance == nullptr) { + PX4_ERR("Failed to allocate RoverInterface object"); + return -1; + } + + _instance->ScheduleOnInterval(ScheduleIntervalMs); + + return PX4_OK; +} + + +void RoverInterface::Init() +{ + _initialized = false; + + // Check rover type + switch (_rover_type) { + case 0: + // Scout Mini + PX4_INFO("Scout Mini (rover type 0) is supported. Initializing..."); + break; + + case 1: + // Scout + PX4_INFO("Scout (rover type 1) is not supported. Aborted"); + return; + + case 2: + // Scout Pro + PX4_INFO("Scout Pro (rover type 2) is not supported. Aborted"); + return; + + case 3: + // Scout 2 + PX4_INFO("Scout 2 (rover type 3) is not supported. Aborted"); + return; + + case 4: + // Scout 2 Pro + PX4_INFO("Scout 2 Pro (rover type 4) is not supported. Aborted"); + return; + + case 5: + // Bunker + PX4_INFO("Bunker (rover type 5) is supported. Initializing..."); + break; + + case 6: + // Bunker Mini + PX4_INFO("Bunker Mini (rover type 6) is supported. Initializing..."); + break; + + default: + // Unknown Rover type + PX4_INFO("Unknown rover type. Aborted"); + return; + } + + // Check protocol version and create ScoutRobot object + if (_protocol_version == scoutsdk::ProtocolVersion::AGX_V1) { + PX4_INFO("AGX V1 protocol version is not supported. Aborted"); + + } else if (_protocol_version == scoutsdk::ProtocolVersion::AGX_V2) { + PX4_INFO("Detected AGX V2 protocol"); + _scout = new scoutsdk::ScoutRobot(scoutsdk::ProtocolVersion::AGX_V2, true); + + } else { + PX4_INFO("Unknown protocol version"); + } + + if (_scout == nullptr) { + PX4_ERR("Failed to create the ScoutRobot object. Aborted"); + return; + } + + // Finally establish connection to rover + _scout->Connect(CAN_IFACE, _bitrate); + + if (!_scout->GetCANConnected()) { + PX4_ERR("Failed to connect to the rover CAN bus"); + return; + } + + // Use CAN command mode + _scout->EnableCommandMode(); + + // Retrieve system version (blocking) + _scout->QuerySystemVersion(SystemVersionQueryLimitMs); + + // Setup rover state publisher + if (!orb_advert_valid(_rover_status_pub)) { + _rover_status_pub = orb_advertise(ORB_ID(rover_status), &_rover_status_msg); + } + + // Breathing mode light by default when not armed + _scout->SetLightCommand(LightMode::BREATH, 0); + + _initialized = true; +} + + +void RoverInterface::parameters_update(bool force) +{ + // check for parameter updates + if (_parameter_update_sub.updated() || force) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } +} + + +void RoverInterface::Run() +{ + pthread_mutex_lock(&_node_mutex); + + if (_instance != nullptr && _task_should_exit.load()) { + ScheduleClear(); + + if (_initialized) { _initialized = false; } + + // Clean up + if (_scout != nullptr) { + delete _scout; + _scout = nullptr; + } + + _instance = nullptr; + pthread_mutex_unlock(&_node_mutex); + return; + } + + if (_instance != nullptr && !_initialized) { + // Try initializing for the first time + if (!_init_try_count) { + Init(); + _init_try_count++; + } + + // Return early if still not initialized + if (!_initialized) { + pthread_mutex_unlock(&_node_mutex); + return; + } + } + + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // Update parameters + parameters_update(); + + // Check for actuator armed command to rover + ActuatorArmedUpdate(); + + // Check for action request (kill switch) + ActionRequestUpdate(); + + // Check for actuator controls command to rover + if (!_kill_switch && _armed) { VehicleTorqueAndThrustUpdate(); } + + // Check for vehicle control mode + VehicleControlModeUpdate(); + + // Check for receive msgs from the rover + _scout->CheckUpdateFromRover(); + + // Update from rover and publish the rover state + if (hrt_elapsed_time(&_last_rover_status_publish_time) > RoverStatusPublishIntervalMs) { + PublishRoverState(); + } + + perf_end(_cycle_perf); + + pthread_mutex_unlock(&_node_mutex); +} + + +void RoverInterface::VehicleTorqueAndThrustUpdate() +{ + bool do_update = false; + + // Check torque setppoint update + if (_vehicle_torque_setpoint_sub.updated()) { + vehicle_torque_setpoint_s vehicle_torque_setpoint_msg; + + if (_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint_msg)) { + _yaw_control = vehicle_torque_setpoint_msg.xyz[2]; + do_update = true; + } + } + + // Check thrust setpoint update + if (_vehicle_thrust_setpoint_sub.updated()) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_msg; + + if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint_msg)) { + _throttle_control = vehicle_thrust_setpoint_msg.xyz[0]; + do_update = true; + } + } + + if (do_update) { + auto throttle = (_is_manual_mode ? _param_man_speed_scale.get() : _vehicle_speed_max) * _throttle_control; + auto steering = _yaw_control; + _scout->SetMotionCommand(throttle, steering); + } +} + + +void RoverInterface::ActuatorArmedUpdate() +{ + if (_actuator_armed_sub.updated()) { + actuator_armed_s actuator_armed_msg; + + if (_actuator_armed_sub.copy(&actuator_armed_msg)) { + // Arm or disarm the rover + if (!_armed && actuator_armed_msg.armed) { + _scout->SetLightCommand(LightMode::CONST_ON, 0); + _armed = true; + + } else if (_armed && !actuator_armed_msg.armed) { + _scout->SetLightCommand(LightMode::BREATH, 0); + _armed = false; + } + } + } +} + + +void RoverInterface::ActionRequestUpdate() +{ + if (_action_request_sub.updated()) { + action_request_s action_request_msg; + + if (_action_request_sub.copy(&action_request_msg)) { + // Check for kill switch (expected pub rate is 5Hz) + switch (action_request_msg.action) { + case action_request_s::ACTION_KILL: + _kill_switch = true; + _scout->SetMotionCommand(0.0, 0.0); + break; + + case action_request_s::ACTION_UNKILL: + _kill_switch = false; + break; + } + } + } +} + + +void RoverInterface::VehicleControlModeUpdate() +{ + if (_vehicle_control_mode_sub.updated()) { + vehicle_control_mode_s vehicle_control_mode_msg; + + if (_vehicle_control_mode_sub.copy(&vehicle_control_mode_msg)) { + _is_manual_mode = vehicle_control_mode_msg.flag_control_manual_enabled; + } + } +} + + +void RoverInterface::PublishRoverState() +{ + // Get rover state + auto &robot_state = _scout->GetRobotState(); + + // Assign the values to the PX4 side ORB msg + _rover_status_msg.timestamp = hrt_absolute_time(); + _rover_status_msg.linear_velocity = robot_state.motion_state.linear_velocity; + _rover_status_msg.angular_velocity = robot_state.motion_state.angular_velocity; + _rover_status_msg.vehicle_state = robot_state.system_state.vehicle_state; + _rover_status_msg.control_mode = robot_state.system_state.control_mode; + _rover_status_msg.error_code = robot_state.system_state.error_code; + _rover_status_msg.battery_voltage = robot_state.system_state.battery_voltage; + _rover_status_msg.light_control_enable = robot_state.light_state.enable_cmd_ctrl; + _rover_status_msg.front_light_mode = robot_state.light_state.front_light.mode; + _rover_status_msg.front_light_custom_value = robot_state.light_state.front_light.custom_value; + _rover_status_msg.rear_light_mode = robot_state.light_state.rear_light.mode; + _rover_status_msg.rear_light_custom_value = robot_state.light_state.rear_light.custom_value; + + if (orb_advert_valid(_rover_status_pub)) { + orb_publish(ORB_ID(rover_status), &_rover_status_pub, &_rover_status_msg); + _last_rover_status_publish_time = _rover_status_msg.timestamp; + } +} + + +void RoverInterface::print_status() +{ + pthread_mutex_lock(&_node_mutex); + + if (_scout == nullptr) { + PX4_ERR("Scout Robot object not initialized"); + pthread_mutex_unlock(&_node_mutex); + return; + } + + // CAN connection info + PX4_INFO("CAN interface: %s. Status: %s", + RoverInterface::CAN_IFACE, _scout->GetCANConnected() ? "connected" : "disconnected"); + + // Rover info + if (_scout->GetCANConnected()) { + PX4_INFO("Rover Type: %d. Protocol Version: %s", + _rover_type, + _protocol_version == scoutsdk::ProtocolVersion::AGX_V2 ? "AGX_V2" : "Unknown"); + PX4_INFO("Rover system version: %s", _scout->GetSystemVersion()); + } + + // Arm / disarm / kill switch status + PX4_INFO("Rover is armed: %s. Kill switch: %s", _armed ? "true" : "false", _kill_switch ? "true" : "false"); + + // Subscription info + PX4_INFO("Subscribed to topics: %s, %s, %s", + _vehicle_thrust_setpoint_sub.get_topic()->o_name, + _vehicle_torque_setpoint_sub.get_topic()->o_name, + _actuator_armed_sub.get_topic()->o_name); + + // Publication info + if (orb_advert_valid(_rover_status_pub)) { PX4_INFO("Publishing rover_status topic"); } + + // Performance counters + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + + pthread_mutex_unlock(&_node_mutex); +} + + +static void print_usage() +{ + PX4_INFO("Usage: \n\trover_interface {start|status|stop}"); +} + + +extern "C" __EXPORT int rover_interface_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + if (RoverInterface::instance()) { + PX4_ERR("Already started"); + return 1; + } + + // Rover type | default is Scout Mini + int32_t rover_type = 0; + param_get(param_find("RI_ROVER_TYPE"), &rover_type); + + // Rover interface CAN bitrate | default is 500Kbit/s + int32_t can_bitrate = 0; + param_get(param_find("RI_CAN_BITRATE"), &can_bitrate); + + // Vehicle speed max | depending on rover type + float vehicle_speed_max = 0.0f; + param_get(param_find("GND_SPEED_MAX"), &vehicle_speed_max); + + // Start + PX4_INFO("Start Rover Interface to rover type %d at CAN iface %s with bitrate %d bit/s", + rover_type, RoverInterface::CAN_IFACE, can_bitrate); + return RoverInterface::start(static_cast(rover_type), + can_bitrate, + vehicle_speed_max + ); + } + + /* commands below assume that the app has been already started */ + RoverInterface *const inst = RoverInterface::instance(); + + if (!inst) { + PX4_ERR("Application not running"); + return 1; + } + + if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) { + inst->print_status(); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + delete inst; + return 0; + } + + print_usage(); + return 1; +} diff --git a/src/drivers/rover_interface/RoverInterface.hpp b/src/drivers/rover_interface/RoverInterface.hpp new file mode 100644 index 000000000000..888b06bba5cb --- /dev/null +++ b/src/drivers/rover_interface/RoverInterface.hpp @@ -0,0 +1,121 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "scout_sdk/ScoutRobot.hpp" + +using namespace time_literals; + +class RoverInterface : public ModuleParams, public px4::ScheduledWorkItem +{ + /* + * Base interval, has to be compliant with the rate of the actuator_controls + * topic subscription and CAN bus update rate + */ + static constexpr uint64_t ScheduleIntervalMs{10_ms}; + + static constexpr uint64_t RoverStatusPublishIntervalMs{1000_ms}; + + static constexpr uint64_t SystemVersionQueryLimitMs{10_ms}; + + static constexpr uint64_t ControlSubIntervalMs{50_ms}; + +public: + static const char *const CAN_IFACE; + + RoverInterface(uint8_t rover_type, uint32_t bitrate, float vehicle_speed_max); + ~RoverInterface() override; + + static int start(uint8_t rover_type, uint32_t bitrate, float vehicle_speed_max); + + void print_status(); + + static RoverInterface *instance() { return _instance; } + +private: + void Init(); + void Run() override; + + void VehicleTorqueAndThrustUpdate(); + void ActuatorArmedUpdate(); + void ActionRequestUpdate(); + void VehicleControlModeUpdate(); + void PublishRoverState(); + + // Flag to indicate to tear down the rover interface + px4::atomic_bool _task_should_exit{false}; + + bool _armed{false}; + + bool _kill_switch{false}; + + bool _initialized{false}; + + uint8_t _init_try_count{0}; + + bool _is_manual_mode{false}; + + static RoverInterface *_instance; + + pthread_mutex_t _node_mutex; + + uint8_t _rover_type; + + uint32_t _bitrate; + + float _vehicle_speed_max; + + float _throttle_control; + + float _yaw_control; + + scoutsdk::ProtocolVersion _protocol_version{scoutsdk::ProtocolVersion::AGX_V2}; + + const char *_can_iface{nullptr}; + + scoutsdk::ScoutRobot *_scout{nullptr}; + + // Subscription + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint), ControlSubIntervalMs}; + uORB::SubscriptionInterval _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint), ControlSubIntervalMs}; + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _action_request_sub{ORB_ID(action_request)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + + // Publication + orb_advert_t _rover_status_pub{ORB_ADVERT_INVALID}; + rover_status_s _rover_status_msg{}; + hrt_abstime _last_rover_status_publish_time{0}; + + // Performance counters + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_man_speed_scale + ) + + /** + * Update our local parameter cache. + */ + void parameters_update(bool force = false); +}; diff --git a/src/drivers/rover_interface/rover_interface_params.c b/src/drivers/rover_interface/rover_interface_params.c new file mode 100644 index 000000000000..b8b4f629ef20 --- /dev/null +++ b/src/drivers/rover_interface/rover_interface_params.c @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * Rover type. + * + * @value -1 Not selected + * @value 0 Scout Mini + * @value 1 Scout + * @value 2 Scout Pro + * @value 3 Scout 2 + * @value 4 Scout 2 Pro + * @value 5 Bunker + * @value 6 Bunker Mini + * @group Rover Interface + */ +PARAM_DEFINE_INT32(RI_ROVER_TYPE, -1); + +/** + * Rover interface CAN bitrate. + * + * @unit bit/s + * @min 20000 + * @max 1000000 + * @reboot_required true + * @group Rover Interface + */ +PARAM_DEFINE_INT32(RI_CAN_BITRATE, 500000); + + +/** + * Rover interface manual control speed scale. + * + * @unit %m/s + * @min 1.0 + * @max 3.0 + * @group Rover Interface + */ +PARAM_DEFINE_FLOAT(RI_MAN_SPD_SC, 1.0); diff --git a/src/drivers/rover_interface/scout_sdk/CMakeLists.txt b/src/drivers/rover_interface/scout_sdk/CMakeLists.txt new file mode 100644 index 000000000000..fb21949f80ac --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/CMakeLists.txt @@ -0,0 +1,20 @@ +# Include directories +include_directories( + include/scout_sdk/ + include/scout_sdk/agilex_protocol + include/scout_sdk/CAN +) + +# Source files +set(SOURCES + src/agilex_protocol/agilex_msg_parser_v2.c + src/agilex_protocol/agilex_protocol_v2_parser.cpp + src/CAN/SocketCAN.cpp + src/Utilities.cpp + src/ScoutRobot.cpp +) + +# Build library +add_library(scout_sdk STATIC ${SOURCES}) +target_include_directories(scout_sdk PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) +target_link_libraries(scout_sdk PRIVATE nuttx_arch) diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/SocketCAN.hpp b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/SocketCAN.hpp new file mode 100644 index 000000000000..34f731a84021 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/SocketCAN.hpp @@ -0,0 +1,83 @@ +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include "can_frame.h" + +namespace scoutsdk +{ +/// RX Frame with its reception timestamp. +struct RxFrame { + uint64_t timestamp_usec; + + /// The actual CAN frame data. + CANFrame frame; +}; + +/// TX Frame with its transmission deadline. +struct TxFrame { + uint64_t tx_deadline_usec; + + /// The actual CAN frame data. + CANFrame frame; +}; + + +class SocketCAN +{ +public: + /// Creates a SocketCAN socket for corresponding iface can_iface_name + /// Also sets up the message structures required for socketcanTransmit & socketcanReceive + /// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0 + /// The return value is 0 on success and -1 on error + int Init(const char *const can_iface_name, const uint32_t can_bitrate); + + /// Close socket connection + int Close() + { + return ::close(_fd); + } + + /// Send a Frame to the SocketInstance socket + /// This function is blocking + /// The return value is number of bytes transferred, negative value on error. + int16_t SendFrame(const TxFrame &txframe, int timeout_ms = 0); + + /// Receive a Frame from the SocketInstance socket + /// This function is blocking + /// The return value is number of bytes received, negative value on error. + int16_t ReceiveFrame(RxFrame *rxf); + +private: + + int _fd{-1}; + bool _can_fd{false}; + + //// Send msg structure + struct iovec _send_iov {}; + struct canfd_frame _send_frame {}; + struct msghdr _send_msg {}; + struct cmsghdr *_send_cmsg {}; + struct timeval *_send_tv {}; /* TX deadline timestamp */ + uint8_t _send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; + + //// Receive msg structure + struct iovec _recv_iov {}; + struct canfd_frame _recv_frame {}; + struct msghdr _recv_msg {}; + struct cmsghdr *_recv_cmsg {}; + uint8_t _recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; +}; +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/can_frame.h b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/can_frame.h new file mode 100644 index 000000000000..28d49067f616 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/CAN/can_frame.h @@ -0,0 +1,23 @@ +#ifndef CAN_FRAME_H +#define CAN_FRAME_H + +#include +#include + +/// CAN data frame with support for 11-bit ID and extended 29-bit ID. +typedef struct { + /// 11-bit or 29-bit extended ID. The bits above 29-th shall be zero. + uint32_t can_id; + + /// The useful data in the frame. The length value is not to be confused with DLC! + /// If the payload is empty (payload_size = 0), the payload pointer may be NULL. + /// For RX frames: the library does not expect the lifetime of the pointee to extend beyond the point of return + /// from the API function. That is, the pointee can be invalidated immediately after the frame has been processed. + /// For TX frames: the frame and the payload are allocated within the same dynamic memory fragment, so their + /// lifetimes are identical; when the frame is freed, the payload is invalidated. + /// A more detailed overview of the dataflow and related resource management issues is provided in the API docs. + size_t payload_size; + void *payload; +} CANFrame; + +#endif /* CAN_FRAME_H */ diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/ScoutRobot.hpp b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/ScoutRobot.hpp new file mode 100644 index 000000000000..21fee4b0d8c9 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/ScoutRobot.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include +#include +#include +#include + +#include "agilex_protocol/agilex_message.h" +#include "agilex_protocol/agilex_protocol_v2_parser.hpp" + +namespace scoutsdk +{ +struct ScoutCoreState { + SystemStateMessage system_state; + MotionStateMessage motion_state; + LightStateMessage light_state; + RcStateMessage rc_state; +}; + +struct ScoutActuatorState { + ActuatorHSStateMessage actuator_hs_state[4]; + ActuatorLSStateMessage actuator_ls_state[4]; +}; + +struct ScoutMotorState { + MotorAngleMessage MotorAngle; + MotorSpeedMessage MotorSpeed; +}; + +struct VersionResponse { + const char *str_version_response; + VersionResponseMessage version_response; +}; + + +class ScoutRobot +{ +public: + ScoutRobot(ProtocolVersion protocol = ProtocolVersion::AGX_V2, + bool is_mini_model = false); + ~ScoutRobot(); + + // do not allow copy or assignment + ScoutRobot(const ScoutRobot &robot) = delete; + ScoutRobot &operator=(const ScoutRobot &robot) = delete; + + // CAN connection + void Connect(const char *const can_dev, const uint32_t can_bitrate); + void Disconnect(); + + // Send commands + void EnableCommandMode(); + void QuerySystemVersion(const uint64_t timeout_msec); + void SetMotionCommand(float linear_vel, float angular_vel); + void SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode = LightMode::CONST_ON, uint8_t r_value = 0); + void DisableLightControl(); + + // Receive status from rover + void CheckUpdateFromRover(); + + // Get connection and rover general info + bool GetCANConnected() const { return _can_connected; } + const char *GetSystemVersion() const { return _version_response_msgs.str_version_response; } + + // Return rover states + const ScoutCoreState &GetRobotState() const { return _core_state_msgs; } + const ScoutActuatorState &GetActuatorState() const { return _actuator_state_msgs; } + const ScoutMotorState &GetMotorState() const { return _motor_state_msgs; } + +private: + void UpdateRobotCoreState(const AgxMessage &status_msg); + void UpdateActuatorState(const AgxMessage &status_msg); + void UpdateMotorState(const AgxMessage &status_msg); + int UpdateVersionResponse(const AgxMessage &status_msg); + + AgileXProtocolV2Parser _parser; + + // Feedback group 1: core state + ScoutCoreState _core_state_msgs; + + // Feedback group 2: actuator state + ScoutActuatorState _actuator_state_msgs; + + ScoutMotorState _motor_state_msgs; + + VersionResponse _version_response_msgs; + + char _version_response_string[9] {}; + + // TX buffer + uint8_t _tx_data[8] {}; + TxFrame _tx_frame{}; + + // RX buffer + uint8_t _rx_data[8] {}; + RxFrame _rx_frame{}; + + // Communication interface + bool _can_connected = false; + SocketCAN *_can{nullptr}; +}; +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/Utilities.hpp b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/Utilities.hpp new file mode 100644 index 000000000000..fb9362067d15 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/Utilities.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include + +#include "CAN/SocketCAN.hpp" +#include "agilex_protocol/agilex_protocol_v2_parser.hpp" + +namespace scoutsdk +{ +class ProtocolDetector +{ +public: + ProtocolDetector(); + ~ProtocolDetector(); + + bool Connect(const char *const can_name, const uint32_t bitrate); + + ProtocolVersion DetectProtocolVersion(const uint64_t timeout_sec); + +private: + SocketCAN *_can; + void ParseCANFrame(); + AgileXProtocolV2Parser _parser; + + bool _msg_v1_detected; + bool _msg_v2_detected; +}; +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_message.h b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_message.h new file mode 100644 index 000000000000..677cab4db75f --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_message.h @@ -0,0 +1,345 @@ +/* + * agilex_message.h + * + * Created on: Dec 10, 2020 11:47 + * Description: + * all values are using SI units (e.g. meter/second/radian) + * + * Copyright (c) 2020 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_MESSAGE_H +#define AGILEX_MESSAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#include "agilex_types.h" + +/***************** Control messages *****************/ + +typedef struct { + float linear_velocity; + float angular_velocity; + float lateral_velocity; + float steering_angle; +} MotionCommandMessage; + +typedef struct { + bool enable_cmd_ctrl; + LightOperation front_light; + LightOperation rear_light; +} LightCommandMessage; + +typedef struct { + bool enable_braking; +} BrakingCommandMessage; + +typedef struct { + uint8_t motion_mode; +} MotionModeCommandMessage; + +/**************** Feedback messages *****************/ + +#define SYSTEM_ERROR_MOTOR_DRIVER_MASK ((uint16_t)0x0100) +#define SYSTEM_ERROR_HL_COMM_MASK ((uint16_t)0x0200) +#define SYSTEM_ERROR_BATTERY_FAULT_MASK ((uint16_t)0x0001) +#define SYSTEM_ERROR_BATTERY_WARNING_MASK ((uint16_t)0x0002) +#define SYSTEM_ERROR_RC_SIGNAL_LOSS_MASK ((uint16_t)0x0004) +#define SYSTEM_ERROR_MOTOR1_COMM_MASK ((uint16_t)0x0008) +#define SYSTEM_ERROR_MOTOR2_COMM_MASK ((uint16_t)0x0010) +#define SYSTEM_ERROR_MOTOR3_COMM_MASK ((uint16_t)0x0020) +#define SYSTEM_ERROR_MOTOR4_COMM_MASK ((uint16_t)0x0040) +#define SYSTEM_ERROR_STEER_ENCODER_MASK ((uint16_t)0x0080) + +typedef struct { + VehicleState vehicle_state; + ControlMode control_mode; + float battery_voltage; + uint16_t error_code; +} SystemStateMessage; + +typedef struct { + float linear_velocity; + float angular_velocity; // only valid for differential drivering + float lateral_velocity; + float steering_angle; // only valid for ackermann steering +} MotionStateMessage; + +typedef LightCommandMessage LightStateMessage; + +typedef struct { + RcSwitchState swa; + RcSwitchState swb; + RcSwitchState swc; + RcSwitchState swd; + int8_t stick_right_v; + int8_t stick_right_h; + int8_t stick_left_v; + int8_t stick_left_h; + int8_t var_a; +} RcStateMessage; + +typedef struct { + uint8_t motor_id; + int16_t rpm; + float current; + int32_t pulse_count; +} ActuatorHSStateMessage; + +#define DRIVER_STATE_INPUT_VOLTAGE_LOW_MASK ((uint8_t)0x01) +#define DRIVER_STATE_MOTOR_OVERHEAT_MASK ((uint8_t)0x02) +#define DRIVER_STATE_DRIVER_OVERLOAD_MASK ((uint8_t)0x04) +#define DRIVER_STATE_DRIVER_OVERHEAT_MASK ((uint8_t)0x08) +#define DRIVER_STATE_SENSOR_FAULT_MASK ((uint8_t)0x10) +#define DRIVER_STATE_DRIVER_FAULT_MASK ((uint8_t)0x20) +#define DRIVER_STATE_DRIVER_ENABLED_MASK ((uint8_t)0x40) +#define DRIVER_STATE_DRIVER_RESET_MASK ((uint8_t)0x80) + +typedef struct { + uint8_t motor_id; + float driver_voltage; + float driver_temp; + float motor_temp; + uint8_t driver_state; +} ActuatorLSStateMessage; + +typedef struct { + uint8_t motion_mode; + uint8_t mode_changing; +} MotionModeStateMessage; + +/***************** Sensor messages ******************/ + +typedef struct { + float left_wheel; + float right_wheel; +} OdometryMessage; + +typedef struct { + float accel_x; + float accel_y; + float accel_z; +} ImuAccelMessage; + +typedef struct { + float gyro_x; + float gyro_y; + float gyro_z; +} ImuGyroMessage; + +typedef struct { + float yaw; + float pitch; + float roll; +} ImuEulerMessage; + +typedef struct { + uint8_t trigger_state; +} SafetyBumperMessage; + +typedef struct { + uint8_t sensor_id; + uint8_t distance[8]; +} UltrasonicMessage; + +typedef struct { + uint8_t sensor_id; + float relative_distance; + float relative_angle; + bool is_normal; + int8_t channels[3]; +} UwbMessage; + +typedef struct { + uint8_t battery_soc; + uint8_t battery_soh; + float voltage; + float current; + float temperature; +} BmsBasicMessage; + +typedef struct { + float angle_5; + float angle_6; + float angle_7; + float angle_8; +} MotorAngleMessage; + +typedef struct { + float speed_1; + float speed_2; + float speed_3; + float speed_4; +} MotorSpeedMessage; + +#define BMS_PROT1_CHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x01) +#define BMS_PROT1_CHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x02) +#define BMS_PROT1_DISCHARGING_CURRENT_NONZERO_MASK ((uint8_t)0x10) +#define BMS_PROT1_DISCHARGING_OVERCURRENT_SET_MASK ((uint8_t)0x20) +#define BMS_PROT1_DISCHARGING_SHORTCIRCUIT_SET_MASK ((uint8_t)0x40) + +#define BMS_PROT2_CORE_OPENCIRCUIT_SET_MASK ((uint8_t)0x01) +#define BMS_PROT2_TEMP_SENSOR_OPENCIRCUIT_SET_MASK ((uint8_t)0x02) +#define BMS_PROT2_CORE_OVERVOLTAGE_SET_MASK ((uint8_t)0x10) +#define BMS_PROT2_CORE_UNDERVOLTAGE_SET_MASK ((uint8_t)0x20) +#define BMS_PROT2_TOTAL_OVERVOLTAGE_SET_MASK ((uint8_t)0x40) +#define BMS_PROT2_TOTAL_UNDERVOLTAGE_SET_MASK ((uint8_t)0x80) + +#define BMS_PROT3_CHARGING_OVERTEMP_SET_MASK ((uint8_t)0x04) +#define BMS_PROT3_DISCHARGING_OVERTEMP_SET_MASK ((uint8_t)0x08) +#define BMS_PROT3_CHARGING_UNDERTEMP_SET_MASK ((uint8_t)0x10) +#define BMS_PROT3_DISCHARGING_UNDERTEMP_SET_MASK ((uint8_t)0x20) +#define BMS_PROT3_CHARGING_TEMPDIFF_SET_MASK ((uint8_t)0x40) +#define BMS_PROT3_DISCHARGING_TEMPDIFF_SET_MASK ((uint8_t)0x80) + +#define BMS_PROT4_CHARGING_MOS_STATE_SET_MASK ((uint8_t)0x01) +#define BMS_PROT4_DISCHARGING_MOS_STATE_SET_MASK ((uint8_t)0x02) +#define BMS_PROT4_CHARGING_MOS_FAILURE_SET_MASK ((uint8_t)0x04) +#define BMS_PROT4_DISCHARGING_MOS_FAILURE_SET_MASK ((uint8_t)0x08) +#define BMS_PROT4_WEAK_SIGNAL_SWITCH_OPEN_SET_MASK ((uint8_t)0x10) + +typedef struct { + uint8_t protection_code1; + uint8_t protection_code2; + uint8_t protection_code3; + uint8_t protection_code4; + uint8_t battery_max_teperature; + uint8_t battery_min_teperature; +} BmsExtendedMessage; + +/************ Query/config messages ****************/ + +typedef struct { + bool request; +} VersionRequestMessage; + +typedef struct { + uint16_t controller_hw_version; + uint16_t motor_driver_hw_version; + uint16_t controller_sw_version; + uint16_t motor_driver_sw_version; +} VersionResponseMessage; + +typedef struct { + ControlMode mode; +} ControlModeConfigMessage; + +typedef struct { + BrakeMode mode; +} BrakeModeConfigMessage; + +typedef struct { + bool set_as_neutral; +} SteerNeutralRequestMessage; + +typedef struct { + bool neutral_set_successful; +} SteerNeutralResponseMessage; + +typedef enum { + CLEAR_ALL_FAULT = 0x00, + CLEAR_MOTOR1_FAULT = 0x01, + CLEAR_MOTOR2_FAULT = 0x02, + CLEAR_MOTOR3_FAULT = 0x03, + CLEAR__MOTOR4_FAULT = 0x04 +} FaultClearCode; + +typedef struct { + uint8_t error_clear_byte; +} StateResetConfigMessage; + +////////////////////////////////////////////////////// + +typedef enum { + AgxMsgUnknown = 0x00, + // command + AgxMsgMotionCommand, + AgxMsgLightCommand, + AgxMsgBrakingCommand, + AgxMsgSetMotionModeCommand, + // state feedback + AgxMsgSystemState, + AgxMsgMotionState, + AgxMsgLightState, + AgxMsgMotionModeState, + AgxMsgRcState, + // actuator feedback + AgxMsgActuatorHSState, + AgxMsgActuatorLSState, + AgxMsgMotorAngle, + AgxMsgMotorSpeed, + // sensor + AgxMsgOdometry, + AgxMsgImuAccel, + AgxMsgImuGyro, + AgxMsgImuEuler, + AgxMsgSafetyBumper, + AgxMsgUltrasonic, + AgxMsgUwb, + AgxMsgBmsBasic, + AgxMsgBmsExtended, + // query/config + AgxMsgVersionRequest, + AgxMsgVersionResponse, + AgxMsgControlModeConfig, + AgxMsgSteerNeutralRequest, + AgxMsgSteerNeutralResponse, + AgxMsgStateResetConfig, + AgxMsgBrakeModeConfig +} MsgType; + +typedef struct { + MsgType type; + union { + // command + MotionCommandMessage motion_command_msg; + LightCommandMessage light_command_msg; + BrakingCommandMessage braking_command_msg; + MotionModeCommandMessage motion_mode_msg; + // core state feedback + SystemStateMessage system_state_msg; + MotionStateMessage motion_state_msg; + LightStateMessage light_state_msg; + MotionModeStateMessage motion_mode_state_msg; + RcStateMessage rc_state_msg; + // actuator feedback + ActuatorHSStateMessage actuator_hs_state_msg; + ActuatorLSStateMessage actuator_ls_state_msg; + // sensor + OdometryMessage odometry_msg; + ImuAccelMessage imu_accel_msg; + ImuGyroMessage imu_gyro_msg; + ImuEulerMessage imu_euler_msg; + SafetyBumperMessage safety_bumper_msg; + UltrasonicMessage ultrasonic_msg; + UwbMessage uwb_msg; + BmsBasicMessage bms_basic_msg; + BmsExtendedMessage bms_extended_msg; + // query/config + VersionRequestMessage version_request_msg; + VersionResponseMessage version_response_msg; +// uint8_t version_str[10][8]; + uint8_t version_str[8]; + ControlModeConfigMessage control_mode_config_msg; + BrakeModeConfigMessage brake_mode_config_msg; + SteerNeutralRequestMessage steer_neutral_request_msg; + SteerNeutralResponseMessage steer_neutral_response_msg; + StateResetConfigMessage state_reset_config_msg; + + MotorAngleMessage motor_angle_msg; + MotorSpeedMessage motor_speed_msg; + + } body; +} AgxMessage; + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_MESSAGE_H */ diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_msg_parser_v2.h b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_msg_parser_v2.h new file mode 100644 index 000000000000..39dfe02445ec --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_msg_parser_v2.h @@ -0,0 +1,23 @@ +#ifndef AGILEX_MSG_PARSER_H +#define AGILEX_MSG_PARSER_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include + +#include "agilex_message.h" +#include "../CAN/can_frame.h" + +bool DecodeCanFrameV2(const CANFrame *can_frame, AgxMessage *msg); +bool EncodeCanFrameV2(const AgxMessage *msg, CANFrame *can_frame); +uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc); + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_MSG_PARSER_H */ diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2.h b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2.h new file mode 100644 index 000000000000..58d97a058ca3 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2.h @@ -0,0 +1,439 @@ +/* + * agilex_protocol_v2.h + * + * Created on: Nov 04, 2020 13:54 + * Description: + * + * Copyright (c) 2020 Weston Robot Pte. Ltd. + */ + +#ifndef AGILEX_PROTOCOL_V2_H +#define AGILEX_PROTOCOL_V2_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +// define endianess of the platform +#if (!defined(USE_LITTLE_ENDIAN) && !defined(USE_BIG_ENDIAN)) +#define USE_LITTLE_ENDIAN +#endif + +#ifdef USE_BIG_ENDIAN +#error "BIG ENDIAN IS CURRENTLY NOT SUPPORTED" +#endif + +/*---------------------------- Motor IDs -------------------------------*/ + +#define ACTUATOR1_ID ((uint8_t)0x00) +#define ACTUATOR2_ID ((uint8_t)0x01) +#define ACTUATOR3_ID ((uint8_t)0x02) +#define ACTUATOR4_ID ((uint8_t)0x03) +#define ACTUATOR5_ID ((uint8_t)0x04) +#define ACTUATOR6_ID ((uint8_t)0x05) +#define ACTUATOR7_ID ((uint8_t)0x06) +#define ACTUATOR8_ID ((uint8_t)0x07) + +/*--------------------------- Message IDs ------------------------------*/ + +// control group: 0x1 +#define CAN_MSG_MOTION_COMMAND_ID ((uint32_t)0x111) +#define CAN_MSG_LIGHT_COMMAND_ID ((uint32_t)0x121) +#define CAN_MSG_BRAKING_COMMAND_ID ((uint32_t)0x131) +#define CAN_MSG_SET_MOTION_MODE_ID ((uint32_t)0x141) + +// state feedback group: 0x2 +#define CAN_MSG_SYSTEM_STATE_ID ((uint32_t)0x211) +#define CAN_MSG_MOTION_STATE_ID ((uint32_t)0x221) +#define CAN_MSG_LIGHT_STATE_ID ((uint32_t)0x231) +#define CAN_MSG_RC_STATE_ID ((uint32_t)0x241) + +#define CAN_MSG_ACTUATOR1_HS_STATE_ID ((uint32_t)0x251) +#define CAN_MSG_ACTUATOR2_HS_STATE_ID ((uint32_t)0x252) +#define CAN_MSG_ACTUATOR3_HS_STATE_ID ((uint32_t)0x253) +#define CAN_MSG_ACTUATOR4_HS_STATE_ID ((uint32_t)0x254) +#define CAN_MSG_ACTUATOR5_HS_STATE_ID ((uint32_t)0x255) +#define CAN_MSG_ACTUATOR6_HS_STATE_ID ((uint32_t)0x256) +#define CAN_MSG_ACTUATOR7_HS_STATE_ID ((uint32_t)0x257) +#define CAN_MSG_ACTUATOR8_HS_STATE_ID ((uint32_t)0x258) + +#define CAN_MSG_ACTUATOR1_LS_STATE_ID ((uint32_t)0x261) +#define CAN_MSG_ACTUATOR2_LS_STATE_ID ((uint32_t)0x262) +#define CAN_MSG_ACTUATOR3_LS_STATE_ID ((uint32_t)0x263) +#define CAN_MSG_ACTUATOR4_LS_STATE_ID ((uint32_t)0x264) +#define CAN_MSG_ACTUATOR5_LS_STATE_ID ((uint32_t)0x265) +#define CAN_MSG_ACTUATOR6_LS_STATE_ID ((uint32_t)0x266) +#define CAN_MSG_ACTUATOR7_LS_STATE_ID ((uint32_t)0x267) +#define CAN_MSG_ACTUATOR8_LS_STATE_ID ((uint32_t)0x268) + +#define CAN_MSG_MOTOR_ANGLE_INFO ((uint32_t)0x271) +#define CAN_MSG_MOTOR_SPEED_INFO ((uint32_t)0x281) +#define CAN_MSG_CURRENT_CTRL_MODE ((uint32_t)0x291) + +// sensor data group: 0x3 +#define CAN_MSG_ODOMETRY_ID ((uint32_t)0x311) + +#define CAN_MSG_IMU_ACCEL_ID ((uint32_t)0x321) +#define CAN_MSG_IMU_GYRO_ID ((uint32_t)0x322) +#define CAN_MSG_IMU_EULER_ID ((uint32_t)0x323) + +#define CAN_MSG_SAFETY_BUMPER_ID ((uint32_t)0x331) + +#define CAN_MSG_ULTRASONIC_1_ID ((uint32_t)0x341) +#define CAN_MSG_ULTRASONIC_2_ID ((uint32_t)0x342) +#define CAN_MSG_ULTRASONIC_3_ID ((uint32_t)0x343) +#define CAN_MSG_ULTRASONIC_4_ID ((uint32_t)0x344) +#define CAN_MSG_ULTRASONIC_5_ID ((uint32_t)0x345) +#define CAN_MSG_ULTRASONIC_6_ID ((uint32_t)0x346) +#define CAN_MSG_ULTRASONIC_7_ID ((uint32_t)0x347) +#define CAN_MSG_ULTRASONIC_8_ID ((uint32_t)0x348) + +#define CAN_MSG_UWB_1_ID ((uint32_t)0x351) +#define CAN_MSG_UWB_2_ID ((uint32_t)0x352) +#define CAN_MSG_UWB_3_ID ((uint32_t)0x353) +#define CAN_MSG_UWB_4_ID ((uint32_t)0x354) + +#define CAN_MSG_BMS_BASIC_ID ((uint32_t)0x361) +#define CAN_MSG_BMS_EXTENDED_ID ((uint32_t)0x362) + +// query/config group: 0x4 +#define CAN_MSG_VERSION_REQUEST_ID ((uint32_t)0x411) +#define CAN_MSG_VERSION_RESPONSE_ID ((uint32_t)0x4a1) + +#define CAN_MSG_CTRL_MODE_CONFIG_ID ((uint32_t)0x421) + +#define CAN_MSG_STEER_NEUTRAL_REQUEST_ID ((uint32_t)0x431) +#define CAN_MSG_STEER_NEUTRAL_RESPONSE_ID ((uint32_t)0x43a) + +#define CAN_MSG_STATE_RESET_CONFIG_ID ((uint32_t)0x441) + +/*------------------------ Frame Memory Layout -------------------------*/ + +/* No padding in the struct */ +// reference: https://stackoverflow.com/questions/3318410/pragma-pack-effect +#pragma pack(push, 1) + +#ifdef USE_LITTLE_ENDIAN +typedef struct { + uint8_t high_byte; + uint8_t low_byte; +} struct16_t; +typedef struct { + uint8_t msb; + uint8_t high_byte; + uint8_t low_byte; + uint8_t lsb; +} struct32_t; +#elif defined(USE_BIG_ENDIAN) +typedef struct { + uint8_t low_byte; + uint8_t high_byte; +} struct16_t; +typedef struct { + uint8_t lsb; + uint8_t low_byte; + uint8_t high_byte; + uint8_t msb; +} struct32_t; +#endif + +// Control messages +typedef struct { + struct16_t linear_velocity; + struct16_t angular_velocity; + struct16_t lateral_velocity; + struct16_t steering_angle; +} MotionCommandFrame; + +#define LIGHT_ENABLE_CMD_CTRL ((uint8_t)0x01) +#define LIGHT_DISABLE_CMD_CTRL ((uint8_t)0x00) + +typedef struct { + uint8_t enable_cmd_ctrl; + uint8_t front_mode; + uint8_t front_custom; + uint8_t rear_mode; + uint8_t rear_custom; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; +} LightCommandFrame; + +typedef struct { + uint8_t enable_brake; + uint8_t count; +} BrakingCommandFrame; + +typedef struct { + uint8_t motion_mode; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} SetMotionModeFrame; + +// State feedback messages +typedef struct { + uint8_t vehicle_state; + uint8_t control_mode; + struct16_t battery_voltage; + struct16_t error_code; + uint8_t reserved0; + uint8_t count; +} SystemStateFrame; + +typedef struct { + struct16_t linear_velocity; + struct16_t angular_velocity; + struct16_t lateral_velocity; + struct16_t steering_angle; +} MotionStateFrame; + +typedef struct { + uint8_t enable_cmd_ctrl; + uint8_t front_mode; + uint8_t front_custom; + uint8_t rear_mode; + uint8_t rear_custom; + uint8_t reserved0; + uint8_t reserved1; + uint8_t count; +} LightStateFrame; + +#define RC_SWA_MASK ((uint8_t)0b00000011) +#define RC_SWA_UP_MASK ((uint8_t)0b00000010) +#define RC_SWA_DOWN_MASK ((uint8_t)0b00000011) + +#define RC_SWB_MASK ((uint8_t)0b00001100) +#define RC_SWB_UP_MASK ((uint8_t)0b00001000) +#define RC_SWB_MIDDLE_MASK ((uint8_t)0b00000100) +#define RC_SWB_DOWN_MASK ((uint8_t)0b00001100) + +#define RC_SWC_MASK ((uint8_t)0b00110000) +#define RC_SWC_UP_MASK ((uint8_t)0b00100000) +#define RC_SWC_MIDDLE_MASK ((uint8_t)0b00010000) +#define RC_SWC_DOWN_MASK ((uint8_t)0b00110000) + +#define RC_SWD_MASK ((uint8_t)0b11000000) +#define RC_SWD_UP_MASK ((uint8_t)0b10000000) +#define RC_SWD_DOWN_MASK ((uint8_t)0b11000000) + +typedef struct { + uint8_t sws; + int8_t stick_right_h; + int8_t stick_right_v; + int8_t stick_left_v; + int8_t stick_left_h; + int8_t var_a; + uint8_t reserved0; + uint8_t count; +} RcStateFrame; + +typedef struct { + struct16_t rpm; + struct16_t current; + struct32_t pulse_count; +} ActuatorHSStateFrame; + +typedef struct { + struct16_t driver_voltage; + struct16_t driver_temp; + int8_t motor_temp; + uint8_t driver_state; + uint8_t reserved0; + uint8_t reserved1; +} ActuatorLSStateFrame; + +// 0x291 +typedef struct { + uint8_t motion_mode; + uint8_t mode_changing; +} MotionModeStateFrame; + +// sensors +typedef struct { + struct32_t left_wheel; + struct32_t right_wheel; +} OdometryFrame; + +typedef struct { + struct16_t accel_x; + struct16_t accel_y; + struct16_t accel_z; + uint8_t reserverd0; + uint8_t count; +} ImuAccelFrame; + +typedef struct { + struct16_t gyro_x; + struct16_t gyro_y; + struct16_t gyro_z; + uint8_t reserverd0; + uint8_t count; +} ImuGyroFrame; + +typedef struct { + struct16_t yaw; + struct16_t pitch; + struct16_t roll; + uint8_t reserverd0; + uint8_t count; +} ImuEulerFrame; + +typedef struct { + uint8_t trigger_state; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} SafetyBumperFrame; + +typedef struct { + uint8_t distance[8]; +} UltrasonicFrame; + +typedef struct { + struct16_t relative_distance; + struct16_t relative_angle; + uint8_t is_normal; + int8_t channels[3]; +} UwbFrame; + +typedef struct { + uint8_t battery_soc; + uint8_t battery_soh; + struct16_t voltage; + struct16_t current; + struct16_t temperature; +} BmsBasicFrame; + +typedef struct { + uint8_t protection_code1; + uint8_t protection_code2; + uint8_t protection_code3; + uint8_t protection_code4; + uint8_t battery_max_teperature; + uint8_t battery_min_teperature; + struct16_t count; +} BmsExtendedFrame; + +// query/config +#define VERSION_REQUEST_VALUE ((uint8_t)0x01) +#define STEER_NEUTRAL_REQUEST_VALUE ((uint8_t)0xee) +#define STEER_NEUTRAL_RESPONSE_SUCCESS_VALUE ((uint8_t)0xee) +#define STEER_NEUTRAL_RESPONSE_FAILURE_VALUE ((uint8_t)0xff) + +typedef struct { + uint8_t request; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} VersionRequestFrame; + +//typedef struct { +// struct16_t controller_hw_version; +// struct16_t motor_driver_hw_version; +// struct16_t controller_sw_version; +// struct16_t motor_driver_sw_version; +//} VersionResponseFrame; + +typedef struct { + uint8_t res0; + uint8_t res1; + uint8_t res2; + uint8_t res3; + uint8_t res4; + uint8_t res5; + uint8_t res6; + uint8_t res7; +} VersionResponseFrame; + + +typedef struct { + uint8_t mode; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} ControlModeConfigFrame; + +typedef struct { + uint8_t mode; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} BrakeModeConfigFrame; + +typedef struct { + uint8_t set_as_neutral; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} SteerNeutralRequestFrame; + +typedef struct { + uint8_t neutral_set_successful; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} SteerNeutralResponseFrame; + +typedef struct { + uint8_t error_clear_byte; + uint8_t reserved0; + uint8_t reserved1; + uint8_t reserved2; + uint8_t reserved3; + uint8_t reserved4; + uint8_t reserved5; + uint8_t reserved6; +} StateResetConfigFrame; + +typedef struct { + struct16_t angle_5; + struct16_t angle_6; + struct16_t angle_7; + struct16_t angle_8; +} MoterAngleFrame; + +typedef struct { + struct16_t speed_1; + struct16_t speed_2; + struct16_t speed_3; + struct16_t speed_4; +} MoterSpeedFrame; + +#pragma pack(pop) + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_PROTOCOL_V2_H */ diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2_parser.hpp b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2_parser.hpp new file mode 100644 index 000000000000..94fd66796240 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_protocol_v2_parser.hpp @@ -0,0 +1,20 @@ +#ifndef AGILEX_PROTOCOL_V2_PARSER_HPP +#define AGILEX_PROTOCOL_V2_PARSER_HPP + +#include "agilex_message.h" +#include "agilex_msg_parser_v2.h" +#include "../CAN/SocketCAN.hpp" + +namespace scoutsdk +{ +enum class ProtocolVersion { UNKNOWN, AGX_V1, AGX_V2 }; + +class AgileXProtocolV2Parser +{ +public: + bool DecodeMessage(const RxFrame *rxf, AgxMessage *msg); + bool EncodeMessage(const AgxMessage *msg, TxFrame *txf); + uint8_t CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc); +}; +} // namespace scoutsdk +#endif /* AGILEX_PROTOCOL_V2_PARSER_HPP */ diff --git a/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_types.h b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_types.h new file mode 100644 index 000000000000..a177302229d3 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/include/scout_sdk/agilex_protocol/agilex_types.h @@ -0,0 +1,60 @@ +/* + * agilex_types.h + * + * Created on: Jul 09, 2021 21:57 + * Description: + * + * Copyright (c) 2021 Ruixiang Du (rdu) + */ + +#ifndef AGILEX_TYPES_H +#define AGILEX_TYPES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +typedef enum { + CONST_OFF = 0x00, + CONST_ON = 0x01, + BREATH = 0x02, + CUSTOM = 0x03 +} LightMode; + +typedef struct { + LightMode mode; + uint8_t custom_value; +} LightOperation; + +typedef enum { + VehicleStateNormal = 0x00, + VehicleStateEStop = 0x01, + VehicleStateException = 0x02 +} VehicleState; + +typedef enum { + // CONTROL_MODE_STANDBY = 0x00, + CONTROL_MODE_RC = 0x00, + CONTROL_MODE_CAN = 0x01, + CONTROL_MODE_UART = 0x02 +} ControlMode; + +typedef enum { + // CONTROL_MODE_STANDBY = 0x00, + BRAKE_MODE_UNLOCK = 0x00, + BRAKE_MODE_LOCK = 0x01 +} BrakeMode; + +typedef enum { + RC_SWITCH_UP = 0, + RC_SWITCH_MIDDLE, + RC_SWITCH_DOWN +} RcSwitchState; + +#ifdef __cplusplus +} +#endif + +#endif /* AGILEX_TYPES_H */ diff --git a/src/drivers/rover_interface/scout_sdk/src/CAN/SocketCAN.cpp b/src/drivers/rover_interface/scout_sdk/src/CAN/SocketCAN.cpp new file mode 100644 index 000000000000..3821af16eeb6 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/src/CAN/SocketCAN.cpp @@ -0,0 +1,251 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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 "scout_sdk/CAN/SocketCAN.hpp" + +#include +#include + +#define MODULE_NAME "SCOUT_SDK" + +#include + +uint64_t getMonotonicTimestampUSec(void) +{ + struct timespec ts {}; + + clock_gettime(CLOCK_MONOTONIC, &ts); + + return ts.tv_sec * 1000000ULL + ts.tv_nsec / 1000ULL; +} + + +namespace scoutsdk +{ +int SocketCAN::Init(const char *const can_iface_name, const uint32_t can_bitrate) +{ + struct sockaddr_can addr; + struct ifreq ifr; + + // Disable CAN FD mode + bool can_fd = 0; + + _can_fd = can_fd; + + /* Open socket */ + if ((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + PX4_ERR("Opening socket failed"); + return -1; + } + + strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + + if (!ifr.ifr_ifindex) { + PX4_ERR("if_nametoindex"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + const int on = 1; + /* RX Timestamping */ + + if (setsockopt(_fd, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { + PX4_ERR("SO_TIMESTAMP is disabled"); + return -1; + } + + /* NuttX Feature: Enable TX deadline when sending CAN frames + * When a deadline occurs the driver will remove the CAN frame + */ + + if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) { + PX4_ERR("CAN_RAW_TX_DEADLINE is disabled"); + return -1; + } + + if (can_fd) { + if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) { + PX4_ERR("no CAN FD support"); + return -1; + } + } + + if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + PX4_ERR("bind"); + return -1; + } + + // Setup TX msg + _send_iov.iov_base = &_send_frame; + + if (_can_fd) { _send_iov.iov_len = sizeof(struct canfd_frame); } + + else { _send_iov.iov_len = sizeof(struct can_frame); } + + memset(&_send_control, 0x00, sizeof(_send_control)); + + _send_msg.msg_iov = &_send_iov; + _send_msg.msg_iovlen = 1; + _send_msg.msg_control = &_send_control; + _send_msg.msg_controllen = sizeof(_send_control); + + _send_cmsg = CMSG_FIRSTHDR(&_send_msg); + _send_cmsg->cmsg_level = SOL_CAN_RAW; + _send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE; + _send_cmsg->cmsg_len = sizeof(struct timeval); + _send_tv = (struct timeval *)CMSG_DATA(_send_cmsg); + + // Setup RX msg + _recv_iov.iov_base = &_recv_frame; + + if (can_fd) { _recv_iov.iov_len = sizeof(struct canfd_frame); } + + else { _recv_iov.iov_len = sizeof(struct can_frame); } + + memset(_recv_control, 0x00, sizeof(_recv_control)); + + _recv_msg.msg_iov = &_recv_iov; + _recv_msg.msg_iovlen = 1; + _recv_msg.msg_control = &_recv_control; + _recv_msg.msg_controllen = sizeof(_recv_control); + _recv_cmsg = CMSG_FIRSTHDR(&_recv_msg); + + // Setup bitrate + ifr.ifr_ifru.ifru_can_data.arbi_bitrate = can_bitrate / 1000; + ifr.ifr_ifru.ifru_can_data.arbi_samplep = 88; + ifr.ifr_ifru.ifru_can_data.data_bitrate = 4 * can_bitrate / 1000; + ifr.ifr_ifru.ifru_can_data.data_samplep = 75; + + if (ioctl(_fd, SIOCSCANBITRATE, &ifr) < 0) { + PX4_ERR("Setting CAN bitrate to %d bit/s failed", can_bitrate); + return -1; + } + + // Setup RX range filter [ RANGE FILTER NEEDS TO BE SET BEFORE ANY BIT FILTER] + ifr.ifr_ifru.ifru_can_filter.fid1 = 0x211; // lower end + ifr.ifr_ifru.ifru_can_filter.fid2 = 0x231; // higher end + ifr.ifr_ifru.ifru_can_filter.ftype = CAN_FILTER_RANGE; + ifr.ifr_ifru.ifru_can_filter.fprio = CAN_MSGPRIO_LOW; + + if (ioctl(_fd, SIOCACANSTDFILTER, &ifr) < 0) { + PX4_ERR("Setting RX range filter failed"); + return -1; + } + + // Setup RX bit filter + ifr.ifr_ifru.ifru_can_filter.fid1 = 0x41A; // value + ifr.ifr_ifru.ifru_can_filter.fid2 = 0x7FF; // mask + ifr.ifr_ifru.ifru_can_filter.ftype = CAN_FILTER_MASK; + ifr.ifr_ifru.ifru_can_filter.fprio = CAN_MSGPRIO_LOW; + + if (ioctl(_fd, SIOCACANSTDFILTER, &ifr) < 0) { + PX4_ERR("Setting RX bit filter A failed"); + return -1; + } + + return 0; +} + +int16_t SocketCAN::SendFrame(const TxFrame &txf, int timeout_ms) +{ + /* Copy Frame to can_frame/canfd_frame */ + if (_can_fd) { + _send_frame.can_id = txf.frame.can_id | CAN_EFF_FLAG; + _send_frame.len = txf.frame.payload_size; + memcpy(&_send_frame.data, txf.frame.payload, txf.frame.payload_size); + + } else { + struct can_frame *frame = (struct can_frame *)&_send_frame; + //frame->can_id = txf.frame.can_id | CAN_EFF_FLAG; + frame->can_id = txf.frame.can_id; + frame->can_dlc = txf.frame.payload_size; + memcpy(&frame->data, txf.frame.payload, txf.frame.payload_size); + PX4_DEBUG("cansend len %d; can_id: %03x", frame->can_dlc, frame->can_id); + } + + uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.tx_deadline_usec - hrt_absolute_time()) + + CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick + + /* Set CAN_RAW_TX_DEADLINE timestamp */ + _send_tv->tv_usec = deadline_systick % 1000000ULL; + _send_tv->tv_sec = (deadline_systick - _send_tv->tv_usec) / 1000000ULL; + + auto ret = sendmsg(_fd, &_send_msg, 0); + + return ret; +} + +int16_t SocketCAN::ReceiveFrame(RxFrame *rxf) +{ + int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); + + if (result <= 0) { return result; } + + /* Copy CAN frame to Frame */ + + if (_can_fd) { + struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame; + rxf->frame.can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->frame.payload_size = recv_frame->len; + + if (recv_frame->len > 0) { + memcpy(rxf->frame.payload, &recv_frame->data, recv_frame->len); + } + + } else { + struct can_frame *recv_frame = (struct can_frame *)&_recv_frame; + rxf->frame.can_id = recv_frame->can_id & CAN_SFF_MASK; + rxf->frame.payload_size = recv_frame->can_dlc; + + if (recv_frame->can_dlc > 0 && recv_frame->can_dlc <= CAN_MAX_DLEN) { + rxf->frame.payload_size = recv_frame->can_dlc; + memcpy(rxf->frame.payload, &recv_frame->data, recv_frame->can_dlc); + PX4_DEBUG("can len %d; can_id: %03x", recv_frame->can_dlc, recv_frame->can_id); + } + } + + /* Read SO_TIMESTAMP value */ + + if (_recv_cmsg->cmsg_level == SOL_SOCKET && _recv_cmsg->cmsg_type == SO_TIMESTAMP) { + struct timeval *tv = (struct timeval *)CMSG_DATA(_recv_cmsg); + rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec; + } + + return result; +} +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/src/ScoutRobot.cpp b/src/drivers/rover_interface/scout_sdk/src/ScoutRobot.cpp new file mode 100644 index 000000000000..e327c4898c84 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/src/ScoutRobot.cpp @@ -0,0 +1,274 @@ +#include "scout_sdk/ScoutRobot.hpp" + +#define MODULE_NAME "SCOUT_SDK" + +#include + +namespace scoutsdk +{ +ScoutRobot::ScoutRobot(ProtocolVersion protocol, bool is_mini_model) +{ + if (!is_mini_model) { + PX4_ERR("Only Scout Mini model is supported. Aborted"); + + } else { + if (protocol == ProtocolVersion::AGX_V1) { + PX4_ERR("Protocol AGX V1 is not supported. Aborted"); + + } else if (protocol == ProtocolVersion::AGX_V2) { + PX4_INFO("Protocol AGX V2 is supported"); + } + } + + // Setup buffers + _tx_frame.frame.payload = &_tx_data; + _rx_frame.frame.payload = &_rx_data; + + _version_response_msgs.str_version_response = (const char *)_version_response_string; +} + + +ScoutRobot::~ScoutRobot() +{ + Disconnect(); +} + + +void ScoutRobot::Connect(const char *const can_dev, const uint32_t can_bitrate) +{ + _can = new SocketCAN(); + + if (_can->Init(can_dev, can_bitrate) == PX4_OK) { _can_connected = true; } +} + + +void ScoutRobot::Disconnect() +{ + if (_can_connected) { _can->Close(); } + + if (_can != nullptr) { + delete _can; + _can = nullptr; + } +} + + +void ScoutRobot::UpdateRobotCoreState(const AgxMessage &status_msg) +{ + switch (status_msg.type) { + case AgxMsgSystemState: { + // std::cout << "system status feedback received" << std::endl; + _core_state_msgs.system_state = status_msg.body.system_state_msg; + break; + } + + case AgxMsgMotionState: { + // std::cout << "motion control feedback received" << std::endl; + _core_state_msgs.motion_state = status_msg.body.motion_state_msg; + break; + } + + case AgxMsgLightState: { + // std::cout << "light control feedback received" << std::endl; + _core_state_msgs.light_state = status_msg.body.light_state_msg; + break; + } + + case AgxMsgRcState: { + // std::cout << "rc feedback received" << std::endl; + _core_state_msgs.rc_state = status_msg.body.rc_state_msg; + break; + } + + default: + break; + } +} + +void ScoutRobot::UpdateActuatorState(const AgxMessage &status_msg) +{ + switch (status_msg.type) { + case AgxMsgActuatorHSState: { + // std::cout << "actuator hs feedback received" << std::endl; + _actuator_state_msgs + .actuator_hs_state[status_msg.body.actuator_hs_state_msg.motor_id] = + status_msg.body.actuator_hs_state_msg; + break; + } + + case AgxMsgActuatorLSState: { + // std::cout << "actuator ls feedback received" << std::endl; + _actuator_state_msgs + .actuator_ls_state[status_msg.body.actuator_ls_state_msg.motor_id] = + status_msg.body.actuator_ls_state_msg; + break; + } + + default: + break; + } +} + + +void ScoutRobot::UpdateMotorState(const AgxMessage &status_msg) +{ + switch (status_msg.type) { + case AgxMsgMotorAngle: { + _motor_state_msgs.MotorAngle.angle_5 = status_msg.body.motor_angle_msg.angle_5; + _motor_state_msgs.MotorAngle.angle_6 = status_msg.body.motor_angle_msg.angle_6; + _motor_state_msgs.MotorAngle.angle_7 = status_msg.body.motor_angle_msg.angle_7; + _motor_state_msgs.MotorAngle.angle_8 = status_msg.body.motor_angle_msg.angle_8; + break; + } + + case AgxMsgMotorSpeed: { + _motor_state_msgs.MotorSpeed.speed_1 = status_msg.body.motor_speed_msg.speed_1; + _motor_state_msgs.MotorSpeed.speed_2 = status_msg.body.motor_speed_msg.speed_2; + _motor_state_msgs.MotorSpeed.speed_3 = status_msg.body.motor_speed_msg.speed_3; + _motor_state_msgs.MotorSpeed.speed_4 = status_msg.body.motor_speed_msg.speed_4; + break; + } + + default: + break; + } +} + + +int ScoutRobot::UpdateVersionResponse(const AgxMessage &status_msg) +{ + switch (status_msg.type) { + case AgxMsgVersionResponse: { + char temp_version_response[9] = {0}; + + for (int i = 0; i < 8; i++) { + uint8_t data = status_msg.body.version_str[i]; + + if (data < 32 || data > 126) { data = 32; } + + snprintf(temp_version_response + i, 2, "%c", data); + } + + strcpy(_version_response_string, temp_version_response); + return PX4_OK; + } + + default: + break; + } + + return PX4_ERROR; +} + + +/****************** + * PUBLIC METHODS * +******************/ +void ScoutRobot::CheckUpdateFromRover() +{ + _can->ReceiveFrame(&_rx_frame); + + AgxMessage status_msg; + + if (_parser.DecodeMessage(&_rx_frame, &status_msg)) { + UpdateRobotCoreState(status_msg); // 0x211 + UpdateActuatorState(status_msg); // 0x251-0x254 + UpdateMotorState(status_msg); + } +} + +void ScoutRobot::EnableCommandMode() +{ + PX4_INFO("EnableCommandMode"); + // Construct message + AgxMessage msg; + msg.type = AgxMsgControlModeConfig; + msg.body.control_mode_config_msg.mode = CONTROL_MODE_CAN; + + // Encode msg to can frame and send to bus + if (_parser.EncodeMessage(&msg, &_tx_frame)) { + uint64_t count = 0; + + while (count < 100) { + count++; + _can->SendFrame(_tx_frame); + } + } +} + +void ScoutRobot::QuerySystemVersion(const uint64_t timeout_msec) +{ + // Construct message + AgxMessage msg; + msg.type = AgxMsgVersionRequest; + msg.body.version_request_msg.request = 1; + + const hrt_abstime begin = hrt_absolute_time(); + + while (hrt_elapsed_time(&begin) < timeout_msec) { + // Send request + if (_parser.EncodeMessage(&msg, &_tx_frame)) { _can->SendFrame(_tx_frame); } + + // Receive response + _can->ReceiveFrame(&_rx_frame); + + AgxMessage status_msg; + + if (_parser.DecodeMessage(&_rx_frame, &status_msg)) { + if (UpdateVersionResponse(status_msg) == PX4_OK) { break; } + } + } +} + + +void ScoutRobot::SetMotionCommand(float linear_vel, float angular_vel) +{ + if (_can_connected) { + // motion control message + AgxMessage msg; + msg.type = AgxMsgMotionCommand; + msg.body.motion_command_msg.linear_velocity = linear_vel; + msg.body.motion_command_msg.angular_velocity = -angular_vel; + msg.body.motion_command_msg.lateral_velocity = 0.0; + msg.body.motion_command_msg.steering_angle = 0.0; + + PX4_DEBUG("SetMotionCommand: linear_vel: %f, angular_vel: %f", static_cast(linear_vel), + static_cast(angular_vel)); + + // send to can bus + if (_parser.EncodeMessage(&msg, &_tx_frame)) { _can->SendFrame(_tx_frame); } + } +} + + +void ScoutRobot::SetLightCommand(LightMode f_mode, uint8_t f_value, + LightMode r_mode, uint8_t r_value) +{ + if (_can_connected) { + AgxMessage msg; + msg.type = AgxMsgLightCommand; + msg.body.light_command_msg.enable_cmd_ctrl = true; + msg.body.light_command_msg.front_light.mode = f_mode; + msg.body.light_command_msg.front_light.custom_value = f_value; + msg.body.light_command_msg.rear_light.mode = r_mode; + msg.body.light_command_msg.rear_light.custom_value = r_value; + + // send to can bus + if (_parser.EncodeMessage(&msg, &_tx_frame)) { _can->SendFrame(_tx_frame); } + } +} + + +void ScoutRobot::DisableLightControl() +{ + if (_can_connected) { + AgxMessage msg; + msg.type = AgxMsgLightCommand; + + msg.body.light_command_msg.enable_cmd_ctrl = false; + + // send to can bus + if (_parser.EncodeMessage(&msg, &_tx_frame)) { _can->SendFrame(_tx_frame); } + } +} +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/src/Utilities.cpp b/src/drivers/rover_interface/scout_sdk/src/Utilities.cpp new file mode 100644 index 000000000000..58b2613d878d --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/src/Utilities.cpp @@ -0,0 +1,86 @@ +#include "scout_sdk/Utilities.hpp" +#include +#include +#include + +#define MODULE_NAME "SCOUT_SDK" + +namespace scoutsdk +{ + +ProtocolDetector::ProtocolDetector() : _can(nullptr), _msg_v1_detected(false), _msg_v2_detected(false) {} + +ProtocolDetector::~ProtocolDetector() +{ + if (_can != nullptr) { + _can->Close(); + delete _can; + _can = nullptr; + } +} + +bool ProtocolDetector::Connect(const char *const _canname, const uint32_t bitrate) +{ + _can = new SocketCAN(); + + if (_can->Init(_canname, bitrate) == PX4_OK) { return true; } else { return false; } +} + +ProtocolVersion ProtocolDetector::DetectProtocolVersion(const uint64_t timeout_msec) +{ + _msg_v1_detected = false; + _msg_v2_detected = false; + + const hrt_abstime start_time = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_time) < timeout_msec) { + //ParseCANFrame(); + if (_msg_v1_detected || _msg_v2_detected) { break; } + } + + // remove + _msg_v2_detected = true; + + // make sure only one version is detected + if (_msg_v1_detected && _msg_v2_detected) { + return ProtocolVersion::UNKNOWN; + + } else if (_msg_v1_detected) { + return ProtocolVersion::AGX_V1; + + } else if (_msg_v2_detected) { + return ProtocolVersion::AGX_V2; + + } else { + return ProtocolVersion::UNKNOWN; + } +}; + +void ProtocolDetector::ParseCANFrame() +{ + uint8_t data[8] {0}; + RxFrame rxf{}; + rxf.frame.payload = &data; + + if (_can->ReceiveFrame(&rxf) <= 0) { return; } + + switch (rxf.frame.can_id) { + // state feedback frame with id 0x151 is unique to V1 protocol + case 0x151: { + PX4_INFO("Protocol V1 detected"); + _msg_v1_detected = true; + break; + } + + // motion control feedback frame with id 0x221 is unique to V2 protocol + case 0x221: + + // rc state feedback frame with id 0x241 is unique to V2 protocol + case 0x241: { + PX4_INFO("Protocol V2 detected"); + _msg_v2_detected = true; + break; + } + } +} +} // namespace scoutsdk diff --git a/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_msg_parser_v2.c b/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_msg_parser_v2.c new file mode 100644 index 000000000000..2f0901a67582 --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_msg_parser_v2.c @@ -0,0 +1,527 @@ +#include "scout_sdk/agilex_protocol/agilex_protocol_v2.h" +#include "scout_sdk/agilex_protocol/agilex_msg_parser_v2.h" + +#include "stdio.h" +#include "string.h" + +bool DecodeCanFrameV2(const CANFrame *can_frame, AgxMessage *msg) +{ + msg->type = AgxMsgUnknown; + + switch (can_frame->can_id) { + /***************** command frame *****************/ + case CAN_MSG_MOTION_COMMAND_ID: { + msg->type = AgxMsgMotionCommand; + // parse frame buffer to message + MotionCommandFrame *frame = (MotionCommandFrame *)(can_frame->payload); + msg->body.motion_command_msg.linear_velocity = + (int16_t)((uint16_t)(frame->linear_velocity.low_byte) | + (uint16_t)(frame->linear_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_command_msg.angular_velocity = + (int16_t)((uint16_t)(frame->angular_velocity.low_byte) | + (uint16_t)(frame->angular_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_command_msg.lateral_velocity = + (int16_t)((uint16_t)(frame->lateral_velocity.low_byte) | + (uint16_t)(frame->lateral_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_command_msg.steering_angle = + (int16_t)((uint16_t)(frame->steering_angle.low_byte) | + (uint16_t)(frame->steering_angle.high_byte) << 8) / + 1000.0; + break; + } + + case CAN_MSG_LIGHT_COMMAND_ID: { + msg->type = AgxMsgLightCommand; + // parse frame buffer to message + LightCommandFrame *frame = (LightCommandFrame *)(can_frame->payload); + msg->body.light_command_msg.enable_cmd_ctrl = + (frame->enable_cmd_ctrl != 0) ? true : false; + msg->body.light_command_msg.front_light.mode = frame->front_mode; + msg->body.light_command_msg.front_light.custom_value = + frame->front_custom; + msg->body.light_command_msg.rear_light.mode = frame->rear_mode; + msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom; + break; + } + + case CAN_MSG_BRAKING_COMMAND_ID: { + msg->type = AgxMsgBrakingCommand; + break; + } + + /***************** feedback frame ****************/ + case CAN_MSG_SYSTEM_STATE_ID: { + msg->type = AgxMsgSystemState; + SystemStateFrame *frame = (SystemStateFrame *)(can_frame->payload); + msg->body.system_state_msg.vehicle_state = frame->vehicle_state; + msg->body.system_state_msg.control_mode = frame->control_mode; + msg->body.system_state_msg.battery_voltage = + (int16_t)((uint16_t)(frame->battery_voltage.low_byte) | + (uint16_t)(frame->battery_voltage.high_byte) << 8) * + 0.1; + msg->body.system_state_msg.error_code = + (uint16_t)(frame->error_code.low_byte) | + (uint16_t)(frame->error_code.high_byte) << 8; + break; + } + + case CAN_MSG_MOTION_STATE_ID: { + msg->type = AgxMsgMotionState; + MotionStateFrame *frame = (MotionStateFrame *)(can_frame->payload); + msg->body.motion_state_msg.linear_velocity = + (int16_t)((uint16_t)(frame->linear_velocity.low_byte) | + (uint16_t)(frame->linear_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_state_msg.angular_velocity = + (int16_t)((uint16_t)(frame->angular_velocity.low_byte) | + (uint16_t)(frame->angular_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_state_msg.lateral_velocity = + (int16_t)((uint16_t)(frame->lateral_velocity.low_byte) | + (uint16_t)(frame->lateral_velocity.high_byte) << 8) / + 1000.0; + msg->body.motion_state_msg.steering_angle = + (int16_t)((uint16_t)(frame->steering_angle.low_byte) | + (uint16_t)(frame->steering_angle.high_byte) << 8) / + 100.0; + break; + } + + case CAN_MSG_LIGHT_STATE_ID: { + msg->type = AgxMsgLightState; + LightStateFrame *frame = (LightStateFrame *)(can_frame->payload); + msg->body.light_command_msg.enable_cmd_ctrl = + (frame->enable_cmd_ctrl != 0) ? true : false; + msg->body.light_command_msg.front_light.mode = frame->front_mode; + msg->body.light_command_msg.front_light.custom_value = + frame->front_custom; + msg->body.light_command_msg.rear_light.mode = frame->rear_mode; + msg->body.light_command_msg.rear_light.custom_value = frame->rear_custom; + break; + } + + case CAN_MSG_RC_STATE_ID: { + msg->type = AgxMsgRcState; + RcStateFrame *frame = (RcStateFrame *)(can_frame->payload); + + // switch a + if ((frame->sws & RC_SWA_MASK) == RC_SWA_UP_MASK) { + msg->body.rc_state_msg.swa = RC_SWITCH_UP; + + } else if ((frame->sws & RC_SWA_MASK) == RC_SWA_DOWN_MASK) { + msg->body.rc_state_msg.swa = RC_SWITCH_DOWN; + } + + // switch b + if ((frame->sws & RC_SWB_MASK) == RC_SWB_UP_MASK) { + msg->body.rc_state_msg.swb = RC_SWITCH_UP; + + } else if ((frame->sws & RC_SWB_MASK) == RC_SWB_MIDDLE_MASK) { + msg->body.rc_state_msg.swb = RC_SWITCH_MIDDLE; + + } else if ((frame->sws & RC_SWB_MASK) == RC_SWB_DOWN_MASK) { + msg->body.rc_state_msg.swb = RC_SWITCH_DOWN; + } + + // switch c + if ((frame->sws & RC_SWC_MASK) == RC_SWC_UP_MASK) { + msg->body.rc_state_msg.swc = RC_SWITCH_UP; + + } else if ((frame->sws & RC_SWC_MASK) == RC_SWC_MIDDLE_MASK) { + msg->body.rc_state_msg.swc = RC_SWITCH_MIDDLE; + + } else if ((frame->sws & RC_SWC_MASK) == RC_SWC_DOWN_MASK) { + msg->body.rc_state_msg.swc = RC_SWITCH_DOWN; + } + + // switch d + if ((frame->sws & RC_SWD_MASK) == RC_SWD_UP_MASK) { + msg->body.rc_state_msg.swd = RC_SWITCH_UP; + + } else if ((frame->sws & RC_SWD_MASK) == RC_SWD_DOWN_MASK) { + msg->body.rc_state_msg.swd = RC_SWITCH_DOWN; + } + + msg->body.rc_state_msg.stick_right_v = frame->stick_right_v; + msg->body.rc_state_msg.stick_right_h = frame->stick_right_h; + msg->body.rc_state_msg.stick_left_v = frame->stick_left_v; + msg->body.rc_state_msg.stick_left_h = frame->stick_left_h; + msg->body.rc_state_msg.var_a = frame->var_a; + break; + } + + case CAN_MSG_ACTUATOR1_HS_STATE_ID: + case CAN_MSG_ACTUATOR2_HS_STATE_ID: + case CAN_MSG_ACTUATOR3_HS_STATE_ID: + case CAN_MSG_ACTUATOR4_HS_STATE_ID: + case CAN_MSG_ACTUATOR5_HS_STATE_ID: + case CAN_MSG_ACTUATOR6_HS_STATE_ID: + case CAN_MSG_ACTUATOR7_HS_STATE_ID: + case CAN_MSG_ACTUATOR8_HS_STATE_ID: { + msg->type = AgxMsgActuatorHSState; + ActuatorHSStateFrame *frame = (ActuatorHSStateFrame *)(can_frame->payload); + msg->body.actuator_hs_state_msg.motor_id = + can_frame->can_id - CAN_MSG_ACTUATOR1_HS_STATE_ID; + msg->body.actuator_hs_state_msg.rpm = + (int16_t)((uint16_t)(frame->rpm.low_byte) | + (uint16_t)(frame->rpm.high_byte) << 8); + msg->body.actuator_hs_state_msg.current = + ((uint16_t)(frame->current.low_byte) | + (uint16_t)(frame->current.high_byte) << 8) * 0.1; + msg->body.actuator_hs_state_msg.pulse_count = + (int32_t)((uint32_t)(frame->pulse_count.lsb) | + (uint32_t)(frame->pulse_count.low_byte) << 8 | + (uint32_t)(frame->pulse_count.high_byte) << 16 | + (uint32_t)(frame->pulse_count.msb) << 24); + break; + } + + case CAN_MSG_ACTUATOR1_LS_STATE_ID: + case CAN_MSG_ACTUATOR2_LS_STATE_ID: + case CAN_MSG_ACTUATOR3_LS_STATE_ID: + case CAN_MSG_ACTUATOR4_LS_STATE_ID: + case CAN_MSG_ACTUATOR5_LS_STATE_ID: + case CAN_MSG_ACTUATOR6_LS_STATE_ID: + case CAN_MSG_ACTUATOR7_LS_STATE_ID: + case CAN_MSG_ACTUATOR8_LS_STATE_ID: { + msg->type = AgxMsgActuatorLSState; + ActuatorLSStateFrame *frame = (ActuatorLSStateFrame *)(can_frame->payload); + msg->body.actuator_hs_state_msg.motor_id = + can_frame->can_id - CAN_MSG_ACTUATOR1_LS_STATE_ID; + msg->body.actuator_ls_state_msg.driver_voltage = + ((uint16_t)(frame->driver_voltage.low_byte) | + (uint16_t)(frame->driver_voltage.high_byte) << 8) * + 0.1; + msg->body.actuator_ls_state_msg.driver_temp = + (int16_t)((uint16_t)(frame->driver_temp.low_byte) | + (uint16_t)(frame->driver_temp.high_byte) << 8); + msg->body.actuator_ls_state_msg.motor_temp = + frame->motor_temp; + msg->body.actuator_ls_state_msg.driver_state = frame->driver_state; + break; + } + + case CAN_MSG_CURRENT_CTRL_MODE: { + msg->type = AgxMsgMotionModeState; + MotionModeStateFrame *frame = (MotionModeStateFrame *)(can_frame->payload); + msg->body.motion_mode_state_msg.motion_mode = frame->motion_mode; + msg->body.motion_mode_state_msg.mode_changing = frame->mode_changing; + break; + } + + /****************** sensor frame *****************/ + case CAN_MSG_ODOMETRY_ID: { + msg->type = AgxMsgOdometry; + OdometryFrame *frame = (OdometryFrame *)(can_frame->payload); + msg->body.odometry_msg.left_wheel = + (int32_t)((uint32_t)(frame->left_wheel.lsb) | + (uint32_t)(frame->left_wheel.low_byte) << 8 | + (uint32_t)(frame->left_wheel.high_byte) << 16 | + (uint32_t)(frame->left_wheel.msb) << 24); + msg->body.odometry_msg.right_wheel = + (int32_t)((uint32_t)(frame->right_wheel.lsb) | + (uint32_t)(frame->right_wheel.low_byte) << 8 | + (uint32_t)(frame->right_wheel.high_byte) << 16 | + (uint32_t)(frame->right_wheel.msb) << 24); + break; + } + + case CAN_MSG_IMU_ACCEL_ID: { + msg->type = AgxMsgImuAccel; + break; + } + + case CAN_MSG_IMU_GYRO_ID: { + msg->type = AgxMsgImuGyro; + break; + } + + case CAN_MSG_IMU_EULER_ID: { + msg->type = AgxMsgImuEuler; + break; + } + + case CAN_MSG_SAFETY_BUMPER_ID: { + msg->type = AgxMsgSafetyBumper; + break; + } + + case CAN_MSG_ULTRASONIC_1_ID: + case CAN_MSG_ULTRASONIC_2_ID: + case CAN_MSG_ULTRASONIC_3_ID: + case CAN_MSG_ULTRASONIC_4_ID: + case CAN_MSG_ULTRASONIC_5_ID: + case CAN_MSG_ULTRASONIC_6_ID: + case CAN_MSG_ULTRASONIC_7_ID: + case CAN_MSG_ULTRASONIC_8_ID: { + msg->type = AgxMsgUltrasonic; + break; + } + + case CAN_MSG_UWB_1_ID: + case CAN_MSG_UWB_2_ID: + case CAN_MSG_UWB_3_ID: + case CAN_MSG_UWB_4_ID: { + msg->type = AgxMsgUwb; + break; + } + + case CAN_MSG_BMS_BASIC_ID: { + msg->type = AgxMsgBmsBasic; + BmsBasicFrame *frame = (BmsBasicFrame *)(can_frame->payload); + msg->body.bms_basic_msg.battery_soc = frame->battery_soc; + msg->body.bms_basic_msg.battery_soh = frame->battery_soh; + msg->body.bms_basic_msg.voltage = + (int16_t)((uint16_t)(frame->voltage.low_byte) | + (uint16_t)(frame->voltage.high_byte) << 8) * 0.1; + msg->body.bms_basic_msg.current = + (int16_t)((uint16_t)(frame->current.low_byte) | + (uint16_t)(frame->current.high_byte) << 8) * 0.1; + msg->body.bms_basic_msg.temperature = + (int16_t)((uint16_t)(frame->temperature.low_byte) | + (uint16_t)(frame->temperature.high_byte) << 8) * 0.1; + break; + } + + case CAN_MSG_BMS_EXTENDED_ID: { + msg->type = AgxMsgBmsExtended; + break; + } + + /*************** query/config frame **************/ + case CAN_MSG_VERSION_REQUEST_ID: { + msg->type = AgxMsgVersionRequest; + break; + } + + case CAN_MSG_VERSION_RESPONSE_ID: { + msg->type = AgxMsgVersionResponse; + VersionResponseFrame *frame = (VersionResponseFrame *)(can_frame->payload); + memcpy(msg->body.version_str, (uint8_t *)frame, 8); + break; + } + + case CAN_MSG_CTRL_MODE_CONFIG_ID: { + msg->type = AgxMsgControlModeConfig; + ControlModeConfigFrame *frame = (ControlModeConfigFrame *)(can_frame->payload); + msg->body.control_mode_config_msg.mode = frame->mode; + break; + } + + case CAN_MSG_STEER_NEUTRAL_REQUEST_ID: { + msg->type = AgxMsgSteerNeutralRequest; + break; + } + + case CAN_MSG_STEER_NEUTRAL_RESPONSE_ID: { + msg->type = AgxMsgSteerNeutralResponse; + break; + } + + case CAN_MSG_STATE_RESET_CONFIG_ID: { + msg->type = AgxMsgStateResetConfig; + StateResetConfigFrame *frame = (StateResetConfigFrame *)(can_frame->payload); + msg->body.state_reset_config_msg.error_clear_byte = + frame->error_clear_byte; + break; + } + + case CAN_MSG_MOTOR_ANGLE_INFO: { + msg->type = AgxMsgMotorAngle; + MoterAngleFrame *frame = (MoterAngleFrame *)(can_frame->payload); + msg->body.motor_angle_msg.angle_5 = + (int16_t)((uint16_t)(frame->angle_5.low_byte) | + (uint16_t)(frame->angle_5.high_byte) << 8) * 0.01; + msg->body.motor_angle_msg.angle_6 = + (int16_t)((uint16_t)(frame->angle_6.low_byte) | + (uint16_t)(frame->angle_6.high_byte) << 8) * 0.01; + msg->body.motor_angle_msg.angle_7 = + (int16_t)((uint16_t)(frame->angle_7.low_byte) | + (uint16_t)(frame->angle_7.high_byte) << 8) * 0.01; + msg->body.motor_angle_msg.angle_8 = + (int16_t)((uint16_t)(frame->angle_8.low_byte) | + (uint16_t)(frame->angle_8.high_byte) << 8) * 0.01; + break; + } + + case CAN_MSG_MOTOR_SPEED_INFO: { + msg->type = AgxMsgMotorSpeed; + MoterSpeedFrame *frame = (MoterSpeedFrame *)(can_frame->payload); + msg->body.motor_speed_msg.speed_1 = + (int16_t)((uint16_t)(frame->speed_1.low_byte) | + (uint16_t)(frame->speed_1.high_byte) << 8) * 0.001; + msg->body.motor_speed_msg.speed_2 = + (int16_t)((uint16_t)(frame->speed_2.low_byte) | + (uint16_t)(frame->speed_2.high_byte) << 8) * 0.001; + msg->body.motor_speed_msg.speed_3 = + (int16_t)((uint16_t)(frame->speed_3.low_byte) | + (uint16_t)(frame->speed_3.high_byte) << 8) * 0.001; + msg->body.motor_speed_msg.speed_4 = + (int16_t)((uint16_t)(frame->speed_4.low_byte) | + (uint16_t)(frame->speed_4.high_byte) << 8) * 0.001; + break; + } + + default: + break; + } + + return true; +} + +bool EncodeCanFrameV2(const AgxMessage *msg, CANFrame *can_frame) +{ + bool ret = true; + + switch (msg->type) { + /***************** command frame *****************/ + case AgxMsgMotionCommand: { + can_frame->can_id = CAN_MSG_MOTION_COMMAND_ID; + can_frame->payload_size = 8; + MotionCommandFrame frame; + int16_t linear_cmd = + (int16_t)(msg->body.motion_command_msg.linear_velocity * 1000); + int16_t angular_cmd = + (int16_t)(msg->body.motion_command_msg.angular_velocity * 1000); + int16_t lateral_cmd = + (int16_t)(msg->body.motion_command_msg.lateral_velocity * 1000); + int16_t steering_cmd = + (int16_t)(msg->body.motion_command_msg.steering_angle * 1000); + frame.linear_velocity.high_byte = (uint8_t)(linear_cmd >> 8); + frame.linear_velocity.low_byte = (uint8_t)(linear_cmd & 0x00ff); + frame.angular_velocity.high_byte = (uint8_t)(angular_cmd >> 8); + frame.angular_velocity.low_byte = (uint8_t)(angular_cmd & 0x00ff); + frame.lateral_velocity.high_byte = (uint8_t)(lateral_cmd >> 8); + frame.lateral_velocity.low_byte = (uint8_t)(lateral_cmd & 0x00ff); + frame.steering_angle.high_byte = (uint8_t)(steering_cmd >> 8); + frame.steering_angle.low_byte = (uint8_t)(steering_cmd & 0x00ff); + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + case AgxMsgLightCommand: { + static uint8_t count = 0; + can_frame->can_id = CAN_MSG_LIGHT_COMMAND_ID; + can_frame->payload_size = 8; + LightCommandFrame frame; + + if (msg->body.light_command_msg.enable_cmd_ctrl) { + frame.enable_cmd_ctrl = LIGHT_ENABLE_CMD_CTRL; + frame.front_mode = msg->body.light_command_msg.front_light.mode; + frame.front_custom = + msg->body.light_command_msg.front_light.custom_value; + frame.rear_mode = msg->body.light_command_msg.rear_light.mode; + frame.rear_custom = msg->body.light_command_msg.rear_light.custom_value; + + } else { + frame.enable_cmd_ctrl = LIGHT_DISABLE_CMD_CTRL; + frame.front_mode = 0; + frame.front_custom = 0; + frame.rear_mode = 0; + frame.rear_custom = 0; + } + + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.count = count++; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + case AgxMsgBrakingCommand: { + static uint8_t count = 0; + can_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID; + can_frame->payload_size = 2; + BrakingCommandFrame frame; + frame.enable_brake = msg->body.braking_command_msg.enable_braking; + frame.count = count++; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + case AgxMsgSetMotionModeCommand: { + can_frame->can_id = CAN_MSG_SET_MOTION_MODE_ID; + can_frame->payload_size = 8; + SetMotionModeFrame frame; + frame.motion_mode = msg->body.motion_mode_msg.motion_mode; + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.reserved2 = 0; + frame.reserved3 = 0; + frame.reserved4 = 0; + frame.reserved5 = 0; + frame.reserved6 = 0; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + /*************** query/config frame **************/ + case AgxMsgVersionRequest: { + can_frame->can_id = CAN_MSG_VERSION_REQUEST_ID; + can_frame->payload_size = 8; + VersionRequestFrame frame; + frame.request = msg->body.version_request_msg.request; + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.reserved2 = 0; + frame.reserved3 = 0; + frame.reserved4 = 0; + frame.reserved5 = 0; + frame.reserved6 = 0; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + case AgxMsgControlModeConfig: { + can_frame->can_id = CAN_MSG_CTRL_MODE_CONFIG_ID; + can_frame->payload_size = 8; + ControlModeConfigFrame frame; + frame.mode = msg->body.control_mode_config_msg.mode; + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.reserved2 = 0; + frame.reserved3 = 0; + frame.reserved4 = 0; + frame.reserved5 = 0; + frame.reserved6 = 0; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + case AgxMsgBrakeModeConfig: { + can_frame->can_id = CAN_MSG_BRAKING_COMMAND_ID; + can_frame->payload_size = 8; + BrakeModeConfigFrame frame; + frame.mode = msg->body.control_mode_config_msg.mode; + frame.reserved0 = 0; + frame.reserved1 = 0; + frame.reserved2 = 0; + frame.reserved3 = 0; + frame.reserved4 = 0; + frame.reserved5 = 0; + frame.reserved6 = 0; + memcpy(can_frame->payload, (uint8_t *)(&frame), can_frame->payload_size); + break; + } + + default: { + ret = false; + break; + } + } + + return ret; +} + +uint8_t CalcCanFrameChecksumV2(uint16_t id, uint8_t *data, uint8_t dlc) +{ + uint8_t checksum = 0x00; + checksum = (uint8_t)(id & 0x00ff) + (uint8_t)(id >> 8) + dlc; + + for (int i = 0; i < (dlc - 1); ++i) { checksum += data[i]; } + + return checksum; +} diff --git a/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_protocol_v2_parser.cpp b/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_protocol_v2_parser.cpp new file mode 100644 index 000000000000..73b73d37427b --- /dev/null +++ b/src/drivers/rover_interface/scout_sdk/src/agilex_protocol/agilex_protocol_v2_parser.cpp @@ -0,0 +1,19 @@ +#include "scout_sdk/agilex_protocol/agilex_protocol_v2_parser.hpp" + +namespace scoutsdk +{ +bool AgileXProtocolV2Parser::DecodeMessage(const RxFrame *rxf, AgxMessage *msg) +{ + return DecodeCanFrameV2(&rxf->frame, msg); +} + +bool AgileXProtocolV2Parser::EncodeMessage(const AgxMessage *msg, TxFrame *txf) +{ + return EncodeCanFrameV2(msg, &txf->frame); +} + +uint8_t AgileXProtocolV2Parser::CalculateChecksum(uint16_t id, uint8_t *data, uint8_t dlc) +{ + return CalcCanFrameChecksumV2(id, data, dlc); +} +} // namespace scoutsdk \ No newline at end of file diff --git a/src/drivers/stub_keystore/CMakeLists.txt b/src/drivers/stub_keystore/CMakeLists.txt index ecbac32daa0d..d3f71ed5a863 100644 --- a/src/drivers/stub_keystore/CMakeLists.txt +++ b/src/drivers/stub_keystore/CMakeLists.txt @@ -36,6 +36,10 @@ px4_add_library(keystore_backend stub_keystore.c) target_include_directories(keystore_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +# This interface library can be used to get the backend headers +add_library(keystore_backend_interface INTERFACE) +target_include_directories(keystore_backend_interface INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) + # Parse keyfile locations from boardconfig # If the key file path is defined in environment variable, use it from there diff --git a/src/drivers/stub_keystore/stub_keystore.c b/src/drivers/stub_keystore/stub_keystore.c index 0f33436479a5..1e5054021ad3 100644 --- a/src/drivers/stub_keystore/stub_keystore.c +++ b/src/drivers/stub_keystore/stub_keystore.c @@ -45,6 +45,10 @@ void keystore_init(void) { } +void keystore_deinit(void) +{ +} + keystore_session_handle_t keystore_open(void) { keystore_session_handle_t ret; diff --git a/src/drivers/sw_crypto/CMakeLists.txt b/src/drivers/sw_crypto/CMakeLists.txt index 16d2d215cc1d..0e4535e33c1a 100644 --- a/src/drivers/sw_crypto/CMakeLists.txt +++ b/src/drivers/sw_crypto/CMakeLists.txt @@ -45,5 +45,9 @@ target_link_libraries(crypto_backend monocypher libtomcrypt ) - target_include_directories(crypto_backend PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +# This interface library can be used to get the backend headers +add_library(crypto_backend_interface INTERFACE) +target_link_libraries(crypto_backend_interface INTERFACE keystore_backend_interface) +target_include_directories(crypto_backend_interface INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/drivers/sw_crypto/crypto.c b/src/drivers/sw_crypto/crypto.c index ca0a23de8e66..3ef3cc206aa3 100644 --- a/src/drivers/sw_crypto/crypto.c +++ b/src/drivers/sw_crypto/crypto.c @@ -157,6 +157,7 @@ void crypto_init() void crypto_deinit() { + keystore_deinit(); } crypto_session_handle_t crypto_open(px4_crypto_algorithm_t algorithm) @@ -233,7 +234,14 @@ bool crypto_signature_check(crypto_session_handle_t handle, switch (handle.algorithm) { case CRYPTO_ED25519: - ret = crypto_ed25519_check(signature, public_key, message, message_size) == 0; + if (keylen >= 32) { + /* In the DER format ed25510 key the raw public key part is always the last 32 bytes. + * This simple "parsing" works for both "raw" key and DER format + */ + public_key += keylen - 32; + ret = crypto_ed25519_check(signature, public_key, message, message_size) == 0; + } + break; default: @@ -248,7 +256,9 @@ bool crypto_encrypt_data(crypto_session_handle_t handle, const uint8_t *message, size_t message_size, uint8_t *cipher, - size_t *cipher_size) + size_t *cipher_size, + uint8_t *mac, + size_t *mac_size) { bool ret = false; @@ -276,8 +286,9 @@ bool crypto_encrypt_data(crypto_session_handle_t handle, uint8_t *key = (uint8_t *)crypto_get_key_ptr(handle.keystore_handle, key_idx, &key_sz); chacha20_context_t *context = handle.context; - if (key_sz == 32) { - context->ctr = crypto_xchacha20_ctr(cipher, message, *cipher_size, key, context->nonce, context->ctr); + if (key_sz == 32 && *cipher_size >= message_size) { + context->ctr = crypto_xchacha20_ctr(cipher, message, message_size, key, context->nonce, context->ctr); + *cipher_size = message_size; ret = true; } } @@ -378,12 +389,15 @@ bool crypto_get_encrypted_key(crypto_session_handle_t handle, // Encrypt it if (key != NULL) { + size_t mac_size = 0; ret = crypto_encrypt_data(handle, encryption_key_idx, plain_key, key_sz, key, - max_len); + max_len, + NULL, + &mac_size); } else { switch (handle.algorithm) { @@ -447,3 +461,24 @@ size_t crypto_get_min_blocksize(crypto_session_handle_t handle, uint8_t key_idx) return ret; } + +bool crypto_renew_nonce(crypto_session_handle_t handle, + const uint8_t *nonce, + size_t nonce_size) +{ + /* unimplemented */ + return false; +} + +bool crypto_decrypt_data(crypto_session_handle_t handle, + uint8_t key_index, + const uint8_t *cipher, + size_t cipher_size, + const uint8_t *mac, + size_t mac_size, + uint8_t *message, + size_t *message_size) +{ + /* unimplemented */ + return false; +} diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp index 1924b3529fbe..9310e9413a16 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp @@ -396,7 +396,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) float filtered_alt = NAN; float last_baro_alt = 0.f; - int airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); + orb_sub_t airdata_sub = orb_subscribe(ORB_ID(vehicle_air_data)); uint32_t lastBATV_ms = 0; uint32_t lastCUR_ms = 0; diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 96c2dd338685..55dee09de5d0 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -58,14 +58,14 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -static int _battery_sub = -1; -static int _gps_sub = -1; -static int _home_sub = -1; -static int _airdata_sub = -1; -static int _airspeed_sub = -1; -static int _esc_sub = -1; +static orb_sub_t _battery_sub = ORB_SUB_INVALID; +static orb_sub_t _gps_sub = ORB_SUB_INVALID; +static orb_sub_t _home_sub = ORB_SUB_INVALID; +static orb_sub_t _airdata_sub = ORB_SUB_INVALID; +static orb_sub_t _airspeed_sub = ORB_SUB_INVALID; +static orb_sub_t _esc_sub = ORB_SUB_INVALID; -static orb_advert_t _esc_pub = nullptr; +static orb_advert_t _esc_pub = ORB_ADVERT_INVALID; static bool _home_position_set = false; static double _home_lat = 0.0d; @@ -122,8 +122,8 @@ publish_gam_message(const uint8_t *buffer) esc.esc[0].esc_current = static_cast((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1F; /* announce the esc if needed, just publish else */ - if (_esc_pub != nullptr) { - orb_publish(ORB_ID(esc_status), _esc_pub, &esc); + if (orb_advert_valid(_esc_pub)) { + orb_publish(ORB_ID(esc_status), &_esc_pub, &esc); } else { _esc_pub = orb_advertise(ORB_ID(esc_status), &esc); diff --git a/src/drivers/tone_alarm/ToneAlarm.cpp b/src/drivers/tone_alarm/ToneAlarm.cpp index 5893f9018f7e..77b7d241fc55 100644 --- a/src/drivers/tone_alarm/ToneAlarm.cpp +++ b/src/drivers/tone_alarm/ToneAlarm.cpp @@ -42,6 +42,8 @@ using namespace time_literals; +bool ToneAlarm::_stop_note = false; + ToneAlarm::ToneAlarm() : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { @@ -51,13 +53,11 @@ ToneAlarm::ToneAlarm() : ToneAlarm::~ToneAlarm() { - ToneAlarmInterface::stop_note(); + // stop_note() is handled via request_stop() } bool ToneAlarm::Init() { - // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. - ToneAlarmInterface::init(); _tune_control_sub.set_interval_us(10_ms); @@ -73,11 +73,23 @@ bool ToneAlarm::Init() void ToneAlarm::InterruptStopNote(void *arg) { - ToneAlarmInterface::stop_note(); + _stop_note = true; } void ToneAlarm::Run() { + + if (!_initialized) { + // NOTE: Implement hardware specific detail in the ToneAlarmInterface class implementation. + ToneAlarmInterface::init(); + _initialized = true; + } + + if (_stop_note) { + ToneAlarmInterface::stop_note(); + _stop_note = false; + } + // Check if circuit breaker is enabled. if (!_circuit_break_initialized) { if (circuit_breaker_enabled("CBRK_BUZZER", CBRK_BUZZER_KEY)) { @@ -88,6 +100,7 @@ void ToneAlarm::Run() } if (should_exit()) { + ToneAlarmInterface::stop_note(); _tune_control_sub.unregisterCallback(); exit_and_cleanup(); return; @@ -211,8 +224,8 @@ void ToneAlarm::Run() hrt_call_at(&_hrt_call, time_started + duration, (hrt_callout)&InterruptStopNote, this); _next_note_time = time_started + duration + silence_length; - // schedule next note - ScheduleAt(_next_note_time); + // schedule at stop time + ScheduleAt(time_started + duration); } } else { @@ -235,6 +248,12 @@ void ToneAlarm::Run() } } +void ToneAlarm::request_stop() +{ + ModuleBase::request_stop(); + ScheduleNow(); +} + int ToneAlarm::task_spawn(int argc, char *argv[]) { ToneAlarm *instance = new ToneAlarm(); diff --git a/src/drivers/tone_alarm/ToneAlarm.h b/src/drivers/tone_alarm/ToneAlarm.h index fa04993e8c8c..ab1f4a89b49f 100644 --- a/src/drivers/tone_alarm/ToneAlarm.h +++ b/src/drivers/tone_alarm/ToneAlarm.h @@ -70,6 +70,8 @@ class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); + void request_stop() override; + private: static void InterruptStopNote(void *arg); @@ -86,4 +88,6 @@ class ToneAlarm : public ModuleBase, public px4::ScheduledWorkItem bool _circuit_break_initialized{false}; bool _play_tone{false}; + bool _initialized{false}; + static bool _stop_note; }; diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 37993c97ce51..fedc501f0676 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -78,7 +78,7 @@ add_definitions( -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 - -DUAVCAN_MEM_POOL_BLOCK_SIZE=48 + -DUAVCAN_MEM_POOL_BLOCK_SIZE=64 -DUAVCAN_NO_ASSERTIONS -DUAVCAN_PLATFORM=${UAVCAN_PLATFORM} ) diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 2031b3187750..4220fed727c6 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -51,6 +51,7 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) : _uavcan_sub_status(node) { _uavcan_pub_raw_cmd.setPriority(uavcan::TransferPriority::NumericallyMin); // Highest priority + param_get(param_find("UAVCAN_ESC_RL"), &_uavcan_rate_limit_enable); } int @@ -75,7 +76,7 @@ UavcanEscController::update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUA */ const auto timestamp = _node.getMonotonicTime(); - if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { + if (_uavcan_rate_limit_enable == 1 && (timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) { return; } @@ -137,7 +138,7 @@ UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructurenode_id = node_id; DEVICE_LOG("node %d instance %d ok", channel->node_id, channel->instance); - if (channel->orb_advert == nullptr) { + if (!orb_advert_valid(channel->orb_advert)) { DEVICE_LOG("uORB advertise failed. Out of instances?"); *channel = uavcan_bridge::Channel(); _out_of_channels = true; @@ -223,7 +223,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) assert(channel != nullptr); - (void)orb_publish(_orb_topic, channel->orb_advert, report); + (void)orb_publish(_orb_topic, &channel->orb_advert, report); } uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id) diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp index b3a8ce183625..c46a7ceca757 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -86,7 +86,7 @@ uavcan::uint32_t CanIface::socketInit(uint32_t index) return -1; } - snprintf(ifr.ifr_name, IFNAMSIZ, "can%li", index); + snprintf(ifr.ifr_name, IFNAMSIZ, "can%" PRIu32, index); ifr.ifr_name[IFNAMSIZ - 1] = '\0'; ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index c054304d76fc..99026e821a87 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -69,6 +69,10 @@ // #include and leaving OK undefined # define OK 0 +#if defined(__PX4_NUTTX) +#include +#endif + /* * UavcanNode */ @@ -512,10 +516,15 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } - ret = _safety_state_controller.init(); + int32_t safety_state_pub_enable = 0; + (void)param_get(param_find("UAVCAN_PUB_SS"), &safety_state_pub_enable); - if (ret < 0) { - return ret; + if (safety_state_pub_enable == 1) { + ret = _safety_state_controller.init(); + + if (ret < 0) { + return ret; + } } ret = _log_message_controller.init(); @@ -524,10 +533,15 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) return ret; } - ret = _rgbled_controller.init(); + int32_t rgbled_pub_enable = 0; + (void)param_get(param_find("UAVCAN_PUB_RGB"), &rgbled_pub_enable); - if (ret < 0) { - return ret; + if (rgbled_pub_enable == 1) { + ret = _rgbled_controller.init(); + + if (ret < 0) { + return ret; + } } /* Start node info retriever to fetch node info from new nodes */ @@ -566,11 +580,12 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanNode::cb_opcode)); _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart)); - int32_t uavcan_enable = 1; (void)param_get(param_find("UAVCAN_ENABLE"), &uavcan_enable); + int32_t uavcan_dynamic_node_server_enable = 1; + (void)param_get(param_find("UAVCAN_DNS"), &uavcan_dynamic_node_server_enable); - if (uavcan_enable > 1) { + if (uavcan_enable > 1 && uavcan_dynamic_node_server_enable == 1) { _servers = new UavcanServers(_node, _node_info_retriever); if (_servers) { @@ -770,7 +785,7 @@ UavcanNode::Run() int call_res = _param_getset_client.call(request.node_id, req); if (call_res < 0) { - PX4_ERR("couldn't send GetSet: %d", call_res); + PX4_DEBUG("couldn't send GetSet: %d", call_res); } else { _param_in_progress = true; @@ -856,7 +871,7 @@ UavcanNode::Run() if (call_res < 0) { _param_list_in_progress = false; - PX4_ERR("couldn't send param list GetSet: %d", call_res); + PX4_DEBUG("couldn't send param list GetSet: %d", call_res); } else { _param_in_progress = true; @@ -1081,7 +1096,7 @@ UavcanNode::cb_getset(const uavcan::ServiceCallResult(); } + PX4_DEBUG("Got node : %d, param index %d", response.node_id, response.param_index); _param_response_pub.publish(response); } else { - PX4_ERR("GetSet error"); + PX4_DEBUG("GetSet error at node : %d, param index %d, need resend...", result.getCallID().server_node_id.get(), + _param_index); + + uavcan::protocol::param::GetSet::Request req; + + req.index = _param_index; + + int call_res = _param_getset_client.call(result.getCallID().server_node_id.get(), req); + + if (call_res < 0) { + PX4_DEBUG("resend failed"); + } + + _param_index--; } _param_in_progress = false; @@ -1146,7 +1175,7 @@ UavcanNode::param_count(uavcan::NodeID node_id) // -ErrInvalidParam is returned when no UAVCAN device is connected to the CAN bus if ((call_res < 0) && (-uavcan::ErrInvalidParam != call_res)) { - PX4_ERR("couldn't start parameter count: %d", call_res); + PX4_DEBUG("couldn't start parameter count: %d", call_res); } else { _count_in_progress = true; diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 7fcc86e96839..4e4c8a27c771 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -154,7 +154,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame; static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1); - static constexpr unsigned ScheduleIntervalMs = 3; + static constexpr unsigned ScheduleIntervalMs = 1; /* @@ -267,7 +267,7 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vcmd_sub{ORB_ID(vehicle_command)}; - uORB::Subscription _param_request_sub{ORB_ID(uavcan_parameter_request)}; + uORB::SubscriptionInterval _param_request_sub{ORB_ID(uavcan_parameter_request), 50_ms}; uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 21672f7c1edc..0b0939687fa4 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -221,6 +221,30 @@ PARAM_DEFINE_INT32(UAVCAN_PUB_RTCM, 0); */ PARAM_DEFINE_INT32(UAVCAN_PUB_MBD, 0); +/** + * publish safety state + * + * Enable UAVCAN safety state publish to control CAN SafetyState led + * ardupilot::indication::SafetyState + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_SS, 0); + +/** + * publish rgb led + * + * Enable UAVCAN rgb led control + * uavcan::equipment::indication::LightsCommand + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_PUB_RGB, 0); + /** * subscription airspeed * @@ -378,3 +402,25 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_RNG, 0); * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_SUB_BTN, 0); + +/** + * use rate limit of ESC output + * + * Enable UAVCAN ESC rate limit for output update + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ESC_RL, 0); + +/** + * use UAVCAN dynamic node server + * + * Enable UAVCAN dynamic node server for automatic node enumeration + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_DNS, 0); diff --git a/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp b/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp index 97047f074842..568526c74310 100644 --- a/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp +++ b/src/examples/px4_mavlink_debug/px4_mavlink_debug.cpp @@ -93,19 +93,19 @@ int px4_mavlink_debug_main(int argc, char *argv[]) /* send one named value */ dbg_key.value = value_counter; dbg_key.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_key_value), pub_dbg_key, &dbg_key); + orb_publish(ORB_ID(debug_key_value), &pub_dbg_key, &dbg_key); /* send one indexed value */ dbg_ind.value = 0.5f * value_counter; dbg_ind.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_value), pub_dbg_ind, &dbg_ind); + orb_publish(ORB_ID(debug_value), &pub_dbg_ind, &dbg_ind); /* send one vector */ dbg_vect.x = 1.0f * value_counter; dbg_vect.y = 2.0f * value_counter; dbg_vect.z = 3.0f * value_counter; dbg_vect.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_vect), pub_dbg_vect, &dbg_vect); + orb_publish(ORB_ID(debug_vect), &pub_dbg_vect, &dbg_vect); /* send one array */ for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) { @@ -113,7 +113,7 @@ int px4_mavlink_debug_main(int argc, char *argv[]) } dbg_array.timestamp = timestamp_us; - orb_publish(ORB_ID(debug_array), pub_dbg_array, &dbg_array); + orb_publish(ORB_ID(debug_array), &pub_dbg_array, &dbg_array); warnx("sent one more value.."); diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 25e89efcf1f3..72e8abcf51e5 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[]) PX4_INFO("Hello Sky!"); /* subscribe to vehicle_acceleration topic */ - int sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_acceleration)); + orb_sub_t sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_acceleration)); /* limit the update rate to 5 Hz */ orb_set_interval(sensor_sub_fd, 200); @@ -115,7 +115,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.q[1] = accel.xyz[1]; att.q[2] = accel.xyz[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); + orb_publish(ORB_ID(vehicle_attitude), &att_pub, &att); } /* there could be more file descriptors here, in the form like: diff --git a/src/include/containers/BlockingQueue.hpp b/src/include/containers/BlockingQueue.hpp index 3b1ec77a8668..00b64d10534a 100644 --- a/src/include/containers/BlockingQueue.hpp +++ b/src/include/containers/BlockingQueue.hpp @@ -46,6 +46,8 @@ class BlockingQueue { px4_sem_init(&_sem_head, 0, N); px4_sem_init(&_sem_tail, 0, 0); + px4_sem_setprotocol(&_sem_head, SEM_PRIO_NONE); + px4_sem_setprotocol(&_sem_tail, SEM_PRIO_NONE); } ~BlockingQueue() @@ -56,7 +58,7 @@ class BlockingQueue void push(T newItem) { - px4_sem_wait(&_sem_head); + do {} while (px4_sem_wait(&_sem_head) != 0); _data[_tail] = newItem; _tail = (_tail + 1) % N; @@ -66,7 +68,7 @@ class BlockingQueue T pop() { - px4_sem_wait(&_sem_tail); + do {} while (px4_sem_wait(&_sem_tail) != 0); T ret = _data[_head]; _head = (_head + 1) % N; diff --git a/src/include/image_toc.h b/src/include/image_toc.h index 066e4021aa67..048eaeeb24da 100644 --- a/src/include/image_toc.h +++ b/src/include/image_toc.h @@ -48,6 +48,7 @@ #define TOC_FLAG1_DECRYPT 0x8 #define TOC_FLAG1_COPY 0x10 +#define TOC_FLAG1_ICFG 0x40 #define TOC_FLAG1_RDCT 0x80 #define TOC_START_MAGIC 0x00434f54 /* "TOC" */ @@ -60,6 +61,14 @@ #define TOC_VERSION BOARD_IMAGE_TOC_VERSION #endif +typedef enum { + IMAGE_TYPE_PX4 = 0, + IMAGE_TYPE_UBOOT = 1, + IMAGE_TYPE_RSVD1 = 2, + IMAGE_TYPE_RSVD2 = 3, + IMAGE_NUM_TYPES = 4 +} image_type_t; + /* Markers for TOC start and end in the image */ typedef const struct __attribute__((__packed__)) image_toc_start { @@ -78,7 +87,9 @@ typedef struct __attribute__((__packed__)) image_toc_entry { uint8_t signature_key; /* Key index for the signature */ uint8_t encryption_key; /* Key index for encryption */ uint8_t flags1; /* Flags */ - uint32_t reserved; /* e.g. for more flags */ + uint8_t flags2; /* More flags */ + uint8_t image_type; /* PX4 / Linux ... */ + uint16_t arb; /* Anti-rollback counter for this image */ } image_toc_entry_t; #define IMAGE_MAIN_TOC(len) \ @@ -94,8 +105,6 @@ typedef struct __attribute__((__packed__)) image_toc_entry { Certificate signature follows the data */ -#define RDCT_CAPS0_ALLOW_UNSIGNED_BOOT 0x1 - typedef struct __attribute__((__packed__)) { uint8_t device_uuid[16]; diff --git a/src/include/visibility.h b/src/include/visibility.h index bc38ffb8a1ac..858dde4d6c42 100644 --- a/src/include/visibility.h +++ b/src/include/visibility.h @@ -60,7 +60,7 @@ #endif -#define system_exit exit +#define system_exit _exit #define system_clock_gettime clock_gettime #define system_clock_settime clock_settime #define system_pthread_cond_timedwait pthread_cond_timedwait @@ -123,7 +123,20 @@ /* We should include cstdlib or stdlib.h but this doesn't * compile because many C++ files include stdlib.h and would * need to get changed. */ +#ifndef CONFIG_LIB_SYSCALL +/* The syscall interface puts these into the syscall LUT. When PX4 and NuttX + * are compiled as one, it is impossible to reliably limit visibility to these + * symbols only for the kernel. + * + * This produces false alarms via: + * /sys/syscall_lookup.h:344:18: error: attempt to use poisoned "getenv" + * 344 | SYSCALL_LOOKUP(getenv, 1) + * + * This alarm is false, as the symbol is not really _used_ from anywhere, the + * kernel simply populates it (and the empty handler) into the syscall table. + */ #pragma GCC poison getenv setenv putenv +#endif #endif // defined(__PX4_NUTTX) #endif // PX4_DISABLE_GCC_POISON diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b6a..479b0fa141f2 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -45,6 +45,7 @@ add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) add_subdirectory(crypto EXCLUDE_FROM_ALL) +add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index cb40f27a7df6..09b4ee94a413 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -383,7 +383,7 @@ void AdsbConflict::fake_traffic(const char *callsign, float distance, float dire #endif /* BOARD_HAS_NO_UUID */ - orb_publish(ORB_ID(transponder_report), fake_traffic_report_publisher, &tr); + orb_publish(ORB_ID(transponder_report), &fake_traffic_report_publisher, &tr); } diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index b5bcce2dd34b..0b90dc845c45 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -67,10 +67,6 @@ CDev::~CDev() unregister_driver(_devname); } - if (_pollset) { - delete[](_pollset); - } - px4_sem_destroy(&_lock); } @@ -191,190 +187,4 @@ CDev::close(file_t *filep) return ret; } -int -CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) -{ - PX4_DEBUG("CDev::Poll %s", setup ? "setup" : "teardown"); - int ret = PX4_OK; - - if (setup) { - /* - * Save the file pointer in the pollfd for the subclass' - * benefit. - */ - fds->priv = (void *)filep; - PX4_DEBUG("CDev::poll: fds->priv = %p", filep); - - /* - * Lock against poll_notify() and possibly other callers (protect _pollset). - */ - ATOMIC_ENTER; - - /* - * Try to store the fds for later use and handle array resizing. - */ - while ((ret = store_poll_waiter(fds)) == -ENFILE) { - - // No free slot found. Resize the pollset. This is expensive, but it's only needed initially. - - if (_max_pollwaiters >= 256 / 2) { //_max_pollwaiters is uint8_t - ret = -ENOMEM; - break; - } - - const uint8_t new_count = _max_pollwaiters > 0 ? _max_pollwaiters * 2 : 1; - px4_pollfd_struct_t **prev_pollset = _pollset; - -#ifdef __PX4_NUTTX - // malloc uses a semaphore, we need to call it enabled IRQ's - px4_leave_critical_section(flags); -#endif - px4_pollfd_struct_t **new_pollset = new px4_pollfd_struct_t *[new_count]; - -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - - if (prev_pollset == _pollset) { - // no one else updated the _pollset meanwhile, so we're good to go - if (!new_pollset) { - ret = -ENOMEM; - break; - } - - if (_max_pollwaiters > 0) { - memset(new_pollset + _max_pollwaiters, 0, sizeof(px4_pollfd_struct_t *) * (new_count - _max_pollwaiters)); - memcpy(new_pollset, _pollset, sizeof(px4_pollfd_struct_t *) * _max_pollwaiters); - } - - _pollset = new_pollset; - _pollset[_max_pollwaiters] = fds; - _max_pollwaiters = new_count; - - // free the previous _pollset (we need to unlock here which is fine because we don't access _pollset anymore) -#ifdef __PX4_NUTTX - px4_leave_critical_section(flags); -#endif - - if (prev_pollset) { - delete[](prev_pollset); - } - -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - - // Success - ret = PX4_OK; - break; - } - -#ifdef __PX4_NUTTX - px4_leave_critical_section(flags); -#endif - // We have to retry - delete[] new_pollset; -#ifdef __PX4_NUTTX - flags = px4_enter_critical_section(); -#endif - } - - if (ret == PX4_OK) { - - /* - * Check to see whether we should send a poll notification - * immediately. - */ - fds->revents |= fds->events & poll_state(filep); - - /* yes? post the notification */ - if (fds->revents != 0) { - px4_sem_post(fds->sem); - } - - } - - ATOMIC_LEAVE; - - } else { - ATOMIC_ENTER; - /* - * Handle a teardown request. - */ - ret = remove_poll_waiter(fds); - ATOMIC_LEAVE; - } - - return ret; -} - -void -CDev::poll_notify(px4_pollevent_t events) -{ - PX4_DEBUG("CDev::poll_notify events = %0x", events); - - /* lock against poll() as well as other wakeups */ - ATOMIC_ENTER; - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (nullptr != _pollset[i]) { - poll_notify_one(_pollset[i], events); - } - } - - ATOMIC_LEAVE; -} - -void -CDev::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) -{ - PX4_DEBUG("CDev::poll_notify_one"); - - /* update the reported event set */ - fds->revents |= fds->events & events; - - PX4_DEBUG(" Events fds=%p %0x %0x %0x", fds, fds->revents, fds->events, events); - - if (fds->revents != 0) { - px4_sem_post(fds->sem); - } -} - -int -CDev::store_poll_waiter(px4_pollfd_struct_t *fds) -{ - // Look for a free slot. - PX4_DEBUG("CDev::store_poll_waiter"); - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (nullptr == _pollset[i]) { - - /* save the pollfd */ - _pollset[i] = fds; - - return PX4_OK; - } - } - - return -ENFILE; -} - -int -CDev::remove_poll_waiter(px4_pollfd_struct_t *fds) -{ - PX4_DEBUG("CDev::remove_poll_waiter"); - - for (unsigned i = 0; i < _max_pollwaiters; i++) { - if (fds == _pollset[i]) { - - _pollset[i] = nullptr; - return PX4_OK; - - } - } - - PX4_DEBUG("poll: bad fd state"); - return -EINVAL; -} - } // namespace cdev diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index 071ae618fe97..41818f5fb630 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -148,19 +148,6 @@ class __EXPORT CDev */ virtual int ioctl(file_t *filep, int cmd, unsigned long arg) { return -ENOTTY; }; - /** - * Perform a poll setup/teardown operation. - * - * This is handled internally and should not normally be overridden. - * - * @param filep Pointer to the internal file structure. - * @param fds Poll descriptor being waited on. - * @param setup True if this is establishing a request, false if - * it is being torn down. - * @return OK on success, or -errno otherwise. - */ - int poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup); - /** * Get the device name. * @@ -175,38 +162,6 @@ class __EXPORT CDev */ static const px4_file_operations_t fops; - /** - * Check the current state of the device for poll events from the - * perspective of the file. - * - * This function is called by the default poll() implementation when - * a poll is set up to determine whether the poll should return immediately. - * - * The default implementation returns no events. - * - * @param filep The file that's interested. - * @return The current set of poll events. - */ - virtual px4_pollevent_t poll_state(file_t *filep) { return 0; } - - /** - * Report new poll events. - * - * This function should be called anytime the state of the device changes - * in a fashion that might be interesting to a poll waiter. - * - * @param events The new event(s) being announced. - */ - void poll_notify(px4_pollevent_t events); - - /** - * Internal implementation of poll_notify. - * - * @param fds A poll waiter to notify. - * @param events The event(s) to send to the waiter. - */ - virtual void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events); - /** * Notification of the first open. * @@ -273,29 +228,10 @@ class __EXPORT CDev private: const char *_devname{nullptr}; /**< device node name */ - px4_pollfd_struct_t **_pollset{nullptr}; - bool _registered{false}; /**< true if device name was registered */ - uint8_t _max_pollwaiters{0}; /**< size of the _pollset array */ uint16_t _open_count{0}; /**< number of successful opens */ - /** - * Store a pollwaiter in a slot where we can find it later. - * - * Expands the pollset as required. Must be called with the driver locked. - * - * @return OK, or -errno on error. - */ - inline int store_poll_waiter(px4_pollfd_struct_t *fds); - - /** - * Remove a poll waiter. - * - * @return OK, or -errno on error. - */ - inline int remove_poll_waiter(px4_pollfd_struct_t *fds); - }; } // namespace cdev diff --git a/src/lib/cdev/nuttx/cdev_platform.cpp b/src/lib/cdev/nuttx/cdev_platform.cpp index 39de49a55c05..3d0c01e5ae0d 100644 --- a/src/lib/cdev/nuttx/cdev_platform.cpp +++ b/src/lib/cdev/nuttx/cdev_platform.cpp @@ -60,7 +60,6 @@ static ssize_t cdev_read(file_t *filp, char *buffer, size_t buflen); static ssize_t cdev_write(file_t *filp, const char *buffer, size_t buflen); static off_t cdev_seek(file_t *filp, off_t offset, int whence); static int cdev_ioctl(file_t *filp, int cmd, unsigned long arg); -static int cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup); /** * Character device indirection table. @@ -78,7 +77,7 @@ read : cdev_read, write : cdev_write, seek : cdev_seek, ioctl : cdev_ioctl, -poll : cdev_poll, +poll : nullptr, #ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS unlink : nullptr #endif @@ -156,16 +155,4 @@ cdev_ioctl(file_t *filp, int cmd, unsigned long arg) return cdev->ioctl(filp, cmd, arg); } -static int -cdev_poll(file_t *filp, px4_pollfd_struct_t *fds, bool setup) -{ - if ((filp->f_inode->i_flags & FSNODEFLAG_DELETED) != 0) { - return -ENODEV; - } - - cdev::CDev *cdev = (cdev::CDev *)(filp->f_inode->i_private); - - return cdev->poll(filp, fds, setup); -} - } // namespace cdev diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp index 0a977a1bba16..4f17d473e7d7 100644 --- a/src/lib/cdev/posix/cdev_platform.cpp +++ b/src/lib/cdev/posix/cdev_platform.cpp @@ -76,8 +76,7 @@ class VFile : public cdev::CDev ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen) override { - // ignore what was written, but let pollers know something was written - poll_notify(POLLIN); + // ignore what was written return buflen; } }; @@ -329,119 +328,6 @@ extern "C" { return ret; } - int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout) - { - if (nfds == 0) { - PX4_WARN("px4_poll with no fds"); - return -1; - } - - px4_sem_t sem; - int count = 0; - int ret = -1; - - const unsigned NAMELEN = 32; - char thread_name[NAMELEN] {}; - -#ifndef __PX4_QURT - int nret = pthread_getname_np(pthread_self(), thread_name, NAMELEN); - - if (nret || thread_name[0] == 0) { - PX4_WARN("failed getting thread name"); - } - -#endif - - PX4_DEBUG("Called px4_poll timeout = %d", timeout); - - px4_sem_init(&sem, 0, 0); - - // sem use case is a signal - px4_sem_setprotocol(&sem, SEM_PRIO_NONE); - - // Go through all fds and check them for a pollable state - bool fd_pollable = false; - - for (unsigned int i = 0; i < nfds; ++i) { - fds[i].sem = &sem; - fds[i].revents = 0; - fds[i].priv = nullptr; - - cdev::CDev *dev = getFile(fds[i].fd); - - // If fd is valid - if (dev) { - PX4_DEBUG("%s: px4_poll: CDev->poll(setup) %d", thread_name, fds[i].fd); - ret = dev->poll(&filemap[fds[i].fd], &fds[i], true); - - if (ret < 0) { - PX4_WARN("%s: px4_poll() error: %s", thread_name, strerror(errno)); - break; - } - - if (ret >= 0) { - fd_pollable = true; - } - } - } - - // If any FD can be polled, lock the semaphore and - // check for new data - if (fd_pollable) { - if (timeout > 0) { - // Get the current time - struct timespec ts; - // Note, we can't actually use CLOCK_MONOTONIC on macOS - // but that's hidden and implemented in px4_clock_gettime. - px4_clock_gettime(CLOCK_MONOTONIC, &ts); - - // Calculate an absolute time in the future - const unsigned billion = (1000 * 1000 * 1000); - uint64_t nsecs = ts.tv_nsec + ((uint64_t)timeout * 1000 * 1000); - ts.tv_sec += nsecs / billion; - nsecs -= (nsecs / billion) * billion; - ts.tv_nsec = nsecs; - - ret = px4_sem_timedwait(&sem, &ts); - - if (ret && errno != ETIMEDOUT) { - PX4_WARN("%s: px4_poll() sem error: %s", thread_name, strerror(errno)); - } - - } else if (timeout < 0) { - px4_sem_wait(&sem); - } - - // We have waited now (or not, depending on timeout), - // go through all fds and count how many have data - for (unsigned int i = 0; i < nfds; ++i) { - - cdev::CDev *dev = getFile(fds[i].fd); - - // If fd is valid - if (dev) { - PX4_DEBUG("%s: px4_poll: CDev->poll(teardown) %d", thread_name, fds[i].fd); - ret = dev->poll(&filemap[fds[i].fd], &fds[i], false); - - if (ret < 0) { - PX4_WARN("%s: px4_poll() 2nd poll fail", thread_name); - break; - } - - if (fds[i].revents) { - count += 1; - } - } - } - } - - px4_sem_destroy(&sem); - - // Return the positive count if present, - // return the negative error number if failed - return (count) ? count : ret; - } - int px4_access(const char *pathname, int mode) { if (mode != F_OK) { diff --git a/src/lib/cdev/test/cdevtest_example.cpp b/src/lib/cdev/test/cdevtest_example.cpp index 48bed08d0b82..9944ff746b4e 100644 --- a/src/lib/cdev/test/cdevtest_example.cpp +++ b/src/lib/cdev/test/cdevtest_example.cpp @@ -162,9 +162,6 @@ ssize_t CDevNode::write(cdev::file_t *handlep, const char *buffer, size_t buflen _write_offset++; } - // ignore what was written, but let pollers know something was written - poll_notify(POLLIN); - return buflen; } @@ -191,59 +188,6 @@ CDevExample::~CDevExample() } } -int CDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll) -{ - int pollret, readret; - int loop_count = 0; - char readbuf[10]; - px4_pollfd_struct_t fds[1]; - - fds[0].fd = fd; - fds[0].events = POLLIN; - fds[0].revents = 0; - - bool mustblock = (timeout < 0); - - // Test indefinte blocking poll - while ((!appState.exitRequested()) && (loop_count < iterations)) { - pollret = px4_poll(fds, 1, timeout); - - if (pollret < 0) { - PX4_ERR("Reader: px4_poll failed %d FAIL", pollret); - goto fail; - } - - PX4_INFO("Reader: px4_poll returned %d", pollret); - - if (pollret) { - readret = px4_read(fd, readbuf, 10); - - if (readret != 1) { - if (mustblock) { - PX4_ERR("Reader: read failed %d FAIL", readret); - goto fail; - - } else { - PX4_INFO("Reader: read failed %d FAIL", readret); - } - - } else { - readbuf[readret] = '\0'; - PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf); - } - } - - if (delayms_after_poll) { - px4_usleep(delayms_after_poll * 1000); - } - - loop_count++; - } - - return 0; -fail: - return 1; -} int CDevExample::main() { appState.setRunning(true); @@ -277,52 +221,7 @@ int CDevExample::main() int ret = 0; - PX4_INFO("TEST: BLOCKING POLL ---------------"); - - if (do_poll(fd, -1, 3, 0)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - goto fail2; - } - - PX4_INFO("TEST: ZERO TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 3, 0)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: 100ms TIMEOUT POLL -----------"); - - if (do_poll(fd, 0, 30, 100)) { - ret = 1; - goto fail2; - } - - PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------"); - - if (do_poll(fd, 1000, 3, 0)) { - ret = 1; - goto fail2; - } - PX4_INFO("TEST: waiting for writer to stop"); -fail2: g_exit = true; px4_close(fd); PX4_INFO("TEST: waiting for writer to stop"); diff --git a/src/lib/collision_prevention/CollisionPreventionTest.cpp b/src/lib/collision_prevention/CollisionPreventionTest.cpp index 9de0a208aa00..ef8cc169438f 100644 --- a/src/lib/collision_prevention/CollisionPreventionTest.cpp +++ b/src/lib/collision_prevention/CollisionPreventionTest.cpp @@ -170,8 +170,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithObstacleMessage) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint1 = original_setpoint1; matrix::Vector2f modified_setpoint2 = original_setpoint2; cp.modifySetpoint(modified_setpoint1, max_speed, curr_pos, curr_vel); @@ -228,8 +228,8 @@ TEST_F(CollisionPreventionTest, testBehaviorOnWithDistanceMessage) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(distance_sensor), distance_sensor_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(distance_sensor), &distance_sensor_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); //WHEN: We run the setpoint modification matrix::Vector2f modified_setpoint1 = original_setpoint1; @@ -302,8 +302,8 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { @@ -312,7 +312,7 @@ TEST_F(CollisionPreventionTest, testPurgeOldData) mocked_time = mocked_time + 100000; //advance time by 0.1 seconds message_lost_data.timestamp = mocked_time; - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message_lost_data); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message_lost_data); //at iteration 8 change the CP_GO_NO_DATA to True if (i == 8) { @@ -380,8 +380,8 @@ TEST_F(CollisionPreventionTest, testNoRangeData) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); for (int i = 0; i < 10; i++) { @@ -437,7 +437,7 @@ TEST_F(CollisionPreventionTest, noBias) // WHEN: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -492,7 +492,7 @@ TEST_F(CollisionPreventionTest, outsideFOV) matrix::Vector2f original_setpoint = {10.f * cosf(angle_rad), 10.f * sinf(angle_rad)}; matrix::Vector2f modified_setpoint = original_setpoint; message.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); //THEN: if the resulting setpoint demands velocities bigger zero, it must lie inside the FOV @@ -567,7 +567,7 @@ TEST_F(CollisionPreventionTest, goNoData) //THEN: As soon as the range data contains any valid number, flying outside the FOV is allowed message.timestamp = hrt_absolute_time(); orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); @@ -606,7 +606,7 @@ TEST_F(CollisionPreventionTest, jerkLimit) // AND: we publish the message and set the parameter and then run the setpoint modification orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &message); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &message); matrix::Vector2f modified_setpoint_default_jerk = original_setpoint; cp.modifySetpoint(modified_setpoint_default_jerk, max_speed, curr_pos, curr_vel); orb_unadvertise(obstacle_distance_pub); @@ -1112,8 +1112,8 @@ TEST_F(CollisionPreventionTest, overlappingSensors) //WHEN: we publish the long range sensor message orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &long_range_msg); orb_advert_t vehicle_attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &attitude); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); - orb_publish(ORB_ID(vehicle_attitude), vehicle_attitude_pub, &attitude); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(vehicle_attitude), &vehicle_attitude_pub, &attitude); matrix::Vector2f modified_setpoint = original_setpoint; cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); @@ -1123,10 +1123,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // CASE 2 // WHEN: we publish the short range message followed by a long range message short_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &short_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); // THEN: the internal map data should contain the short range measurement @@ -1135,10 +1135,10 @@ TEST_F(CollisionPreventionTest, overlappingSensors) // CASE 3 // WHEN: we publish the short range message with values out of range followed by a long range message short_range_msg_no_obstacle.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &short_range_msg_no_obstacle); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &short_range_msg_no_obstacle); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); long_range_msg.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(obstacle_distance), obstacle_distance_pub, &long_range_msg); + orb_publish(ORB_ID(obstacle_distance), &obstacle_distance_pub, &long_range_msg); cp.modifySetpoint(modified_setpoint, max_speed, curr_pos, curr_vel); // THEN: the internal map data should contain the short range measurement diff --git a/src/lib/controllib/controllib_test/controllib_test_main.cpp b/src/lib/controllib/controllib_test/controllib_test_main.cpp index cbe816a76ee6..c5fa45d175d7 100644 --- a/src/lib/controllib/controllib_test/controllib_test_main.cpp +++ b/src/lib/controllib/controllib_test/controllib_test_main.cpp @@ -46,7 +46,7 @@ using namespace control; -#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; } +#define ASSERT_CL(T) if (!(T)) { PX4_INFO("FAIL"); return -1; } int basicBlocksTest(); int blockLimitTest(); @@ -102,7 +102,7 @@ int blockLimitTest() ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); ASSERT_CL(equal(1.0f, limit.update(2.0f))); ASSERT_CL(equal(0.0f, limit.update(0.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -117,7 +117,7 @@ int blockLimitSymTest() ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); ASSERT_CL(equal(1.0f, limit.update(2.0f))); ASSERT_CL(equal(0.0f, limit.update(0.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -145,7 +145,7 @@ int blockLowPassTest() ASSERT_CL(equal(2.0f, lowPass.getState())); ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; }; @@ -176,7 +176,7 @@ int blockHighPassTest() ASSERT_CL(equal(0.0f, highPass.getY())); ASSERT_CL(equal(0.0f, highPass.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -204,7 +204,7 @@ int blockLowPass2Test() ASSERT_CL(equal(2.0f, lowPass.getState())); ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; }; @@ -241,7 +241,7 @@ int blockIntegralTest() integral.setY(0.1f); ASSERT_CL(equal(0.2f, integral.update(1.0))); ASSERT_CL(equal(0.2f, integral.getY())); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -284,7 +284,7 @@ int blockIntegralTrapTest() ASSERT_CL(equal(0.25f, integral.update(1.0))); ASSERT_CL(equal(0.25f, integral.getY())); ASSERT_CL(equal(1.0f, integral.getU())); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -307,7 +307,7 @@ int blockDerivativeTest() // test update ASSERT_CL(equal(8.6269744f, derivative.update(2.0f))); ASSERT_CL(equal(2.0f, derivative.getU())); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -323,7 +323,7 @@ int blockPTest() ASSERT_CL(equal(0.1f, blockP.getDt())); // test update ASSERT_CL(equal(0.4f, blockP.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -345,7 +345,7 @@ int blockPITest() // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 ASSERT_CL(equal(0.43f, blockPI.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -370,7 +370,7 @@ int blockPDTest() // test update // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -400,7 +400,7 @@ int blockPIDTest() // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -422,7 +422,7 @@ int blockOutputTest() // test trim blockOutput.update(0.0f); ASSERT_CL(equal(0.5f, blockOutput.get())); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -448,7 +448,7 @@ int blockRandUniformTest() ASSERT_CL(equal(mean, (blockRandUniform.getMin() + blockRandUniform.getMax()) / 2, 1e-1)); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -478,7 +478,7 @@ int blockRandGaussTest() (void)(stdDev); ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1)); ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -495,7 +495,7 @@ int blockStatsTest() stats.reset(); ASSERT_CL(equal(0.0f, stats.getMean()(0))); ASSERT_CL(equal(0.0f, stats.getStdDev()(0))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } @@ -523,7 +523,7 @@ int blockDelayTest() Vector2f y4 = delay.update(u4); ASSERT_CL(equal(y4(0), u2(0))); ASSERT_CL(equal(y4(1), u2(1))); - printf("PASS\n"); + PX4_INFO("PASS"); return 0; } diff --git a/src/lib/dataman_client/CMakeLists.txt b/src/lib/dataman_client/CMakeLists.txt new file mode 100644 index 000000000000..8d60c9935d60 --- /dev/null +++ b/src/lib/dataman_client/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_library(dataman_client DatamanClient.cpp) diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp new file mode 100644 index 000000000000..aeaa65c55c1a --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -0,0 +1,644 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file DatamanClient.cpp + */ + +#include + +DatamanClient::DatamanClient() +{ + _dataman_request_pub.advertise(); + _dataman_response_sub = orb_subscribe(ORB_ID(dataman_response)); + + if (!orb_sub_valid(_dataman_response_sub)) { + PX4_ERR("Failed to subscribe (%i)", errno); + + } else { + // make sure we don't get any stale response by doing an orb_copy + dataman_response_s response{}; + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + _fds.fd = _dataman_response_sub; + _fds.events = POLLIN; + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.request_type = DM_GET_ID; + request.client_id = CLIENT_ID_NOT_SET; + + bool success = syncHandler(request, response, timestamp, 1000_ms); + + if (success && (response.client_id > CLIENT_ID_NOT_SET)) { + + _client_id = response.client_id; + + } else { + PX4_ERR("Failed to get client ID!"); + } + } +} + +DatamanClient::~DatamanClient() +{ + if (orb_sub_valid(_dataman_response_sub)) { + orb_unsubscribe(_dataman_response_sub); + } +} + +bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout) +{ + bool response_received = false; + int32_t ret = 0; + hrt_abstime time_elapsed = hrt_elapsed_time(&start_time); + _dataman_request_pub.publish(request); + + while (!response_received && (time_elapsed < timeout)) { + + uint32_t timeout_ms = 100; + ret = px4_poll(&_fds, 1, timeout_ms); + + if (ret < 0) { + PX4_ERR("px4_poll returned error: %" PRIu32, ret); + break; + + } else if (ret == 0) { + + // No response received, send new request + _dataman_request_pub.publish(request); + + } else { + + bool updated = false; + orb_check(_dataman_response_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + if (response.client_id == request.client_id) { + + if ((response.request_type == request.request_type) && + (response.item == request.item) && + (response.index == request.index)) { + response_received = true; + break; + } + + } else if (request.client_id == CLIENT_ID_NOT_SET) { + + // validate timestamp from response.data + if (0 == memcmp(&(request.timestamp), &(response.data), sizeof(hrt_abstime))) { + response_received = true; + break; + } + } + } + } + + time_elapsed = hrt_elapsed_time(&start_time); + } + + if (!response_received && ret >= 0) { + PX4_ERR("timeout after %" PRIu32 " ms!", static_cast(timeout / 1000)); + } + + return response_received; +} + +bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_READ; + request.item = static_cast(item); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("readSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + + } else { + memcpy(buffer, response.data, length); + } + } + + return success; +} + +bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_WRITE; + request.item = static_cast(item); + + memcpy(request.data, buffer, length); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("writeSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + } + } + + return success; +} + +bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout) +{ + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.client_id = _client_id; + request.request_type = DM_CLEAR; + request.item = static_cast(item); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("clearSync failed! status=%" PRIu8 ", item=%" PRIu8, + response.status, static_cast(item)); + } + } + + return success; +} + +bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_READ; + request.item = static_cast(item); + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_READ; + _active_request.item = item; + _active_request.index = index; + _active_request.buffer = buffer; + _active_request.length = length; + + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_WRITE; + request.item = static_cast(item); + + memcpy(request.data, buffer, length); + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_WRITE; + _active_request.item = item; + _active_request.index = index; + _active_request.buffer = buffer; + _active_request.length = length; + + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +bool DatamanClient::clearAsync(dm_item_t item) +{ + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.client_id = _client_id; + request.request_type = DM_CLEAR; + request.item = static_cast(item); + request.index = 0; + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_CLEAR; + _active_request.item = item; + _active_request.index = request.index; + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +void DatamanClient::update() +{ + if (_state == State::RequestSent) { + + bool updated = false; + orb_check(_dataman_response_sub, &updated); + + dataman_response_s response; + + if (updated) { + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + if ((response.client_id == _client_id) && + (response.request_type == _active_request.request_type) && + (response.item == _active_request.item) && + (response.index == _active_request.index)) { + + if (response.request_type == DM_READ) { + memcpy(_active_request.buffer, response.data, _active_request.length); + } + + _response_status = response.status; + + if (_response_status != dataman_response_s::STATUS_SUCCESS) { + + PX4_ERR("Async request type %" PRIu8 " failed! status=%" PRIu8 " item=%" PRIu8 " index=%" PRIu32, + response.request_type, response.status, static_cast(_active_request.item), _active_request.index); + } + + _state = State::ResponseReceived; + } + } + + if (_state == State::RequestSent) { + + /* Retry the request if there is no answer */ + if (((_active_request.request_type != DM_CLEAR) && (hrt_elapsed_time(&_active_request.timestamp) > 100_ms)) || + (hrt_elapsed_time(&_active_request.timestamp) > 1000_ms) + ) { + + hrt_abstime timestamp = hrt_absolute_time(); + + _active_request.timestamp = timestamp; + + dataman_request_s request; + request.timestamp = timestamp; + request.index = _active_request.index; + request.data_length = _active_request.length; + request.client_id = _client_id; + request.request_type = static_cast(_active_request.request_type); + request.item = static_cast(_active_request.item); + + if (_active_request.request_type == DM_WRITE) { + memcpy(request.data, _active_request.buffer, _active_request.length); + } + + _dataman_request_pub.publish(request); + + _state = State::RequestSent; + } + } + } +} + +bool DatamanClient::lastOperationCompleted(bool &success) +{ + bool completed = false; + success = false; + + if (_state == State::ResponseReceived) { + + if (_response_status == dataman_response_s::STATUS_SUCCESS) { + success = true; + } + + _state = State::Idle; + completed = true; + } + + return completed; +} + +void DatamanClient::abortCurrentOperation() +{ + _state = State::Idle; +} + +DatamanCache::DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items) + : _cache_miss_perf(perf_alloc(PC_COUNT, cache_miss_perf_counter_name)) +{ + _items = new Item[num_items] {}; + + if (_items != nullptr) { + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +DatamanCache::~DatamanCache() +{ + delete[] _items; + perf_free(_cache_miss_perf); +} + +void DatamanCache::resize(uint32_t num_items) +{ + Item *new_items = new Item[num_items] {}; + + if (new_items != nullptr) { + uint32_t num_min = num_items < _num_items ? num_items : _num_items; + + for (uint32_t i = 0; i < num_min; ++i) { + new_items[i] = _items[i]; + } + + delete[] _items; + _items = new_items; + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +bool DatamanCache::load(dm_item_t item, uint32_t index) +{ + if (!_items) { + return false; + } + + bool success = false; + bool duplicate = false; + + //Prevent duplicates + for (uint32_t i = 0; i < _num_items; ++i) { + + if (_items[i].cache_state != State::Idle && + _items[i].cache_state != State::Error && + _items[i].response.item == item && + _items[i].response.index == index) { + duplicate = true; + break; + } + } + + if (!duplicate && (_item_counter < _num_items)) { + + _items[_load_index].cache_state = State::RequestPrepared; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; + + success = true; + } + + return success; +} + +bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + if (!_items) { + return false; + } + + bool success = false; + bool item_found = false; + + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index)) { + item_found = true; + + if (_items[i].cache_state == State::ResponseReceived) { + memcpy(buffer, _items[i].response.data, length); + success = true; + break; + } + } + } + + if (!success && (timeout > 0)) { + perf_count(_cache_miss_perf); + success = _client.readSync(item, index, buffer, length, timeout); + + // Cache the item if not found already (it could be in the process of being loaded) + if (success && !item_found && _item_counter < _num_items) { + _items[_load_index].cache_state = State::ResponseReceived; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + memcpy(_items[_load_index].response.data, buffer, length); + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; // Still increase the counter here + } + } + + return success; +} + +void DatamanCache::update() +{ + if (_item_counter > 0) { + + _client.update(); + + bool success = false; + bool response_success = false; + + switch (_items[_update_index].cache_state) { + case State::Idle: + break; + + case State::ResponseReceived: + // Skip it + changeUpdateIndex(); + break; + + case State::RequestPrepared: + + success = _client.readAsync(static_cast(_items[_update_index].response.item), + _items[_update_index].response.index, + _items[_update_index].response.data, + g_per_item_size[_items[_update_index].response.item]); + + if (success) { + _items[_update_index].cache_state = State::RequestSent; + + } else { + _items[_update_index].cache_state = State::Error; + } + + break; + + case State::RequestSent: + + if (_client.lastOperationCompleted(response_success)) { + + if (response_success) { + + _items[_update_index].cache_state = State::ResponseReceived; + changeUpdateIndex(); + + } else { + _items[_update_index].cache_state = State::Error; + } + } + + break; + + case State::Error: + // Handled below + break; + } + + if (_items[_update_index].cache_state == State::Error) { + PX4_ERR("Caching: item %" PRIu8 ", index %" PRIu32", status %" PRIu8, + _items[_update_index].response.item, _items[_update_index].response.index, + _items[_update_index].response.status); + + changeUpdateIndex(); + } + + } +} + +void DatamanCache::invalidate() +{ + for (uint32_t i = 0; i < _num_items; ++i) { + _items[i].cache_state = State::Idle; + } + + _update_index = 0; + _item_counter = 0; + _load_index = 0; + _client.abortCurrentOperation(); +} + +inline void DatamanCache::changeUpdateIndex() +{ + _update_index = (_update_index + 1) % _num_items; + + if (_item_counter > 0) { + --_item_counter; + } +} diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp new file mode 100644 index 000000000000..4d6dbd12cbf4 --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class DatamanClient +{ +public: + DatamanClient(); + ~DatamanClient(); + + DatamanClient(const DatamanClient &) = delete; + DatamanClient &operator=(const DatamanClient &) = delete; + + /** + * @brief Reads data synchronously from the dataman for the specified item and index. + * + * @param[in] item The item to read data from. + * @param[in] index The index of the item to read data from. + * @param[out] buffer Pointer to the buffer to store the read data. + * @param[in] length The length of the data to read. + * @param[in] timeout The timeout in microseconds for waiting for the response. + * + * @return true if data was read successfully within the timeout, false otherwise. + */ + bool readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + + /** + * @brief Write data to the dataman synchronously. + * + * @param[in] item The data item type to write. + * @param[in] index The index of the data item. + * @param[in] buffer The buffer that contains the data to write. + * @param[in] length The length of the data to write. + * @param[in] timeout The maximum time in microseconds to wait for the response. + * + * @return True if the write operation succeeded, false otherwise. + */ + bool writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + + /** + * @brief Clears the data in the specified dataman item. + * + * @param[in] item The dataman item to clear. + * @param[in] timeout The timeout for the operation. + * + * @return True if the operation was successful, false otherwise. + */ + bool clearSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + + /** + * @brief Initiates an asynchronous request to read the data from dataman for a specific item and index. + * + * @param[in] item The item to read from. + * @param[in] index The index within the item to read from. + * @param[out] buffer The buffer to store the read data in. + * @param[in] length The length of the data to read. + * + * @return True if the read request was successfully queued, false otherwise. + * + * @note The buffer must be kept alive as long as the request did not finish. + * The completion status of the request can be obtained with the + * lastOperationCompleted() function. + */ + bool readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length); + + /** + * @brief Initiates an asynchronous request to write the data to dataman for a specific item and index. + * + * @param[in] item The item to write data to. + * @param[in] index The index of the item to write data to. + * @param[in] buffer The buffer containing the data to write. + * @param[in] length The length of the data to write. + * + * @return True if the write request was successfully sent, false otherwise. + * + * @note The buffer must be kept alive as long as the request did not finish. + * The completion status of the request can be obtained with the + * lastOperationCompleted() function. + */ + bool writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length); + + /** + * @brief Initiates an asynchronous request to clear an item in dataman. + * + * The request is only initiated if the DatamanClient is in the Idle state. + * + * @param[in] item The item to clear. + * @return True if the request was successfully initiated, false otherwise. + */ + bool clearAsync(dm_item_t item); + + /** + * @brief Updates the state of the dataman client for asynchronous functions. + * + * This function shall be called regularly. It checks if there is any response from the dataman, + * and updates the state accordingly. If there is no response for a request, it retries the + * request after a timeout. + * + * @see readAsync(), writeAsync(), clearAsync(), lastOperationCompleted() + */ + void update(); + + /** + * @brief Check if the last dataman operation has completed and whether it was successful. + * + * @param[out] success Output parameter indicating whether the last operation was successful. + * @return true if the last operation has completed, false otherwise. + */ + bool lastOperationCompleted(bool &success); + + /** + * Abort any async operation currently in progress + */ + void abortCurrentOperation(); + +private: + + enum class State { + Idle, + RequestSent, + ResponseReceived + }; + + struct Request { + hrt_abstime timestamp; + dm_function_t request_type; + dm_item_t item; + uint32_t index; + uint8_t *buffer; + uint32_t length; + }; + + /* Synchronous response/request handler */ + bool syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout); + + State _state{State::Idle}; + Request _active_request{}; + uint8_t _response_status{}; + + orb_sub_t _dataman_response_sub{}; + uORB::Publication _dataman_request_pub{ORB_ID(dataman_request)}; + + px4_pollfd_struct_t _fds; + + uint8_t _client_id{0}; + + static constexpr uint8_t CLIENT_ID_NOT_SET{0}; +}; + + +class DatamanCache +{ +public: + DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items); + ~DatamanCache(); + + /** + * @brief Resizes the cache to hold the specified number of items. + * + * @param[in] num_items The number of items the cache should hold. + */ + void resize(uint32_t num_items); + + /** + * @brief Invalidates all cached items in the cache by resetting their cache_state to Idle. + */ + void invalidate(); + + /** + * @brief Adds an index for items to be cached. + * + * Calling this function will exit immediately. Data shall be acquired with 'update()' function and + * it will be cached at full size. Later it can be retrieved with 'loadWait()' function. + * + * @param[in] item The item to load. + * @param[in] index The index of the item to load. + * + * @return true if the item was added to be cached, false otherwise if the size of the cache is reached. + */ + bool load(dm_item_t item, uint32_t index); + + /** + * @brief Loads for a specific item from the cache or acquires and wait for it if not found in the cache. + * + * @param[in] item Dataman item type + * @param[in] index Item index + * @param[out] buffer Buffer for the data to be stored + * @param[in] length Length of the buffer in bytes to be stored + * @param[in] timeout Maximum time to wait for the item to be available in microseconds, 0 to return immediately + * + * @return true if item was successfully loaded from cache or acquired through the client, false otherwise. + * + * @note This function will block if timeout is set differently than 0 and data doesn't exist in cache. + */ + bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0); + + /** + * @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them. + * + * If there are items in the cache, this function will call the DatamanClient's 'update()' function to check for responses. + * Depending on the state of each item, it will either send a request, wait for a response, or report an error. + * If a response is received for an item, it will be marked as "response received" and the update index will be changed + * to the next item in the cache. This function does not block and returns immediately. + * The data can be acquired with the 'loadWait()' function after it has been cached. + */ + void update(); + + /** + * @brief Function providing info if there items to be loaded to dataman cache. + * + * @return true if there are items to be processed. + */ + bool isLoading() { return (_item_counter > 0); } + + /** + * @brief Returns a reference to the DatamanClient instance used by the DatamanCache. + * + * @return A reference to the DatamanClient instance used by the DatamanCache. + */ + DatamanClient &client() { return _client; } + + int size() const { return _num_items; } + +private: + + enum class State { + Idle, + RequestPrepared, + RequestSent, + ResponseReceived, + Error + }; + + struct Item { + dataman_response_s response; + State cache_state; + }; + + inline void changeUpdateIndex(); + + Item *_items{nullptr}; + uint32_t _load_index{0}; ///< index for tracking last index used by load function + uint32_t _update_index{0}; ///< index for tracking last index used by update function + uint32_t _item_counter{0}; ///< number of items to process with update function + uint32_t _num_items{0}; ///< number of items that cache can store + + DatamanClient _client{}; + + perf_counter_t _cache_miss_perf; +}; diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp index 176928923af0..ad817e983ad9 100644 --- a/src/lib/drivers/smbus_sbs/SBS.hpp +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -321,6 +321,6 @@ void SMBUS_SBS_BaseClass::RunImpl() // Only publish if no errors. if (!ret) { - orb_publish(ORB_ID(battery_status), _batt_topic, &new_report); + orb_publish(ORB_ID(battery_status), &_batt_topic, &new_report); } } diff --git a/src/lib/events/CMakeLists.txt b/src/lib/events/CMakeLists.txt index b3540ffb3d78..eea3e9b2ecec 100644 --- a/src/lib/events/CMakeLists.txt +++ b/src/lib/events/CMakeLists.txt @@ -110,3 +110,13 @@ add_custom_command(OUTPUT ${generated_events_header} ) add_custom_target(events_header DEPENDS ${generated_events_header}) add_dependencies(prebuild_targets events_header) + +# Build the interface(s) +if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) + list(APPEND EXTRA_SRCS events_ioctl.cpp) + add_library(usr_events usr_events.cpp) + add_dependencies(usr_events prebuild_targets) +endif() + +add_library(events events.cpp ${EXTRA_SRCS}) +add_dependencies(events prebuild_targets) diff --git a/platforms/common/events.cpp b/src/lib/events/events.cpp similarity index 94% rename from platforms/common/events.cpp rename to src/lib/events/events.cpp index c4d6867020c0..969cd2c51e58 100644 --- a/platforms/common/events.cpp +++ b/src/lib/events/events.cpp @@ -38,7 +38,7 @@ #include #include -static orb_advert_t orb_event_pub = nullptr; +static orb_advert_t orb_event_pub = ORB_ADVERT_INVALID; static pthread_mutex_t publish_event_mutex = PTHREAD_MUTEX_INITIALIZER; static uint16_t event_sequence{events::initial_event_sequence}; @@ -57,8 +57,8 @@ void send(EventType &event) pthread_mutex_lock(&publish_event_mutex); event.event_sequence = ++event_sequence; // Set the sequence here so we're able to detect uORB queue overflows - if (orb_event_pub != nullptr) { - orb_publish(ORB_ID(event), orb_event_pub, &event); + if (orb_advert_valid(orb_event_pub)) { + orb_publish(ORB_ID(event), &orb_event_pub, &event); } else { orb_event_pub = orb_advertise_queue(ORB_ID(event), &event, event_s::ORB_QUEUE_LENGTH); diff --git a/src/lib/events/events.h b/src/lib/events/events.h new file mode 100644 index 000000000000..929dbc498279 --- /dev/null +++ b/src/lib/events/events.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +void events_ioctl_init(void); diff --git a/src/lib/events/events_ioctl.cpp b/src/lib/events/events_ioctl.cpp new file mode 100644 index 000000000000..a3c8260ddd8b --- /dev/null +++ b/src/lib/events/events_ioctl.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file events_ioctl.cpp + * + * Interface to send events from user space. + */ + +#include + +#include + +#include "events_ioctl.h" + +static int events_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case EVENTSIOCSEND: { + eventiocsend_t *data = (eventiocsend_t *)arg; + events::send(data->event); + } + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +void events_ioctl_init(void) +{ + px4_register_boardct_ioctl(_EVENTSIOCBASE, events_ioctl); +} diff --git a/src/lib/events/events_ioctl.h b/src/lib/events/events_ioctl.h new file mode 100644 index 000000000000..7b97c66aa41a --- /dev/null +++ b/src/lib/events/events_ioctl.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file events_ioctl.h + * + * User space - kernel space interface for dispatching events. + */ + +#pragma once + +#include +#include +#include + +#define _EVENTSIOC(_n) (_PX4_IOC(_EVENTSIOCBASE, _n)) + +#define EVENTSIOCSEND _EVENTSIOC(1) +typedef struct eventiocsend { + events::EventType &event; +} eventiocsend_t; diff --git a/src/lib/events/usr_events.cpp b/src/lib/events/usr_events.cpp new file mode 100644 index 000000000000..a2f076d06555 --- /dev/null +++ b/src/lib/events/usr_events.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file usr_events.cpp + * + * User space interface for dispatching events. + */ + +#include + +#include "events_ioctl.h" + +namespace events +{ + +void send(EventType &event) +{ + eventiocsend_t data = {event}; + boardctl(EVENTSIOCSEND, reinterpret_cast(&data)); +} + +} /* namespace events */ diff --git a/src/lib/geo/CMakeLists.txt b/src/lib/geo/CMakeLists.txt index 04c8eae037d7..9fbf5165caf4 100644 --- a/src/lib/geo/CMakeLists.txt +++ b/src/lib/geo/CMakeLists.txt @@ -31,11 +31,11 @@ # ############################################################################ -add_library(geo +px4_add_library(geo geo.cpp geo.h ) -add_dependencies(geo prebuild_targets) + target_compile_options(geo PRIVATE ${MAX_CUSTOM_OPT_LEVEL}) px4_add_unit_gtest(SRC test_geo.cpp LINKLIBS geo) diff --git a/src/lib/mathlib/math/filter/second_order_reference_model.hpp b/src/lib/mathlib/math/filter/second_order_reference_model.hpp index 08fd1202f730..3842c0f04fca 100644 --- a/src/lib/mathlib/math/filter/second_order_reference_model.hpp +++ b/src/lib/mathlib/math/filter/second_order_reference_model.hpp @@ -97,7 +97,7 @@ class SecondOrderReferenceModel // Zero natural frequency means our time step is irrelevant cutoff_freq_ = FLT_EPSILON; - max_time_step_ = INFINITY; + max_time_step_ = (float)INFINITY; // Fail return false; @@ -198,7 +198,7 @@ class SecondOrderReferenceModel T last_rate_sample_{}; // [units/s] // Maximum time step [s] - float max_time_step_{INFINITY}; + float max_time_step_{(float)INFINITY}; // The selected time discretization method used for state integration DiscretizationMethod discretization_method_{kBilinear}; diff --git a/src/lib/parameters/ParameterTest.cpp b/src/lib/parameters/ParameterTest.cpp index b0e04923e915..c3a16f02c386 100644 --- a/src/lib/parameters/ParameterTest.cpp +++ b/src/lib/parameters/ParameterTest.cpp @@ -92,7 +92,7 @@ TEST_F(ParameterTest, testUorbSendReceive) // WHEN we send the message orb_advert_t obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance), &message); - ASSERT_TRUE(obstacle_distance_pub != nullptr); + ASSERT_TRUE(orb_advert_valid(obstacle_distance_pub)); // THEN: the subscriber should receive the message sub_obstacle_distance.update(); diff --git a/src/lib/parameters/flashparams/flashfs.c b/src/lib/parameters/flashparams/flashfs.c index a692b62048dc..bbbab3a0abb8 100644 --- a/src/lib/parameters/flashparams/flashfs.c +++ b/src/lib/parameters/flashparams/flashfs.c @@ -44,7 +44,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include #include diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 84a5da768287..061b925fc751 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -47,7 +47,11 @@ #include #include +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include @@ -77,6 +81,10 @@ using namespace time_literals; #include "parameters_ioctl.h" #endif +#if defined(__PX4_NUTTX) +#include +#endif + #if defined(FLASH_BASED_PARAMS) #include "flashparams/flashparams.h" #else @@ -113,7 +121,7 @@ UT_array *param_custom_default_values{nullptr}; const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr}; /** parameter update topic handle */ -static orb_advert_t param_topic = nullptr; +static orb_advert_t param_topic = ORB_ADVERT_INVALID; static unsigned int param_instance = 0; // the following implements an RW-lock using 2 semaphores (used as mutexes). It gives @@ -260,11 +268,11 @@ param_notify_changes() pup.custom_default = params_custom_default.count(); pup.timestamp = hrt_absolute_time(); - if (param_topic == nullptr) { + if (!orb_advert_valid(param_topic)) { param_topic = orb_advertise(ORB_ID(parameter_update), &pup); } else { - orb_publish(ORB_ID(parameter_update), param_topic, &pup); + orb_publish(ORB_ID(parameter_update), ¶m_topic, &pup); } } diff --git a/src/lib/perf/perf_counter.cpp b/src/lib/perf/perf_counter.cpp index 8cd3ddddeb90..91b6d1616ad8 100644 --- a/src/lib/perf/perf_counter.cpp +++ b/src/lib/perf/perf_counter.cpp @@ -37,24 +37,44 @@ * @brief Performance measuring tools. */ +#ifndef MODULE_NAME +#define MODULE_NAME "perf" +#endif + #include #include #include #include #include #include -#include #include +#include +#include +#include +#include + +#include #include "perf_counter.h" +#define PERF_SHMNAME_PREFIX "_perf_" +#define PERF_SHMNAME_STR PERF_SHMNAME_PREFIX "%s" +#define PERF_SHMNAME_MAX NAME_MAX + 1 + +#ifndef CONFIG_FS_SHMFS_VFS_PATH +#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm" +#endif + /** * Header common to all counters. */ struct perf_ctr_header { - sq_entry_t link; /**< list linkage */ enum perf_counter_type type; /**< counter type */ +#ifdef CONFIG_BUILD_FLAT const char *name; /**< counter name */ +#else + char name[PERF_SHMNAME_MAX]; /**< counter name */ +#endif }; /** @@ -91,51 +111,124 @@ struct perf_ctr_interval : public perf_ctr_header { float M2{0.0f}; }; -/** - * List of all known counters. - */ -static sq_queue_t perf_counters = { nullptr, nullptr }; - -/** - * mutex protecting access to the perf_counters linked list (which is read from & written to by different threads) - */ -pthread_mutex_t perf_counters_mutex = PTHREAD_MUTEX_INITIALIZER; -// FIXME: the mutex does **not** protect against access to/from the perf -// counter's data. It can still happen that a counter is updated while it is -// printed. This can lead to inconsistent output, or completely bogus values -// (especially the 64bit values which are in general not atomically updated). -// The same holds for shared perf counters (perf_alloc_once), that can be updated -// concurrently (this affects the 'ctrl_latency' counter). - - -perf_counter_t -perf_alloc(enum perf_counter_type type, const char *name) +static size_t perf_size(enum perf_counter_type type) { - perf_counter_t ctr = nullptr; - switch (type) { case PC_COUNT: - ctr = new perf_ctr_count(); - break; + return sizeof(perf_ctr_count); case PC_ELAPSED: - ctr = new perf_ctr_elapsed(); - break; + return sizeof(perf_ctr_elapsed); case PC_INTERVAL: - ctr = new perf_ctr_interval(); - break; + return sizeof(perf_ctr_interval); default: break; } - if (ctr != nullptr) { + return 0; +} + +static size_t perf_shmsize(int fd) +{ + struct stat sb; + + if (fstat(fd, &sb) < 0) { + PX4_ERR("Cannot stat %d", fd); + } + + return sb.st_size; +} + +static int perf_shmname(char *shmname, const char *name, size_t size) +{ + int ret = snprintf(shmname, size, PERF_SHMNAME_STR, name); + + if ((size_t)ret >= size) { + return -ENAMETOOLONG; + } + + return OK; +} + +static void perf_foreach(perf_callback cb, void *arg = NULL) +{ + DIR *dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *perf; + + while ((perf = readdir(dir)) != nullptr) { + if (!strncmp(perf->d_name, PERF_SHMNAME_PREFIX, sizeof(PERF_SHMNAME_PREFIX) - 1)) { + /* found the counter, map it for us */ + int fd = shm_open(perf->d_name, O_RDWR, 0666); + + if (fd >= 0) { + size_t size = perf_shmsize(fd); + void *p = px4_mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + /* the file can be closed now */ + close(fd); + + if (p != MAP_FAILED) { + cb((perf_counter_t)p, arg); + px4_munmap(p, size); + } + } + } + } +} + +static void perf_print_cb(perf_counter_t handle, void *arg) +{ + perf_print_counter(handle); +} + +static void perf_reset_cb(perf_counter_t handle, void *arg) +{ + perf_reset(handle); +} + +perf_counter_t +perf_alloc(enum perf_counter_type type, const char *name) +{ + perf_counter_t ctr; + char shmname[PERF_SHMNAME_MAX]; + int ret; + int fd; + + /* generate name for shmfs file */ + ret = perf_shmname(shmname, name, sizeof(shmname)); + + if (ret < 0) { + PX4_ERR("failed to allocate perf counter %s", name); + return nullptr; + } + + /* try to allocate new shm object and map it */ + ctr = nullptr; + fd = shm_open(shmname, O_CREAT | O_RDWR | O_EXCL, 0666); + + if (fd >= 0) { + ret = ftruncate(fd, perf_size(type)); + + if (ret == 0) { + void *p = px4_mmap(0, perf_size(type), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + + if (p != MAP_FAILED) { + ctr = (perf_counter_t)p; + } + } + } + + close(fd); + + if (ctr) { ctr->type = type; +#ifdef CONFIG_BUILD_FLAT ctr->name = name; - pthread_mutex_lock(&perf_counters_mutex); - sq_addfirst(&ctr->link, &perf_counters); - pthread_mutex_unlock(&perf_counters_mutex); +#else + strncpy(ctr->name, name, PERF_SHMNAME_MAX); +#endif } return ctr; @@ -144,27 +237,38 @@ perf_alloc(enum perf_counter_type type, const char *name) perf_counter_t perf_alloc_once(enum perf_counter_type type, const char *name) { - pthread_mutex_lock(&perf_counters_mutex); - perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); - - while (handle != nullptr) { - if (!strcmp(handle->name, name)) { - if (type == handle->type) { - /* they are the same counter */ - pthread_mutex_unlock(&perf_counters_mutex); - return handle; - - } else { - /* same name but different type, assuming this is an error and not intended */ - pthread_mutex_unlock(&perf_counters_mutex); - return nullptr; - } - } + DIR *dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *perf; + char shmname[PERF_SHMNAME_MAX]; + int ret; + + /* generate name for shmfs file */ + ret = perf_shmname(shmname, name, sizeof(shmname)); - handle = (perf_counter_t)sq_next(&handle->link); + if (ret < 0) { + PX4_ERR("failed to allocate perf counter %s", name); + return nullptr; } - pthread_mutex_unlock(&perf_counters_mutex); + while ((perf = readdir(dir)) != nullptr) { + if (!strncmp(perf->d_name, shmname, sizeof(shmname))) { + /* found the counter, map it for us */ + int fd = shm_open(perf->d_name, O_RDWR, 0666); + + if (fd >= 0) { + void *p = px4_mmap(0, perf_size(type), PROT_READ, MAP_SHARED, fd, 0); + + /* the file can be closed now */ + close(fd); + + if (p == MAP_FAILED) { + return nullptr; + } + + return (perf_counter_t)p; + } + } + } /* if the execution reaches here, no existing counter of that name was found */ return perf_alloc(type, name); @@ -173,29 +277,15 @@ perf_alloc_once(enum perf_counter_type type, const char *name) void perf_free(perf_counter_t handle) { - if (handle == nullptr) { - return; - } - - pthread_mutex_lock(&perf_counters_mutex); - sq_rem(&handle->link, &perf_counters); - pthread_mutex_unlock(&perf_counters_mutex); - - switch (handle->type) { - case PC_COUNT: - delete (struct perf_ctr_count *)handle; - break; + if (handle) { + char shmname[PERF_SHMNAME_MAX]; - case PC_ELAPSED: - delete (struct perf_ctr_elapsed *)handle; - break; - - case PC_INTERVAL: - delete (struct perf_ctr_interval *)handle; - break; + /* should not fail, if object creation succeeded ? */ + if (perf_shmname(shmname, handle->name, sizeof(shmname) >= 0)) { + shm_unlink(shmname); + } - default: - break; + px4_munmap(handle, perf_size(handle->type)); } } @@ -586,29 +676,13 @@ perf_mean(perf_counter_t handle) void perf_iterate_all(perf_callback cb, void *user) { - pthread_mutex_lock(&perf_counters_mutex); - perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); - - while (handle != nullptr) { - cb(handle, user); - handle = (perf_counter_t)sq_next(&handle->link); - } - - pthread_mutex_unlock(&perf_counters_mutex); + perf_foreach(cb, user); } void perf_print_all(void) { - pthread_mutex_lock(&perf_counters_mutex); - perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); - - while (handle != nullptr) { - perf_print_counter(handle); - handle = (perf_counter_t)sq_next(&handle->link); - } - - pthread_mutex_unlock(&perf_counters_mutex); + perf_foreach(perf_print_cb); } void @@ -630,15 +704,6 @@ perf_print_latency(void) void perf_reset_all(void) { - pthread_mutex_lock(&perf_counters_mutex); - perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters); - - while (handle != nullptr) { - perf_reset(handle); - handle = (perf_counter_t)sq_next(&handle->link); - } - - pthread_mutex_unlock(&perf_counters_mutex); - + perf_foreach(perf_reset_cb); reset_latency_counters(); } diff --git a/src/lib/systemlib/hardfault_log.h b/src/lib/systemlib/hardfault_log.h index 574a2628cafe..7540776e49f9 100644 --- a/src/lib/systemlib/hardfault_log.h +++ b/src/lib/systemlib/hardfault_log.h @@ -208,7 +208,7 @@ typedef struct { _stack_s interrupt; #endif -} stack_t; +} fault_stack_t; /* Not Used for reference only */ @@ -321,7 +321,7 @@ typedef struct { int pid; /* Process ID */ uint32_t regs[XCPTCONTEXT_REGS]; /* Interrupt register save area */ fault_regs_s fault_regs; /* NVIC status */ - stack_t stacks; /* Stack info */ + fault_stack_t stacks; /* Stack info */ #if CONFIG_TASK_NAME_SIZE > 0 char name[CONFIG_TASK_NAME_SIZE + 1]; /* Task name (with NULL * terminator) */ diff --git a/src/lib/systemlib/mavlink_log.cpp b/src/lib/systemlib/mavlink_log.cpp index d31136f21530..9526e9934434 100644 --- a/src/lib/systemlib/mavlink_log.cpp +++ b/src/lib/systemlib/mavlink_log.cpp @@ -69,8 +69,8 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con vsnprintf((char *)log_msg.text, sizeof(log_msg.text), fmt, ap); va_end(ap); - if (*mavlink_log_pub != nullptr) { - orb_publish(ORB_ID(mavlink_log), *mavlink_log_pub, &log_msg); + if (orb_advert_valid(*mavlink_log_pub)) { + orb_publish(ORB_ID(mavlink_log), mavlink_log_pub, &log_msg); } else { *mavlink_log_pub = orb_advertise_queue(ORB_ID(mavlink_log), &log_msg, mavlink_log_s::ORB_QUEUE_LENGTH); diff --git a/src/modules/cdcacm/CMakeLists.txt b/src/modules/cdcacm/CMakeLists.txt new file mode 100644 index 000000000000..da479aba99ed --- /dev/null +++ b/src/modules/cdcacm/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2023 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ + +px4_add_module( + MODULE modules__cdcacm + MAIN cdcacm + SRCS + cdcacm.cpp + ) diff --git a/src/modules/cdcacm/Kconfig b/src/modules/cdcacm/Kconfig new file mode 100644 index 000000000000..b6c56b297ee9 --- /dev/null +++ b/src/modules/cdcacm/Kconfig @@ -0,0 +1,12 @@ +menuconfig MODULES_CDCACM + bool "CDC ACM monitor process" + default n + ---help--- + Enable support for CDC ACM monitor + +menuconfig USER_CDCACM + bool "CDC ACM monitor running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_CDCACM + ---help--- + Put CDC ACM monitor in userspace memory diff --git a/src/modules/cdcacm/cdcacm.cpp b/src/modules/cdcacm/cdcacm.cpp new file mode 100644 index 000000000000..d68e04a52c63 --- /dev/null +++ b/src/modules/cdcacm/cdcacm.cpp @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file cdcacm.cpp + * Main thread of CDC ACM connection monitor + */ +#include "cdcacm.hpp" + +extern void cdcacm_init(void); + +CdcAcm::CdcAcm() +{ + px4_sem_init(&_exit_wait, 0, 0); +} + +int CdcAcm::task_spawn(int argc, char *argv[]) +{ + _task_id = px4_task_spawn_cmd("cdcacm", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2048, + (px4_main_t)&run_trampoline, + (char *const *)argv); + + if (_task_id < 0) { + _task_id = -1; + return -errno; + } + + // wait until task is up & running + if (wait_until_running() < 0) { + _task_id = -1; + return -1; + } + + return 0; +} + +CdcAcm *CdcAcm::instantiate(int argc, char *argv[]) +{ + CdcAcm *instance = new CdcAcm(); + return instance; +} + +int CdcAcm::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +void CdcAcm::request_stop() +{ + ModuleBase::request_stop(); + px4_sem_post(&_exit_wait); +} + +void CdcAcm::run() +{ + cdcacm_init(); + + while (!should_exit()) { + px4_sem_wait(&_exit_wait); + } +} + +int CdcAcm::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +CDC ACM monitor process, used as host for cdc_acm_check(). + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("cdcacm", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int cdcacm_main(int argc, char *argv[]) +{ + return CdcAcm::main(argc, argv); +} diff --git a/src/modules/cdcacm/cdcacm.hpp b/src/modules/cdcacm/cdcacm.hpp new file mode 100644 index 000000000000..0f3e207e4615 --- /dev/null +++ b/src/modules/cdcacm/cdcacm.hpp @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +class CdcAcm : public ModuleBase +{ +public: + CdcAcm(); + ~CdcAcm() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static CdcAcm *instantiate(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + void request_stop() override; + + /** @see ModuleBase::run() */ + void run() override; +private: + px4_sem_t _exit_wait; +}; diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp index bfcff4a3aead..a9975f225445 100644 --- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp @@ -46,7 +46,7 @@ using namespace time_literals; static orb_advert_t *mavlink_log_pub; -static int command_ack_sub = -1; +static orb_sub_t command_ack_sub = ORB_SUB_INVALID; static hrt_abstime auth_timeout; static hrt_abstime auth_req_time; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a9061ad33ba1..8ca650156535 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -98,7 +98,7 @@ static constexpr bool operator ==(const actuator_armed_s &a, const actuator_arme static_assert(sizeof(actuator_armed_s) == 16, "actuator_armed equality operator review"); #if defined(BOARD_HAS_POWER_CONTROL) -static orb_advert_t tune_control_pub = nullptr; +static orb_advert_t tune_control_pub = ORB_ADVERT_INVALID; static void play_power_button_down_tune() { @@ -111,10 +111,10 @@ static void stop_tune() tune_control_s tune_control{}; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); } -static orb_advert_t power_button_state_pub = nullptr; +static orb_advert_t power_button_state_pub = ORB_ADVERT_INVALID; static int power_button_state_notification_cb(board_power_button_state_notification_e request) { // Note: this can be called from IRQ handlers, so we publish a message that will be handled @@ -147,8 +147,8 @@ static int power_button_state_notification_cb(board_power_button_state_notificat return ret; } - if (power_button_state_pub != nullptr) { - orb_publish(ORB_ID(power_button_state), power_button_state_pub, &button_state); + if (!orb_advert_valid(power_button_state_pub)) { + orb_publish(ORB_ID(power_button_state), &power_button_state_pub, &button_state); } else { PX4_ERR("power_button_state_pub not properly initialized"); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3678b9ea9d77..a29ac4b65411 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -33,6 +33,8 @@ #pragma once +#include + /* Helper classes */ #include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "failure_detector/FailureDetector.hpp" diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index f642541fb74b..6460fabcdae4 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -127,7 +127,7 @@ static hrt_abstime blink_msg_end = 0; static int fd_leds{-1}; static led_control_s led_control {}; -static orb_advert_t led_control_pub = nullptr; +static orb_advert_t led_control_pub = ORB_ADVERT_INVALID; // Static array that defines the duration of each tune, 0 if it's a repeating tune (therefore no fixed duration) static unsigned int tune_durations[tune_control_s::NUMBER_OF_TUNES] {}; @@ -139,7 +139,7 @@ static hrt_abstime tune_end = 0; static uint8_t tune_current = tune_control_s::TUNE_ID_STOP; static tune_control_s tune_control {}; -static orb_advert_t tune_control_pub = nullptr; +static orb_advert_t tune_control_pub = ORB_ADVERT_INVALID; int buzzer_init() @@ -169,7 +169,7 @@ void set_tune_override(const int tune_id) tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; tune_control.tune_override = true; tune_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); } void set_tune(const int tune_id) @@ -204,7 +204,7 @@ void set_tune(const int tune_id) tune_control.volume = tune_control_s::VOLUME_LEVEL_DEFAULT; tune_control.tune_override = false; tune_control.timestamp = current_time; - orb_publish(ORB_ID(tune_control), tune_control_pub, &tune_control); + orb_publish(ORB_ID(tune_control), &tune_control_pub, &tune_control); tune_current = tune_id; @@ -390,7 +390,7 @@ void rgbled_set_color_and_mode(uint8_t color, uint8_t mode, uint8_t blinks, uint led_control.num_blinks = blinks; led_control.priority = prio; led_control.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(led_control), led_control_pub, &led_control); + orb_publish(ORB_ID(led_control), &led_control_pub, &led_control); } void rgbled_set_color_and_mode(uint8_t color, uint8_t mode) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index fcb35ca78242..b2895ddbb079 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -417,11 +417,11 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } } - if (worker_data->mag_worker_data_pub == nullptr) { + if (!orb_advert_valid(worker_data->mag_worker_data_pub)) { worker_data->mag_worker_data_pub = orb_advertise(ORB_ID(mag_worker_data), &status); } else { - orb_publish(ORB_ID(mag_worker_data), worker_data->mag_worker_data_pub, &status); + orb_publish(ORB_ID(mag_worker_data), &worker_data->mag_worker_data_pub, &status); } calibration_counter_side++; @@ -484,7 +484,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma mag_worker_data_t worker_data{}; - worker_data.mag_worker_data_pub = nullptr; + worker_data.mag_worker_data_pub = ORB_ADVERT_INVALID; // keep and update the existing calibration when we are not doing a full 6-axis calibration worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 9faae22ca434..37a0cf1624fa 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -141,7 +141,7 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::VectorgetActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); - actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + actuator_motors.control[motors_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : (float)NAN; if (stopped_motors & (1u << motors_idx)) { - actuator_motors.control[motors_idx] = NAN; + actuator_motors.control[motors_idx] = (float)NAN; } ++actuator_idx_matrix[selected_matrix]; @@ -669,7 +669,7 @@ ControlAllocator::publish_actuator_controls() } for (int i = motors_idx; i < actuator_motors_s::NUM_CONTROLS; i++) { - actuator_motors.control[i] = NAN; + actuator_motors.control[i] = (float)NAN; } _actuator_motors_pub.publish(actuator_motors); @@ -681,13 +681,13 @@ ControlAllocator::publish_actuator_controls() for (servos_idx = 0; servos_idx < _num_actuators[1] && servos_idx < actuator_servos_s::NUM_CONTROLS; servos_idx++) { int selected_matrix = _control_allocation_selection_indexes[actuator_idx]; float actuator_sp = _control_allocation[selected_matrix]->getActuatorSetpoint()(actuator_idx_matrix[selected_matrix]); - actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : NAN; + actuator_servos.control[servos_idx] = PX4_ISFINITE(actuator_sp) ? actuator_sp : (float)NAN; ++actuator_idx_matrix[selected_matrix]; ++actuator_idx; } for (int i = servos_idx; i < actuator_servos_s::NUM_CONTROLS; i++) { - actuator_servos.control[i] = NAN; + actuator_servos.control[i] = (float)NAN; } _actuator_servos_pub.publish(actuator_servos); diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index f74cec5cb60a..1b1a0b600ec0 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -52,13 +52,18 @@ #include #include +#include +#include +#include +#include + #include "dataman.h" __BEGIN_DECLS __EXPORT int dataman_main(int argc, char *argv[]); __END_DECLS -static constexpr int TASK_STACK_SIZE = 1320; +static constexpr int TASK_STACK_SIZE = 1420; /* Private File based Operations */ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count); @@ -117,74 +122,25 @@ static struct { bool silence = false; } dm_operations_data; -/** Types of function calls supported by the worker task */ -typedef enum { - dm_write_func = 0, - dm_read_func, - dm_clear_func, - dm_number_of_funcs -} dm_function_t; - -/** Work task work item */ -typedef struct { - sq_entry_t link; /**< list linkage */ - px4_sem_t wait_sem; - unsigned char first; - unsigned char func; - ssize_t result; - union { - struct { - dm_item_t item; - unsigned index; - const void *buf; - size_t count; - } write_params; - struct { - dm_item_t item; - unsigned index; - void *buf; - size_t count; - } read_params; - struct { - dm_item_t item; - } clear_params; - }; -} work_q_item_t; - -const size_t k_work_item_allocation_chunk_size = 8; - /* Usage statistics */ -static unsigned g_func_counts[dm_number_of_funcs]; - -/* table of maximum number of instances for each item type */ -static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { - DM_KEY_SAFE_POINTS_MAX, - DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_MISSION_STATE_MAX, - DM_KEY_COMPAT_MAX -}; +static unsigned g_func_counts[DM_NUMBER_OF_FUNCS]; #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ -/* Table of the len of each item type */ -static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { - sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct dataman_compat_s) + DM_SECTOR_HDR_SIZE +/* Table of the len of each item type including HDR size */ +static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = { + g_per_item_size[DM_KEY_SAFE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_1] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_MISSION_STATE] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_COMPAT] + DM_SECTOR_HDR_SIZE }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; -/* Item type lock mutexes */ -static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS]; -static px4_sem_t g_sys_state_mutex_mission; -static px4_sem_t g_sys_state_mutex_fence; +static uint8_t dataman_clients_count = 1; static perf_counter_t _dm_read_perf{nullptr}; static perf_counter_t _dm_write_perf{nullptr}; @@ -200,160 +156,12 @@ static enum { BACKEND_LAST } backend = BACKEND_NONE; -/* The data manager work queues */ - -typedef struct { - sq_queue_t q; /* Nuttx queue */ - px4_sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; /* Current size of queue */ - unsigned max_size; /* Maximum queue size reached */ -} work_q_t; - -static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ -static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ - -static px4_sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ static px4_sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -static void init_q(work_q_t *q) -{ - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ - px4_sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ -} - -static inline void -destroy_q(work_q_t *q) -{ - px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ -} - -static inline void -lock_queue(work_q_t *q) -{ - px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */ -} - -static inline void -unlock_queue(work_q_t *q) -{ - px4_sem_post(&(q->mutex)); /* Release the queue lock */ -} - -static work_q_item_t * -create_work_item() -{ - work_q_item_t *item; - - /* Try to reuse item from free item queue */ - lock_queue(&g_free_q); - - if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { - g_free_q.size--; - } - - unlock_queue(&g_free_q); - - /* If we there weren't any free items then obtain memory for a new ones */ - if (item == nullptr) { - item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); - - if (item) { - item->first = 1; - lock_queue(&g_free_q); - - for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { - (item + i)->first = 0; - sq_addfirst(&(item + i)->link, &(g_free_q.q)); - } - - /* Update the queue size and potentially the maximum queue size */ - g_free_q.size += k_work_item_allocation_chunk_size - 1; - - if (g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); - } - } - - /* If we got one then lock the item*/ - if (item) { - px4_sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ - - /* item->wait_sem use case is a signal */ - - px4_sem_setprotocol(&item->wait_sem, SEM_PRIO_NONE); - } - - /* return the item pointer, or nullptr if all failed */ - return item; -} - /* Work queue management functions */ -static inline void -destroy_work_item(work_q_item_t *item) -{ - px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ - /* Return the item to the free item queue for later reuse */ - lock_queue(&g_free_q); - sq_addfirst(&item->link, &(g_free_q.q)); - - /* Update the queue size and potentially the maximum queue size */ - if (++g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); -} - -static inline work_q_item_t * -dequeue_work_item() -{ - work_q_item_t *work; - - /* retrieve the 1st item on the work queue */ - lock_queue(&g_work_q); - - if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { - g_work_q.size--; - } - - unlock_queue(&g_work_q); - return work; -} - -static int -enqueue_work_item_and_wait_for_result(work_q_item_t *item) -{ - /* put the work item at the end of the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - - /* Adjust the queue size and potentially the maximum queue size */ - if (++g_work_q.size > g_work_q.max_size) { - g_work_q.max_size = g_work_q.size; - } - - unlock_queue(&g_work_q); - - /* tell the work thread that work is available */ - px4_sem_post(&g_work_queued_sema); - - /* wait for the result */ - px4_sem_wait(&item->wait_sem); - - int result = item->result; - - destroy_work_item(item); - - return result; -} - static bool is_running() { return dm_operations_data.running; @@ -375,7 +183,7 @@ calculate_offset(dm_item_t item, unsigned index) } /* Calculate and return the item index based on type and index */ - return g_key_offsets[item] + (index * g_per_item_size[item]); + return g_key_offsets[item] + (index * g_per_item_size_with_hdr[item]); } /* Each data item is stored as follows @@ -392,6 +200,10 @@ calculate_offset(dm_item_t item, unsigned index) /* write to the data manager RAM buffer */ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_t count) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -401,7 +213,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -429,7 +241,11 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) { - unsigned char buffer[g_per_item_size[item]]; + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ const int offset = calculate_offset(item, index); @@ -440,7 +256,7 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -502,6 +318,10 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) /* Retrieve from the data manager RAM buffer*/ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -511,7 +331,7 @@ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -546,7 +366,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) return -1; } - unsigned char buffer[g_per_item_size[item]]; + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -557,7 +377,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -610,6 +430,9 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) /* Looks good, copy it to the caller's buffer */ memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + + } else { + memset(buf, 0, count); } /* Return the number of bytes of caller data read */ @@ -618,6 +441,10 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) static int _ram_clear(dm_item_t item) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + int i; int result = 0; @@ -639,7 +466,7 @@ static int _ram_clear(dm_item_t item) } buf[0] = 0; - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } return result; @@ -648,6 +475,10 @@ static int _ram_clear(dm_item_t item) static int _file_clear(dm_item_t item) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + int i, result = 0; /* Get the offset of 1st item of this type */ @@ -687,7 +518,7 @@ _file_clear(dm_item_t item) } } - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } /* Make sure data is actually written to physical media */ @@ -698,31 +529,6 @@ _file_clear(dm_item_t item) static int _file_initialize(unsigned max_offset) { - /* See if the data manage file exists and is a multiple of the sector size */ - dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); - - if (dm_operations_data.file.fd >= 0) { - // Read the mission state and check the hash - struct dataman_compat_s compat_state; - dm_operations_data.silence = true; - int ret = g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); - dm_operations_data.silence = false; - - bool incompat = true; - - if (ret == sizeof(compat_state)) { - if (compat_state.key == DM_COMPAT_KEY) { - incompat = false; - } - } - - close(dm_operations_data.file.fd); - - if (incompat) { - unlink(k_data_manager_device_path); - } - } - /* Open or create the data manager file */ dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); @@ -739,16 +545,43 @@ _file_initialize(unsigned max_offset) return -1; } - /* Write current compat info */ - struct dataman_compat_s compat_state; - compat_state.key = DM_COMPAT_KEY; - int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + dataman_compat_s compat_state{}; + + dm_operations_data.silence = true; + + g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + + dm_operations_data.silence = false; + + if (compat_state.key != DM_COMPAT_KEY) { + + /* Write current compat info */ + compat_state.key = DM_COMPAT_KEY; + int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + + if (ret != sizeof(compat_state)) { + PX4_ERR("Failed writing compat: %d", ret); + } + + for (uint32_t item = DM_KEY_SAFE_POINTS; item <= DM_KEY_MISSION_STATE; ++item) { + g_dm_ops->clear((dm_item_t)item); + } + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; - if (ret != sizeof(compat_state)) { - PX4_ERR("Failed writing compat: %d", ret); + mission_stats_entry_s stats; + stats.num_items = 0; + stats.update_counter = 1; + + g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + g_dm_ops->write(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); + g_dm_ops->write(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); } - fsync(dm_operations_data.file.fd); dm_operations_data.running = true; return 0; @@ -787,156 +620,6 @@ _ram_shutdown() dm_operations_data.running = false; } -/** Write to the data manager file */ -__EXPORT ssize_t -dm_write(dm_item_t item, unsigned index, const void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_write_perf); - - /* get a work item and queue up a write request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_write create_work_item failed"); - perf_end(_dm_write_perf); - return -1; - } - - work->func = dm_write_func; - work->write_params.item = item; - work->write_params.index = index; - work->write_params.buf = buf; - work->write_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_write_perf); - return ret; -} - -/** Retrieve from the data manager file */ -__EXPORT ssize_t -dm_read(dm_item_t item, unsigned index, void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_read_perf); - - /* get a work item and queue up a read request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_read create_work_item failed"); - perf_end(_dm_read_perf); - return -1; - } - - work->func = dm_read_func; - work->read_params.item = item; - work->read_params.index = index; - work->read_params.buf = buf; - work->read_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_read_perf); - return ret; -} - -/** Clear a data Item */ -__EXPORT int -dm_clear(dm_item_t item) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - /* get a work item and queue up a clear request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_clear create_work_item failed"); - return -1; - } - - work->func = dm_clear_func; - work->clear_params.item = item; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - return enqueue_work_item_and_wait_for_result(work); -} - -__EXPORT int -dm_lock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_wait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -__EXPORT int -dm_trylock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_trywait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -/** Unlock a data Item */ -__EXPORT void -dm_unlock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return; - } - - if (item >= DM_KEY_NUM_KEYS) { - return; - } - - if (g_item_locks[item]) { - px4_sem_post(g_item_locks[item]); - } -} - static int task_main(int argc, char *argv[]) { @@ -955,43 +638,28 @@ task_main(int argc, char *argv[]) return -1; } - work_q_item_t *work; - /* Initialize global variables */ g_key_offsets[0] = 0; for (int i = 0; i < ((int)DM_KEY_NUM_KEYS - 1); i++) { - g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size[i]); + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size_with_hdr[i]); } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * - g_per_item_size[DM_KEY_NUM_KEYS - 1]); + g_per_item_size_with_hdr[DM_KEY_NUM_KEYS - 1]); - for (unsigned i = 0; i < dm_number_of_funcs; i++) { + for (unsigned i = 0; i < DM_NUMBER_OF_FUNCS; i++) { g_func_counts[i] = 0; } - /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */ - px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */ - px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */ - - for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { - g_item_locks[i] = nullptr; - } - - g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission; - g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence; - g_task_should_exit = false; - init_q(&g_work_q); - init_q(&g_free_q); - - px4_sem_init(&g_work_queued_sema, 1, 0); + uORB::Publication dataman_response_pub{ORB_ID(dataman_response)}; + const orb_sub_t dataman_request_sub = orb_subscribe(ORB_ID(dataman_request)); - /* g_work_queued_sema use case is a signal */ - - px4_sem_setprotocol(&g_work_queued_sema, SEM_PRIO_NONE); + if (!orb_sub_valid(dataman_request_sub)) { + PX4_ERR("Failed to subscribe (%i)", errno); + } _dm_read_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": read"); _dm_write_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": write"); @@ -1017,47 +685,109 @@ task_main(int argc, char *argv[]) break; } + px4_pollfd_struct_t fds; + fds.fd = dataman_request_sub; + fds.events = POLLIN; + /* Tell startup that the worker thread has completed its initialization */ px4_sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ while (true) { - /* do we need to exit ??? */ - if (!g_task_should_exit) { - /* wait for work */ - g_dm_ops->wait(&g_work_queued_sema); - } + ret = px4_poll(&fds, 1, 1000); - /* Empty the work queue */ - while ((work = dequeue_work_item())) { + if (ret > 0) { - /* handle each work item with the appropriate handler */ - switch (work->func) { - case dm_write_func: - g_func_counts[dm_write_func]++; - work->result = - g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.buf, work->write_params.count); - break; + bool updated = false; + orb_check(dataman_request_sub, &updated); - case dm_read_func: - g_func_counts[dm_read_func]++; - work->result = - g_dm_ops->read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); - break; + if (updated) { - case dm_clear_func: - g_func_counts[dm_clear_func]++; - work->result = g_dm_ops->clear(work->clear_params.item); - break; + dataman_request_s request; + orb_copy(ORB_ID(dataman_request), dataman_request_sub, &request); - default: /* should never happen */ - work->result = -1; - break; - } + dataman_response_s response{}; + response.client_id = request.client_id; + response.request_type = request.request_type; + response.item = request.item; + response.index = request.index; + response.status = dataman_response_s::STATUS_FAILURE_NO_DATA; + + ssize_t result; + + switch (request.request_type) { + + case DM_GET_ID: + if (dataman_clients_count < UINT8_MAX) { + response.client_id = dataman_clients_count++; + /* Send the timestamp of the request over the data buffer so that the "dataman client" + * can distinguish whether the request was made by it. */ + memcpy(response.data, &request.timestamp, sizeof(hrt_abstime)); + + } else { + PX4_ERR("Max Dataman clients reached!"); + } + + break; - /* Inform the caller that work is done */ - px4_sem_post(&work->wait_sem); + case DM_WRITE: + + g_func_counts[DM_WRITE]++; + perf_begin(_dm_write_perf); + result = g_dm_ops->write(static_cast(request.item), request.index, + &(request.data), request.data_length); + perf_end(_dm_write_perf); + + if (result > 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_WRITE_FAILED; + } + + break; + + case DM_READ: + + g_func_counts[DM_READ]++; + perf_begin(_dm_read_perf); + result = g_dm_ops->read(static_cast(request.item), request.index, + &(response.data), request.data_length); + + perf_end(_dm_read_perf); + + if (result >= 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_READ_FAILED; + } + + break; + + case DM_CLEAR: + + g_func_counts[DM_CLEAR]++; + result = g_dm_ops->clear(static_cast(request.item)); + + if (result == 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_CLEAR_FAILED; + } + + break; + + default: + break; + + } + + response.timestamp = hrt_absolute_time(); + dataman_response_pub.publish(response); + } } /* time to go???? */ @@ -1066,26 +796,12 @@ task_main(int argc, char *argv[]) } } - g_dm_ops->shutdown(); - - /* The work queue is now empty, empty the free queue */ - for (;;) { - if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == nullptr) { - break; - } + orb_unsubscribe(dataman_request_sub); - if (work->first) { - free(work); - } - } + g_dm_ops->shutdown(); end: backend = BACKEND_NONE; - destroy_q(&g_work_q); - destroy_q(&g_free_q); - px4_sem_destroy(&g_work_queued_sema); - px4_sem_destroy(&g_sys_state_mutex_mission); - px4_sem_destroy(&g_sys_state_mutex_fence); perf_free(_dm_read_perf); _dm_read_perf = nullptr; @@ -1127,10 +843,10 @@ static void status() { /* display usage statistics */ - PX4_INFO("Writes %u", g_func_counts[dm_write_func]); - PX4_INFO("Reads %u", g_func_counts[dm_read_func]); - PX4_INFO("Clears %u", g_func_counts[dm_clear_func]); - PX4_INFO("Max Q lengths work %u, free %u", g_work_q.max_size, g_free_q.max_size); + PX4_INFO("Writes %u", g_func_counts[DM_WRITE]); + PX4_INFO("Reads %u", g_func_counts[DM_READ]); + PX4_INFO("Clears %u", g_func_counts[DM_CLEAR]); + perf_print_counter(_dm_read_perf); perf_print_counter(_dm_write_perf); } @@ -1140,7 +856,6 @@ stop() { /* Tell the worker task to shut down */ g_task_should_exit = true; - px4_sem_post(&g_work_queued_sema); } static void @@ -1158,13 +873,11 @@ It is used to store structured data of different types: mission waypoints, missi Each type has a specific type and a fixed maximum amount of storage items, so that fast random access is possible. ### Implementation -Reading and writing a single item is always atomic. If multiple items need to be read/modified atomically, there is -an additional lock per item type via `dm_lock`. +Reading and writing a single item is always atomic. **DM_KEY_FENCE_POINTS** and **DM_KEY_SAFE_POINTS** items: the first data element is a `mission_stats_entry_s` struct, which stores the number of items for these types. These items are always updated atomically in one transaction (from -the mavlink mission manager). During that time, navigator will try to acquire the geofence item lock, fail, and will not -check for geofence violations. +the mavlink mission manager). )DESCR_STR"); @@ -1274,3 +987,11 @@ dataman_main(int argc, char *argv[]) return 0; } + +static_assert(sizeof(dataman_request_s::data) == sizeof(dataman_response_s::data), "request and response data are not the same size"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SAFE_POINT_SIZE, "mission_safe_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_FENCE_POINT_SIZE, "mission_fance_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_ITEM_SIZE, "mission_item_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SIZE, "mission_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= DATAMAN_COMPAT_SIZE, "dataman_compat_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= sizeof(hrt_abstime), "hrt_abstime can't fit in the response data"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index bcc5b2984a8e..48d048ca4436 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,21 +42,27 @@ #include #include -#ifdef __cplusplus -extern "C" { -#endif - /** Types of items that the data manager can store */ typedef enum { - DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ - DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ - DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_SAFE_POINTS = 0, ///< Safe points coordinates, safe point 0 is home point + DM_KEY_FENCE_POINTS, ///< Fence vertex coordinates + DM_KEY_WAYPOINTS_OFFBOARD_0, ///< Mission way point coordinates sent over mavlink + DM_KEY_WAYPOINTS_OFFBOARD_1, ///< (alternate between 0 and 1) + DM_KEY_MISSION_STATE, ///< Persistent mission state DM_KEY_COMPAT, - DM_KEY_NUM_KEYS /* Total number of item types defined */ + DM_KEY_NUM_KEYS ///< Total number of item types defined } dm_item_t; +/** Types of function calls supported by the worker task */ +typedef enum { + DM_GET_ID = 0, ///< Get dataman client ID + DM_WRITE, ///< Write index for given item + DM_READ, ///< Read index for given item + DM_CLEAR, ///< Clear all index for given item + DM_NUMBER_OF_FUNCS +} dm_function_t; + +/** The maximum number of instances for each item type */ #if defined(MEMORY_CONSTRAINED_SYSTEM) enum { DM_KEY_SAFE_POINTS_MAX = 8, @@ -67,7 +73,6 @@ enum { DM_KEY_COMPAT_MAX = 1 }; #else -/** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = 64, @@ -78,68 +83,40 @@ enum { }; #endif +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, + DM_KEY_MISSION_STATE_MAX, + DM_KEY_COMPAT_MAX +}; + +constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_safe_point_s); +constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s); +constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s); +constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s); +constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct mission_s); + +/** The table of the size of each item type */ +static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { + MISSION_SAFE_POINT_SIZE, + MISSION_FENCE_POINT_SIZE, + MISSION_ITEM_SIZE, + MISSION_ITEM_SIZE, + MISSION_SIZE, + DATAMAN_COMPAT_SIZE +}; + struct dataman_compat_s { uint64_t key; }; /* increment this define whenever a binary incompatible change is performed */ -#define DM_COMPAT_VERSION 2ULL +#define DM_COMPAT_VERSION 3ULL #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \ (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \ (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \ sizeof(struct dataman_compat_s)) - -/** Retrieve from the data manager store */ -__EXPORT ssize_t -dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** write to the data manager store */ -__EXPORT ssize_t -dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ -__EXPORT int -dm_lock( - dm_item_t item /* The item type to lock */ -); - -/** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ -__EXPORT int -dm_trylock( - dm_item_t item /* The item type to lock */ -); - -/** Unlock all items of a type */ -__EXPORT void -dm_unlock( - dm_item_t item /* The item type to unlock */ -); - -/** Erase all items of this type */ -__EXPORT int -dm_clear( - dm_item_t item /* The item type to clear */ -); - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt index 34ac465f488a..c8d44ad807fe 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/requirements.txt @@ -2,4 +2,4 @@ matplotlib==3.5.1 numpy==1.22.2 pyulog==0.9.0 quaternion==3.5.2.post4 -scipy==1.8.0 +scipy==1.10.0 diff --git a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt index 10d3fbc54990..a371f5a549e1 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt +++ b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/requirements.txt @@ -2,5 +2,5 @@ matplotlib==3.5.1 numpy==1.22.2 pyulog==0.9.0 quaternion==3.5.2.post4 -scipy==1.8.0 +scipy==1.10.0 sympy==1.10.1 diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index ec274f28dac2..1dfdad7a1384 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp @@ -466,7 +466,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) break; } - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_info(&mavlink_log_pub, "Autotune returned to idle"); _state = state::idle; _param_fw_at_start.set(false); @@ -482,7 +482,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now) if (now - _state_start_time > 20_s || (_param_fw_at_man_aux.get() && !_aux_switch_en) || _start_flight_mode != _nav_state) { - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing"); _state = state::fail; _state_start_time = now; diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index 78cf6ae806f7..5216bfef64e5 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -51,11 +51,11 @@ InputMavlinkROI::InputMavlinkROI(Parameters ¶meters) : InputMavlinkROI::~InputMavlinkROI() { - if (_vehicle_roi_sub >= 0) { + if (orb_sub_valid(_vehicle_roi_sub)) { orb_unsubscribe(_vehicle_roi_sub); } - if (_position_setpoint_triplet_sub >= 0) { + if (orb_sub_valid(_position_setpoint_triplet_sub)) { orb_unsubscribe(_position_setpoint_triplet_sub); } } @@ -64,13 +64,13 @@ int InputMavlinkROI::initialize() { _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); - if (_vehicle_roi_sub < 0) { + if (!orb_sub_valid(_vehicle_roi_sub)) { return -errno; } _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - if (_position_setpoint_triplet_sub < 0) { + if (!orb_sub_valid(_position_setpoint_triplet_sub)) { return -errno; } @@ -171,14 +171,14 @@ InputMavlinkCmdMount::InputMavlinkCmdMount(Parameters ¶meters) : InputMavlinkCmdMount::~InputMavlinkCmdMount() { - if (_vehicle_command_sub >= 0) { + if (orb_sub_valid(_vehicle_command_sub)) { orb_unsubscribe(_vehicle_command_sub); } } int InputMavlinkCmdMount::initialize() { - if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + if (!orb_sub_valid(_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)))) { return -errno; } @@ -379,23 +379,23 @@ InputMavlinkGimbalV2::InputMavlinkGimbalV2(Parameters ¶meters) : InputMavlinkGimbalV2::~InputMavlinkGimbalV2() { - if (_vehicle_roi_sub >= 0) { + if (orb_sub_valid(_vehicle_roi_sub)) { orb_unsubscribe(_vehicle_roi_sub); } - if (_position_setpoint_triplet_sub >= 0) { + if (orb_sub_valid(_position_setpoint_triplet_sub)) { orb_unsubscribe(_position_setpoint_triplet_sub); } - if (_gimbal_manager_set_attitude_sub >= 0) { + if (orb_sub_valid(_gimbal_manager_set_attitude_sub)) { orb_unsubscribe(_gimbal_manager_set_attitude_sub); } - if (_vehicle_command_sub >= 0) { + if (orb_sub_valid(_vehicle_command_sub)) { orb_unsubscribe(_vehicle_command_sub); } - if (_gimbal_manager_set_manual_control_sub >= 0) { + if (orb_sub_valid(_gimbal_manager_set_manual_control_sub)) { orb_unsubscribe(_gimbal_manager_set_manual_control_sub); } } @@ -410,27 +410,27 @@ int InputMavlinkGimbalV2::initialize() { _vehicle_roi_sub = orb_subscribe(ORB_ID(vehicle_roi)); - if (_vehicle_roi_sub < 0) { + if (!orb_sub_valid(_vehicle_roi_sub)) { return -errno; } _position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); - if (_position_setpoint_triplet_sub < 0) { + if (!orb_sub_valid(_position_setpoint_triplet_sub)) { return -errno; } _gimbal_manager_set_attitude_sub = orb_subscribe(ORB_ID(gimbal_manager_set_attitude)); - if (_gimbal_manager_set_attitude_sub < 0) { + if (!orb_sub_valid(_gimbal_manager_set_attitude_sub)) { return -errno; } - if ((_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command))) < 0) { + if (!orb_sub_valid(_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command)))) { return -errno; } - if ((_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control))) < 0) { + if (!orb_sub_valid(_gimbal_manager_set_manual_control_sub = orb_subscribe(ORB_ID(gimbal_manager_set_manual_control)))) { return -errno; } diff --git a/src/modules/gimbal/input_mavlink.h b/src/modules/gimbal/input_mavlink.h index 04264410d7d1..59e6b9d5b827 100644 --- a/src/modules/gimbal/input_mavlink.h +++ b/src/modules/gimbal/input_mavlink.h @@ -68,8 +68,8 @@ class InputMavlinkROI : public InputBase private: void _read_control_data_from_position_setpoint_sub(ControlData &control_data); - int _vehicle_roi_sub = -1; - int _position_setpoint_triplet_sub = -1; + orb_sub_t _vehicle_roi_sub = ORB_SUB_INVALID; + orb_sub_t _position_setpoint_triplet_sub = ORB_SUB_INVALID; uint8_t _cur_roi_mode {vehicle_roi_s::ROI_NONE}; }; @@ -89,7 +89,7 @@ class InputMavlinkCmdMount : public InputBase UpdateResult _process_command(ControlData &control_data, const vehicle_command_s &vehicle_command); void _ack_vehicle_command(const vehicle_command_s &cmd); - int _vehicle_command_sub = -1; + orb_sub_t _vehicle_command_sub = ORB_SUB_INVALID; }; class InputMavlinkGimbalV2 : public InputBase @@ -118,11 +118,11 @@ class InputMavlinkGimbalV2 : public InputBase void _stream_gimbal_manager_status(const ControlData &control_data); void _read_control_data_from_position_setpoint_sub(ControlData &control_data); - int _vehicle_roi_sub = -1; - int _gimbal_manager_set_attitude_sub = -1; - int _gimbal_manager_set_manual_control_sub = -1; - int _position_setpoint_triplet_sub = -1; - int _vehicle_command_sub = -1; + orb_sub_t _vehicle_roi_sub = ORB_SUB_INVALID; + orb_sub_t _gimbal_manager_set_attitude_sub = ORB_SUB_INVALID; + orb_sub_t _gimbal_manager_set_manual_control_sub = ORB_SUB_INVALID; + orb_sub_t _position_setpoint_triplet_sub = ORB_SUB_INVALID; + orb_sub_t _vehicle_command_sub = ORB_SUB_INVALID; uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)}; uORB::Publication _gimbal_manager_info_pub{ORB_ID(gimbal_manager_information)}; diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index c512cda254ee..60ecf620c00a 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -51,7 +51,7 @@ InputRC::InputRC(Parameters ¶meters) : InputRC::~InputRC() { - if (_manual_control_setpoint_sub >= 0) { + if (orb_sub_valid(_manual_control_setpoint_sub)) { orb_unsubscribe(_manual_control_setpoint_sub); } } @@ -60,7 +60,7 @@ int InputRC::initialize() { _manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - if (_manual_control_setpoint_sub < 0) { + if (!orb_sub_valid(_manual_control_setpoint_sub)) { return -errno; } diff --git a/src/modules/gimbal/input_rc.h b/src/modules/gimbal/input_rc.h index 2bf0c2f51f05..5483cab6314a 100644 --- a/src/modules/gimbal/input_rc.h +++ b/src/modules/gimbal/input_rc.h @@ -59,7 +59,7 @@ class InputRC : public InputBase virtual UpdateResult _read_control_data_from_subscription(ControlData &control_data, bool already_active); float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); - int _manual_control_setpoint_sub{-1}; + orb_sub_t _manual_control_setpoint_sub{ORB_SUB_INVALID}; float _last_set_aux_values[3] {}; }; diff --git a/src/modules/gyro_fft/GyroFFT.cpp b/src/modules/gyro_fft/GyroFFT.cpp index 51818c342552..6ea2bd3dc1bd 100644 --- a/src/modules/gyro_fft/GyroFFT.cpp +++ b/src/modules/gyro_fft/GyroFFT.cpp @@ -135,6 +135,11 @@ bool GyroFFT::init() buffers_allocated = AllocateBuffers<256>(); _param_imu_gyro_fft_len.set(256); _param_imu_gyro_fft_len.commit(); + _rfft_q15.fftLenReal = 256; + _rfft_q15.twidCoefRModifier = 32U; + _rfft_q15.pCfft = &arm_cfft_sR_q15_len128; + PX4_INFO("Setting IMU_GYRO_FFT_LEN=%" PRId32 ", as default", _param_imu_gyro_fft_len.get()); + break; } diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index 78e153ee8995..0409eae5ef49 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -298,7 +298,7 @@ bool MulticopterLandDetector::_get_landed_state() bool MulticopterLandDetector::_get_ground_effect_state() { - return (_in_descend && !_horizontal_movement) || + return (_below_gnd_effect_hgt && _in_descend && !_horizontal_movement) || (_below_gnd_effect_hgt && _takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT) || _takeoff_state == takeoff_status_s::TAKEOFF_STATE_RAMPUP; } diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index 80c54f482694..b9ae25f1f19b 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -259,7 +259,8 @@ void LoadMon::stack_usage() if (system_load.tasks[_stack_task_index].valid && (system_load.tasks[_stack_task_index].tcb->pid > 0)) { - stack_free = up_check_tcbstack_remain(system_load.tasks[_stack_task_index].tcb); + stack_free = system_load.tasks[_stack_task_index].tcb->adj_stack_size - up_check_tcbstack( + system_load.tasks[_stack_task_index].tcb); strncpy((char *)task_stack_info.task_name, system_load.tasks[_stack_task_index].tcb->name, CONFIG_TASK_NAME_SIZE - 1); task_stack_info.task_name[CONFIG_TASK_NAME_SIZE - 1] = '\0'; diff --git a/src/modules/load_mon/LoadMon.hpp b/src/modules/load_mon/LoadMon.hpp index 212aef8016ba..7e1eab5b7c79 100644 --- a/src/modules/load_mon/LoadMon.hpp +++ b/src/modules/load_mon/LoadMon.hpp @@ -34,7 +34,7 @@ #pragma once #if defined(__PX4_NUTTX) -#include +#include #endif #include #include diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 508a5427549f..4c42073297bf 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -5,7 +5,7 @@ #include #include -orb_advert_t mavlink_log_pub = nullptr; +orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; // required standard deviation of estimate for estimator to publish data static const uint32_t EST_STDDEV_XY_VALID = 2.0; // 2.0 m diff --git a/src/modules/logger/CMakeLists.txt b/src/modules/logger/CMakeLists.txt index ec07d9adae95..477c87df976e 100644 --- a/src/modules/logger/CMakeLists.txt +++ b/src/modules/logger/CMakeLists.txt @@ -30,6 +30,9 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ +if(CONFIG_LOGGER_PARALLEL_LOGGING) + set(LOGGER_PARALLEL_COMPILE_FLAG "-DLOGGER_PARALLEL_LOGGING") +endif() px4_add_module( MODULE modules__logger @@ -38,6 +41,7 @@ px4_add_module( COMPILE_FLAGS ${MAX_CUSTOM_OPT_LEVEL} -Wno-cast-align # TODO: fix and enable + ${LOGGER_PARALLEL_COMPILE_FLAG} SRCS logged_topics.cpp logger.cpp diff --git a/src/modules/logger/Kconfig b/src/modules/logger/Kconfig index db2c5a75460e..519afccd190c 100644 --- a/src/modules/logger/Kconfig +++ b/src/modules/logger/Kconfig @@ -10,3 +10,10 @@ menuconfig USER_LOGGER depends on BOARD_PROTECTED && MODULES_LOGGER ---help--- Put logger in userspace memory + +menuconfig LOGGER_PARALLEL_LOGGING + bool "Custom mavlink logging protocol in logger" + default n + depends on MODULES_LOGGER + ---help--- + Utilize custom mavlink logging protocol for speed up logging start phase diff --git a/src/modules/logger/log_writer.cpp b/src/modules/logger/log_writer.cpp index 8511f9e92a3d..402bc888486f 100644 --- a/src/modules/logger/log_writer.cpp +++ b/src/modules/logger/log_writer.cpp @@ -158,7 +158,7 @@ void LogWriter::thread_stop() } } -int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start) +int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start, bool reliable, bool wait) { int ret_file = 0, ret_mavlink = 0; @@ -167,7 +167,16 @@ int LogWriter::write_message(LogType type, void *ptr, size_t size, uint64_t drop } if (_log_writer_mavlink_for_write && type == LogType::Full) { - ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size); +#ifdef LOGGER_PARALLEL_LOGGING + + if (reliable) { + ret_mavlink = _log_writer_mavlink_for_write->write_reliable_message(ptr, size, wait); + + } else +#endif + { + ret_mavlink = _log_writer_mavlink_for_write->write_message(ptr, size); + } } // file backend errors takes precedence diff --git a/src/modules/logger/log_writer.h b/src/modules/logger/log_writer.h index 9635d22dadf0..59ec75bc9945 100644 --- a/src/modules/logger/log_writer.h +++ b/src/modules/logger/log_writer.h @@ -73,6 +73,19 @@ class LogWriter void stop_log_mavlink(); +#ifdef LOGGER_PARALLEL_LOGGING + void wait_fifo_empty() + { + if (_log_writer_mavlink) { + _log_writer_mavlink->wait_fifo_count(0); + + while (_log_writer_mavlink->reliable_fifo_is_sending()) { + usleep(10000); + } + } + } +#endif + /** * whether logging is currently active or not (any of the selected backends). */ @@ -90,7 +103,8 @@ class LogWriter * -1 if not enough space in the buffer left (file backend), -2 mavlink backend failed * add type -> pass through, but not to mavlink if mission log */ - int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0); + int write_message(LogType type, void *ptr, size_t size, uint64_t dropout_start = 0, bool reliable = false, + bool wait = false); /** * Select a backend, so that future calls to write_message() only write to the selected @@ -98,7 +112,7 @@ class LogWriter * @param backend */ void select_write_backend(Backend sel_backend); - void unselect_write_backend() { select_write_backend(BackendAll); } + void unselect_write_backend() { select_write_backend(_backend); } /* file logging methods */ @@ -154,7 +168,11 @@ class LogWriter { if (_log_writer_file) { _log_writer_file->set_need_reliable_transfer(need_reliable); } +#ifndef LOGGER_PARALLEL_LOGGING + if (_log_writer_mavlink) { _log_writer_mavlink->set_need_reliable_transfer(need_reliable && mavlink_backed_too); } + +#endif } bool need_reliable_transfer() const @@ -165,7 +183,6 @@ class LogWriter return false; } - #if defined(PX4_CRYPTO) void set_encryption_parameters(px4_crypto_algorithm_t algorithm, uint8_t key_idx, uint8_t exchange_key_idx) { diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp index 873da36d3ed9..dae17c2efb0c 100644 --- a/src/modules/logger/log_writer_file.cpp +++ b/src/modules/logger/log_writer_file.cpp @@ -40,9 +40,10 @@ #include #include +#include #include #include -#ifdef __PX4_NUTTX +#if defined(__PX4_NUTTX) && defined(SYSTEMCMDS_HARDFAULT_LOG) #include #endif /* __PX4_NUTTX */ @@ -256,7 +257,7 @@ void LogWriterFile::start_log(LogType type, const char *filename) int LogWriterFile::hardfault_store_filename(const char *log_file) { -#if defined(__PX4_NUTTX) && defined(px4_savepanic) +#if defined(__PX4_NUTTX) && defined(px4_savepanic) && defined(SYSTEMCMDS_HARDFAULT_LOG) int fd = open(HARDFAULT_ULOG_PATH, O_TRUNC | O_WRONLY | O_CREAT); if (fd < 0) { @@ -304,7 +305,7 @@ int LogWriterFile::thread_start() param.sched_priority = SCHED_PRIORITY_DEFAULT - 40; (void)pthread_attr_setschedparam(&thr_attr, ¶m); - pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1170)); + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1370)); int ret = pthread_create(&_thread, &thr_attr, &LogWriterFile::run_helper, this); pthread_attr_destroy(&thr_attr); @@ -412,12 +413,15 @@ void LogWriterFile::run() size_t out = available; if (_algorithm != CRYPTO_NONE) { + _mac_size = sizeof(_mac); _crypto.encrypt_data( _key_idx, (uint8_t *)read_ptr, available, (uint8_t *)read_ptr, - &out); + &out, + _mac, + &_mac_size); if (out != available) { PX4_ERR("Encryption output size mismatch, logfile corrupted"); diff --git a/src/modules/logger/log_writer_file.h b/src/modules/logger/log_writer_file.h index 218f1272581f..4884f4472b2e 100644 --- a/src/modules/logger/log_writer_file.h +++ b/src/modules/logger/log_writer_file.h @@ -225,6 +225,8 @@ class LogWriterFile px4_crypto_algorithm_t _algorithm; uint8_t _key_idx; uint8_t _exchange_key_idx; + uint8_t _mac[16]; + size_t _mac_size; #endif }; diff --git a/src/modules/logger/log_writer_mavlink.cpp b/src/modules/logger/log_writer_mavlink.cpp index 1c2a7b4bc499..971c6913a5c4 100644 --- a/src/modules/logger/log_writer_mavlink.cpp +++ b/src/modules/logger/log_writer_mavlink.cpp @@ -48,23 +48,147 @@ namespace logger LogWriterMavlink::LogWriterMavlink() { _ulog_stream_data.length = 0; +#ifdef LOGGER_PARALLEL_LOGGING + _ulog_stream_acked_data.length = 0; + pthread_mutex_init(&_fifo.mtx, nullptr); + pthread_cond_init(&_fifo.cv, nullptr); +#endif } +#ifdef LOGGER_PARALLEL_LOGGING +ReliableMsg *LogWriterMavlink::reliable_fifo_pop() +{ + pthread_mutex_lock(&_fifo.mtx); + PX4_DEBUG("reliable POP - wait.."); + + while (_fifo.empty() && !_fifo.sender_should_exit.load()) { + pthread_cond_wait(&_fifo.cv, &_fifo.mtx); + } + + PX4_DEBUG("reliable POP: signalled"); + ReliableMsg *node = _fifo.getHead(); + _fifo.remove(node); + + if (node) { + _fifo.sending = true; + } + + pthread_mutex_unlock(&_fifo.mtx); + return node; +} + +bool LogWriterMavlink::reliable_fifo_push(ReliableMsg *node) +{ + if (!node) { + PX4_ERR("[reliable_fifo_push] nullptr"); + return false; + } + + pthread_mutex_lock(&_fifo.mtx); + _fifo.add(node); + PX4_DEBUG("reliable PUSH - signal sender"); + pthread_cond_signal(&_fifo.cv); + pthread_mutex_unlock(&_fifo.mtx); + return true; +} + +void LogWriterMavlink::reliable_fifo_set_sender_idle() +{ + pthread_mutex_lock(&_fifo.mtx); + _fifo.sending = false; + pthread_mutex_unlock(&_fifo.mtx); +} + +bool LogWriterMavlink::reliable_fifo_is_sending() +{ + bool sending; + pthread_mutex_lock(&_fifo.mtx); + sending = _fifo.sending; + pthread_mutex_unlock(&_fifo.mtx); + return sending; +} + +void LogWriterMavlink::wait_fifo_count(size_t count) +{ + while (reliable_fifo_count() > count) { + usleep(30000); + } +} + +size_t LogWriterMavlink::reliable_fifo_count() +{ + size_t count = 0; + pthread_mutex_lock(&_fifo.mtx); + count = _fifo.size(); + pthread_mutex_unlock(&_fifo.mtx); + return count; +} + +void *LogWriterMavlink::mav_reliable_sender_helper(void *context) +{ + px4_prctl(PR_SET_NAME, "log_writer_mav_reliable_sender", px4_getpid()); + static_cast(context)->mav_reliable_sender(); + return nullptr; +} + +void LogWriterMavlink::mav_reliable_sender() +{ + while (!_fifo.sender_should_exit.load()) { + ReliableMsg *msg = reliable_fifo_pop(); + + PX4_DEBUG("[sender] - msg:%p", msg); + + if (msg) { + write_message(msg->data, msg->len, true); + PX4_DEBUG("[sender] - delete msg"); + delete msg; + reliable_fifo_set_sender_idle(); + } + } +} +#endif + bool LogWriterMavlink::init() { +#ifdef LOGGER_PARALLEL_LOGGING + pthread_attr_t thr_attr; + pthread_attr_init(&thr_attr); + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(8500)); + PX4_INFO("create mav_reliable_sender_thread"); + int ret = pthread_create(&_mav_reliable_sender_thread, &thr_attr, &LogWriterMavlink::mav_reliable_sender_helper, this); + + if (ret) { + PX4_ERR("mav_reliable_sender_thread create failed: %d", ret); + } + + pthread_attr_destroy(&thr_attr); +#endif return true; } LogWriterMavlink::~LogWriterMavlink() { - if (_ulog_stream_ack_sub >= 0) { +#ifdef LOGGER_PARALLEL_LOGGING + pthread_mutex_lock(&_fifo.mtx); + _fifo.sender_should_exit.store(true); + pthread_cond_signal(&_fifo.cv); + pthread_mutex_unlock(&_fifo.mtx); + pthread_join(_mav_reliable_sender_thread, nullptr); +#endif + + if (orb_sub_valid(_ulog_stream_ack_sub)) { orb_unsubscribe(_ulog_stream_ack_sub); } + +#ifdef LOGGER_PARALLEL_LOGGING + pthread_mutex_destroy(&_fifo.mtx); + pthread_cond_destroy(&_fifo.cv); +#endif } void LogWriterMavlink::start_log() { - if (_ulog_stream_ack_sub == -1) { + if (!orb_sub_valid(_ulog_stream_ack_sub)) { _ulog_stream_ack_sub = orb_subscribe(ORB_ID(ulog_stream_ack)); } @@ -76,37 +200,60 @@ void LogWriterMavlink::start_log() _ulog_stream_data.length = 0; _ulog_stream_data.first_message_offset = 0; +#ifdef LOGGER_PARALLEL_LOGGING + _ulog_stream_acked_data.msg_sequence = 0; + _ulog_stream_acked_data.length = 0; + _ulog_stream_acked_data.first_message_offset = 0; +#endif _is_started = true; } void LogWriterMavlink::stop_log() { _ulog_stream_data.length = 0; +#ifdef LOGGER_PARALLEL_LOGGING + _ulog_stream_acked_data.length = 0; +#endif _is_started = false; } -int LogWriterMavlink::write_message(void *ptr, size_t size) +int LogWriterMavlink::write_message(void *ptr, size_t size, bool reliable) { if (!is_started()) { return 0; } - const uint8_t data_len = (uint8_t)sizeof(_ulog_stream_data.data); + ulog_stream_s *ulog_s_p; + +#ifdef LOGGER_PARALLEL_LOGGING + + if (reliable) { + ulog_s_p = &_ulog_stream_acked_data; + + } else { + ulog_s_p = &_ulog_stream_data; + } + +#else + ulog_s_p = &_ulog_stream_data; +#endif + + const uint8_t data_len = (uint8_t)sizeof(ulog_s_p->data); uint8_t *ptr_data = (uint8_t *)ptr; - if (_ulog_stream_data.first_message_offset == 255) { - _ulog_stream_data.first_message_offset = _ulog_stream_data.length; + if (ulog_s_p->first_message_offset == 255) { + ulog_s_p->first_message_offset = ulog_s_p->length; } while (size > 0) { - size_t send_len = math::min((size_t)data_len - _ulog_stream_data.length, size); - memcpy(_ulog_stream_data.data + _ulog_stream_data.length, ptr_data, send_len); - _ulog_stream_data.length += send_len; + size_t send_len = math::min((size_t)data_len - ulog_s_p->length, size); + memcpy(ulog_s_p->data + ulog_s_p->length, ptr_data, send_len); + ulog_s_p->length += send_len; ptr_data += send_len; size -= send_len; - if (_ulog_stream_data.length >= data_len) { - if (publish_message()) { + if (ulog_s_p->length >= data_len) { + if (publish_message(reliable)) { return -2; } } @@ -115,6 +262,30 @@ int LogWriterMavlink::write_message(void *ptr, size_t size) return 0; } +#ifdef LOGGER_PARALLEL_LOGGING +int LogWriterMavlink::write_reliable_message(void *ptr, size_t size, bool wait) +{ + if (wait) { + wait_fifo_count(LOGGER_RELIABLE_FIFO_WAIT_THRESHOLD); + } + + uint8_t *p = (uint8_t *) ptr; + + while (size > 0) { + size_t len = math::min(size, LOGGER_ULOG_STREAM_DATA_LEN); + size -= len; + ReliableMsg *msg = new ReliableMsg(); + memcpy(msg->data, p, len); + p += len; + msg->len = len; + reliable_fifo_push(msg); + } + + return 0; +} +#endif + +#ifndef LOGGER_PARALLEL_LOGGING void LogWriterMavlink::set_need_reliable_transfer(bool need_reliable) { if (!need_reliable && _need_reliable_transfer) { @@ -126,19 +297,46 @@ void LogWriterMavlink::set_need_reliable_transfer(bool need_reliable) _need_reliable_transfer = need_reliable; } +#endif -int LogWriterMavlink::publish_message() +int LogWriterMavlink::publish_message(bool reliable) { - _ulog_stream_data.timestamp = hrt_absolute_time(); - _ulog_stream_data.flags = 0; + ulog_stream_s *ulog_s_p; + +#ifdef LOGGER_PARALLEL_LOGGING + + if (reliable) { + ulog_s_p = &_ulog_stream_acked_data; + + } else { + ulog_s_p = &_ulog_stream_data; + } + +#else + ulog_s_p = &_ulog_stream_data; +#endif + + ulog_s_p->timestamp = hrt_absolute_time(); + ulog_s_p->flags = 0; + +#ifdef LOGGER_PARALLEL_LOGGING + + if (!reliable) { + _ulog_stream_pub.publish(*ulog_s_p); + + } else { + ulog_s_p->flags = ulog_s_p->FLAGS_NEED_ACK; + _ulog_stream_acked_pub.publish(*ulog_s_p); +#else if (_need_reliable_transfer) { - _ulog_stream_data.flags = _ulog_stream_data.FLAGS_NEED_ACK; + ulog_s_p->flags = ulog_s_p->FLAGS_NEED_ACK; } - _ulog_stream_pub.publish(_ulog_stream_data); + _ulog_stream_pub.publish(*ulog_s_p); if (_need_reliable_transfer) { +#endif // we need to wait for an ack. Note that this blocks the main logger thread, so if a file logging // is already running, it will miss samples. px4_pollfd_struct_t fds[1]; @@ -160,7 +358,7 @@ int LogWriterMavlink::publish_message() ulog_stream_ack_s ack; orb_copy(ORB_ID(ulog_stream_ack), _ulog_stream_ack_sub, &ack); - if (ack.msg_sequence == _ulog_stream_data.msg_sequence) { + if (ack.msg_sequence == ulog_s_p->msg_sequence) { got_ack = true; } @@ -178,9 +376,9 @@ int LogWriterMavlink::publish_message() PX4_DEBUG("got ack in %i ms", (int)(hrt_elapsed_time(&started) / 1000)); } - _ulog_stream_data.msg_sequence++; - _ulog_stream_data.length = 0; - _ulog_stream_data.first_message_offset = 255; + ulog_s_p->msg_sequence++; + ulog_s_p->length = 0; + ulog_s_p->first_message_offset = 255; return 0; } diff --git a/src/modules/logger/log_writer_mavlink.h b/src/modules/logger/log_writer_mavlink.h index ad6d80fa3bc6..a3722dbdb64e 100644 --- a/src/modules/logger/log_writer_mavlink.h +++ b/src/modules/logger/log_writer_mavlink.h @@ -37,12 +37,37 @@ #include #include #include +#include + +#ifdef LOGGER_PARALLEL_LOGGING +static constexpr size_t LOGGER_ULOG_STREAM_DATA_LEN {249}; // Size of ulog_stream data buffer +static constexpr size_t LOGGER_RELIABLE_FIFO_WAIT_THRESHOLD{10}; // Msg count threshold for wait fifo trigger +#endif namespace px4 { namespace logger { +#ifdef LOGGER_PARALLEL_LOGGING +class ReliableMsg : public ListNode +{ +public: + uint16_t len; + uint8_t data[LOGGER_ULOG_STREAM_DATA_LEN]; +}; + +class ReliableFifo : public List +{ +public: + pthread_mutex_t mtx; + pthread_cond_t cv; + px4::atomic_bool sender_should_exit{false}; + bool sending{false}; +}; + +#endif + /** * @class LogWriterMavlink * Writes logging data to uORB, and then sent via mavlink @@ -62,9 +87,14 @@ class LogWriterMavlink bool is_started() const { return _is_started; } /** @see LogWriter::write_message() */ - int write_message(void *ptr, size_t size); - + int write_message(void *ptr, size_t size, bool reliable = false); +#ifdef LOGGER_PARALLEL_LOGGING + int write_reliable_message(void *ptr, size_t size, bool wait = false); + bool reliable_fifo_is_sending(); + void wait_fifo_count(size_t count); +#else void set_need_reliable_transfer(bool need_reliable); +#endif bool need_reliable_transfer() const { @@ -72,15 +102,31 @@ class LogWriterMavlink } private: +#ifdef LOGGER_PARALLEL_LOGGING + static void *mav_reliable_sender_helper(void *context); + void mav_reliable_sender(); + + ReliableMsg *reliable_fifo_pop(); + bool reliable_fifo_push(ReliableMsg *node); + void reliable_fifo_set_sender_idle(); + + size_t reliable_fifo_count(); +#endif /** publish message, wait for ack if needed & reset message */ - int publish_message(); + int publish_message(bool reliable = false); ulog_stream_s _ulog_stream_data{}; uORB::Publication _ulog_stream_pub{ORB_ID(ulog_stream)}; - int _ulog_stream_ack_sub{-1}; + orb_sub_t _ulog_stream_ack_sub{ORB_SUB_INVALID}; bool _need_reliable_transfer{false}; bool _is_started{false}; +#ifdef LOGGER_PARALLEL_LOGGING + ulog_stream_s _ulog_stream_acked_data {}; + uORB::Publication _ulog_stream_acked_pub{ORB_ID(ulog_stream_acked)}; + ReliableFifo _fifo; + pthread_t _mav_reliable_sender_thread = 0; +#endif }; } diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index d2a6b906d735..18aaee5893a3 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -64,6 +64,7 @@ #include #include #include +#include //#define DBGPRINT //write status output every few seconds @@ -400,6 +401,18 @@ Logger::Logger(LogWriter::Backend backend, size_t buffer_size, uint32_t log_inte PX4_ERR("Failed to find topic %s", poll_topic_name); } } + +#ifdef LOGGER_PARALLEL_LOGGING + + if (pthread_key_create(&pthread_data_key, NULL) < 0) { + PX4_ERR("Creating pthread data key failed"); + pthread_setspecific(pthread_data_key, (void *)nullptr); + + } else { + pthread_setspecific(pthread_data_key, (void *)&_thread_main_data); + } + +#endif } Logger::~Logger() @@ -593,7 +606,7 @@ void Logger::run() return; } - //all topics added. Get required message buffer size + //Get required message buffer size int max_msg_size = 0; for (int sub = 0; sub < _num_subscriptions; ++sub) { @@ -655,12 +668,12 @@ void Logger::run() /* timer_semaphore use case is a signal */ px4_sem_setprotocol(&_timer_callback_data.semaphore, SEM_PRIO_NONE); - int polling_topic_sub = -1; + orb_sub_t polling_topic_sub = ORB_SUB_INVALID; if (_polling_topic_meta) { polling_topic_sub = orb_subscribe(_polling_topic_meta); - if (polling_topic_sub < 0) { + if (!orb_sub_valid(polling_topic_sub)) { PX4_ERR("Failed to subscribe (%i)", errno); } @@ -683,7 +696,7 @@ void Logger::run() hrt_abstime next_subscribe_check = 0; int next_subscribe_topic_index = -1; // this is used to distribute the checks over time - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { _lockstep_component = px4_lockstep_register_component(); } @@ -890,7 +903,7 @@ void Logger::run() update_params(); // wait for next loop iteration... - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { px4_lockstep_progress(_lockstep_component); px4_pollfd_struct_t fds[1]; @@ -931,13 +944,13 @@ void Logger::run() // stop the writer thread _writer.thread_stop(); - if (polling_topic_sub >= 0) { + if (orb_sub_valid(polling_topic_sub)) { orb_unsubscribe(polling_topic_sub); } - if (_mavlink_log_pub) { + if (orb_advert_valid(_mavlink_log_pub)) { orb_unadvertise(_mavlink_log_pub); - _mavlink_log_pub = nullptr; + _mavlink_log_pub = ORB_ADVERT_INVALID; } px4_unregister_shutdown_hook(&Logger::request_stop_static); @@ -1187,11 +1200,11 @@ void Logger::handle_vehicle_command_update() } } -bool Logger::write_message(LogType type, void *ptr, size_t size) +bool Logger::write_message(LogType type, void *ptr, size_t size, bool reliable, bool wait) { Statistics &stats = _statistics[(int)type]; - if (_writer.write_message(type, ptr, size, stats.dropout_start) != -1) { + if (_writer.write_message(type, ptr, size, stats.dropout_start, reliable, wait) != -1) { if (stats.dropout_start) { float dropout_duration = (float)(hrt_elapsed_time(&stats.dropout_start) / 1000) / 1.e3f; @@ -1455,6 +1468,35 @@ void Logger::stop_log_file(LogType type) _writer.stop_log_file(type); } +#ifdef LOGGER_PARALLEL_LOGGING +void *Logger::mav_start_steps_helper(void *context) +{ + px4_prctl(PR_SET_NAME, "log_writer_mavlink_headers", px4_getpid()); + static_cast(context)->mav_start_steps(); + return nullptr; +} + +void Logger::mav_start_steps() +{ + /* This is running in separate thread to keep logging data while sending header&descriptions */ + _thread_mav_start_sender_data.wait_for_ack = true; + pthread_setspecific(pthread_data_key, (void *)&_thread_mav_start_sender_data); + + PX4_INFO("Write static data - Begin"); + write_header(LogType::Full); + write_version(LogType::Full); + write_formats(LogType::Full); + write_parameters(LogType::Full); + write_parameter_defaults(LogType::Full); + write_perf_data(PrintLoadReason::Preflight); + write_console_output(); + write_events_file(LogType::Full); + write_excluded_optional_topics(LogType::Full); + write_all_add_logged_msg(LogType::Full); + PX4_INFO("Write static data - End"); +} +#endif + void Logger::start_log_mavlink() { if (!can_start_mavlink_log()) { @@ -1472,6 +1514,33 @@ void Logger::start_log_mavlink() _writer.start_log_mavlink(); _writer.select_write_backend(LogWriter::BackendMavlink); + +#ifdef LOGGER_PARALLEL_LOGGING + + for (int sub = 0; sub < _num_subscriptions; ++sub) { + if (_subscriptions[sub].valid() && _subscriptions[sub].msg_id == MSG_ID_INVALID) { + if (_next_topic_id == MSG_ID_INVALID) { + // if we land here an uint8 is too small -> switch to uint16 + PX4_ERR("limit for _next_topic_id reached"); + return; + } + + _subscriptions[sub].msg_id = _next_topic_id++; + } + } + + pthread_attr_t thr_attr; + pthread_attr_init(&thr_attr); + pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(8500)); + PX4_INFO("create mav_start_thread"); + int ret = pthread_create(&_mav_start_thread, &thr_attr, &Logger::mav_start_steps_helper, this); + + if (ret) { + PX4_WARN("mav_start_thread create failed: %d", ret); + } + + pthread_attr_destroy(&thr_attr); +#else _writer.set_need_reliable_transfer(true); write_header(LogType::Full); write_version(LogType::Full); @@ -1484,9 +1553,11 @@ void Logger::start_log_mavlink() write_excluded_optional_topics(LogType::Full); write_all_add_logged_msg(LogType::Full); _writer.set_need_reliable_transfer(false); +#endif _writer.unselect_write_backend(); _writer.notify(); + PX4_INFO("Mavlink logging started"); adjust_subscription_updates(); // redistribute updates as sending the header can take some time } @@ -1499,11 +1570,16 @@ void Logger::stop_log_mavlink() _writer.select_write_backend(LogWriter::BackendMavlink); _writer.set_need_reliable_transfer(true); write_perf_data(PrintLoadReason::Postflight); +#ifdef LOGGER_PARALLEL_LOGGING + _writer.wait_fifo_empty(); +#endif _writer.set_need_reliable_transfer(false); _writer.unselect_write_backend(); _writer.notify(); _writer.stop_log_mavlink(); } + + PX4_INFO("Mavlink logging stopped"); } struct perf_callback_data_t { @@ -1704,7 +1780,12 @@ void Logger::write_format(LogType type, const orb_metadata &meta, WrittenFormats size_t msg_size = sizeof(msg) - sizeof(msg.format) + format_len; msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, &msg, msg_size, true, th_data->wait_for_ack); +#else write_message(type, &msg, msg_size); +#endif if (level > 1 && !written_formats.push_back(&meta)) { PX4_ERR("Array too small"); @@ -1852,7 +1933,12 @@ void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription bool prev_reliable = _writer.need_reliable_transfer(); _writer.set_need_reliable_transfer(true); +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, &msg, msg_size, true, th_data->wait_for_ack); +#else write_message(type, &msg, msg_size); +#endif _writer.set_need_reliable_transfer(prev_reliable); } @@ -1875,7 +1961,12 @@ void Logger::write_info(LogType type, const char *name, const char *value) msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } _writer.unlock(); @@ -1901,7 +1992,12 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } else { PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key_value_str); @@ -1930,6 +2026,10 @@ void Logger::write_info_multiple(LogType type, const char *name, int fd) int file_offset = 0; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); +#endif + while (file_offset < file_size) { _writer.lock(); @@ -1946,7 +2046,11 @@ void Logger::write_info_multiple(LogType type, const char *name, int fd) msg_size += read_length; msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif file_offset += ret; } else { @@ -1989,7 +2093,12 @@ void Logger::write_info_template(LogType type, const char *name, T value, const msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif _writer.unlock(); } @@ -2018,7 +2127,13 @@ void Logger::write_header(LogType type) header.magic[7] = 0x01; //file version 1 header.timestamp = hrt_absolute_time(); _writer.lock(); + +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); + write_message(type, &header, sizeof(header), true, th_data->wait_for_ack); +#else write_message(type, &header, sizeof(header)); +#endif // write the Flags message: this MUST be written right after the ulog header ulog_message_flag_bits_s flag_bits{}; @@ -2028,7 +2143,11 @@ void Logger::write_header(LogType type) flag_bits.msg_size = sizeof(flag_bits) - ULOG_MSG_HEADER_LEN; flag_bits.msg_type = static_cast(ULogMessageType::FLAG_BITS); +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, &flag_bits, sizeof(flag_bits), true, th_data->wait_for_ack); +#else write_message(type, &flag_bits, sizeof(flag_bits)); +#endif _writer.unlock(); } @@ -2125,6 +2244,9 @@ void Logger::write_parameter_defaults(LogType type) msg.msg_type = static_cast(ULogMessageType::PARAMETER_DEFAULT); int param_idx = 0; param_t param = 0; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); +#endif do { // skip over all parameters which are not used @@ -2187,20 +2309,32 @@ void Logger::write_parameter_defaults(LogType type) if (memcmp(&value, &default_value, value_size) != 0) { memcpy(&buffer[msg_size - value_size], default_value, value_size); msg.default_types = ulog_parameter_default_type_t::current_setup | ulog_parameter_default_type_t::system; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } } else { if (memcmp(&value, &default_value, value_size) != 0) { memcpy(&buffer[msg_size - value_size], default_value, value_size); msg.default_types = ulog_parameter_default_type_t::current_setup; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } if (memcmp(&value, &system_default_value, value_size) != 0) { memcpy(&buffer[msg_size - value_size], system_default_value, value_size); msg.default_types = ulog_parameter_default_type_t::system; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } } } @@ -2220,6 +2354,10 @@ void Logger::write_parameters(LogType type) int param_idx = 0; param_t param = 0; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); +#endif + do { // skip over all parameters which are not used do { @@ -2271,7 +2409,11 @@ void Logger::write_parameters(LogType type) msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); @@ -2289,6 +2431,10 @@ void Logger::write_changed_parameters(LogType type) int param_idx = 0; param_t param = 0; +#ifdef LOGGER_PARALLEL_LOGGING + thread_data_t *th_data = (thread_data_t *) pthread_getspecific(pthread_data_key); +#endif + do { // skip over all parameters which are not used do { @@ -2342,7 +2488,11 @@ void Logger::write_changed_parameters(LogType type) // msg_size is now 1 (msg_type) + 2 (msg_size) + 1 (key_len) + key_len + value_size msg.msg_size = msg_size - ULOG_MSG_HEADER_LEN; +#ifdef LOGGER_PARALLEL_LOGGING + write_message(type, buffer, msg_size, true, th_data->wait_for_ack); +#else write_message(type, buffer, msg_size); +#endif } } while ((param != PARAM_INVALID) && (param_idx < (int) param_count())); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 65a9f1d179b3..e215401d0861 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -69,6 +69,14 @@ namespace px4 namespace logger { +#ifdef LOGGER_PARALLEL_LOGGING +typedef struct thread_data { + bool wait_for_ack{false}; +} thread_data_t; + +pthread_key_t pthread_data_key; +#endif + static constexpr uint8_t MSG_ID_INVALID = UINT8_MAX; struct LoggerSubscription : public uORB::SubscriptionInterval { @@ -221,6 +229,16 @@ class Logger : public ModuleBase, public ModuleParams void stop_log_mavlink(); +#ifdef LOGGER_PARALLEL_LOGGING + /** + * Run in separate thread to continue data logging while sending header&descriptions + */ + void mav_start_steps(); + + static void *mav_start_steps_helper(void *); + +#endif + /** check if mavlink logging can be started */ bool can_start_mavlink_log() const { @@ -290,7 +308,7 @@ class Logger : public ModuleBase, public ModuleParams * Must be called with _writer.lock() held. * @return true if data written, false otherwise (on overflow) */ - bool write_message(LogType type, void *ptr, size_t size); + bool write_message(LogType type, void *ptr, size_t size, bool reliable = false, bool wait = false); /** * Add topic subscriptions from SD file if it exists, otherwise add topics based on the configured profile. @@ -341,6 +359,12 @@ class Logger : public ModuleBase, public ModuleParams void adjust_subscription_updates(); +#ifdef LOGGER_PARALLEL_LOGGING + pthread_t _mav_start_thread {0}; + thread_data_t _thread_main_data; + thread_data_t _thread_mav_start_sender_data; +#endif + uint8_t *_msg_buffer{nullptr}; int _msg_buffer_len{0}; diff --git a/src/modules/logger/parallel_mavlink_logging.md b/src/modules/logger/parallel_mavlink_logging.md new file mode 100644 index 000000000000..d506a53aba65 --- /dev/null +++ b/src/modules/logger/parallel_mavlink_logging.md @@ -0,0 +1,118 @@ +# Parallel data logging over MAVLink + +### Problem +Starting flight logging over mavlink is a slow operation. In case logging is started from ARMING event the log file most likely have several seconds long missing trace period from the takeoff part. This is caused by the protocol that the log is first filled with header and all the static definitions, configuration, boot-log, etc. which are sent using *reliable_transfer*, meaning that every log message/package the px4 logger is sending needs to be acknowleged by the receiver. The round-trip time for this may be long depending on the MAVLink transfer media and the module receiving the log data in the other system behind the Mavlink. + +### Solution +To speed up logging startup and reduce that blackout period in the beginning of the log, the parallel data logging option is implemented. The trick here is that the actual uorb data logging is started as soon as possible when logging is started and the static definitions/configs are sent at the same time through another channel. The receiver end reads both streams and store them to two separate files and in the end of logging it combines them into one ulog file by appending the topic data file in the end of static definition data file. + + +This new protocol is not backward compatible, so BOTH px4 logger and the receiver MUST or MUST NOT implement the parallel logging to make it work! + + +### Original protocol +For Logger, there is one **ulog_stream** uorb channel for transfer data to receiver side and another **ulog_stream_ack** for receiveing ack events. First it collects all the static definitions and send them using *reliable_transfer* method. After static defs are sent it starts sending actual dynamic topic data. + +Mavlink_uorb module listen to **ulog_stream** topic and depending on FLAGS_NEED_ACK flag in the topic msg it sends either **MAVLINK_MSG_LOGGING_DATA** or **MAVLINK_MSG_LOGGING_DATA_ACKED** msg over mavlink. +If **MAVLINK_MSG_LOGGING_DATA_ACKED** is sent it starts waiting for **MAVLINK_MSG_LOGGING_DATA_ACK** and continue sending only after it receives the ack message. publish it to **ulog_stream_ack** + +The receiver reads **MAVLINK_MSG_LOGGING_DATA/_ACKED** messages and store them to ulg file. If **MAVLINK_MSG_LOGGING_DATA_ACKED** is received then **MAVLINK_MSG_LOGGING_DATA_ACK** is sent back to PX4. + +``` ++----------------------------------------------+ +| Logger | +| | +|----------------------------------------------| +| Static data | +| Dyn data | ++----------------------------------------------+ + | ^ + | Publish | Subs + | | + V | ++----------------------------------------------+ +| Mavlink_ulog | +| | +| | +| | ++----------------------------------------------+ + | ^ + Send | +MAVLINK_MSG_LOGGING_DATA Recv +MAVLINK_MSG_LOGGING_DATA_ACKED MAVLINK_MSG_ + | LOGGING_DATA_ACK + V | ++----------------------------------------------+ +| Receiver | +|----------------------------------------------| +| Static data | +| Dyn data | ++----------------------------------------------+ + | + | + | + V ++------------+ +| .ulg file | ++------------+ + +``` + + +### Parallel logging protocol +Logger spawns new thread for sending Static definitions data (reliable transfer enabled) and continues to send dynamic topic data at the same time. Static data is published into **ulog_stream_acked** topic and the dynamic data into **ulog_stream** topic. The thread sending dynamic data does not need to wait anything, but continuously sending the data without waiting any ack. The static data sender thread publishes one message at a time and waits for ack until publishing next one. + +mavlink_uorb reads both **ulog_stream** and **ulog_stream_acked** streams and sends either **MAVLINK_MSG_LOGGING_DATA** or **MAVLINK_MSG_LOGGING_DATA_ACKED** mavlink msgs accordingly. Also it listens to **MAVLINK_MSG_LOGGING_DATA_ACK** messages and publish to **ulog_stream_ack** if one received. +Sending **MAVLINK_MSG_LOGGING_DATA_ACKED** raises a flag to wait for ack. A **MAVLINK_MSG_LOGGING_DATA_ACK** message with expected sequence number shall be received before next _acked message can be sent, but the **MAVLINK_MSG_LOGGING_DATA** messages are always sent in parallel of that without any blocking. + +Receiver listens to both **MAVLINK_MSG_LOGGING_DATA** and **MAVLINK_MSG_LOGGING_DATA_ACKED** messages and write them to separate files accordingly: _DATA into .data file and _DATA_ACKED into .ulg file. For each **MAVLINK_MSG_LOGGING_DATA_ACKED** message it sends back a **MAVLINK_MSG_LOGGING_DATA_ACK** message. When logging is stopped, the receiver append the .data file content into the end of .ulg file and removes the .data file. + +``` ++----------------------------------------------+ +| Logger | +| | +|-----------+ +--------------------------| +| Dyn data | | Static data | ++----------------------------------------------+ + | | ^ + | Publish | Publish | Subs + | | | + v v | ++----------------------------------------------+ +| MAvlink_ulog | +| | +| | +| | ++----------------------------------------------+ + | | ^ +MAVLINK_MSG_ MAVLINK_MSG_ | +LOGGING_DATA LOGGING_DATA_ACKED | + | | MAVLINK_MSG_ + | | LOGGING_DATA_ACK + v v | ++----------------------------------------------+ +| Receiver | +|-----------+ +--------------------------| +| Dyn data | | Static data | ++----------------------------------------------+ + | | + | | + | | + v v ++------------+ +------------+ +| .data file | | .ulg file | +| DATA | | DEFS | ++------------+ +------------+ + | | + | ---- Stop logging---- | + | | + +-------------------------->+ + | + v + +------------+ + | .ulg file | + | DEFS | + | DATA | + +------------+ + +``` diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index c3b42c258e2c..7f24098032b2 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -104,7 +104,7 @@ void ManualControl::Run() _param_man_arm_gesture.set(0); // disable arm gesture _param_man_arm_gesture.commit(); - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t") /* EVENT * @description MAN_ARM_GESTURE is now set to disable arm/disarm stick gesture. @@ -126,7 +126,7 @@ void ManualControl::Run() airmode = 1; // change to roll/pitch airmode param_set(param_mc_airmode, &airmode); - orb_advert_t mavlink_log_pub = nullptr; + orb_advert_t mavlink_log_pub = ORB_ADVERT_INVALID; mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t") /* EVENT * @description MC_AIRMODE is now set to roll/pitch airmode. diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737e6e03..2052fe01062c 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -131,6 +131,7 @@ px4_add_module( adsb airspeed component_general_json # for checksums.h + dataman_client drivers_accelerometer drivers_gyroscope drivers_magnetometer @@ -144,6 +145,14 @@ px4_add_module( UNITY_BUILD ) +px4_add_module( + MODULE modules__mavlink_shell + MAIN mavlink_shell + NO_DAEMON + SRCS + mavlink_shell_main.cpp + ) + if(PX4_TESTING) add_subdirectory(mavlink_tests) endif() diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 9d078b16b090..5770880a0572 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,7 +34,11 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne +#if defined(__PX4_NUTTX) +#include +#else #include +#endif #include #include #include diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 40f6cbf8191b..c2f5cf9bdddf 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1910,7 +1910,7 @@ Mavlink::task_main(int argc, char *argv[]) int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:i:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { @@ -1996,8 +1996,6 @@ Mavlink::task_main(int argc, char *argv[]) _mav_broadcast = BROADCAST_MODE_ON; break; -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) - // multicast case 'c': _src_addr.sin_family = AF_INET; @@ -2012,13 +2010,37 @@ Mavlink::task_main(int argc, char *argv[]) } break; -#else - case 'c': - PX4_ERR("Multicast option is not supported on this platform"); - err_flag = true; + case 'i': { + _src_addr.sin_family = AF_INET; + int mav_id = atoi(myoptarg); + char tmp_str[32]; + uint8_t digits[4] = {0, 0, 0, 0}; + + for (int i = 0; i < 4; i++) { + snprintf(tmp_str, 32, "p:MAV_%d_REMOTE_IP%d", mav_id, i); + int ret = px4_get_parameter_value(tmp_str, temp_int_arg); + + if (!ret) { + digits[i] = (uint8_t)(temp_int_arg & 0xff); + + } else { + PX4_ERR("parse_ip: cant parse %s -> %d", tmp_str, ret); + } + } + + snprintf(tmp_str, 32, "%d.%d.%d.%d", digits[0], digits[1], digits[2], digits[3]); + + if (inet_aton(tmp_str, &_src_addr.sin_addr)) { + PX4_INFO("UDP mavlink remote address: %s", tmp_str); + _src_addr_initialized = true; + + } else { + PX4_ERR("invalid partner ip '%s'", tmp_str); + err_flag = true; + } + } break; -#endif #else case 'p': @@ -2994,10 +3016,8 @@ Mavlink::display_status() printf("UDP (%hu, remote port: %hu)\n", _network_port, _remote_port); printf("\tBroadcast enabled: %s\n", broadcast_enabled() ? "YES" : "NO"); -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) printf("\tMulticast enabled: %s\n", multicast_enabled() ? "YES" : "NO"); -#endif #ifdef __PX4_POSIX if (get_client_source_initialized()) { @@ -3354,14 +3374,13 @@ Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: PRINT_MODULE_USAGE_PARAM_FLAG('p', "Enable Broadcast", true); PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true); PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 3, "Partner IP from MAV__REMOTE_IPn params, where is the argument value", true); PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via -p flag)", true); #endif PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal|uavionix", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); -#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_{i}_BROADCAST param)", true); -#endif PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true); diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e89f0f1f42fd..b20480e79121 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -62,6 +62,7 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; +uint16_t MavlinkMissionManager::_mission_update_counter = 0; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; @@ -83,28 +84,18 @@ MavlinkMissionManager::init_offboard_mission() if (!_dataman_init) { _dataman_init = true; - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); - - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); - } - mission_s mission_state; - int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); - } - - if (ret > 0) { + if (success) { _dataman_id = (dm_item_t)mission_state.dataman_id; _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; _current_seq = mission_state.current_seq; + _mission_update_counter = mission_state.mission_update_counter; - } else if (ret < 0) { - PX4_WARN("offboard mission init failed (%i)", ret); + } else { + PX4_WARN("offboard mission init failed"); } load_geofence_stats(); @@ -115,87 +106,60 @@ MavlinkMissionManager::init_offboard_mission() _my_dataman_id = _dataman_id; } -int +bool MavlinkMissionManager::load_geofence_stats() { mission_stats_entry_s stats; // initialize fence points count - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _geofence_update_counter = stats.update_counter; } - return ret; + return success; } -int +bool MavlinkMissionManager::load_safepoint_stats() { mission_stats_entry_s stats; // initialize safe points count - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; + _safepoint_update_counter = stats.update_counter; } - return ret; + return success; } /** * Publish mission topic to notify navigator about changes. */ -int +void MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) { - // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; mission.timestamp = hrt_absolute_time(); mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; + mission.mission_update_counter = _mission_update_counter; + mission.geofence_update_counter = _geofence_update_counter; + mission.safe_points_update_counter = _safepoint_update_counter; + + /* update active mission state */ + _dataman_id = dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = count; + _current_seq = seq; + _my_dataman_id = _dataman_id; - /* update mission state in dataman */ - - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); - - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); - } - - int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); - - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); - } - - if (res == sizeof(mission_s)) { - /* update active mission state */ - _dataman_id = dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = count; - _current_seq = seq; - _my_dataman_id = _dataman_id; - - /* mission state saved successfully, publish offboard_mission topic */ - _offboard_mission_pub.publish(mission); - - return PX4_OK; - - } else { - PX4_ERR("WPM: can't save mission state"); - - if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); - events::send(events::ID("mavlink_mission_storage_write_failure"), events::Log::Critical, - "Mission: Unable to write to storage"); - } - - return PX4_ERROR; - } + _offboard_mission_pub.publish(mission); } int @@ -203,12 +167,13 @@ MavlinkMissionManager::update_geofence_count(unsigned count) { mission_stats_entry_s stats; stats.num_items = count; - stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data + stats.update_counter = ++_geofence_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = count; } else { @@ -222,6 +187,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) return PX4_ERROR; } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq); return PX4_OK; } @@ -233,9 +199,10 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) stats.update_counter = ++_safepoint_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = count; } else { @@ -249,6 +216,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) return PX4_ERROR; } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq); return PX4_OK; } @@ -298,21 +266,20 @@ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { mission_item_s mission_item{}; - int16_t bytes_read = 0; bool read_success = false; switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: { - bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)); - read_success = (bytes_read == sizeof(mission_item_s)); + read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast(&mission_item), + sizeof(mission_item_s)); } break; case MAV_MISSION_TYPE_FENCE: { // Read a geofence point mission_fence_point_s mission_fence_point; - bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)); - read_success = (bytes_read == sizeof(mission_fence_point_s)); + read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); mission_item.nav_cmd = mission_fence_point.nav_cmd; mission_item.frame = mission_fence_point.frame; @@ -332,8 +299,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point mission_safe_point_s mission_safe_point; - bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)); - read_success = (bytes_read == sizeof(mission_safe_point_s)); + read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; mission_item.frame = mission_safe_point.frame; @@ -385,13 +352,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { mavlink_log_critical(_mavlink->get_mavlink_log_pub(), - "Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t", - (uint8_t)_mission_type, bytes_read); + "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type); /* EVENT - * @description Mission type: {1}. Number of bytes read: {2} + * @description Mission type: {1} */ - events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, - "Mission: Unable to read from storage", _mission_type, bytes_read); + events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, + "Mission: Unable to read from storage", _mission_type); } PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); @@ -679,16 +645,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { - if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); - - } else { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); - - _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID\t"); - events::send(events::ID("mavlink_mission_err_id"), events::Log::Error, - "Failed to write current mission ID to storage"); - } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq); } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); @@ -916,6 +873,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: + ++_mission_update_counter; + /* alternate dataman ID anyway to let navigator know about changes */ if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { @@ -956,21 +915,6 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission _transfer_current_seq = -1; - if (_mission_type == MAV_MISSION_TYPE_FENCE) { - // We're about to write new geofence items, so take the lock. It will be released when - // switching back to idle - PX4_DEBUG("locking fence dataman items"); - - int ret = dm_lock(DM_KEY_FENCE_POINTS); - - if (ret == 0) { - _geofence_locked = true; - - } else { - PX4_ERR("locking failed (%i)", errno); - } - } - } else if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -1006,15 +950,6 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) void MavlinkMissionManager::switch_to_idle_state() { - // when switching to idle, we *always* check if the lock was held and release it. - // This is to ensure we don't end up in a state where we forget to release it. - if (_geofence_locked) { - dm_unlock(DM_KEY_FENCE_POINTS); - _geofence_locked = false; - - PX4_DEBUG("unlocking geofence"); - } - _state = MAVLINK_WPM_STATE_IDLE; } @@ -1129,9 +1064,9 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) check_failed = true; } else { - dm_item_t dm_item = _transfer_dataman_id; - write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); if (!write_failed) { /* waypoint marked as current */ @@ -1167,8 +1102,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.frame = mission_item.frame; if (!check_failed) { - write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } } @@ -1180,8 +1115,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_safe_point.lon = mission_item.lon; mission_safe_point.alt = mission_item.altitude; mission_safe_point.frame = mission_item.frame; - write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point, - sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_safe_point), sizeof(mission_safe_point_s), 2_s); } break; @@ -1226,7 +1161,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + ++_mission_update_counter; + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); break; case MAV_MISSION_TYPE_FENCE: @@ -1280,8 +1216,9 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ++_mission_update_counter; + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; case MAV_MISSION_TYPE_FENCE: @@ -1293,9 +1230,10 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_ALL: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); - ret = update_geofence_count(0) || ret; + ++_mission_update_counter; + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ret = update_geofence_count(0); ret = update_safepoint_count(0) || ret; break; diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 64f6607ef343..d66eddc524ed 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -45,7 +45,7 @@ #pragma once -#include +#include #include #include #include @@ -95,6 +95,8 @@ class MavlinkMissionManager enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) + DatamanClient _dataman_client{}; + uint64_t _time_last_recv{0}; uint64_t _time_last_sent{0}; @@ -130,9 +132,9 @@ class MavlinkMissionManager uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; + static uint16_t _mission_update_counter; static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; - bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz @@ -159,7 +161,7 @@ class MavlinkMissionManager void init_offboard_mission(); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); @@ -168,10 +170,10 @@ class MavlinkMissionManager int update_safepoint_count(unsigned count); /** load geofence stats from dataman */ - int load_geofence_stats(); + bool load_geofence_stats(); /** load safe point stats from dataman */ - int load_safepoint_stats(); + bool load_safepoint_stats(); /** * @brief Sends an waypoint ack message diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 96f34c6903c8..f44287955d3c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2226,6 +2226,9 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mavlink_hil_sensor_t hil_sensor; mavlink_msg_hil_sensor_decode(msg, &hil_sensor); + // Workaround for mavlink2 msg generator bug, just in case. + hil_sensor.id = 0; + const uint64_t timestamp = hrt_absolute_time(); // temperature only updated with baro @@ -2318,7 +2321,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; hil_battery_status.remaining = 0.70; - hil_battery_status.time_remaining_s = NAN; + hil_battery_status.time_remaining_s = (float)NAN; _battery_pub.publish(hil_battery_status); } @@ -2644,119 +2647,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } - - /* attitude */ - { - vehicle_attitude_s hil_attitude{}; - hil_attitude.timestamp_sample = timestamp_sample; - matrix::Quatf q(hil_state.attitude_quaternion); - q.copyTo(hil_attitude.q); - hil_attitude.timestamp = hrt_absolute_time(); - _attitude_pub.publish(hil_attitude); - } - - /* global position */ - { - vehicle_global_position_s hil_global_pos{}; - - hil_global_pos.timestamp_sample = timestamp_sample; - hil_global_pos.lat = hil_state.lat / ((double)1e7); - hil_global_pos.lon = hil_state.lon / ((double)1e7); - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.eph = 2.f; - hil_global_pos.epv = 4.f; - hil_global_pos.timestamp = hrt_absolute_time(); - _global_pos_pub.publish(hil_global_pos); - } - - /* local position */ - { - const double lat = hil_state.lat * 1e-7; - const double lon = hil_state.lon * 1e-7; - - if (!_global_local_proj_ref.isInitialized() || !PX4_ISFINITE(_global_local_alt0)) { - _global_local_proj_ref.initReference(lat, lon); - _global_local_alt0 = hil_state.alt / 1000.f; - } - - float x = 0.f; - float y = 0.f; - _global_local_proj_ref.project(lat, lon, x, y); - - vehicle_local_position_s hil_local_pos{}; - hil_local_pos.timestamp_sample = timestamp_sample; - hil_local_pos.ref_timestamp = _global_local_proj_ref.getProjectionReferenceTimestamp(); - hil_local_pos.ref_lat = _global_local_proj_ref.getProjectionReferenceLat(); - hil_local_pos.ref_lon = _global_local_proj_ref.getProjectionReferenceLon(); - hil_local_pos.ref_alt = _global_local_alt0; - hil_local_pos.xy_valid = true; - hil_local_pos.z_valid = true; - hil_local_pos.v_xy_valid = true; - hil_local_pos.v_z_valid = true; - hil_local_pos.x = x; - hil_local_pos.y = y; - hil_local_pos.z = _global_local_alt0 - hil_state.alt / 1000.f; - hil_local_pos.vx = hil_state.vx / 100.f; - hil_local_pos.vy = hil_state.vy / 100.f; - hil_local_pos.vz = hil_state.vz / 100.f; - - matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; - hil_local_pos.heading = euler.psi(); - hil_local_pos.xy_global = true; - hil_local_pos.z_global = true; - hil_local_pos.vxy_max = INFINITY; - hil_local_pos.vz_max = INFINITY; - hil_local_pos.hagl_min = INFINITY; - hil_local_pos.hagl_max = INFINITY; - hil_local_pos.timestamp = hrt_absolute_time(); - _local_pos_pub.publish(hil_local_pos); - } - - /* accelerometer */ - { - if (_px4_accel == nullptr) { - // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - _px4_accel = new PX4Accelerometer(1310988); - - if (_px4_accel == nullptr) { - PX4_ERR("PX4Accelerometer alloc failed"); - } - } - - if (_px4_accel != nullptr) { - // accel in mG - _px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f); - _px4_accel->update(timestamp_sample, hil_state.xacc, hil_state.yacc, hil_state.zacc); - } - } - - /* gyroscope */ - { - if (_px4_gyro == nullptr) { - // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - _px4_gyro = new PX4Gyroscope(1310988); - - if (_px4_gyro == nullptr) { - PX4_ERR("PX4Gyroscope alloc failed"); - } - } - - if (_px4_gyro != nullptr) { - _px4_gyro->update(timestamp_sample, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed); - } - } - - /* battery status */ - { - battery_status_s hil_battery_status{}; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; - hil_battery_status.current_a = 10.0f; - hil_battery_status.discharged_mah = -1.0f; - hil_battery_status.timestamp = hrt_absolute_time(); - hil_battery_status.time_remaining_s = NAN; - _battery_pub.publish(hil_battery_status); - } } #if !defined(CONSTRAINED_FLASH) diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index 4ff139d0d721..5d746b5c37f5 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -141,15 +141,19 @@ int MavlinkShell::start() #endif if (ret == 0) { +#ifdef __PX4_NUTTX + _task = px4_exec("mavlink_shell", nullptr, nullptr, 0); +#else _task = px4_task_spawn_cmd("mavlink_shell", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + PX4_STACK_ADJUSTED(2048), &MavlinkShell::shell_start_thread, #ifdef __PX4_POSIX argv); #else nullptr); +#endif #endif if (_task < 0) { @@ -183,12 +187,6 @@ int MavlinkShell::start() int MavlinkShell::shell_start_thread(int argc, char *argv[]) { -#ifdef __PX4_NUTTX - dup2(1, 2); //redirect stderror to stdout - - nsh_consolemain(0, NULL); -#endif /* __PX4_NUTTX */ - #ifdef __PX4_POSIX if (argc != 3) { diff --git a/src/modules/mavlink/mavlink_shell_main.cpp b/src/modules/mavlink/mavlink_shell_main.cpp new file mode 100644 index 000000000000..9fee3eb64f82 --- /dev/null +++ b/src/modules/mavlink/mavlink_shell_main.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file mavlink_shell_main.cpp + * Main thread of mavlink shell + */ + +#include + +#ifdef __PX4_NUTTX +#include +#endif /* __PX4_NUTTX */ + +extern "C" __EXPORT int mavlink_shell_main(int argc, char *argv[]) +{ +#ifdef __PX4_NUTTX + dup2(1, 2); //redirect stderror to stdout + + nsh_consolemain(0, NULL); +#endif /* __PX4_NUTTX */ + return 0; +} diff --git a/src/modules/mavlink/mavlink_ulog.cpp b/src/modules/mavlink/mavlink_ulog.cpp index 64b9ec981d20..e7f58bb72529 100644 --- a/src/modules/mavlink/mavlink_ulog.cpp +++ b/src/modules/mavlink/mavlink_ulog.cpp @@ -60,6 +60,12 @@ MavlinkULog::MavlinkULog(int datarate, float max_rate_factor, uint8_t target_sys } + PX4_INFO("[Mavlink_ulog] Parallel logging supported"); + + while (_ulog_stream_acked_sub.update()) { + + } + _waiting_for_initial_ack = true; _last_sent_time = hrt_absolute_time(); //(ab)use this timestamp during initialization _next_rate_check = _last_sent_time + _rate_calculation_delta_t; @@ -84,7 +90,7 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN, "Invalid uorb ulog_stream.data length"); static_assert(sizeof(ulog_stream_s::data) == MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN, - "Invalid uorb ulog_stream.data length"); + "Invalid uorb ulog_stream_acked.data length"); if (_waiting_for_initial_ack) { if (hrt_elapsed_time(&_last_sent_time) > 3e5) { @@ -96,8 +102,10 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) } // check if we're waiting for an ACK + bool check_for_updates = true; + if (_last_sent_time) { - bool check_for_updates = false; + check_for_updates = false; if (_ack_received) { _last_sent_time = 0; @@ -113,7 +121,7 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) PX4_DEBUG("re-sending ulog mavlink message (try=%i)", _sent_tries); _last_sent_time = hrt_absolute_time(); - const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); + const ulog_stream_s &ulog_data = _ulog_stream_acked_sub.get(); mavlink_logging_data_acked_t msg; msg.sequence = ulog_data.msg_sequence; @@ -126,25 +134,59 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) } } } - - if (!check_for_updates) { - return 0; - } } + while ((_current_num_msgs < _max_num_messages) && + (_ulog_stream_sub.updated() || (check_for_updates && _ulog_stream_acked_sub.updated()))) { + if (_ulog_stream_sub.updated()) { + const unsigned last_generation = _ulog_stream_sub.get_last_generation(); + _ulog_stream_sub.update(); + + if (_ulog_stream_sub.get_last_generation() != last_generation + 1) { + perf_count(_msg_missed_ulog_stream_perf); + } + + const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); + + if (ulog_data.timestamp > 0) { + if (ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { + _sent_tries = 1; + _last_sent_time = hrt_absolute_time(); + lock(); + _wait_for_ack_sequence = ulog_data.msg_sequence; + _ack_received = false; + unlock(); + + mavlink_logging_data_acked_t msg; + msg.sequence = ulog_data.msg_sequence; + msg.length = ulog_data.length; + msg.first_message_offset = ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_acked_send_struct(channel, &msg); - while ((_current_num_msgs < _max_num_messages) && _ulog_stream_sub.updated()) { - const unsigned last_generation = _ulog_stream_sub.get_last_generation(); - _ulog_stream_sub.update(); + } else { + mavlink_logging_data_t msg; + msg.sequence = ulog_data.msg_sequence; + msg.length = ulog_data.length; + msg.first_message_offset = ulog_data.first_message_offset; + msg.target_system = _target_system; + msg.target_component = _target_component; + memcpy(msg.data, ulog_data.data, sizeof(msg.data)); + mavlink_msg_logging_data_send_struct(channel, &msg); + } + } - if (_ulog_stream_sub.get_last_generation() != last_generation + 1) { - perf_count(_msg_missed_ulog_stream_perf); + ++_current_num_msgs; } - const ulog_stream_s &ulog_data = _ulog_stream_sub.get(); + if (check_for_updates && _ulog_stream_acked_sub.updated()) { + _ulog_stream_acked_sub.update(); + + const ulog_stream_s &ulog_data = _ulog_stream_acked_sub.get(); - if (ulog_data.timestamp > 0) { - if (ulog_data.flags & ulog_stream_s::FLAGS_NEED_ACK) { + if (ulog_data.timestamp > 0) { _sent_tries = 1; _last_sent_time = hrt_absolute_time(); lock(); @@ -161,19 +203,10 @@ int MavlinkULog::handle_update(mavlink_channel_t channel) memcpy(msg.data, ulog_data.data, sizeof(msg.data)); mavlink_msg_logging_data_acked_send_struct(channel, &msg); - } else { - mavlink_logging_data_t msg; - msg.sequence = ulog_data.msg_sequence; - msg.length = ulog_data.length; - msg.first_message_offset = ulog_data.first_message_offset; - msg.target_system = _target_system; - msg.target_component = _target_component; - memcpy(msg.data, ulog_data.data, sizeof(msg.data)); - mavlink_msg_logging_data_send_struct(channel, &msg); } - } - ++_current_num_msgs; + ++_current_num_msgs; + } } //need to update the rate? @@ -247,6 +280,7 @@ void MavlinkULog::handle_ack(mavlink_logging_ack_t ack) lock(); if (_instance) { // make sure stop() was not called right before + //PX4_INFO("<< %u (wait: %u)", ack.sequence, _wait_for_ack_sequence); if (_wait_for_ack_sequence == ack.sequence) { _ack_received = true; publish_ack(ack.sequence); diff --git a/src/modules/mavlink/mavlink_ulog.h b/src/modules/mavlink/mavlink_ulog.h index 7c96553c6d49..9e534075920a 100644 --- a/src/modules/mavlink/mavlink_ulog.h +++ b/src/modules/mavlink/mavlink_ulog.h @@ -125,7 +125,9 @@ class MavlinkULog static constexpr hrt_abstime _rate_calculation_delta_t = 100_ms; ///< rate update interval uORB::SubscriptionData _ulog_stream_sub{ORB_ID(ulog_stream)}; - uORB::Publication _ulog_stream_ack_pub{ORB_ID(ulog_stream_ack)}; + uORB::SubscriptionData _ulog_stream_acked_sub {ORB_ID(ulog_stream_acked)}; + + uORB::Publication _ulog_stream_ack_pub {ORB_ID(ulog_stream_ack)}; uint16_t _wait_for_ack_sequence; uint8_t _sent_tries = 0; volatile bool _ack_received = false; ///< set to true if a matching ack received diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index dfa9a2da7ff3..a24be95ec329 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -15,6 +15,26 @@ serial_config: then set MAV_ARGS "${MAV_ARGS} -c" fi + set USE_REMOTE_IP 0 + if param greater MAV_${i}_REMOTE_IP0 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP1 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP2 0 + then + set USE_REMOTE_IP 1 + fi + if param greater MAV_${i}_REMOTE_IP3 0 + then + set USE_REMOTE_IP 1 + fi + if [ ${USE_REMOTE_IP} != 0 ]; then + set MAV_ARGS "${MAV_ARGS} -i ${i}" + fi fi if param compare MAV_${i}_FORWARD 1 then @@ -178,3 +198,63 @@ parameters: num_instances: *max_num_config_instances default: [2, 2, 2] reboot_required: true + + MAV_${i}_REMOTE_IP0: + description: + short: MAVLink 1st digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 1st digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP1: + description: + short: MAVLink 2nd digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 2nd digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP2: + description: + short: MAVLink 3rd digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 3rd digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true + + MAV_${i}_REMOTE_IP3: + description: + short: MAVLink 4th digit of Remote IP Address for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected 4th digit of remote ip address will be set and used in MAVLink instance ${i}. + + type: int32 + min: 0 + max: 255 + reboot_required: true + num_instances: *max_num_config_instances + default: [0, 0, 0] + requires_ethernet: true diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 30725a05d097..4532c08d1766 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -51,6 +51,7 @@ px4_add_module( geofence.cpp vtol_takeoff.cpp DEPENDS + dataman_client geo adsb geofence_breach_avoidance diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index 84e1991b3469..4441a251662a 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -34,7 +34,6 @@ #include #include "geofence_breach_avoidance.h" #include "fake_geofence.hpp" -#include "dataman_mocks.hpp" #include using namespace matrix; @@ -88,7 +87,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.fence_violation = true; gf_avoidance.setHorizontalTestPointDistance(20.0f); @@ -148,7 +147,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) value = 8; param_set(param, &value); - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.fence_violation = true; gf_avoidance.setHorizontalTestPointDistance(30.0f); @@ -198,7 +197,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist); gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.max_altitude_exceeded = true; float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); @@ -217,7 +216,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) GeofenceBreachAvoidance gf_avoidance(nullptr); const float climbrate = 10.0f; const float current_alt_amsl = 100.0f; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.max_altitude_exceeded = true; gf_avoidance.setClimbRate(climbrate); @@ -242,7 +241,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) FakeGeofence geo; Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.dist_to_home_exceeded = true; const float hor_vel = 8.0f; @@ -274,7 +273,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) FakeGeofence geo; Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.dist_to_home_exceeded = true; const float test_point_distance = 30.0f; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp deleted file mode 100644 index 5811863889c0..000000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * 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. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * 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 OWNER 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. - * - ****************************************************************************/ -/** - * @file dataman_mocks.h - * Provides a minimal dataman implementation to compile against for testing - * - * @author Roman Bapst - * @author Julian Kent - */ -#pragma once - -#include -extern "C" { - __EXPORT ssize_t - dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** write to the data manager store */ - __EXPORT ssize_t - dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ - __EXPORT int - dm_lock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ - __EXPORT int - dm_trylock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** Unlock all items of a type */ - __EXPORT void - dm_unlock( - dm_item_t item /* The item type to unlock */ - ) {}; - - /** Erase all items of this type */ - __EXPORT int - dm_clear( - dm_item_t item /* The item type to clear */ - ) {return 0;}; -} - diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index fa5882690537..023e097bbae0 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -62,7 +62,7 @@ class TestFeasibilityChecker : public FeasibilityChecker home.lon = lon; home.valid_hpos = true; orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home); - orb_publish(ORB_ID(home_position), home_pub, &home); + orb_publish(ORB_ID(home_position), &home_pub, &home); } void publishCurrentPosition(double lat, double lon) @@ -71,7 +71,7 @@ class TestFeasibilityChecker : public FeasibilityChecker gpos.lat = lat; gpos.lon = lon; orb_advert_t gpos_pub = orb_advertise(ORB_ID(vehicle_global_position), &gpos); - orb_publish(ORB_ID(vehicle_global_position), gpos_pub, &gpos); + orb_publish(ORB_ID(vehicle_global_position), &gpos_pub, &gpos); } void publishLanded(bool landed) @@ -79,7 +79,7 @@ class TestFeasibilityChecker : public FeasibilityChecker vehicle_land_detected_s land_detected = {}; land_detected.landed = true; orb_advert_t landed_pub = orb_advertise(ORB_ID(vehicle_land_detected), &land_detected); - orb_publish(ORB_ID(vehicle_land_detected), landed_pub, &land_detected); + orb_publish(ORB_ID(vehicle_land_detected), &landed_pub, &land_detected); } void publishVehicleType(int vehicle_type) @@ -87,7 +87,7 @@ class TestFeasibilityChecker : public FeasibilityChecker vehicle_status_s status = {}; status.vehicle_type = vehicle_type; orb_advert_t status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + orb_publish(ORB_ID(vehicle_status), &status_pub, &status); } }; diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f131130b901d..19ba42d821da 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,7 +43,7 @@ #include -#include +#include #include #include #include @@ -58,9 +58,8 @@ Geofence::Geofence(Navigator *navigator) : _navigator(navigator), _sub_airdata(ORB_ID(vehicle_air_data)) { - // we assume there's no concurrent fence update on startup if (_navigator != nullptr) { - _updateFence(); + updateFence(); } } @@ -71,42 +70,113 @@ Geofence::~Geofence() } } -void Geofence::updateFence() +void Geofence::run() { - // Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer. - // However this is currently not used - if (dm_lock(DM_KEY_FENCE_POINTS) != 0) { - PX4_ERR("lock failed"); - return; + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_fence_updated) { + _initiate_fence_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client.update(); + + if (_dataman_client.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _fence_updated = false; + + _dataman_cache.invalidate(); + + if (_dataman_cache.size() != _stats.num_items) { + _dataman_cache.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache.size(); ++index) { + _dataman_cache.load(DM_KEY_FENCE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache.update(); + + if (!_dataman_cache.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _updateFence(); + _fence_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Geofence update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + } +} - _updateFence(); - dm_unlock(DM_KEY_FENCE_POINTS); +void Geofence::updateFence() +{ + _initiate_fence_updated = true; } void Geofence::_updateFence() { - // initialize fence points count - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - int num_fence_items = 0; - - if (ret == sizeof(mission_stats_entry_s)) { - num_fence_items = stats.num_items; - _update_counter = stats.update_counter; - } + mission_fence_point_s mission_fence_point; + bool is_circle_area = false; // iterate over all polygons and store their starting vertices _num_polygons = 0; int current_seq = 1; - while (current_seq <= num_fence_items) { - mission_fence_point_s mission_fence_point; - bool is_circle_area = false; + while (current_seq <= _dataman_cache.size()) { - if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != - sizeof(mission_fence_point_s)) { - PX4_ERR("dm_read failed"); + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq, + reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (!success) { + PX4_ERR("loadWait failed, seq: %i", current_seq); break; } @@ -172,9 +242,7 @@ void Geofence::_updateFence() ++current_seq; break; } - } - } bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) @@ -306,22 +374,7 @@ bool Geofence::isBelowMaxAltitude(float altitude) bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { - // the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means - // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now - if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) { - return true; - } - - // we got the lock, now check if the fence data got updated - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - - if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) { - _updateFence(); - } - if (isEmpty()) { - dm_unlock(DM_KEY_FENCE_POINTS); /* Empty fence -> accept all points */ return true; } @@ -329,12 +382,10 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) /* Vertical check */ if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly if (altitude > _altitude_max || altitude < _altitude_min) { - dm_unlock(DM_KEY_FENCE_POINTS); return false; } } - /* Horizontal check: iterate all polygons & circles */ bool outside_exclusion = true; bool inside_inclusion = false; @@ -375,8 +426,6 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) } } - dm_unlock(DM_KEY_FENCE_POINTS); - return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; } @@ -394,13 +443,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, bool c = false; for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, + reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); + + if (!success) { break; } - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, + reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); + + if (!success) { break; } @@ -426,9 +480,10 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, { mission_fence_point_s circle_point{}; + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index, + reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + if (!success) { PX4_ERR("dm_read failed"); return false; } @@ -531,7 +586,10 @@ Geofence::loadFromFile(const char *filename) } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, &vertex, sizeof(vertex)) != sizeof(vertex)) { + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast(&vertex), + sizeof(vertex)); + + if (!success) { goto error; } @@ -555,22 +613,32 @@ Geofence::loadFromFile(const char *filename) if (gotVertical && pointCounter > 2) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t"); events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported"); - ret_val = PX4_OK; + ret_val = PX4_ERROR; /* do a second pass, now that we know the number of vertices */ for (int seq = 1; seq <= pointCounter; ++seq) { mission_fence_point_s mission_fence_point; - if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == - sizeof(mission_fence_point_s)) { + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (success) { mission_fence_point.vertex_count = pointCounter; - dm_write(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)); + _dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); } } mission_stats_entry_s stats; stats.num_items = pointCounter; - ret_val = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + stats.update_counter = _update_counter + 1; + + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (success) { + ret_val = PX4_OK; + } } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); @@ -586,7 +654,7 @@ Geofence::loadFromFile(const char *filename) int Geofence::clearDm() { - dm_clear(DM_KEY_FENCE_POINTS); + _dataman_client.clearSync(DM_KEY_FENCE_POINTS); updateFence(); return PX4_OK; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index b7386db83091..1804d151d539 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,7 @@ #include +#include #include #include #include @@ -77,9 +78,13 @@ class Geofence : public ModuleParams GF_SOURCE_GPS = 1 }; + /** + * @brief function to call regularly to do background work + */ + void run(); + /** * update the geofence from dataman. - * It's generally not necessary to call this as it will automatically update when the data is changed. */ void updateFence(); @@ -136,7 +141,7 @@ class Geofence : public ModuleParams */ int loadFromFile(const char *filename); - bool isEmpty() { return _num_polygons == 0; } + bool isEmpty() { return (!_fence_updated || (_num_polygons == 0)); } int getSource() { return _param_gf_source.get(); } int getGeofenceAction() { return _param_gf_action.get(); } @@ -154,6 +159,14 @@ class Geofence : public ModuleParams private: + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error + }; + struct PolygonInfo { uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region) uint16_t dataman_index; @@ -166,6 +179,12 @@ class Geofence : public ModuleParams Navigator *_navigator{nullptr}; PolygonInfo *_polygons{nullptr}; + mission_stats_entry_s _stats; + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; + DatamanClient &_dataman_client = _dataman_cache.client(); + hrt_abstime _last_horizontal_range_warning{0}; hrt_abstime _last_vertical_range_warning{0}; @@ -179,10 +198,12 @@ class Geofence : public ModuleParams uORB::SubscriptionData _sub_airdata; int _outside_counter{0}; - uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, polygon data was updated + bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache + bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed /** - * implementation of updateFence(), but without locking + * implementation of updateFence() */ void _updateFence(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e9270246f473..c260603d252c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include #include @@ -70,27 +70,48 @@ Mission::Mission(Navigator *navigator) : mission_init(); } +void +Mission::run() +{ + if ((_mission.count > 0) && (_current_mission_index != _load_mission_index)) { + + uint32_t start_index = _current_mission_index; + uint32_t end_index = start_index + DATAMAN_CACHE_SIZE; + + end_index = math::min(end_index, static_cast(_mission.count)); + + for (uint32_t index = start_index; index < end_index; ++index) { + + _dataman_cache.load(static_cast(_mission.dataman_id), index); + } + + _load_mission_index = _current_mission_index; + } + + _dataman_cache.update(); +} + void Mission::mission_init() { // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start - mission_s mission{}; - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { - if ((mission.timestamp != 0) - && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { - if (mission.count > 0) { - PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count); + if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s))) { + if ((_mission.timestamp != 0) + && (_mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { + if (_mission.count > 0) { + PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", _mission.dataman_id, _mission.count); } } else { - PX4_ERR("reading mission state failed"); - // initialize mission state in dataman - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.timestamp = hrt_absolute_time(); - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + _mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + _mission.timestamp = hrt_absolute_time(); + _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), sizeof(mission_s)); } } + + _current_mission_index = _mission.current_seq; + } void @@ -101,6 +122,10 @@ Mission::on_inactive() return; } + if (_need_mission_save && _navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + save_mission_state(); + } + if (_inited) { if (_mission_sub.updated()) { update_mission(); @@ -124,14 +149,11 @@ Mission::on_inactive() /* load missions from storage */ mission_s mission_state = {}; - dm_lock(DM_KEY_MISSION_STATE); - /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - dm_unlock(DM_KEY_MISSION_STATE); - - if (read_res == sizeof(mission_s)) { + if (success) { _mission.dataman_id = mission_state.dataman_id; _mission.count = mission_state.count; _current_mission_index = mission_state.current_seq; @@ -161,14 +183,7 @@ Mission::on_inactive() void Mission::on_inactivation() { - // Disable camera trigger - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // Pause trigger - cmd.param1 = -1.0f; - cmd.param3 = 1.0f; - _navigator->publish_vehicle_cmd(&cmd); - + _navigator->disable_camera_trigger(); _navigator->stop_capturing_images(); _navigator->release_gimbal_control(); @@ -178,6 +193,8 @@ Mission::on_inactivation() /* reset so current mission item gets restarted if mission was paused */ _work_item_type = WORK_ITEM_TYPE_DEFAULT; + + _inactivation_index = _current_mission_index; } void @@ -195,15 +212,28 @@ Mission::on_activation() // we already reset the mission items _execution_mode_changed = false; - set_mission_items(); + // reset the cache and fill it with the camera and gimbal items up to the previous item + if (_current_mission_index > 0) { + resetItemCache(); + updateCachedItemsUpToIndex(_current_mission_index - 1); + } + + unsigned resume_index; + + if (_inactivation_index > 0 && cameraWasTriggering() + && getPreviousPositionItemIndex(_mission, _inactivation_index - 1, resume_index)) { + // The mission we are resuming had camera triggering enabled. In order to not lose any images + // we restart the mission at the previous position item. + // We will replay the cached commands once we reach the previous position item and have yaw aligned. + set_current_mission_index(resume_index); - // unpause triggering if it was paused - vehicle_command_s cmd = {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // unpause trigger - cmd.param1 = -1.0f; - cmd.param3 = 0.0f; - _navigator->publish_vehicle_cmd(&cmd); + _align_heading_necessary = true; + + } else { + set_mission_items(); + } + + _inactivation_index = -1; // reset // reset cruise speed _navigator->reset_cruising_speed(); @@ -242,6 +272,37 @@ Mission::on_active() set_mission_items(); } + // check if heading alignment is necessary, and add it to the current mission item if necessary + if (_align_heading_necessary && is_mission_item_reached_or_completed()) { + mission_item_s next_position_mission_item = {}; + + // add yaw alignment requirement on the current mission item + if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_position_mission_item) + && !PX4_ISFINITE(_mission_item.yaw)) { + _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, + next_position_mission_item.lat, next_position_mission_item.lon)); + _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + reset_mission_item_reached(); + + _navigator->set_position_setpoint_triplet_updated(); + _align_heading_necessary = false; + } + + // replay gimbal and camera commands immediately after resuming mission + if (haveCachedGimbalOrCameraItems()) { + replayCachedGimbalCameraItems(); + } + + // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set + if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { + replayCachedTriggerItems(); + } + /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { /* If we just completed a takeoff which was inserted before the right waypoint, @@ -312,6 +373,11 @@ Mission::set_current_mission_index(uint16_t index) _current_mission_index = index; + // we start from the first item so can reset the cache + if (_current_mission_index == 0) { + resetItemCache(); + } + // a mission index is set manually which has the higher priority than the closest mission item // as it is set by the user _mission_waypoints_changed = false; @@ -387,6 +453,7 @@ Mission::set_execution_mode(const uint8_t mode) // handle switch from reverse to forward mission if (_current_mission_index < 0) { _current_mission_index = 0; + resetItemCache(); // reset cache as we start from the beginning } else if (_current_mission_index < _mission.count - 1) { ++_current_mission_index; @@ -419,10 +486,12 @@ Mission::find_mission_land_start() bool found_land_start_marker = false; for (size_t i = 1; i < _mission.count; i++) { - const ssize_t len = sizeof(missionitem); missionitem_prev = missionitem; // store the last mission item before reading a new one - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s), 500_ms); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; @@ -517,7 +586,10 @@ void Mission::update_mission() { - bool failed = true; + bool failed = !_navigator->get_mission_result()->valid; + + _dataman_cache.invalidate(); + _load_mission_index = -1; /* Reset vehicle_roi * Missions that do not explicitly configure ROI would not override @@ -527,6 +599,14 @@ Mission::update_mission() const mission_s old_mission = _mission; if (_mission_sub.copy(&_mission)) { + + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&_mission), + sizeof(mission_s)); + + if (!success) { + PX4_ERR("Can't update mission state in Dataman"); + } + /* determine current index */ if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { _current_mission_index = _mission.current_seq; @@ -544,33 +624,36 @@ Mission::update_mission() /* otherwise, just leave it */ } - check_mission_valid(true); + if (old_mission.mission_update_counter != _mission.mission_update_counter) { + check_mission_valid(true); - failed = !_navigator->get_mission_result()->valid; + failed = !_navigator->get_mission_result()->valid; - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; + if (!failed) { + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } + /* reset work item if new mission has been accepted */ + _work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_changed = true; + } - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != old_mission.count) || - (_mission.dataman_id != old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; + /* check if the mission waypoints changed while the vehicle is in air + * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ + if (((_mission.count != old_mission.count) || + (_mission.dataman_id != old_mission.dataman_id)) && + !_navigator->get_land_detected()->landed) { + _mission_waypoints_changed = true; + } } } else { PX4_ERR("mission update failed"); + failed = true; } if (failed) { @@ -585,6 +668,14 @@ Mission::update_mission() _current_mission_index = 0; } + // we start from the first item so can reset the cache + if (_current_mission_index == 0) { + resetItemCache(); + } + + // reset as when we update mission we don't want to proceed at previous index + _inactivation_index = -1; + // find and store landing start marker (if available) find_mission_land_start(); @@ -615,9 +706,11 @@ Mission::advance_mission() for (int32_t i = _current_mission_index - 1; i >= 0; i--) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_cache.loadWait(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s), 100_ms); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; @@ -1565,7 +1658,7 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) int index_to_read = current_index + offset; int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; - const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; + const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; /* do not work on empty missions */ if (_mission.count == 0) { @@ -1588,13 +1681,14 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) return false; } - const ssize_t len = sizeof(struct mission_item_s); - /* read mission item to temp storage first to not overwrite current mission item if data damaged */ struct mission_item_s mission_item_tmp; /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { + bool success = _dataman_cache.loadWait(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + sizeof(mission_item_s), 500_ms); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, @@ -1615,7 +1709,10 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) (mission_item_tmp.do_jump_current_count)++; /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { + success = _dataman_client.writeSync(dataman_id, *mission_index_ptr, reinterpret_cast(&mission_item_tmp), + sizeof(struct mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the dataman */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, @@ -1662,19 +1759,20 @@ Mission::read_mission_item(int offset, struct mission_item_s *mission_item) void Mission::save_mission_state() { - mission_s mission_state = {}; - - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); - - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // Save only while disarmed, as this is a blocking operation + _need_mission_save = true; + return; } + _need_mission_save = false; + mission_s mission_state = {}; + /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - if (read_res == sizeof(mission_s)) { + if (success) { /* data read successfully, check dataman ID and items count */ if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { /* navigator may modify only sequence, write modified state only if it changed */ @@ -1682,7 +1780,10 @@ Mission::save_mission_state() mission_state.current_seq = _current_mission_index; mission_state.timestamp = hrt_absolute_time(); - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } @@ -1703,16 +1804,14 @@ Mission::save_mission_state() events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state"); /* write modified state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } } - - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); - } } void @@ -1750,7 +1849,7 @@ Mission::check_mission_valid(bool force) { if ((!_home_inited && _navigator->home_global_position_valid()) || force) { - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); + MissionFeasibilityChecker _missionFeasibilityChecker(_navigator, _dataman_client); _navigator->get_mission_result()->valid = _missionFeasibilityChecker.checkMissionFeasible(_mission); @@ -1768,22 +1867,23 @@ Mission::check_mission_valid(bool force) void Mission::reset_mission(struct mission_s &mission) { - dm_lock(DM_KEY_MISSION_STATE); - - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (_dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s))) { if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { /* set current item to 0 */ mission.current_seq = 0; /* reset jump counters */ if (mission.count > 0) { - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + const dm_item_t dataman_id = (dm_item_t)mission.dataman_id; for (unsigned index = 0; index < mission.count; index++) { struct mission_item_s item; const ssize_t len = sizeof(struct mission_item_s); - if (dm_read(dm_current, index, &item, len) != len) { + bool success = _dataman_client.readSync(dataman_id, index, reinterpret_cast(&item), sizeof(mission_item_s), + 500_ms); + + if (!success) { PX4_WARN("could not read mission item during reset"); break; } @@ -1791,7 +1891,9 @@ Mission::reset_mission(struct mission_s &mission) if (item.nav_cmd == NAV_CMD_DO_JUMP) { item.do_jump_current_count = 0; - if (dm_write(dm_current, index, &item, len) != len) { + success = _dataman_client.writeSync(dataman_id, index, reinterpret_cast(&item), len); + + if (!success) { PX4_WARN("could not save mission item during reset"); break; } @@ -1810,17 +1912,16 @@ Mission::reset_mission(struct mission_s &mission) mission.current_seq = 0; } - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); + _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); } - - dm_unlock(DM_KEY_MISSION_STATE); } bool Mission::need_to_reset_mission() { - /* reset mission state when disarmed */ - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) { + // reset mission when disarmed, mission was actually started and we reached the last mission item + if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset + && (_current_mission_index == _mission.count - 1)) { _need_mission_reset = false; return true; } @@ -1829,7 +1930,7 @@ Mission::need_to_reset_mission() } int32_t -Mission::index_closest_mission_item() const +Mission::index_closest_mission_item() { int32_t min_dist_index(0); float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); @@ -1838,9 +1939,11 @@ Mission::index_closest_mission_item() const for (size_t i = 0; i < _mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read(dm_current, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s), 500_ms); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ PX4_ERR("dataman read failure"); break; @@ -1938,3 +2041,136 @@ void Mission::publish_navigator_mission_item() _navigator_mission_item_pub.publish(navigator_mission_item); } + +bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int inactivation_index, + unsigned &prev_pos_index) const +{ + + for (int index = inactivation_index; index >= 0; index--) { + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, index, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (!success) { + break; + } + + if (MissionBlock::item_contains_position(mission_item)) { + prev_pos_index = index; + return true; + } + } + + return false; +} + +bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item) +{ + const dm_item_t dm_current = (dm_item_t)mission.dataman_id; + + while (start_index < mission.count) { + // start_index is expected to be after _current_mission_index, and the item should therefore be cached + bool success = _dataman_cache.loadWait(dm_current, start_index, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (success && MissionBlock::item_contains_position(mission_item)) { + return true; + } + + start_index++; + } + + return false; +} + +void Mission::cacheItem(const mission_item_s &mission_item) +{ + switch (mission_item.nav_cmd) { + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + _last_gimbal_configure_item = mission_item; + break; + + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + _last_gimbal_control_item = mission_item; + break; + + case NAV_CMD_SET_CAMERA_MODE: + _last_camera_mode_item = mission_item; + break; + + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + _last_camera_trigger_item = mission_item; + break; + + default: + break; + } +} + +void Mission::replayCachedGimbalCameraItems() +{ + if (_last_gimbal_configure_item.nav_cmd > 0) { + issue_command(_last_gimbal_configure_item); + _last_gimbal_configure_item = {}; // delete cached item + } + + if (_last_gimbal_control_item.nav_cmd > 0) { + issue_command(_last_gimbal_control_item); + _last_gimbal_control_item = {}; // delete cached item + } + + if (_last_camera_mode_item.nav_cmd > 0) { + issue_command(_last_camera_mode_item); + _last_camera_mode_item = {}; // delete cached item + } +} + +void Mission::replayCachedTriggerItems() +{ + if (_last_camera_trigger_item.nav_cmd > 0) { + issue_command(_last_camera_trigger_item); + _last_camera_trigger_item = {}; // delete cached item + } +} + +void Mission::resetItemCache() +{ + _last_gimbal_configure_item = {}; + _last_gimbal_control_item = {}; + _last_camera_mode_item = {}; + _last_camera_trigger_item = {}; +} + +bool Mission::haveCachedGimbalOrCameraItems() +{ + return _last_gimbal_configure_item.nav_cmd > 0 || + _last_gimbal_control_item.nav_cmd > 0 || + _last_camera_mode_item.nav_cmd > 0; +} + +bool Mission::cameraWasTriggering() +{ + return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL + && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST + && _last_camera_trigger_item.params[0] > FLT_EPSILON); +} + +void Mission::updateCachedItemsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (success) { + cacheItem(mission_item); + } + } +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 0cdddae4fbe1..be6b217cb535 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -52,7 +52,7 @@ #include -#include +#include #include #include #include @@ -74,6 +74,11 @@ class Mission : public MissionBlock, public ModuleParams Mission(Navigator *navigator); ~Mission() override = default; + /** + * @brief function to call regularly to do background work + */ + void run(); + void on_inactive() override; void on_inactivation() override; void on_activation() override; @@ -234,12 +239,76 @@ class Mission : public MissionBlock, public ModuleParams /** * Return the index of the closest mission item to the current global position. */ - int32_t index_closest_mission_item() const; + int32_t index_closest_mission_item(); bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; void publish_navigator_mission_item(); + + /** + * @brief Get the index associated with the last item that contains a position + * @param mission The mission to search + * @param start_index The index to start searching from + * @param prev_pos_index The index of the previous position item containing a position + * @return true if a previous position item was found + */ + bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index) const; + + /** + * @brief Get the next item after start_index that contains a position + * + * @param mission The mission to search + * @param start_index The index to start searching from + * @param mission_item The mission item to populate + * @return true if successful + */ + bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item); + + /** + * @brief Cache the mission items containing gimbal, camera mode and trigger commands + * + * @param mission_item The mission item to cache if applicable + */ + void cacheItem(const mission_item_s &mission_item); + + /** + * @brief Update the cached items up to the given index + * + * @param end_index The index to update up to + */ + void updateCachedItemsUpToIndex(int end_index); + + /** + * @brief Replay the cached gimbal and camera mode items + */ + void replayCachedGimbalCameraItems(); + + /** + * @brief Replay the cached trigger items + * + */ + void replayCachedTriggerItems(); + + /** + * @brief Reset the item cache + */ + void resetItemCache(); + + /** + * @brief Check if there are cached gimbal or camera mode items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalOrCameraItems(); + + /** + * @brief Check if the camera was triggering + * + * @return true if there was a camera trigger command in the cached items that didn't disable triggering + */ + bool cameraWasTriggering(); + DEFINE_PARAMETERS( (ParamFloat) _param_mis_dist_1wp, (ParamInt) _param_mis_mnt_yaw_ctl @@ -250,6 +319,10 @@ class Mission : public MissionBlock, public ModuleParams uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ mission_s _mission {}; + static constexpr uint32_t DATAMAN_CACHE_SIZE = 10; + DatamanCache _dataman_cache{"mission_dm_cache_miss", DATAMAN_CACHE_SIZE}; + DatamanClient &_dataman_client = _dataman_cache.client(); + int32_t _load_mission_index{-1}; int32_t _current_mission_index{-1}; // track location of planned mission landing @@ -275,6 +348,7 @@ class Mission : public MissionBlock, public ModuleParams bool _inited{false}; bool _home_inited{false}; bool _need_mission_reset{false}; + bool _need_mission_save{false}; bool _mission_waypoints_changed{false}; bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ @@ -291,4 +365,12 @@ class Mission : public MissionBlock, public ModuleParams uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ bool _execution_mode_changed{false}; + + int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. + + mission_item_s _last_gimbal_configure_item {}; + mission_item_s _last_gimbal_control_item {}; + mission_item_s _last_camera_mode_item {}; + mission_item_s _last_camera_trigger_item {}; }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ba21850c6c8a..afe8f3ba00e0 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -78,9 +78,11 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { _navigator->get_mission_result()->warning = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; @@ -116,9 +118,11 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al if (_navigator->get_geofence().valid()) { for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 902d803eda22..0eedaf73668b 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include "MissionFeasibility/FeasibilityChecker.hpp" @@ -54,14 +54,16 @@ class MissionFeasibilityChecker: public ModuleParams { private: Navigator *_navigator{nullptr}; + DatamanClient &_dataman_client; FeasibilityChecker _feasibility_checker; bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); public: - MissionFeasibilityChecker(Navigator *navigator) : + MissionFeasibilityChecker(Navigator *navigator, DatamanClient &dataman_client) : ModuleParams(nullptr), _navigator(navigator), + _dataman_client(dataman_client), _feasibility_checker() { diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b62bffe3349b..2a5a95b953f9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -302,16 +302,18 @@ class Navigator : public ModuleBase, public ModuleParams void acquire_gimbal_control(); void release_gimbal_control(); - void calculate_breaking_stop(double &lat, double &lon, float &yaw); - void stop_capturing_images(); + void calculate_breaking_stop(double &lat, double &lon, float &yaw); + + void stop_capturing_images(); + void disable_camera_trigger(); void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); private: - int _local_pos_sub{-1}; - int _mission_sub{-1}; - int _vehicle_status_sub{-1}; + orb_sub_t _local_pos_sub{ORB_SUB_INVALID}; + orb_sub_t _mission_sub{ORB_SUB_INVALID}; + orb_sub_t _vehicle_status_sub{ORB_SUB_INVALID}; uORB::SubscriptionData _position_controller_status_sub{ORB_ID(position_controller_status)}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cbd0bd8fa2c7..33c35c7bc9f4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include #include @@ -168,6 +168,9 @@ void Navigator::run() fds[2].fd = _mission_sub; fds[2].events = POLLIN; + int16_t geofence_update_counter{0}; + int16_t safe_points_update_counter{0}; + /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -192,9 +195,18 @@ void Navigator::run() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); if (fds[2].revents & POLLIN) { - // copy mission to clear any update mission_s mission; orb_copy(ORB_ID(mission), _mission_sub, &mission); + + if (mission.geofence_update_counter != geofence_update_counter) { + geofence_update_counter = mission.geofence_update_counter; + _geofence.updateFence(); + } + + if (mission.safe_points_update_counter != safe_points_update_counter) { + safe_points_update_counter = mission.safe_points_update_counter; + _rtl.updateSafePoints(); + } } /* gps updated */ @@ -909,6 +921,10 @@ void Navigator::run() publish_mission_result(); } + _mission.run(); + _geofence.run(); + _rtl.run(); + perf_end(_loop_perf); } } @@ -1063,7 +1079,7 @@ int Navigator::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_NAVIGATION, - PX4_STACK_ADJUSTED(1952), + PX4_STACK_ADJUSTED(2300), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -1527,6 +1543,18 @@ void Navigator::mode_completed(uint8_t nav_state, uint8_t result) _mode_completed_pub.publish(mode_completed); } + +void Navigator::disable_camera_trigger() +{ + // Disable camera trigger + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + cmd.param3 = 1.0f; + publish_vehicle_cmd(&cmd); +} + int Navigator::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2a94c906198e..4ec188fd2336 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,7 +42,7 @@ #include "rtl.h" #include "navigator.h" -#include +#include #include #include @@ -67,6 +67,90 @@ RTL::RTL(Navigator *navigator) : _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); } +void RTL::run() +{ + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_safe_points_updated) { + _initiate_safe_points_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client.update(); + + if (_dataman_client.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _safe_points_updated = false; + + _dataman_cache.invalidate(); + + if (_dataman_cache.size() != _stats.num_items) { + _dataman_cache.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache.size(); ++index) { + _dataman_cache.load(DM_KEY_SAFE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache.update(); + + if (!_dataman_cache.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _safe_points_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Safe points update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + + } +} + void RTL::on_inactivation() { if (_navigator->get_precland()->is_activated()) { @@ -178,37 +262,34 @@ void RTL::find_RTL_destination() return; } - // compare to safe landing positions mission_safe_point_s closest_safe_point {}; - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - int num_safe_points = 0; - - if (ret == sizeof(mission_stats_entry_s)) { - num_safe_points = stats.num_items; - } - // check if a safe point is closer than home or landing int closest_index = 0; - for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { - mission_safe_point_s mission_safe_point; + if (_safe_points_updated) { - if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != - sizeof(mission_safe_point_s)) { - PX4_ERR("dm_read failed"); - continue; - } + for (int current_seq = 1; current_seq <= _dataman_cache.size(); ++current_seq) { + mission_safe_point_s mission_safe_point; - // TODO: take altitude into account for distance measurement - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); + bool success = _dataman_cache.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; + if (!success) { + PX4_ERR("dm_read failed"); + continue; + } + + // TODO: take altitude into account for distance measurement + dlat = mission_safe_point.lat - global_position.lat; + dlon = mission_safe_point.lon - global_position.lon; + double dist_squared = coord_dist_sq(dlat, dlon); + + if (dist_squared < min_dist_squared) { + closest_index = current_seq; + min_dist_squared = dist_squared; + closest_safe_point = mission_safe_point; + } } } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3c6dea1a2b7b..9c7fe55d5b52 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -53,6 +53,7 @@ #include #include #include +#include class Navigator; @@ -95,6 +96,11 @@ class RTL : public MissionBlock, public ModuleParams RTL_STATE_HEAD_TO_CENTER, }; + /** + * @brief function to call regularly to do background work + */ + void run(); + void on_inactivation() override; void on_inactive() override; void on_activation() override; @@ -116,6 +122,8 @@ class RTL : public MissionBlock, public ModuleParams void resetRtlState() { _rtl_state = RTL_STATE_NONE; } + void updateSafePoints() { _initiate_safe_points_updated = true; } + private: void set_rtl_item(); @@ -137,6 +145,14 @@ class RTL : public MissionBlock, public ModuleParams RTLState _rtl_state{RTL_STATE_NONE}; + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error + }; + struct RTLPosition { double lat; double lon; @@ -156,6 +172,16 @@ class RTL : public MissionBlock, public ModuleParams } }; + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated + bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache + DatamanCache _dataman_cache{"rtl_dm_cache_miss", 4}; + DatamanClient &_dataman_client = _dataman_cache.client(); + bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed + + mission_stats_entry_s _stats; + RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) hrt_abstime _destination_check_time{0}; diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 6218b09e00da..6c7a4a972468 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -933,9 +933,9 @@ Replay::run() subscription->compat = nullptr; } - if (subscription->orb_advert) { + if (orb_advert_valid(subscription->orb_advert)) { orb_unadvertise(subscription->orb_advert); - subscription->orb_advert = nullptr; + subscription->orb_advert = ORB_ADVERT_INVALID; } } @@ -1015,8 +1015,8 @@ Replay::publishTopic(Subscription &sub, void *data) data = sub.compat->apply(data); } - if (sub.orb_advert) { - orb_publish(sub.orb_meta, sub.orb_advert, data); + if (orb_advert_valid(sub.orb_advert)) { + orb_publish(sub.orb_meta, &sub.orb_advert, data); published = true; } else { @@ -1035,7 +1035,7 @@ Replay::publishTopic(Subscription &sub, void *data) if (subscription->orb_meta) { if (strcmp(sub.orb_meta->o_name, subscription->orb_meta->o_name) == 0 && - subscription->orb_advert && subscription->multi_id == sub.multi_id - 1) { + orb_advert_valid(subscription->orb_advert) && subscription->multi_id == sub.multi_id - 1) { advertised = true; } } diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 94ecc9d416fd..ea32a720a5ad 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -128,7 +128,7 @@ class Replay : public ModuleBase struct Subscription { const orb_metadata *orb_meta = nullptr; ///< if nullptr, this subscription is invalid - orb_advert_t orb_advert = nullptr; + orb_advert_t orb_advert = ORB_ADVERT_INVALID; uint8_t multi_id; int timestamp_offset; ///< marks the field of the timestamp diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..6b0406716aed 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -204,7 +204,10 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, bool setpoint = true; if ((_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_offboard_enabled) && pos_sp_triplet.current.valid) { + _control_mode.flag_control_offboard_enabled) && + pos_sp_triplet.current.valid && + PX4_ISFINITE(pos_sp_triplet.current.lat) && + PX4_ISFINITE(pos_sp_triplet.current.lon)) { /* AUTONOMOUS FLIGHT */ _control_mode_current = UGV_POSCTRL_MODE_AUTO; @@ -307,6 +310,9 @@ RoverPositionControl::control_position(const matrix::Vector2d ¤t_position, _prev_wp = curr_wp; } else { + // Other position control mode is not supported. Stop the rover. + _yaw_control = 0.0f; + _throttle_control = 0.0f; _control_mode_current = UGV_POSCTRL_MODE_OTHER; setpoint = false; } @@ -320,20 +326,46 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) const Vector3f desired_velocity{_trajectory_setpoint.velocity}; float dt = 0.01; // Using non zero value to a avoid division by zero + if (_control_velocity_last_called > 0) { + dt = hrt_elapsed_time(&_control_velocity_last_called) * 1e-6f; + } + + _control_velocity_last_called = hrt_absolute_time(); + const float mission_throttle = _param_throttle_cruise.get(); const float desired_speed = desired_velocity.norm(); + float desired_angular_vel = PX4_ISFINITE(_trajectory_setpoint.yawspeed) ? _trajectory_setpoint.yawspeed : + desired_velocity(1); - if (desired_speed > 0.01f) { + if (desired_speed > 0.01f || desired_angular_vel > 0.01f) { const Dcmf R_to_body(Quatf(_vehicle_att.q).inversed()); const Vector3f vel = R_to_body * Vector3f(current_velocity(0), current_velocity(1), current_velocity(2)); const float x_vel = vel(0); const float x_acc = _vehicle_acceleration_sub.get().xyz[0]; + float control_throttle = 0.0f; + float speed_error = desired_speed - x_vel; + + if (_param_speed_control_mode.get() == 0) { + // Use PID control + control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); - const float control_throttle = pid_calculate(&_speed_ctrl, desired_speed, x_vel, x_acc, dt); + } else if (_param_speed_control_mode.get() == 1) { + if (_param_ang_vel_control_mode.get() == 1) { + speed_error = desired_velocity(0) - x_vel; + } - //Constrain maximum throttle to mission throttle - _throttle_control = math::constrain(control_throttle, 0.0f, mission_throttle); + // Use acc limited direct control + float max_delta_speed = (speed_error > 0 ? _param_speed_acc_limit.get() : _param_speed_dec_limit.get()) * dt; + // Compute the velocity with delta speed and constrain it to GND_SPEED_TRIM + float command_velocity = math::constrain(x_vel + math::constrain(speed_error, -max_delta_speed, max_delta_speed), + -_param_gndspeed_trim.get(), _param_gndspeed_trim.get()); + // Compute the desired velocity and divide it by max speed to get the throttle control + control_throttle = command_velocity / _param_gndspeed_max.get(); + // Still multiplying it with scaler to have the support for simulation. Real hw has 1.0 scaler as default + _throttle_control = control_throttle; + } Vector3f desired_body_velocity; @@ -345,14 +377,21 @@ RoverPositionControl::control_velocity(const matrix::Vector3f ¤t_velocity) desired_body_velocity = R_to_body * desired_velocity; } - const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0)); - float control_effort = desired_theta / _param_max_turn_angle.get(); - control_effort = math::constrain(control_effort, -1.0f, 1.0f); - - _yaw_control = control_effort; + if (_param_ang_vel_control_mode.get() == 0) { + // Determine yaw from XY vector + const float desired_theta = atan2f(desired_body_velocity(1), desired_body_velocity(0)); + float control_effort = desired_theta / _param_max_turn_angle.get(); + control_effort = math::constrain(control_effort, -1.0f, 1.0f); + _yaw_control = control_effort; + + } else if (_param_ang_vel_control_mode.get() == 1) { + // Use direct yaw input from velocity setpoint + // Limit it to max anguler velocity + _yaw_control = math::constrain(desired_angular_vel, -_param_max_angular_velocity.get(), + _param_max_angular_velocity.get()); + } } else { - _throttle_control = 0.0f; _yaw_control = 0.0f; } @@ -414,14 +453,16 @@ RoverPositionControl::Run() // Convert Local setpoints to global setpoints if (_control_mode.flag_control_offboard_enabled) { - _trajectory_setpoint_sub.update(&_trajectory_setpoint); - - // local -> global - _global_local_proj_ref.reproject( - _trajectory_setpoint.position[0], _trajectory_setpoint.position[1], - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - - _pos_sp_triplet.current.valid = true; + //_trajectory_setpoint_sub.update(&_trajectory_setpoint); + if (_trajectory_setpoint_sub.update(&_trajectory_setpoint) && + matrix::Vector3f(_trajectory_setpoint.position).isAllFinite()) { + // local -> global + _global_local_proj_ref.reproject( + _trajectory_setpoint.position[0], _trajectory_setpoint.position[1], + _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); + + _pos_sp_triplet.current.valid = true; + } } // update the reset counters in any case diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index ee1c65efa03d..ace06ae9d8ba 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -133,6 +133,7 @@ class RoverPositionControl final : public ModuleBase, publ perf_counter_t _loop_perf; /**< loop performance counter */ hrt_abstime _control_position_last_called{0}; /**, publ (ParamFloat) _param_gndspeed_max, (ParamInt) _param_speed_control_mode, + (ParamInt) _param_vel_control_mode, + (ParamInt) _param_ang_vel_control_mode, (ParamFloat) _param_speed_p, (ParamFloat) _param_speed_i, (ParamFloat) _param_speed_d, @@ -190,7 +193,10 @@ class RoverPositionControl final : public ModuleBase, publ (ParamFloat) _param_throttle_cruise, (ParamFloat) _param_wheel_base, + (ParamFloat) _param_speed_acc_limit, + (ParamFloat) _param_speed_dec_limit, (ParamFloat) _param_max_turn_angle, + (ParamFloat) _param_max_angular_velocity, (ParamFloat) _param_gnd_man_y_max, (ParamFloat) _param_nav_loiter_rad /**< loiter radius for Rover */ ) diff --git a/src/modules/rover_pos_control/rover_pos_control_params.c b/src/modules/rover_pos_control/rover_pos_control_params.c index 6458fb1fea65..7106cc46b09c 100644 --- a/src/modules/rover_pos_control/rover_pos_control_params.c +++ b/src/modules/rover_pos_control/rover_pos_control_params.c @@ -164,6 +164,32 @@ PARAM_DEFINE_FLOAT(GND_THR_MIN, 0.0f); */ PARAM_DEFINE_INT32(GND_SP_CTRL_MODE, 1); +/** + * Control mode for speed + * + * This allows the user to choose between PID control or direct throttle control to work with + * rover_interface + * @min 0 + * @max 1 + * @value 0 PID control + * @value 1 Direct control + * @group Rover Position Control + */ +PARAM_DEFINE_INT32(GND_VEL_CTRL, 0); + +/** + * Control mode for angular velocity + * + * Whether the angular velocity will be done with XY vel command difference, or the angular + * velocity commands are given directly either in yawspeed or _trajectory_setpoint.velocity(1) + * @min 0 + * @max 1 + * @value 0 Difference in XY vel commands + * @value 1 Raw input from controller + * @group Rover Position Control + */ +PARAM_DEFINE_INT32(GND_ANG_VEL_CTRL, 0); + /** * Speed proportional gain * @@ -260,6 +286,42 @@ PARAM_DEFINE_FLOAT(GND_SPEED_TRIM, 3.0f); */ PARAM_DEFINE_FLOAT(GND_SPEED_MAX, 10.0f); +/** + * Velocity control acceleration limit + * + * @unit m/s^2 + * @min 0.01 + * @max 100.0 + * @decimal 2 + * @increment 0.05 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_ACC_LIMIT, 1.0f); + +/** + * Velocity control deceleration limit + * + * @unit m/s^2 + * @min 0.005 + * @max 100.0 + * @decimal 2 + * @increment 0.05 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_DEC_LIMIT, 1.0f); + +/** + * Limit the given angular velocity + * + * @unit rad/s + * @min 0.0 + * @max 3.14159 + * @decimal 3 + * @increment 0.01 + * @group Rover Position Control + */ +PARAM_DEFINE_FLOAT(GND_MAX_ANG_VEL, 0.4f); + /** * Maximum turn angle for Ackerman steering. * diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 7b94bd63bdef..6bfbee43dcdc 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -40,11 +40,12 @@ #include +#include + #include #include -GZBridge::GZBridge(const char *world, const char *name, const char *model, - const char *pose_str) : +GZBridge::GZBridge(const char *world, const char *name, const char *model, const char *pose_str) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), _world_name(world), @@ -55,6 +56,14 @@ GZBridge::GZBridge(const char *world, const char *name, const char *model, pthread_mutex_init(&_node_mutex, nullptr); updateParams(); + + // get current simulated vehicle airframe type + param_get(param_find("CA_AIRFRAME"), &_airframe); + + // get rover max speed + if (_airframe == 6) { + param_get(param_find("GND_SPEED_MAX"), &_rover_max_speed); + } } GZBridge::~GZBridge() @@ -152,8 +161,7 @@ int GZBridge::init() return PX4_ERROR; } - - // IMU: /world/$WORLD/model/$MODEL/link/base_link/sensor/imu_sensor/imu + // Odometry: /world/$WORLD/model/$MODEL/odometry_with_covariance std::string odometry_topic = "/model/" + _model_name + "/odometry_with_covariance"; if (!_node.Subscribe(odometry_topic, &GZBridge::odometryCallback, this)) { @@ -191,6 +199,17 @@ int GZBridge::init() return PX4_ERROR; } + // output (rover airframe type) eg /model/$MODEL_NAME/cmd_vel + if (_airframe == 6) { + std::string cmd_vel_topic = "/model/" + _model_name + "/cmd_vel"; + _cmd_vel_pub = _node.Advertise(cmd_vel_topic); + + if (!_cmd_vel_pub.Valid()) { + PX4_ERR("failed to advertise %s", cmd_vel_topic.c_str()); + return PX4_ERROR; + } + } + ScheduleNow(); return OK; } @@ -210,7 +229,7 @@ int GZBridge::task_spawn(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:v:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'w': // world @@ -664,6 +683,49 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry pthread_mutex_unlock(&_node_mutex); } +void GZBridge::updateCmdVel() +{ + bool do_update = false; + float rover_throttle_control = 0.0f; + float rover_yaw_control = 0.0f; + + // Check torque setppoint update + if (_vehicle_torque_setpoint_sub.updated()) { + vehicle_torque_setpoint_s vehicle_torque_setpoint_msg; + + if (_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint_msg)) { + rover_yaw_control = vehicle_torque_setpoint_msg.xyz[2]; + do_update = true; + } + } + + // Check thrust setpoint update + if (_vehicle_thrust_setpoint_sub.updated()) { + vehicle_thrust_setpoint_s vehicle_thrust_setpoint_msg; + + if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint_msg)) { + /** + * On physical rover the max speed scale back is only for mission mode + * But here, we simplify the logic and apply the scale back for both + * manual and mission mode + */ + rover_throttle_control = vehicle_thrust_setpoint_msg.xyz[0] * _rover_max_speed; + do_update = true; + } + } + + if (do_update) { + // publish cmd_vel + gz::msgs::Twist cmd_vel_message; + cmd_vel_message.mutable_linear()->set_x(rover_throttle_control); + cmd_vel_message.mutable_angular()->set_z(rover_yaw_control); + + if (_cmd_vel_pub.Valid()) { + _cmd_vel_pub.Publish(cmd_vel_message); + } + } +} + void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU) { // FLU (ROS) to FRD (PX4) static rotation @@ -706,6 +768,9 @@ void GZBridge::Run() _mixing_interface_servo.updateParams(); } + // In case of differential drive rover airframe type, publish gz cmd_vel + if (_airframe == 6) { updateCmdVel(); } + ScheduleDelayed(10_ms); pthread_mutex_unlock(&_node_mutex); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index b7d40eef36cd..0ed1ca3e4f9c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -54,6 +54,8 @@ #include #include #include +#include +#include #include #include @@ -95,6 +97,12 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec); + /** + * In case of rover: listen to vehicle thrust and torque setpoint and + * publish to simulated rover cmd_vel + */ + void updateCmdVel(); + void clockCallback(const gz::msgs::Clock &clock); // void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure); @@ -115,7 +123,10 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint), 50_ms}; + uORB::SubscriptionInterval _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint), 50_ms}; + // Publications //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; @@ -146,8 +157,14 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S const std::string _model_sim; const std::string _model_pose; + int32_t _airframe; + + float _rover_max_speed{0.0}; + float _temperature{288.15}; // 15 degrees + gz::transport::Node::Publisher _cmd_vel_pub; + gz::transport::Node _node; DEFINE_PARAMETERS( diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c index 513c67436cef..8f4a81eec170 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -36,10 +36,19 @@ * * @boolean * @reboot_required true - * @group UAVCAN + * @group Simulator */ PARAM_DEFINE_INT32(SIM_GZ_EN, 0); +/** + * Simulator Gazebo run enable + * + * @boolean + * @reboot_required true + * @group Simulator + */ +PARAM_DEFINE_INT32(SIM_GZ_RUN_GZSIM, 1); + /** * simulator origin latitude * diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 1e1b5be22c06..283e171ec37f 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -114,9 +114,9 @@ void SensorGpsSim::Run() vehicle_global_position_s gpos{}; _vehicle_global_position_sub.copy(&gpos); - double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - float altitude = gpos.alt + (generate_wgn() * 0.5f); + double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.1 / CONSTANTS_RADIUS_OF_EARTH); + double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.1 / CONSTANTS_RADIUS_OF_EARTH); + float altitude = gpos.alt + (generate_wgn() * 0.1f); Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index e3bc981805c1..2b8916277a4e 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -58,6 +58,7 @@ #include +static int _tcp_server_fd; static int _fd; static unsigned char _buf[2048]; static sockaddr_in _srcaddr; @@ -443,6 +444,7 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) // New publishers will be created based on the HIL_GPS ID's being different or not for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { + gps.device_id = _gps_dev_ids[i]; _sensor_gps_pubs[i]->publish(gps); break; } @@ -457,6 +459,7 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) device_id.devid_s.address = i; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; gps.device_id = device_id.devid; + _gps_dev_ids[i] = device_id.devid; _sensor_gps_pubs[i]->publish(gps); break; @@ -470,6 +473,9 @@ void SimulatorMavlink::handle_message_hil_sensor(const mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); + // Workaround for mavlink2 msg generator bug + imu.id = 0; + // Assume imu with id 0 is the primary imu an base lockstep based on this. if (imu.id == 0) { if (_lockstep_component == -1) { @@ -1085,6 +1091,7 @@ void SimulatorMavlink::run() if (_ip == InternetProtocol::UDP) { + // UDP 'server' mode if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { PX4_ERR("Creating UDP socket failed: %s", strerror(errno)); return; @@ -1113,8 +1120,80 @@ void SimulatorMavlink::run() PX4_INFO("Simulator connected on UDP port %u.", _port); + } else if (_server_mode) { + + // TCP Server mode + PX4_INFO("TCP Server: Waiting for simulator to connect on TCP port %u", _port); + + if ((_tcp_server_fd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + PX4_ERR("TCP Server: Creating TCP socket failed: %s", strerror(errno)); + return; + } + + int yes = 1; + int ret = setsockopt(_tcp_server_fd, IPPROTO_TCP, TCP_NODELAY, (char *) &yes, sizeof(int)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + struct linger nolinger {}; + + nolinger.l_onoff = 1; + + nolinger.l_linger = 0; + + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_LINGER, &nolinger, sizeof(nolinger)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + // The socket reuse is necessary for reconnecting to the same address + // if the socket does not close but gets stuck in TIME_WAIT. This can happen + // if the server is suddenly closed. + int socket_reuse = 1; + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_REUSEADDR, &socket_reuse, sizeof(socket_reuse)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + // Same as above but for a given port + ret = setsockopt(_tcp_server_fd, SOL_SOCKET, SO_REUSEPORT, &socket_reuse, sizeof(socket_reuse)); + + if (ret != 0) { + PX4_ERR("TCP Server: setsockopt failed: %s", strerror(errno)); + } + + socklen_t myaddr_len = sizeof(_myaddr); + + if (bind(_tcp_server_fd, (struct sockaddr *)&_myaddr, myaddr_len) < 0) { + PX4_ERR("TCP Server: Bind for TCP port %u failed: %s", _port, strerror(errno)); + ::close(_tcp_server_fd); + return; + } + + if (listen(_tcp_server_fd, 0) < 0) { + PX4_ERR("TCP Server: listen failed: %s", strerror(errno)); + } + + while (true) { + _fd = accept(_tcp_server_fd, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen); + + if (_fd >= 0) { + break; + } + + PX4_WARN("TCP Server: Accepting client connection failed: %s", strerror(errno)); + } + + PX4_INFO("TCP Server: Accepting TCP client connection from %s:%u", inet_ntoa(_srcaddr.sin_addr), + ntohs(_srcaddr.sin_port)); + } else { + // TCP Client mode PX4_INFO("Waiting for simulator to accept connection on TCP port %u", _port); while (true) { @@ -1558,6 +1637,12 @@ int SimulatorMavlink::start(int argc, char *argv[]) _instance->set_port(atoi(argv[4])); } + if (argc == 5 && strcmp(argv[3], "-s") == 0) { + _instance->set_ip(InternetProtocol::TCP); + _instance->set_port(atoi(argv[4])); + _instance->set_server_mode(); + } + if (argc == 6 && strcmp(argv[3], "-t") == 0) { PX4_INFO("using TCP on remote host %s port %s", argv[4], argv[5]); PX4_WARN("Please ensure port %s is not blocked by a firewall.", argv[5]); diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp index ee4118a302d1..dee8e88e83b6 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp @@ -129,6 +129,7 @@ class SimulatorMavlink : public ModuleParams void set_port(unsigned port) { _port = port; } void set_hostname(const char *hostname) { _hostname = hostname; } void set_tcp_remote_ipaddr(char *tcp_remote_ipaddr) { _tcp_remote_ipaddr = tcp_remote_ipaddr; } + void set_server_mode() { _server_mode = true; } #if defined(ENABLE_LOCKSTEP_SCHEDULER) bool has_initialized() { return _has_initialized.load(); } @@ -212,6 +213,8 @@ class SimulatorMavlink : public ModuleParams std::string _hostname{""}; + bool _server_mode{false}; + char *_tcp_remote_ipaddr{nullptr}; double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time @@ -260,10 +263,11 @@ class SimulatorMavlink : public ModuleParams static constexpr int MAX_GPS = 3; uORB::PublicationMulti *_sensor_gps_pubs[MAX_GPS] {}; uint8_t _gps_ids[MAX_GPS] {}; + uint32_t _gps_dev_ids[MAX_GPS] {}; std::default_random_engine _gen{}; // uORB subscription handlers - int _actuator_outputs_sub{-1}; + orb_sub_t _actuator_outputs_sub{ORB_SUB_INVALID}; actuator_outputs_s _actuator_outputs{}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index ee666d47574b..fd8da11891cc 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -645,7 +645,7 @@ int Sih::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("sih", SCHED_DEFAULT, SCHED_PRIORITY_MAX, - 1250, + 1560, (px4_main_t)&run_trampoline, (char *const *)argv); diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp index 9dc6c071f86f..f5328d705caa 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/accel.cpp @@ -68,7 +68,7 @@ TemperatureCalibrationAccel::~TemperatureCalibrationAccel() } } -int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.h b/src/modules/temperature_compensation/temperature_calibration/accel.h index f464bb0cb564..02bac51e3bfc 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.h +++ b/src/modules/temperature_compensation/temperature_calibration/accel.h @@ -49,7 +49,7 @@ class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> private: - virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual inline int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp index 4d1dc7be9050..3b573a3aee52 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/baro.cpp @@ -68,7 +68,7 @@ TemperatureCalibrationBaro::~TemperatureCalibrationBaro() } } -int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.h b/src/modules/temperature_compensation/temperature_calibration/baro.h index fc74e3033da4..226b7a7d77cd 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.h +++ b/src/modules/temperature_compensation/temperature_calibration/baro.h @@ -52,7 +52,7 @@ class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFI private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/common.h b/src/modules/temperature_compensation/temperature_calibration/common.h index 442ed01af310..dae2ee1f4580 100644 --- a/src/modules/temperature_compensation/temperature_calibration/common.h +++ b/src/modules/temperature_compensation/temperature_calibration/common.h @@ -188,8 +188,8 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase * update a single sensor instance * @return 0 when done, 1 not finished yet, <0 for an error */ - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) = 0; unsigned _num_sensor_instances{0}; - int _sensor_subs[SENSOR_COUNT_MAX]; + orb_sub_t _sensor_subs[SENSOR_COUNT_MAX]; }; diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp index 0f0f9c4a1efc..e75637333765 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp @@ -45,7 +45,7 @@ #include TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, - float max_start_temperature, int gyro_subs[], int num_gyros) + float max_start_temperature, orb_sub_t gyro_subs[], int num_gyros) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { for (int i = 0; i < num_gyros; ++i) { @@ -55,7 +55,7 @@ TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_ris _num_sensor_instances = num_gyros; } -int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int sensor_sub) +int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub) { bool finished = data.hot_soaked; diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.h b/src/modules/temperature_compensation/temperature_calibration/gyro.h index c6f9a5df47f3..f64e5cd0a939 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.h +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.h @@ -40,7 +40,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, - int gyro_subs[], int num_gyros); + orb_sub_t gyro_subs[], int num_gyros); virtual ~TemperatureCalibrationGyro() {} /** @@ -50,7 +50,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> private: - virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, orb_sub_t sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); }; diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 7328784522f7..c9e298b94088 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -100,7 +100,7 @@ class TemperatureCalibration void TemperatureCalibration::task_main() { // subscribe to all gyro instances - int gyro_sub[SENSOR_COUNT_MAX] {-1, -1, -1}; + orb_sub_t gyro_sub[SENSOR_COUNT_MAX] {ORB_SUB_INVALID, ORB_SUB_INVALID, ORB_SUB_INVALID}; px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] {}; unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro)); diff --git a/src/modules/uxrce_dds_client/CMakeLists.txt b/src/modules/uxrce_dds_client/CMakeLists.txt index 2150f9456fd7..c6e407c74746 100644 --- a/src/modules/uxrce_dds_client/CMakeLists.txt +++ b/src/modules/uxrce_dds_client/CMakeLists.txt @@ -113,13 +113,18 @@ else() add_dependencies(microxrceddsclient libmicroxrceddsclient_project) target_include_directories(microxrceddsclient INTERFACE ${microxrceddsclient_build_dir}/include) - + set(microxrceddsclient_topics_yaml ${PX4_BOARD_DIR}/src/${CONFIG_UXRCE_DDS_TOPICS_YAML}) + if(NOT EXISTS "${microxrceddsclient_topics_yaml}") + set(microxrceddsclient_topics_yaml ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml) + endif() + message(STATUS "microdds_client: use dds topics yaml: '${microxrceddsclient_topics_yaml}'") add_custom_command(OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/dds_topics.h COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py + --camel-case-topic-names --topic-msg-dir ${PX4_SOURCE_DIR}/msg --client-outdir ${CMAKE_CURRENT_BINARY_DIR} - --dds-topics-file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.yaml + --dds-topics-file ${microxrceddsclient_topics_yaml} --template_file ${CMAKE_CURRENT_SOURCE_DIR}/dds_topics.h.em DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_dds_topics.py diff --git a/src/modules/uxrce_dds_client/Kconfig b/src/modules/uxrce_dds_client/Kconfig index 6573e0cc0b82..9cbd0be16642 100644 --- a/src/modules/uxrce_dds_client/Kconfig +++ b/src/modules/uxrce_dds_client/Kconfig @@ -3,3 +3,17 @@ menuconfig MODULES_UXRCE_DDS_CLIENT default n ---help--- Enable support for the UXRCE-DDS Client + +menuconfig USER_UXRCE_DDS_CLIENT + bool "uxrce_dds_client running as userspace module" + default y + depends on BOARD_PROTECTED && MODULES_UXRCE_DDS_CLIENT + ---help--- + Put uxrce_dds_client in userspace memory + +menuconfig UXRCE_DDS_TOPICS_YAML + string "dds topics .yaml file name" + default "dds_topics.yaml" + depends on MODULES_UXRCE_DDS_CLIENT + ---help--- + Filename for yaml file for generating topics sources diff --git a/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client b/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client index 4248559f3b11..d44682c0e065 160000 --- a/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client +++ b/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client @@ -1 +1 @@ -Subproject commit 4248559f3b111155c783e524e461ccc83e768103 +Subproject commit d44682c0e065dc5d35d04d053ee211fed3130738 diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index dc79e0fabdb1..ea217536040d 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -23,19 +23,28 @@ import os #include #include + +#include @[for include in type_includes]@ #include +#include @[end for]@ // Subscribers for messages to send struct SendTopicsSubs { @[ for pub in publications]@ - uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))}; uxrObjectId @(pub['topic_simple'])_data_writer{}; @[ end for]@ +@[ for pub in publications]@ + orb_sub_t @(pub['topic_simple'])_sub_fd; +@[ end for]@ + + px4_pollfd_struct_t fds[@(len(publications))] {}; + uint32_t num_payload_sent{}; + void init(); void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); void reset(); }; @@ -47,40 +56,58 @@ void SendTopicsSubs::reset() { @[ end for]@ }; +void SendTopicsSubs::init() +{ +@[ for pub in publications]@ + @(pub['topic_simple'])_sub_fd = orb_subscribe(ORB_ID(@(pub['topic_simple']))); +@[ if pub['topic_pub_thold_us'] > 0 ]@ + orb_set_interval(@(pub['topic_simple'])_sub_fd, @(pub['topic_pub_thold_us'])); +@[ else]@ + orb_set_interval(@(pub['topic_simple'])_sub_fd, 10); +@[ end if ]@ +@[ end for]@ + +@[ for idx, pub in enumerate(publications)]@ + fds[@(idx)].fd = @(pub['topic_simple'])_sub_fd; + fds[@(idx)].events = POLLIN; +@[ end for]@ + +} + void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace) { int64_t time_offset_us = session->time_offset / 1000; // ns -> us @[ for idx, pub in enumerate(publications)]@ + if (fds[@(idx)].revents & POLLIN) { @(pub['simple_base_type'])_s data; - if (@(pub['topic_simple'])_sub.update(&data)) { + orb_copy( ORB_ID(@(pub['simple_base_type'])), @(pub['topic_simple'])_sub_fd, &data); - if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { - // data writer not created yet - create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); - } - - if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + // data writer not created yet + create_data_writer(session, reliable_out_stream_id, participant_id, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_name'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + } - ucdrBuffer ub; - uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); - if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { - ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); - // TODO: fill up the MTU and then flush, which reduces the packet overhead - uxr_flash_output_streams(session); - num_payload_sent += topic_size; + if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { - } else { - //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); - } + ucdrBuffer ub; + uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); + if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + // TODO: fill up the MTU and then flush, which reduces the packet overhead + uxr_flash_output_streams(session); + num_payload_sent += topic_size; } else { - //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); } + } else { + //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); } + } @[ end for]@ } @@ -128,7 +155,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id @[ for idx, sub in enumerate(subscriptions)]@ { uint16_t queue_depth = uORB::DefaultQueueSize<@(sub['simple_base_type'])_s>::value * 2; // use a bit larger queue size than internal - create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_simple'])", "@(sub['dds_type'])", queue_depth); + create_data_reader(session, reliable_out_stream_id, best_effort_in_stream_id, participant_id, @(idx), client_namespace, "@(sub['topic_name'])", "@(sub['dds_type'])", queue_depth); } @[ end for]@ diff --git a/src/modules/uxrce_dds_client/dds_topics.yaml b/src/modules/uxrce_dds_client/dds_topics.yaml index 8b62f01cdb3f..892500a1e0cf 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -11,9 +11,6 @@ publications: - topic: /fmu/out/failsafe_flags type: px4_msgs::msg::FailsafeFlags - - topic: /fmu/out/position_setpoint_triplet - type: px4_msgs::msg::PositionSetpointTriplet - - topic: /fmu/out/sensor_combined type: px4_msgs::msg::SensorCombined @@ -47,6 +44,29 @@ publications: - topic: /fmu/out/vehicle_trajectory_waypoint_desired type: px4_msgs::msg::VehicleTrajectoryWaypoint +# messages added by ssrc + + - topic: /fmu/out/battery_status + type: px4_msgs::msg::BatteryStatus + + - topic: /fmu/out/distance_sensor + type: px4_msgs::msg::DistanceSensor + + - topic: /fmu/out/mission_result + type: px4_msgs::msg::MissionResult + + - topic: /fmu/out/sensor_baro + type: px4_msgs::msg::SensorBaro + + - topic: /fmu/out/sensor_mag + type: px4_msgs::msg::SensorMag + + - topic: /fmu/out/vehicle_land_detected + type: px4_msgs::msg::VehicleLandDetected + + - topic: /fmu/out/home_position + type: px4_msgs::msg::HomePosition + subscriptions: - topic: /fmu/in/offboard_control_mode @@ -87,3 +107,11 @@ subscriptions: - topic: /fmu/in/vehicle_trajectory_waypoint type: px4_msgs::msg::VehicleTrajectoryWaypoint + +# messages added by ssrc + + - topic: /fmu/in/sensor_gps + type: px4_msgs::msg::SensorGps + + - topic: /fmu/in/gps_inject_data + type: px4_msgs::msg::GpsInjectData diff --git a/src/modules/uxrce_dds_client/generate_dds_topics.py b/src/modules/uxrce_dds_client/generate_dds_topics.py index 75fd6dbd9cee..29016b6fc73e 100644 --- a/src/modules/uxrce_dds_client/generate_dds_topics.py +++ b/src/modules/uxrce_dds_client/generate_dds_topics.py @@ -43,6 +43,9 @@ import yaml parser = argparse.ArgumentParser() +parser.add_argument("-c", "--camel-case-topic-names", dest='camelcase', action='store_true', + help="Use CamelCase topic names instead of snake_case") + parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str, help="Topics message, by default using relative path 'msg/'", default="msg") @@ -102,6 +105,20 @@ # topic_simple: eg vehicle_status p['topic_simple'] = p['topic'].split('/')[-1] + if args.camelcase: + simple_topic_name = p['topic_simple'] + topic_name_camel_case = ''.join(part.title() for part in simple_topic_name.split('_')) + p['topic_name'] = topic_name_camel_case + else: + p['topic_name'] = p['topic_simple'] + + # topic_pub_thold: eg. rate: 20Hz => topic_pub_thold_us = 50 (ms) + if 'rate' in p: + p['topic_pub_thold_us'] = int(1000 / p['rate']) + print(p['topic_name'] + " rate: " + str(p['rate']) + " => thold_us: " + str(p['topic_pub_thold_us'])) + else: + print(p['topic_name'] + " rate: N/A") + p['topic_pub_thold_us'] = 0 merged_em_globals['publications'] = msg_map['publications'] @@ -122,6 +139,13 @@ # topic_simple: eg vehicle_status s['topic_simple'] = s['topic'].split('/')[-1] + if args.camelcase: + simple_topic_name = s['topic_simple'] + topic_name_camel_case = ''.join(part.title() for part in simple_topic_name.split('_')) + s['topic_name'] = topic_name_camel_case + else: + s['topic_name'] = s['topic_simple'] + merged_em_globals['subscriptions'] = msg_map['subscriptions'] diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 91e1996fb878..c39464e5cac7 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -54,23 +54,39 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str // data writer + char datawriter_xml[1024]; datawriter_id = uxr_object_id(topic_id.id, UXR_DATAWRITER_ID); + const char *datawriter_xml_format = "" + "" + "" + "NO_KEY" + "%s" + "px4_msgs::msg::dds_::%s_" + "" + "" + "" + "ASYNCHRONOUS" + "" + "" + "PREALLOCATED_WITH_REALLOC" + "" + ""; + + int ret = snprintf(datawriter_xml, 1024, datawriter_xml_format, topic_name, topic_name_simple); + + if (ret < 0) { + PX4_ERR("Can't create datawriter_xml string"); + return false; + } - uxrQoS_t qos = { - .durability = UXR_DURABILITY_TRANSIENT_LOCAL, - .reliability = UXR_RELIABILITY_BEST_EFFORT, - .history = UXR_HISTORY_KEEP_LAST, - .depth = 0, - }; - - uint16_t datawriter_req = uxr_buffer_create_datawriter_bin(session, reliable_out_stream_id, datawriter_id, publisher_id, - topic_id, qos, UXR_REPLACE); + uint16_t datawriter_req = uxr_buffer_create_datawriter_xml(session, reliable_out_stream_id, datawriter_id, publisher_id, + datawriter_xml, UXR_REPLACE); // Send create entities message and wait its status uint16_t requests[3] {topic_req, publisher_req, datawriter_req}; uint8_t status[3]; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + if (!uxr_run_session_until_all_status(session, 5000, requests, status, 3)) { PX4_ERR("create entities failed: %s, topic: %i publisher: %i datawriter: %i", topic_name, status[0], status[1], status[2]); return false; @@ -109,22 +125,33 @@ static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_str // data reader + char datareader_xml[1024]; uxrObjectId datareader_id = uxr_object_id(id, UXR_DATAREADER_ID); + const char *datareader_xml_format = "" + "" + "" + "NO_KEY" + "%s" + "px4_msgs::msg::dds_::%s_" + "" + "PREALLOCATED_WITH_REALLOC" + "" + ""; + + int ret = snprintf(datareader_xml, 1024, datareader_xml_format, topic_name, topic_name_simple); + + if (ret < 0) { + PX4_ERR("Can't create datareader_xml string"); + return false; + } - uxrQoS_t qos = { - .durability = UXR_DURABILITY_VOLATILE, - .reliability = UXR_RELIABILITY_BEST_EFFORT, - .history = UXR_HISTORY_KEEP_LAST, - .depth = queue_depth, - }; - - uint16_t datareader_req = uxr_buffer_create_datareader_bin(session, reliable_out_stream_id, datareader_id, - subscriber_id, topic_id, qos, UXR_REPLACE); + uint16_t datareader_req = uxr_buffer_create_datareader_xml(session, reliable_out_stream_id, datareader_id, + subscriber_id, datareader_xml, UXR_REPLACE); uint16_t requests[3] {topic_req, subscriber_req, datareader_req}; uint8_t status[3]; - if (!uxr_run_session_until_all_status(session, 1000, requests, status, 3)) { + if (!uxr_run_session_until_all_status(session, 5000, requests, status, 3)) { PX4_ERR("create entities failed: %s %i %i %i", topic_name, status[0], status[1], status[2]); return false; diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index b9f1d066e41f..592329190eb3 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -80,7 +80,8 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta } UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, - const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) : + const char *recv_port, const char *send_port, bool localhost_only, bool custom_participant, + const char *client_namespace) : ModuleParams(nullptr), _localhost_only(localhost_only), _custom_participant(custom_participant), _client_namespace(client_namespace) @@ -122,11 +123,12 @@ UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baud #if defined(UXRCE_DDS_CLIENT_UDP) _transport_udp = new uxrUDPTransport(); - strncpy(_port, port, PORT_MAX_LENGTH - 1); + strncpy(_send_port, send_port, PORT_MAX_LENGTH - 1); + strncpy(_recv_port, recv_port, PORT_MAX_LENGTH - 1); strncpy(_agent_ip, agent_ip, AGENT_IP_MAX_LENGTH - 1); if (_transport_udp) { - if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _port)) { + if (uxr_init_udp_transport(_transport_udp, UXR_IPv4, _agent_ip, _recv_port, _send_port)) { _comm = &_transport_udp->comm; _fd = _transport_udp->platform.poll_fd.fd; @@ -174,6 +176,7 @@ void UxrceddsClient::run() while (!should_exit()) { bool got_response = false; + PX4_INFO("Waiting for ping response..."); while (!should_exit() && !got_response) { // Sending ping without initing a XRCE session @@ -199,7 +202,7 @@ void UxrceddsClient::run() // void uxr_create_session_retries(uxrSession* session, size_t retries); if (!uxr_create_session(&session)) { PX4_ERR("uxr_create_session failed"); - return; + continue; } // TODO: uxr_set_status_callback @@ -228,72 +231,20 @@ void UxrceddsClient::run() uint16_t domain_id = _param_xrce_dds_dom_id.get(); - // const char *participant_name = "px4_micro_xrce_dds"; - // uint16_t participant_req = uxr_buffer_create_participant_bin(&session, reliable_out, participant_id, domain_id, - // participant_name, UXR_REPLACE); - - char participant_xml[PARTICIPANT_XML_SIZE]; - int ret = snprintf(participant_xml, PARTICIPANT_XML_SIZE, "%s%s/px4_micro_xrce_dds%s", - _localhost_only ? - "" - "" - "" - "" - "udp_localhost" - "UDPv4" - "
127.0.0.1
" - "" - "" - "" - "" - "" - : - "" - "" - "", - _client_namespace != nullptr ? - _client_namespace - : - "", - _localhost_only ? - "false" - "udp_localhost" - "" - "" - "" - : - "" - "" - "" - ); - - if (ret < 0 || ret >= PARTICIPANT_XML_SIZE) { - PX4_ERR("create entities failed: namespace too long"); - return; - } - - - uint16_t participant_req{}; - - if (_custom_participant) { - participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id, - "px4_participant", UXR_REPLACE); - - } else { - participant_req = uxr_buffer_create_participant_xml(&session, reliable_out, participant_id, domain_id, - participant_xml, UXR_REPLACE); - } + const char *participant_ref = "default_xrce_participant"; + uint16_t participant_req = uxr_buffer_create_participant_ref(&session, reliable_out, participant_id, domain_id, + participant_ref, UXR_REPLACE); uint8_t request_status; if (!uxr_run_session_until_all_status(&session, 1000, &participant_req, &request_status, 1)) { PX4_ERR("create entities failed: participant: %i", request_status); - return; + continue; } if (!_pubs->init(&session, reliable_out, reliable_in, best_effort_in, participant_id, _client_namespace)) { PX4_ERR("pubs init failed"); - return; + continue; } _connected = true; @@ -323,9 +274,31 @@ void UxrceddsClient::run() bool had_ping_reply = false; uint32_t last_num_payload_sent{}; uint32_t last_num_payload_received{}; + int poll_error_counter = 0; + + _subs->init(); while (!should_exit() && _connected) { + /* wait for sensor update for 1000 ms (1 second) */ + int poll_ret = px4_poll(&_subs->fds[0], (sizeof(_subs->fds) / sizeof(_subs->fds[0])), 1000); + + /* handle the poll result */ + if (poll_ret == 0) { + /* Timeout, no updates in selected uorbs */ + continue; + + } else if (poll_ret < 0) { + /* this is seriously bad - should be an emergency */ + if (poll_error_counter < 10 || poll_error_counter % 50 == 0) { + /* use a counter to prevent flooding (and slowing us down) */ + PX4_ERR("ERROR while polling uorbs: %d", poll_ret); + } + + poll_error_counter++; + continue; + } + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); uxr_run_session_timeout(&session, 0); @@ -375,14 +348,28 @@ void UxrceddsClient::run() PX4_INFO("No ping response, disconnecting"); _connected = false; } - - px4_usleep(1000); } uxr_delete_session_retries(&session, _connected ? 1 : 0); _last_payload_tx_rate = 0; _last_payload_tx_rate = 0; - _subs->reset(); + + if (_subs) { + delete _subs; + } + + _subs = new SendTopicsSubs(); + + if (_pubs) { + delete _pubs; + } + + _pubs = new RcvTopicsPubs(); + + if (!_subs || !_pubs) { + PX4_ERR("alloc failed"); + return; + } } } @@ -553,7 +540,7 @@ int UxrceddsClient::print_status() if (_transport_udp != nullptr) { PX4_INFO("Using transport: udp"); PX4_INFO("Agent IP: %s", _agent_ip); - PX4_INFO("Agent port: %s", _port); + PX4_INFO("Agent port: %s", _send_port); } @@ -578,7 +565,8 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; - char port[PORT_MAX_LENGTH] = {0}; + char recv_port[PORT_MAX_LENGTH] = {0}; + char send_port[PORT_MAX_LENGTH] = {'8', '8', '8', '8'}; char agent_ip[AGENT_IP_MAX_LENGTH] = {0}; #if defined(UXRCE_DDS_CLIENT_UDP) @@ -594,7 +582,7 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) const char *client_namespace = nullptr;//"px4"; - while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:lcn:", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "t:d:b:h:p:r:lcn:", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 't': if (!strcmp(myoptarg, "serial")) { @@ -629,7 +617,11 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) break; case 'p': - snprintf(port, PORT_MAX_LENGTH, "%s", myoptarg); + snprintf(send_port, PORT_MAX_LENGTH, "%s", myoptarg); + break; + + case 'r': + snprintf(recv_port, PORT_MAX_LENGTH, "%s", myoptarg); break; case 'l': @@ -658,7 +650,7 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) #if defined(UXRCE_DDS_CLIENT_UDP) - if (port[0] == '\0') { + if (send_port[0] == '\0') { // no port specified, use UXRCE_DDS_PRT int32_t port_i = 0; param_get(param_find("UXRCE_DDS_PRT"), &port_i); @@ -668,7 +660,7 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) return nullptr; } - snprintf(port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i); + snprintf(send_port, PORT_MAX_LENGTH, "%u", (uint16_t)port_i); } if (agent_ip[0] == '\0') { @@ -694,7 +686,8 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) } } - return new UxrceddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant, + return new UxrceddsClient(transport, device, baudrate, agent_ip, recv_port, send_port, localhost_only, + custom_participant, client_namespace); } @@ -721,6 +714,7 @@ UXRCE-DDS Client used to communicate uORB topics with an Agent over serial or UD PRINT_MODULE_USAGE_PARAM_INT('b', 0, 0, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_STRING('h', nullptr, "", "Agent IP. If not provided, defaults to UXRCE_DDS_AG_IP", true); PRINT_MODULE_USAGE_PARAM_INT('p', -1, 0, 65535, "Agent listening port. If not provided, defaults to UXRCE_DDS_PRT", true); + PRINT_MODULE_USAGE_PARAM_INT('r', 0, 0, 65536, "Local Port", true); PRINT_MODULE_USAGE_PARAM_FLAG('l', "Restrict to localhost (use in combination with ROS_LOCALHOST_ONLY=1)", true); PRINT_MODULE_USAGE_PARAM_FLAG('c', "Use custom participant config (profile_name=\"px4_participant\")", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, nullptr, "Client DDS namespace", true); diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index aab381b73dc6..16ae4f4550fe 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -48,8 +48,8 @@ class UxrceddsClient : public ModuleBase, public ModuleParams Udp }; - UxrceddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only, bool custom_participant, const char *client_namespace); + UxrceddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *recv_port, + const char *send_port, bool localhost_only, bool custom_participant, const char *client_namespace); ~UxrceddsClient(); @@ -85,7 +85,8 @@ class UxrceddsClient : public ModuleBase, public ModuleParams static const uint8_t AGENT_IP_MAX_LENGTH = 16; #if defined(CONFIG_NET) || defined(__PX4_POSIX) - char _port[PORT_MAX_LENGTH]; + char _send_port[PORT_MAX_LENGTH]; + char _recv_port[PORT_MAX_LENGTH]; char _agent_ip[AGENT_IP_MAX_LENGTH]; #endif diff --git a/src/systemcmds/launch_kmod/CMakeLists.txt b/src/systemcmds/launch_kmod/CMakeLists.txt index e61fa90845ac..0d1022a1cc4b 100644 --- a/src/systemcmds/launch_kmod/CMakeLists.txt +++ b/src/systemcmds/launch_kmod/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE systemcmds__launch_kmod MAIN launch_kmod COMPILE_FLAGS + NO_DAEMON SRCS launch_kmod.cpp ) diff --git a/src/systemcmds/mtd/Kconfig b/src/systemcmds/mtd/Kconfig index 661e8ee7f68e..d303b34427c7 100644 --- a/src/systemcmds/mtd/Kconfig +++ b/src/systemcmds/mtd/Kconfig @@ -6,7 +6,7 @@ menuconfig SYSTEMCMDS_MTD menuconfig USER_MTD bool "mtd running as userspace module" - default y + default n depends on BOARD_PROTECTED && SYSTEMCMDS_MTD ---help--- Put mtd in userspace memory diff --git a/src/systemcmds/netconfig/CMakeLists.txt b/src/systemcmds/netconfig/CMakeLists.txt new file mode 100644 index 000000000000..e4cec6cd4782 --- /dev/null +++ b/src/systemcmds/netconfig/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2023 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ +px4_add_module( + MODULE modules__netconfig + MAIN netconfig + NO_DAEMON + SRCS + netconfig.cpp + ) diff --git a/src/systemcmds/netconfig/Kconfig b/src/systemcmds/netconfig/Kconfig new file mode 100644 index 000000000000..b99da8811ad6 --- /dev/null +++ b/src/systemcmds/netconfig/Kconfig @@ -0,0 +1,42 @@ +menuconfig SYSTEMCMDS_NETCONFIG + bool "netconfig" + default n + ---help--- + Enable support for netconfig + +menuconfig USER_NETCONFIG + bool "netconfig running as userspace module" + default y + depends on BOARD_PROTECTED && SYSTEMCMDS_NETCONFIG + ---help--- + Put netconfig in userspace memory + +if SYSTEMCMDS_NETCONFIG + +comment "IPv4 Addresses" + +config NETCONFIG_IFNAME + string "Target net interface" + default "eth0" + ---help--- + Set the network interface name for the PX4. Default "eth0" + +config NETCONFIG_IPSUBNET + hex "Target IP subnet" + default 0x00c9a8c0 + ---help--- + Set the subnet for the PX4. The actual IP is generated from this mask and MAV_SYS_ID. Default 192.168.201.0 + +config NETCONFIG_DRIPADDR + hex "Router IPv4 address" + default 0x01c9a8c0 + ---help--- + Set the subnet mask for the PX4. Default 192.168.201.1 + +config NETCONFIG_NETMASK + hex "Target IP subnet mask" + default 0x00ffffff + ---help--- + Set the subnet mask for the PX4 + +endif # SYSTEMCMDS_NETCONFIG diff --git a/src/systemcmds/netconfig/netconfig.cpp b/src/systemcmds/netconfig/netconfig.cpp new file mode 100644 index 000000000000..7f54ce1da2ee --- /dev/null +++ b/src/systemcmds/netconfig/netconfig.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ +/** + * @file netconfig.cpp + * Simple network configuration + * + * @author Jukka Laitinen + */ + +#include +#include +#include +#include +#include + +__BEGIN_DECLS +__EXPORT int netconfig_main(int argc, char *argv[]); +__END_DECLS + +int netconfig_main(int argc, char *argv[]) +{ + struct in_addr addr; + int mav_id; + const char ifname[] = CONFIG_NETCONFIG_IFNAME; + + param_get(param_find("MAV_SYS_ID"), &mav_id); + + if (mav_id < 1) { + return PX4_ERROR; + } + + /* IP: CONFIG_NETCONFIG_IPSUBNET + mav_id */ + + addr.s_addr = CONFIG_NETCONFIG_IPSUBNET; + + mav_id += 100; + + if (mav_id > 253) { + return PX4_ERROR; + } + + addr.s_addr |= ((uint32_t)mav_id << 24); + netlib_set_ipv4addr(ifname, &addr); + + /* GW */ + + addr.s_addr = CONFIG_NETCONFIG_DRIPADDR; + netlib_set_dripv4addr(ifname, &addr); + + /* netmask */ + + addr.s_addr = CONFIG_NETCONFIG_NETMASK; + netlib_set_ipv4netmask(ifname, &addr); + + netlib_ifup(ifname); + + return PX4_OK; +} diff --git a/src/systemcmds/rdfeat/CMakeLists.txt b/src/systemcmds/rdfeat/CMakeLists.txt new file mode 100644 index 000000000000..7551b12ecdb8 --- /dev/null +++ b/src/systemcmds/rdfeat/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2023 Technology Innovation Institute. All rights reserved. +# +# 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. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# 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 OWNER 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__rdfeat + MAIN rdfeat + COMPILE_FLAGS + SRCS + rdfeat.cpp + DEPENDS + ) diff --git a/src/systemcmds/rdfeat/Kconfig b/src/systemcmds/rdfeat/Kconfig new file mode 100644 index 000000000000..6ae336f4860f --- /dev/null +++ b/src/systemcmds/rdfeat/Kconfig @@ -0,0 +1,5 @@ +menuconfig SYSTEMCMDS_RDFEAT + bool "rdfeat" + default y + ---help--- + Enable support for rdfeat diff --git a/src/systemcmds/rdfeat/rdfeat.cpp b/src/systemcmds/rdfeat/rdfeat.cpp new file mode 100644 index 000000000000..bfff55ab6d8a --- /dev/null +++ b/src/systemcmds/rdfeat/rdfeat.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** +* +* Copyright (c) 2023 Technology Innovation Institute. All rights reserved. +* +* 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. +* 3. Neither the name PX4 nor the names of its contributors may be +* used to endorse or promote products derived from this software +* without specific prior written permission. +* +* 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 OWNER 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. +* +****************************************************************************/ + +/** +* @file rdfeat.c +* +* rdfeat command, used to check RD feature flags on console, command line or +* scripts +* +* @author Jukka Laitinen +*/ + +#include +#include +#include +#include + +#ifdef BOARD_RD_FEATURE_FLAGS +static const char cmp_str[] = "cmp"; +static const char print_str[] = "print"; + +static void usage(const char *reason) +{ + if (reason != nullptr) { + PX4_INFO_RAW("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Tool to check R&D feature flag on command line"); + + PRINT_MODULE_USAGE_NAME("rdfeat", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("cmp", + "Check a specific flag (string). Returns 0 on match. Otherwise returns 1"); + PRINT_MODULE_USAGE_ARG("flag", "Flag to check", ""); + PRINT_MODULE_USAGE_COMMAND_DESCR("print", "Outputs all possible flags and the current setting"); +} +#endif + +extern "C" __EXPORT int rdfeat_main(int argc, char *argv[]) +{ +#ifndef BOARD_RD_FEATURE_FLAGS + return 0; +#else + int ret = 1; + unsigned long flags = BOARD_RD_FEATURE_FLAGS; + const size_t num_flags = sizeof(rdct_feat_strings) / sizeof(rdct_feat_strings[0]); + + if (argc < 2 || argc > 3) { + usage("Invalid number of arguments"); + return ret; + } + + if (!strncmp(argv[1], print_str, sizeof(print_str))) { + for (size_t i = 0; i < num_flags; i++) { + PX4_INFO("%s: %s\n", rdct_feat_strings[i].s, (rdct_feat_strings[i].v & flags) != 0 ? "Enabled" : "Disabled"); + ret = 0; + } + } + + else if (argc == 3 && !strncmp(argv[1], cmp_str, sizeof(cmp_str))) { + for (size_t i = 0; i < num_flags; i++) { + if (!strcmp(rdct_feat_strings[i].s, argv[2]) && (rdct_feat_strings[i].v & flags) != 0) { + ret = 0; + } + } + + } else { + usage("Invalid command"); + } + + return ret; +#endif +} diff --git a/src/systemcmds/reboot/CMakeLists.txt b/src/systemcmds/reboot/CMakeLists.txt index 60f4cee88951..ecc7b64870a2 100644 --- a/src/systemcmds/reboot/CMakeLists.txt +++ b/src/systemcmds/reboot/CMakeLists.txt @@ -36,7 +36,5 @@ px4_add_module( COMPILE_FLAGS SRCS reboot.cpp - DEPENDS - px4_platform ) diff --git a/src/systemcmds/reboot/reboot.cpp b/src/systemcmds/reboot/reboot.cpp index 8d0b3419e81b..351ea64c0cfc 100644 --- a/src/systemcmds/reboot/reboot.cpp +++ b/src/systemcmds/reboot/reboot.cpp @@ -51,6 +51,7 @@ static void print_usage() PRINT_MODULE_USAGE_NAME_SIMPLE("reboot", "command"); PRINT_MODULE_USAGE_PARAM_FLAG('b', "Reboot into bootloader", true); + PRINT_MODULE_USAGE_PARAM_FLAG('c', "Bootloader continue boot", true); PRINT_MODULE_USAGE_ARG("lock|unlock", "Take/release the shutdown lock (for testing)", true); } @@ -59,16 +60,21 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[]) { int ch; bool to_bootloader = false; + bool bl_continue_boot = false; int myoptind = 1; const char *myoptarg = nullptr; - while ((ch = px4_getopt(argc, argv, "b", &myoptind, &myoptarg)) != -1) { + while ((ch = px4_getopt(argc, argv, "bc", &myoptind, &myoptarg)) != -1) { switch (ch) { case 'b': to_bootloader = true; break; + case 'c': + bl_continue_boot = true; + break; + default: print_usage(); return 1; @@ -98,7 +104,7 @@ extern "C" __EXPORT int reboot_main(int argc, char *argv[]) return ret; } - int ret = px4_reboot_request(to_bootloader); + int ret = px4_reboot_request(to_bootloader, 0, bl_continue_boot); if (ret < 0) { PX4_ERR("reboot failed (%i)", ret); diff --git a/src/systemcmds/system_time/CMakeLists.txt b/src/systemcmds/system_time/CMakeLists.txt index edb2865f4ea4..b23698f3dd75 100644 --- a/src/systemcmds/system_time/CMakeLists.txt +++ b/src/systemcmds/system_time/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE systemcmds__system_time MAIN system_time + NO_DAEMON SRCS system_time.cpp DEPENDS diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index ab948cf8a238..908a1e013004 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -36,7 +36,7 @@ set(srcs test_bezierQuad.cpp test_bitset.cpp test_bson.cpp - test_dataman.c + test_dataman.cpp test_file.c test_file2.c test_float.cpp @@ -71,6 +71,7 @@ if(${PX4_PLATFORM} STREQUAL "nuttx") test_mount.cpp test_time.c test_uart_break.c + test_crypto.cpp ) endif() @@ -93,6 +94,7 @@ px4_add_module( SRCS ${srcs} DEPENDS + dataman_client version ) diff --git a/src/systemcmds/tests/test_crypto.cpp b/src/systemcmds/tests/test_crypto.cpp new file mode 100644 index 000000000000..dd5e60d31714 --- /dev/null +++ b/src/systemcmds/tests/test_crypto.cpp @@ -0,0 +1,399 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2019 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file test_crypto.cpp + * Tests for crypto interfaces. + */ + +#include +#include +#include +#include +#include + +// Input for encryption (e.g. plaintext message) +const uint32_t test_input[256 / 4] = { + 0xf625d5fc, 0x0253fd3b, 0xe427adf4, 0x824e292e, 0x8f903040, 0xf89a67b3, 0x1ab7cfe4, 0x9a23929c, + 0xb25f43bb, 0xa009a087, 0xa8e13de5, 0x58fc0026, 0x012e9c22, 0xf33be7a2, 0x47e0ce21, 0x596fc612, + 0x6e2d5ece, 0xcdb9aa7c, 0xa2cd3538, 0xac4eb414, 0x51236c03, 0x61704a6f, 0x1992d2a0, 0xb68d766c, + 0xbbfa2886, 0x04e09c5d, 0x1508d501, 0x15f4c9e4, 0xb7f0ce5c, 0x8de0f57d, 0xdd1c9335, 0xb773af67, + 0x8be4289e, 0x98a2cdf3, 0xf6b47000, 0x0d5bca6f, 0x31d062ab, 0x9856426a, 0x00fd1376, 0xa21e101e, + 0xe9907670, 0x61bde6d8, 0x8f2edc79, 0x21839eca, 0x868eb807, 0x7109f785, 0xd5781dd3, 0x87f578a3, + 0x3f445f25, 0x657c2d3b, 0x7d4a8374, 0x7a932bcc, 0xfa0ac524, 0xda203efd, 0x4ff8b7ab, 0x04b3ed62, + 0x5f06f9fe, 0x42f265cd, 0x736487b1, 0x17ffc170, 0x45c37f4f, 0x82f5aeb7, 0xad5906fb, 0xb98b6825, +}; + +// Another input for encryption (e.g. test key) +static const uint32_t test_key[256 / 4] = { + 0x60b6c82d, 0xb51c908f, 0xe67c05cb, 0x1f728c4f, 0x29e4c6b2, 0x5ecb5c27, 0x618f4b47, 0x8be711cd, + 0xf537275d, 0xbfe729db, 0x17e32ac7, 0x832c5e15, 0x1324c3a0, 0xe38a6e44, 0x7b082c31, 0xe26fed61, + 0xa43bafa2, 0xa3183b71, 0xbe3a89fc, 0x7e02d124, 0x5569dc48, 0xc5aeb667, 0x5f6eee55, 0xed528371, + 0x65c15683, 0xfd264695, 0x4f64ea7b, 0x5bdb5278, 0x5780405b, 0x6e0bef86, 0xd847e9ce, 0x1fbcf9e1, + 0x6405ef96, 0x44a143da, 0x6f7f9945, 0xd1355623, 0x98430078, 0xa2e9973a, 0xca5e934a, 0x4e382f89, + 0x13fce2b8, 0x41108c34, 0xc0794ce2, 0x054d56a3, 0x872eda91, 0x511795db, 0x60069323, 0x9607502a, + 0xb96dcf7b, 0x94b77bc0, 0x7a39948a, 0x7d6d3ab0, 0xf1b50de7, 0xb5d201e3, 0xa60e0754, 0x798dcdb4, + 0x4fa58770, 0x8b7e833f, 0x57e0dc52, 0x4f321168, 0x92b7e26c, 0xb3c0e5e3, 0xc0199112, 0x69ea7691, +}; + +// Another input for encryption (e.g. nonce) +static uint32_t test_nonce[64 / 4] = { + 0x9fb7a36e, 0xdafd7f9d, 0x1848d166, 0x398b3c9d, 0xcae67c98, 0x9b1d77e3, 0xbffed534, 0x02dffc3a, +}; + +// Output of the encryption +static uint32_t test_output1[2048 / 4]; +static uint32_t test_output2[2048 / 4]; + +static const uint8_t test_color[4] = {0xef, 0xbe, 0xad, 0xde}; + +static uint32_t rsa_sig[] = { + 0x8ad828c5, 0x7f38a88e, 0x8cd2ea5d, 0xb7b879f0, 0xe40f7fb9, 0x38b63aad, 0xa65cd35b, 0x48762e55, + 0x60544615, 0xf5c97d41, 0xb11661dd, 0x57aadc70, 0xa484687b, 0x329fbde0, 0xb49aa5a6, 0xb77d944b, + 0xe44ceef9, 0xdec23633, 0x624ec5c0, 0xee80b11f, 0xc1bf23f6, 0x0f5179a2, 0xed23a7f0, 0xca15f9cd, + 0xdfde03ca, 0xf969102d, 0x55747617, 0xb8f8c10c, 0xf2593e3a, 0xd7a4741f, 0xe87c87be, 0xae66829f, + 0x3d82069a, 0x0edb316a, 0x9f125c10, 0xbe98f1c1, 0x1fe76e6f, 0x38999bf3, 0xed4f6f7f, 0x80390541, + 0x0ca57ab5, 0x7ee0422b, 0xbf00044f, 0x23c396a3, 0x78465678, 0xfea6d77f, 0xefad237f, 0x363c889b, + 0xc841ddae, 0x6586b9b7, 0xcbf66c5d, 0x831cc395, 0xd0078a79, 0xec833561, 0x261f91eb, 0x2f640291, + 0x9303ebe0, 0x8414b231, 0xbeec5418, 0x088cc7bb, 0x5e345fe3, 0xb7c954a3, 0x54d08308, 0xcaae3c41, +}; + +class CryptoTest : public UnitTest +{ +public: + virtual bool run_tests(); + + bool test_aes(); + bool test_chacha(); + bool test_rsa_sign(); + +private: + void color_output(void *buf, size_t sz); + bool check_color(const void *p1, const void *p2); + void dump_data(const void *d, size_t sz); + void dump_data32(const void *d, size_t sz); +}; + +static bool output_error(PX4Crypto &crypto, const char *str) +{ + printf("%s\n", str); + crypto.close(); + return false; +} + +#define ERROUT(x) return output_error(crypto, x); + +bool CryptoTest::test_aes() +{ + bool ret = true; + const uint8_t *key = (uint8_t *)test_key; + + const uint8_t *plaintext = (uint8_t *)test_input; + uint8_t *enc_data = (uint8_t *)&test_output1[512 / 4]; + uint8_t *dec_data = (uint8_t *)&test_output2[512 / 4]; + const void *outbuf1_end = (uint8_t *)&test_output1[sizeof(test_output1) / 4]; + const void *outbuf2_end = (uint8_t *)&test_output2[sizeof(test_output2) / 4]; + PX4Crypto crypto; + + if (!crypto.open(CRYPTO_AES)) { + ERROUT("Crypto open failed"); + } + + if (!crypto.generate_key(4, false)) { + ERROUT("Generate key failed"); + } + + color_output(test_output1, sizeof(test_output1)); + color_output(test_output2, sizeof(test_output2)); + + /* Encrypt data (in-place) */ + + uint32_t mac[4]; + size_t mac_size = sizeof(mac); + size_t out; + + crypto.renew_nonce((uint8_t *)test_nonce, 12); + + const size_t text_size = 247; + const size_t aligned_text_sz = (text_size + 0x3) & (~0x3); + + /* Encrypt */ + + memcpy(enc_data, plaintext, text_size); + + out = 1024; + ret = crypto.encrypt_data( + 4, + enc_data, + text_size, + enc_data, + &out, + (uint8_t *)mac, + &mac_size); + + if (!ret) { + ERROUT("Encrypt failed"); + } + + /* Check for memory corruption */ + + if (!check_color(test_output1, enc_data) || + !check_color(enc_data + aligned_text_sz, outbuf1_end)) { + dump_data32(enc_data - 16, aligned_text_sz + 32); + ERROUT("Memory corruption in encryption"); + } + + if (out != text_size) { + ERROUT("Encrypt: output size mismatch"); + } + + /* Decrypt data */ + + crypto.renew_nonce((const uint8_t *)test_nonce, 12); + + out = 1024; + ret = crypto.decrypt_data( + 4, + enc_data, + text_size, + (uint8_t *)mac, + mac_size, + dec_data, + &out); + + if (!ret) { + ERROUT("Decrypt failed"); + } + + if (!check_color(test_output2, dec_data) || + !check_color(dec_data + aligned_text_sz, outbuf2_end)) { + dump_data(dec_data - 16, aligned_text_sz + 32); + ERROUT("Memory corruption in decryption"); + } + + if (out != text_size) { + ERROUT("Decrypt: output size mismatch"); + } + + if (memcmp(test_input, dec_data, out)) { + ERROUT("Data doesn't match"); + } + + crypto.close(); + return true; +} + +bool CryptoTest::test_chacha() +{ + bool ret = true; + const uint8_t *key = (uint8_t *)test_key; + + const uint8_t *plaintext = (uint8_t *)test_input; + uint8_t *enc_data = (uint8_t *)&test_output1[512 / 4]; + uint8_t *dec_data = (uint8_t *)&test_output2[512 / 4]; + const void *outbuf1_end = (uint8_t *)&test_output1[sizeof(test_output1) / 4]; + const void *outbuf2_end = (uint8_t *)&test_output2[sizeof(test_output2) / 4]; + PX4Crypto crypto; + + if (!crypto.open(CRYPTO_XCHACHA20)) { + ERROUT("Crypto open failed"); + } + + if (!crypto.generate_key(4, false)) { + ERROUT("Generate key failed"); + } + + color_output(test_output1, sizeof(test_output1)); + color_output(test_output2, sizeof(test_output2)); + + /* Encrypt data (in-place) */ + + size_t out; + + const size_t text_size = 247; + + crypto.renew_nonce((const uint8_t *)test_nonce, 24); + + /* Encrypt */ + + memcpy(enc_data, plaintext, text_size); + + out = 1024; + ret = crypto.encrypt_data( + 4, + enc_data, + text_size, + enc_data, + &out, + NULL, + 0); + + if (!ret) { + ERROUT("Encrypt failed"); + } + + if (out != text_size) { + ERROUT("Encrypt: output size mismatch"); + } + + /* Check for memory corruption */ + + if (!check_color(test_output1, enc_data) || + !check_color(enc_data + text_size, outbuf1_end)) { + dump_data(enc_data - 16, text_size + 32); + ERROUT("Memory corruption in encryption"); + } + + /* Decrypt data */ + + /* Use the same generated key, but re-initialize */ + + crypto.renew_nonce((const uint8_t *)test_nonce, 24); + + out = 1024; + ret = crypto.decrypt_data( + 4, + enc_data, + text_size, + NULL, + 0, + dec_data, + &out); + + if (!ret) { + ERROUT("Decrypt failed"); + } + + if (out != text_size) { + ERROUT("Decrypt: output size mismatch"); + } + + if (!check_color(test_output2, dec_data) || + !check_color(dec_data + text_size, outbuf2_end)) { + dump_data(dec_data - 16, text_size + 32); + ERROUT("Memory corruption in decryption"); + } + + if (memcmp(test_input, dec_data, text_size)) { + ERROUT("Data doesn't match"); + } + + crypto.close(); + return true; +} + +// openssl dgst -sha256 -sign Tools/saluki-sec-scripts/test_keys/saluki-v2/rsa2048_test_key.pem sig_data.bin > rsa_sig.bin +// openssl dgst -sha256 -verify rsa2048.pub -signature rsa_sig.bin sig_data.bin + +bool CryptoTest::test_rsa_sign() +{ + bool ret = true; + + PX4Crypto crypto; + + if (!crypto.open(CRYPTO_RSA_SIG)) { + ERROUT("Crypto open failed"); + } + + ret = crypto.signature_check(2, + (uint8_t *)rsa_sig, + (uint8_t *)test_input, + 5); + + crypto.close(); + + return ret; +} + +bool CryptoTest::run_tests() +{ + ut_run_test(test_aes); + ut_run_test(test_chacha); + // ut_run_test(test_rsa_sign); + + return (_tests_failed == 0); +} + +void CryptoTest::dump_data(const void *d, size_t sz) +{ + const uint8_t *data = (const uint8_t *)d; + + for (size_t i = 0; i < sz; i += 16) { + for (size_t j = 0; j < 16 && i + j < sz; j++) { + printf(" 0x%02x,", data[i + j]); + } + + printf("\n"); + } +} + +void CryptoTest::dump_data32(const void *d, size_t sz) +{ + const uint32_t *data = (const uint32_t *)d; + const size_t sz_w = sz / sizeof(uint32_t); + + for (size_t i = 0; i < sz_w; i += 8) { + for (size_t j = 0; j < 8 && i + j < sz_w; j++) { + printf(" 0x%08x,", data[i + j]); + } + + printf("\n"); + } +} + +void CryptoTest::color_output(void *buf, size_t sz) +{ + uint8_t *p = (uint8_t *)buf; + uint8_t *end = &p[sz]; + + for (; p < end; p++) { + *p = test_color[(uintptr_t)p & 0x3]; + } +} + +bool CryptoTest::check_color(const void *p1, const void *p2) +{ + uint8_t *p = (uint8_t *)p1; + uint8_t *end = (uint8_t *)p2; + bool res = true; + + for (; res && p < end; p++) { + if (*p != test_color[(uintptr_t)p & 0x3]) { + res = false; + } + } + + return res; +} + +ut_declare_test_c(test_crypto, CryptoTest) diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c deleted file mode 100644 index e55332306073..000000000000 --- a/src/systemcmds/tests/test_dataman.c +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 PX4 Development Team. All rights reserved. - * - * 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. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * 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 OWNER 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. - * - ****************************************************************************/ - -/** - * @file test_dataman.c - * Tests for the data manager. - */ - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "tests_main.h" - -#include "dataman/dataman.h" - -static px4_sem_t *sems; -static bool *task_returned_error; -int test_dataman(int argc, char *argv[]); - -#define NUM_MISSIONS_TEST 50 - -#define DM_MAX_DATA_SIZE sizeof(struct mission_s) - -static int -task_main(int argc, char *argv[]) -{ - char buffer[DM_MAX_DATA_SIZE]; - - PX4_INFO("Starting dataman test task %s", argv[2]); - /* try to read an invalid item */ - int my_id = atoi(argv[2]); - - /* try to read an invalid item */ - if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid item failed", my_id); - goto fail; - } - - /* try to read an invalid index */ - if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid index failed", my_id); - goto fail; - } - - srand(hrt_absolute_time() ^ my_id); - unsigned hit = 0; - unsigned miss = 0; - hrt_abstime wstart = hrt_absolute_time(); - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - memset(buffer, my_id, sizeof(buffer)); - buffer[1] = i; - unsigned hash = i ^ my_id; - unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, len); - //PX4_INFO("ret: %d", ret); - - if (ret != len) { - PX4_WARN("task %d: write failed ret=%d, index: %d, length: %d", my_id, ret, hash, len); - goto fail; - } - - if (i % (NUM_MISSIONS_TEST / 10) == 0) { - PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST); - } - - px4_usleep(rand() & ((64 * 1024) - 1)); - } - - hrt_abstime rstart = hrt_absolute_time(); - hrt_abstime wend = rstart; - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - unsigned hash = i ^ my_id; - ssize_t len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer)); - ssize_t len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, ret=%zd, len=%zd", my_id, hash, len2, len); - goto fail; - } - - if (buffer[0] == my_id) { - hit++; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, wanted %zd, got %zd", my_id, hash, len, len2); - goto fail; - } - - if (buffer[1] != i) { - PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); - goto fail; - } - - } else { - miss++; - } - } - - hrt_abstime rend = hrt_absolute_time(); - PX4_INFO("task %d pass, hit %d, miss %d, io time read %" PRIu64 "ms. write %" PRIu64 "ms.", - my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000); - px4_sem_post(sems + my_id); - return 0; - -fail: - PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x", - my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); - px4_sem_post(sems + my_id); - task_returned_error[my_id] = true; - return -1; -} - -int test_dataman(int argc, char *argv[]) -{ - int i = 0; - unsigned num_tasks = 4; - char buffer[DM_MAX_DATA_SIZE]; - - if (argc > 1) { - num_tasks = atoi(argv[1]); - } - - sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t)); - task_returned_error = (bool *)calloc(num_tasks, sizeof(bool)); - PX4_INFO("Running %d tasks", num_tasks); - - for (i = 0; i < num_tasks; i++) { - int task; - - char a[16]; - snprintf(a, 16, "%d", i); - - char *av[] = {"tests_dataman", a, NULL}; - - px4_sem_init(sems + i, 1, 0); - /* sems use case is a signal */ - px4_sem_setprotocol(sems + i, SEM_PRIO_NONE); - - /* start the task */ - if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, task_main, av)) <= 0) { - PX4_ERR("task start failed"); - } - } - - for (i = 0; i < num_tasks; i++) { - px4_sem_wait(sems + i); - px4_sem_destroy(sems + i); - } - - free(sems); - - bool got_error = false; - - for (i = 0; i < num_tasks; i++) { - if (task_returned_error[i]) { - got_error = true; - break; - } - } - - free(task_returned_error); - - if (got_error) { - return -1; - } - - for (i = 0; i < NUM_MISSIONS_TEST; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { - break; - } - } - - return 0; -} diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp new file mode 100644 index 000000000000..dbd1690088b1 --- /dev/null +++ b/src/systemcmds/tests/test_dataman.cpp @@ -0,0 +1,1149 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * 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. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file test_dataman.cpp + * Tests for Dataman. + */ + +#include + +#include +#include +#include +#include +#include + +#include "dataman_client/DatamanClient.hpp" + +class DatamanTest : public UnitTest +{ +public: + DatamanTest(); + virtual bool run_tests(); + +private: + + enum class State { + Write, + WriteWait, + Read, + ReadWait, + Clear, + ClearWait, + OperationCompleted, + CompareBuffers, + Exit + }; + + //Sync + bool testSyncReadInvalidItem(); + bool testSyncWriteInvalidItem(); + bool testSyncReadInvalidIndex(); + bool testSyncWriteInvalidIndex(); + bool testSyncReadBufferOverflow(); + bool testSyncWriteBufferOverflow(); + bool testSyncMutipleClients(); + bool testSyncWriteReadAllItemsMaxSize(); + bool testSyncClearAll(); + + //Async + bool testAsyncReadInvalidItem(); + bool testAsyncWriteInvalidItem(); + bool testAsyncReadInvalidIndex(); + bool testAsyncWriteInvalidIndex(); + bool testAsyncReadBufferOverflow(); + bool testAsyncWriteBufferOverflow(); + bool testAsyncMutipleClients(); + bool testAsyncWriteReadAllItemsMaxSize(); + bool testAsyncClearAll(); + + //Cache + bool testCache(); + + //This will reset the items but it will not restore the compact key. + bool testResetItems(); + + DatamanClient _dataman_client1{}; + DatamanClient _dataman_client2{}; + DatamanClient _dataman_client3{}; + DatamanClient _dataman_client_thread1{}; + DatamanClient _dataman_client_thread2{}; + DatamanClient _dataman_client_thread3{}; + + DatamanCache _dataman_cache{"test_dm_cache_miss", 10}; + + static void *testAsyncThread(void *arg); + + static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_ITEM_SIZE}; + static_assert(sizeof(dataman_response_s::data) == DM_MAX_DATA_SIZE, "data size != DM_MAX_DATA_SIZE"); + + uint8_t _buffer_read[DM_MAX_DATA_SIZE]; + uint8_t _buffer_write[DM_MAX_DATA_SIZE]; + + bool _response_success{false}; + + px4::atomic_int _thread_index{0}; + px4::atomic_bool _thread_tests_success{false}; + + uint16_t _max_index[DM_KEY_NUM_KEYS] {}; + + static constexpr uint32_t OVERFLOW_LENGTH = sizeof(_buffer_write) + 1; +}; + +DatamanTest::DatamanTest() +{ + for (uint32_t i = 0; i < DM_KEY_NUM_KEYS; ++i) { + _max_index[i] = g_per_item_max_index[i]; + } + +#ifndef __PX4_NUTTX + _max_index[DM_KEY_WAYPOINTS_OFFBOARD_0] = 200; + _max_index[DM_KEY_WAYPOINTS_OFFBOARD_1] = 200; +#endif + +} + +bool +DatamanTest::testSyncReadInvalidItem() +{ + + bool success = _dataman_client1.readSync(DM_KEY_NUM_KEYS, 0, _buffer_read, 2); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidItem() +{ + bool success = _dataman_client1.writeSync(DM_KEY_NUM_KEYS, 0, _buffer_write, 2); + return !success; +} + + +bool +DatamanTest::testSyncReadInvalidIndex() +{ + bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidIndex() +{ + bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); + return !success; +} + +bool +DatamanTest::testSyncReadBufferOverflow() +{ + bool success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_read, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testSyncWriteBufferOverflow() +{ + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_write, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testSyncMutipleClients() +{ + // Prepare write buffer + for (uint32_t i = 0; i < DM_MAX_DATA_SIZE; ++i) { + _buffer_write[i] = (uint8_t)i; + } + + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client2.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client3.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + //Compare content from buffers + for (uint32_t i = 0; i < g_per_item_size[DM_KEY_MISSION_STATE]; ++i) { + if (_buffer_read[i] != _buffer_write[i]) { + return false; + } + } + + return success; +} + +bool +DatamanTest::testSyncWriteReadAllItemsMaxSize() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + // writeSync + for (uint32_t index = 0U; index < _max_index[item]; ++index) { + + // Prepare write buffer + for (uint32_t i = 0; i < g_per_item_size[item]; ++i) { + _buffer_write[i] = (uint8_t)(index % UINT8_MAX); + } + + success = _dataman_client1.writeSync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]); + + if (!success) { + PX4_ERR("writeSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + } + + // readSync + for (uint32_t index = 0U; index < _max_index[item]; ++index) { + + success = _dataman_client1.readSync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]); + + if (!success) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + + // Check read buffer + for (uint32_t i = 0U; i < g_per_item_size[item]; ++i) { + + uint8_t expected_value = (index % UINT8_MAX); + + if (expected_value != _buffer_read[i]) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32 ", element= %" PRIu32 ", expected: %" PRIu8 + ", received: %" PRIu8, item, index, i, expected_value, _buffer_read[i]); + return false; + } + } + } + } + + return true; +} + +bool +DatamanTest::testSyncClearAll() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + success = _dataman_client1.clearSync((dm_item_t)item); + + if (!success) { + PX4_ERR("clearSync failed at item = %" PRIu32, item); + return false; + } + } + + return success; +} + +bool +DatamanTest::testAsyncReadInvalidItem() +{ + bool success = true; + + State state = State::Read; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Read: + + state = State::ReadWait; + success = _dataman_client1.readAsync(DM_KEY_NUM_KEYS, 0, _buffer_read, 2); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncWriteInvalidItem() +{ + bool success = true; + + State state = State::Write; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = _dataman_client1.writeAsync(DM_KEY_NUM_KEYS, 0, _buffer_write, 2); + + if (!success) { + return false; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncReadInvalidIndex() +{ + bool success = true; + + State state = State::Read; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Read: + + state = State::ReadWait; + success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncWriteInvalidIndex() +{ + bool success = true; + + State state = State::Write; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); + + if (!success) { + return false; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncReadBufferOverflow() +{ + bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH); + + return !success; +} + +bool +DatamanTest::testAsyncWriteBufferOverflow() +{ + bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, OVERFLOW_LENGTH); + + return !success; +} + +bool +DatamanTest::testAsyncMutipleClients() +{ + pthread_t thread1{}; + pthread_t thread2{}; + pthread_t thread3{}; + + _thread_tests_success.store(true); + _thread_index.store(0); + + // Test multiple dataman clients + uint32_t ret = pthread_create(&thread1, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + ret = pthread_create(&thread2, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + ret = pthread_create(&thread3, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + pthread_join(thread1, nullptr); + pthread_join(thread2, nullptr); + pthread_join(thread3, nullptr); + + return _thread_tests_success.load(); +} + +void *DatamanTest::testAsyncThread(void *arg) +{ + DatamanTest *dataman_test = (DatamanTest *)arg; + const uint32_t index = dataman_test->_thread_index.fetch_add(1); + State state = State::Write; + + hrt_abstime start_time = hrt_absolute_time(); + + uint8_t buffer_read[DM_MAX_DATA_SIZE] = {}; + uint8_t buffer_write[DM_MAX_DATA_SIZE] = {}; + + bool success; + bool response_success; + + // Prepare write buffer + for (uint8_t i = 0; i < g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0]; ++i) { + buffer_write[i] = i * index; + } + + DatamanClient *dataman_client{nullptr}; + + if (index == 0) { + dataman_client = &(dataman_test->_dataman_client_thread1); + + } else if (index == 1) { + dataman_client = &(dataman_test->_dataman_client_thread2); + + } else if (index == 2) { + dataman_client = &(dataman_test->_dataman_client_thread3); + + } else { + PX4_ERR("Unknown thread %" PRIu32 "!", index); + return nullptr; + } + + while (state != State::Exit) { + + dataman_client->update(); + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = dataman_client->writeAsync(DM_KEY_WAYPOINTS_OFFBOARD_0, index, buffer_write, sizeof(buffer_write)); + + if (!success) { + PX4_ERR("writeAsync failed for index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + break; + + case State::WriteWait: + + if (dataman_client->lastOperationCompleted(response_success)) { + state = State::Read; + + if (!response_success) { + PX4_ERR("writeAsync failed to get success operation complete for the index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + } + + break; + + case State::Read: + + state = State::ReadWait; + success = dataman_client->readAsync(DM_KEY_WAYPOINTS_OFFBOARD_0, index, buffer_read, sizeof(buffer_read)); + + if (!success) { + PX4_ERR("readAsync failed for index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + break; + + case State::ReadWait: + if (dataman_client->lastOperationCompleted(response_success)) { + state = State::CompareBuffers; + + if (!response_success) { + PX4_ERR("readAsync failed to get success operation complete for the index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + } + + break; + + case State::CompareBuffers: + + for (uint32_t i = 0; i < g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0]; ++i) { + if (buffer_write[i] != buffer_read[i]) { + PX4_ERR("buffer are not the same for index %" PRIu32 "!", index); + dataman_test->_thread_tests_success.store(false); + break; + } + } + + state = State::Exit; + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout! index=%" PRIu32, index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + PX4_INFO("Thread %" PRIu32 " finished!", index); + px4_usleep(200_ms); + + return nullptr; +} + +bool +DatamanTest::testAsyncWriteReadAllItemsMaxSize() +{ + bool success = false; + State state = State::Write; + + uint32_t item = DM_KEY_SAFE_POINTS; + uint32_t index = 0U; + + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + switch (state) { + + case State::Write: + + state = State::WriteWait; + + // Prepare write buffer + for (uint32_t i = 0; i < g_per_item_size[item]; ++i) { + _buffer_write[i] = (uint8_t)(index % UINT8_MAX); + } + + success = _dataman_client1.writeAsync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]); + + if (!success) { + return false; + } + + break; + + case State::Read: + state = State::ReadWait; + success = _dataman_client1.readAsync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + if (!_response_success) { + return false; + } + + state = State::CompareBuffers; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + if (!_response_success) { + return false; + } + + state = State::Read; + } + + break; + + case State::CompareBuffers: + state = State::Write; + + for (uint32_t i = 0U; i < g_per_item_size[item]; ++i) { + + if (_buffer_write[i] != _buffer_read[i]) { + PX4_ERR("readAsync failed at item = %" PRIu32 ", index = %" PRIu32 ", element= %" PRIu32 ", expected: %" PRIu8 + ", received: %" PRIu8, item, index, i, _buffer_write[i], _buffer_read[i]); + return false; + } + } + + if (index < _max_index[item] - 1) { + ++index; + + } else { + + if (item < DM_KEY_NUM_KEYS - 1) { + index = 0U; + ++item; + + } else { + state = State::Exit; + } + } + + break; + + default: + break; + } + + if (hrt_elapsed_time(&start_time) > 20_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + + } + + return success; +} + +bool +DatamanTest::testAsyncClearAll() +{ + bool success = true; + + State state = State::Clear; + hrt_abstime start_time = hrt_absolute_time(); + uint32_t item = DM_KEY_SAFE_POINTS; + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + switch (state) { + + case State::Clear: + + state = State::ClearWait; + success = _dataman_client1.clearAsync((dm_item_t)item); + + if (!success) { + PX4_ERR("Failed at item %" PRIu32, item); + return false; + } + + break; + + case State::ClearWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + PX4_ERR("Failed at item %" PRIu32, item); + return false; + } + } + + break; + + case State::OperationCompleted: + if (item < DM_KEY_NUM_KEYS - 1) { + state = State::Clear; + ++item; + + } else { + state = State::Exit; + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 5_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return true; +} + +//Cache +bool +DatamanTest::testCache() +{ + bool success = false; + dm_item_t item = DM_KEY_WAYPOINTS_OFFBOARD_0; + uint32_t uniq_number = 13; // Use this to make sure stored data is from this test + + for (uint32_t index = 0; index < 15; ++index) { + uint8_t value = index + uniq_number; + memset(_buffer_write, value, sizeof(_buffer_write)); + success = _dataman_cache.client().writeSync(item, index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + } + + // Write one extra for loadWait with timeout + uint32_t extra_index = 100; + _buffer_write[0] = 123; + success = _dataman_cache.client().writeSync(item, extra_index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + // Load cache + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + if (!_dataman_cache.load(item, index)) { + return false; + } + } + + hrt_abstime start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (_dataman_cache.isLoading()) { + + px4_usleep(1_ms); + _dataman_cache.update(); + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout!"); + return false; + } + } + + // check cached data + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // expected to fail without timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded"); + return false; + } + + // expected to success with timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read), 100_ms); + + if (!success) { + PX4_ERR("loadWait failed"); + return false; + } + + // expected to success without timeout set (item is now cached) + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("loadWait failed"); + return false; + } + + uint32_t old_cache_size = _dataman_cache.size(); + _dataman_cache.resize(5); + + // check cached data after resize (reduced, the first item got overwritten by extra_index) + for (uint32_t index = 1; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + for (uint32_t index = _dataman_cache.size(); index < old_cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + } + + _dataman_cache.invalidate(); + _dataman_cache.resize(15); + + // Load cache + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + _dataman_cache.load(item, index); + } + + start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (_dataman_cache.isLoading()) { + + px4_usleep(1_ms); + _dataman_cache.update(); + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout!"); + return false; + } + } + + // check cached data + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // invalidate and check cached data + _dataman_cache.invalidate(); + + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + // expected to fail + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + + } + + return true; +} + +bool +DatamanTest::testResetItems() +{ + bool success = false; + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; + + success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_MISSION_STATE"); + return false; + } + + success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to read DM_KEY_MISSION_STATE"); + return false; + } + + mission_stats_entry_s stats; + stats.num_items = 0; + stats.update_counter = 1; + + success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_FENCE_POINTS"); + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_SAFE_POINTS"); + return false; + } + + return success; +} + +bool DatamanTest::run_tests() +{ + ut_run_test(testSyncReadInvalidItem); + ut_run_test(testSyncWriteInvalidItem); + ut_run_test(testSyncReadInvalidIndex); + ut_run_test(testSyncWriteInvalidIndex); + ut_run_test(testSyncReadBufferOverflow); + ut_run_test(testSyncWriteBufferOverflow); + ut_run_test(testSyncMutipleClients); + ut_run_test(testSyncWriteReadAllItemsMaxSize); + ut_run_test(testSyncClearAll); + + ut_run_test(testAsyncReadInvalidItem); + ut_run_test(testAsyncWriteInvalidItem); + ut_run_test(testAsyncReadInvalidIndex); + ut_run_test(testAsyncWriteInvalidIndex); + ut_run_test(testAsyncReadBufferOverflow); + ut_run_test(testAsyncWriteBufferOverflow); + ut_run_test(testAsyncMutipleClients); + ut_run_test(testAsyncWriteReadAllItemsMaxSize); + ut_run_test(testAsyncClearAll); + + ut_run_test(testCache); + + ut_run_test(testResetItems); + + return (_tests_failed == 0); +} + +ut_declare_test_c(test_dataman, DatamanTest) diff --git a/src/systemcmds/tests/test_rc.cpp b/src/systemcmds/tests/test_rc.cpp index 97f70651152f..c9a4beb73050 100644 --- a/src/systemcmds/tests/test_rc.cpp +++ b/src/systemcmds/tests/test_rc.cpp @@ -37,6 +37,7 @@ */ #include +#include #include @@ -59,7 +60,7 @@ int test_rc(int argc, char *argv[]) { - int _rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_sub_t _rc_sub = orb_subscribe(ORB_ID(input_rc)); /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct input_rc_s rc_input; @@ -85,7 +86,7 @@ int test_rc(int argc, char *argv[]) rc_last.channel_count = rc_input.channel_count; /* poll descriptor */ - struct pollfd fds[2]; + px4_pollfd_struct_t fds[2]; fds[0].fd = _rc_sub; fds[0].events = POLLIN; fds[1].fd = 0; @@ -93,7 +94,7 @@ int test_rc(int argc, char *argv[]) while (true) { - int ret = poll(fds, 2, 200); + int ret = px4_poll(fds, 2, 200); if (ret > 0) { diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 8a895c157a95..2dc56d160c51 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -75,13 +75,14 @@ const struct { {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST}, {"uart_break", test_uart_break, OPT_NOJIGTEST | OPT_NOALLTEST}, {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"crypto", test_crypto, 0}, #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, - {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, {"file2", test_file2, OPT_NOJIGTEST}, {"float", test_float, 0}, {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743cc9..54a1d25e6d3b 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -77,6 +77,7 @@ extern int test_uart_loopback(int argc, char *argv[]); extern int test_uart_send(int argc, char *argv[]); extern int test_versioning(int argc, char *argv[]); extern int test_cli(int argc, char *argv[]); +extern int test_crypto(int argc, char *argv[]); __END_DECLS diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index e5f127fd1ff9..349a727f4024 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MODULE systemcmds__topic_listener MAIN listener STACK_MAIN 4096 + NO_DAEMON SRCS listener_main.cpp ) diff --git a/src/systemcmds/topic_listener/listener_main.cpp b/src/systemcmds/topic_listener/listener_main.cpp index b4db038c1254..4534406fa6f9 100644 --- a/src/systemcmds/topic_listener/listener_main.cpp +++ b/src/systemcmds/topic_listener/listener_main.cpp @@ -39,6 +39,7 @@ #include #include +#include #include @@ -68,7 +69,7 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, if (instances == 1) { PX4_INFO_RAW("\nTOPIC: %s\n", id->o_name); - int sub = orb_subscribe(id); + orb_sub_t sub = orb_subscribe(id); listener_print_topic(id, sub); orb_unsubscribe(sub); @@ -78,7 +79,7 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) { if (orb_exists(id, i) == PX4_OK) { PX4_INFO_RAW("\nInstance %d:\n", i); - int sub = orb_subscribe_multi(id, i); + orb_sub_t sub = orb_subscribe_multi(id, i); listener_print_topic(id, sub); orb_unsubscribe(sub); } @@ -100,55 +101,51 @@ void listener(const orb_id_t &id, unsigned num_msgs, int topic_instance, return; } - int sub = orb_subscribe_multi(id, topic_instance); + orb_sub_t sub = orb_subscribe_multi(id, topic_instance); orb_set_interval(sub, topic_interval); unsigned msgs_received = 0; - struct pollfd fds[2] {}; - // Poll for user input (for q or escape) - fds[0].fd = 0; /* stdin */ - fds[0].events = POLLIN; + px4_pollfd_struct_t fds[1] {}; // Poll the UOrb subscription - fds[1].fd = sub; - fds[1].events = POLLIN; + fds[0].fd = sub; + fds[0].events = POLLIN; - while (msgs_received < num_msgs) { + int time = 0; - if (poll(&fds[0], 2, int(MESSAGE_TIMEOUT_S * 1000)) > 0) { + while (msgs_received < num_msgs) { - // Received character from stdin - if (fds[0].revents & POLLIN) { - char c = 0; - int ret = read(0, &c, 1); + char c = 0; + int ret = read(0, &c, 1); - if (ret) { - return; - } + if (ret) { - switch (c) { - case 0x03: // ctrl-c - case 0x1b: // esc - case 'q': - return; - /* not reached */ - } + switch (c) { + case 0x03: // ctrl-c + case 0x1b: // esc + case 'q': + return; + /* not reached */ } + } + if (px4_poll(&fds[0], 1, 50) > 0) { // Received message from subscription - if (fds[1].revents & POLLIN) { + + if (fds[0].revents & POLLIN) { msgs_received++; PX4_INFO_RAW("\nTOPIC: %s instance %d #%d\n", id->o_name, topic_instance, msgs_received); - int ret = listener_print_topic(id, sub); + ret = listener_print_topic(id, sub); if (ret != PX4_OK) { PX4_ERR("listener callback failed (%i)", ret); } } + } - } else { + if (time += 50 > MESSAGE_TIMEOUT_S * 1000) { PX4_INFO_RAW("Waited for %.1f seconds without a message. Giving up.\n", (double) MESSAGE_TIMEOUT_S); break; } diff --git a/src/systemcmds/topic_listener/topic_listener.hpp b/src/systemcmds/topic_listener/topic_listener.hpp index 1131f4276fea..dda3816501b4 100644 --- a/src/systemcmds/topic_listener/topic_listener.hpp +++ b/src/systemcmds/topic_listener/topic_listener.hpp @@ -50,7 +50,7 @@ #include #include -inline int listener_print_topic(const orb_id_t &orb_id, int subscription) +inline int listener_print_topic(const orb_id_t &orb_id, orb_sub_t subscription) { static constexpr int max_size = 512; alignas(8) char container[max_size]; diff --git a/src/systemcmds/tune_control/CMakeLists.txt b/src/systemcmds/tune_control/CMakeLists.txt index d00586054fbf..1ad44b6085ba 100644 --- a/src/systemcmds/tune_control/CMakeLists.txt +++ b/src/systemcmds/tune_control/CMakeLists.txt @@ -36,6 +36,7 @@ px4_add_module( PRIORITY "SCHED_PRIORITY_MAX - 16" # max priority below high priority WQ threads COMPILE_FLAGS + NO_DAEMON SRCS tune_control.cpp DEPENDS diff --git a/src/systemcmds/uorb/CMakeLists.txt b/src/systemcmds/uorb/CMakeLists.txt index 2c1362394b47..ccee7e00647c 100644 --- a/src/systemcmds/uorb/CMakeLists.txt +++ b/src/systemcmds/uorb/CMakeLists.txt @@ -34,7 +34,9 @@ px4_add_module( MODULE systemcmds__uorb MAIN uorb COMPILE_FLAGS + NO_DAEMON SRCS uorb.cpp + uORBDeviceMaster.cpp DEPENDS ) diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/src/systemcmds/uorb/uORBDeviceMaster.cpp similarity index 66% rename from platforms/common/uORB/uORBDeviceMaster.cpp rename to src/systemcmds/uorb/uORBDeviceMaster.cpp index c39e4572db5e..c2ccbb2ce65f 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/src/systemcmds/uorb/uORBDeviceMaster.cpp @@ -32,10 +32,11 @@ ****************************************************************************/ #include "uORBDeviceMaster.hpp" -#include "uORBDeviceNode.hpp" -#include "uORBManager.hpp" -#include "uORBUtils.hpp" +#include +#include +#include +#include #include #include @@ -45,6 +46,12 @@ #include #endif // PX4_QURT +#ifndef CONFIG_FS_SHMFS_VFS_PATH +#define CONFIG_FS_SHMFS_VFS_PATH "/dev/shm" +#endif + +#include + uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -55,115 +62,6 @@ uORB::DeviceMaster::~DeviceMaster() px4_sem_destroy(&_lock); } -int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance) -{ - int ret = PX4_ERROR; - - char nodepath[orb_maxpath]; - - /* construct a path to the node - this also checks the node name */ - ret = uORB::Utils::node_mkpath(nodepath, meta, instance); - - if (ret != PX4_OK) { - return ret; - } - - ret = PX4_ERROR; - - /* try for topic groups */ - const unsigned max_group_tries = (instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; - unsigned group_tries = 0; - - if (instance) { - /* for an advertiser, this will be 0, but a for subscriber that requests a certain instance, - * we do not want to start with 0, but with the instance the subscriber actually requests. - */ - group_tries = *instance; - - if (group_tries >= max_group_tries) { - return -ENOMEM; - } - } - - SmartLock smart_lock(_lock); - - do { - /* if path is modifyable change try index */ - if (instance != nullptr) { - /* replace the number at the end of the string */ - nodepath[strlen(nodepath) - 1] = '0' + group_tries; - *instance = group_tries; - } - - /* construct the new node, passing the ownership of path to it */ - uORB::DeviceNode *node = new uORB::DeviceNode(meta, group_tries, nodepath); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) { - return -ENOMEM; - } - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - ret = node->init(); - - /* if init failed, discard the node and its name */ - if (ret != PX4_OK) { - delete node; - - if (ret == -EEXIST) { - /* if the node exists already, get the existing one and check if it's advertised. */ - uORB::DeviceNode *existing_node = getDeviceNodeLocked(meta, group_tries); - - /* - * We can claim an existing node in these cases: - * - The node is not advertised (yet). It means there is already one or more subscribers or it was - * unadvertised. - * - We are a single-instance advertiser requesting the first instance. - * (Usually we don't end up here, but we might in case of a race condition between 2 - * advertisers). - * - We are a subscriber requesting a certain instance. - * (Also we usually don't end up in that case, but we might in case of a race condtion - * between an advertiser and subscriber). - */ - bool is_single_instance_advertiser = is_advertiser && !instance; - - if (existing_node != nullptr && - (!existing_node->is_advertised() || is_single_instance_advertiser || !is_advertiser)) { - if (is_advertiser) { - /* Set as advertised to avoid race conditions (otherwise 2 multi-instance advertisers - * could get the same instance). - */ - existing_node->mark_as_advertised(); - } - - ret = PX4_OK; - - } else { - /* otherwise: already advertised, keep looking */ - } - } - - } else { - if (is_advertiser) { - node->mark_as_advertised(); - } - - // add to the node map. - _node_list.add(node); - _node_exists[node->get_instance()].set((uint8_t)node->id(), true); - } - - group_tries++; - - } while (ret != PX4_OK && (group_tries < max_group_tries)); - - if (ret != PX4_OK && group_tries >= max_group_tries) { - ret = -ENOMEM; - } - - return ret; -} - void uORB::DeviceMaster::printStatistics() { /* Add all nodes to a list while locked, and then print them in unlocked state, to avoid potential @@ -190,6 +88,7 @@ void uORB::DeviceMaster::printStatistics() DeviceNodeStatisticsData *prev = cur_node; cur_node = cur_node->next; + px4_munmap(prev->node, sizeof(uORB::DeviceNode)); delete prev; } } @@ -207,31 +106,74 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, } } - for (const auto &node : _node_list) { + DIR *shm_dir = opendir(CONFIG_FS_SHMFS_VFS_PATH); + struct dirent *shm; + const char orb_name_prefix[] = "_orb_"; + char orb_name[orb_maxpath]; - ++num_topics; + while ((shm = readdir(shm_dir)) != nullptr) { - //check if already added + if (strncmp(orb_name_prefix, shm->d_name, sizeof(orb_name_prefix) - 1)) { + continue; + } + + // check if already added cur_node = *first_node; - while (cur_node && cur_node->node != node) { + while (cur_node) { + int instance = cur_node->node->get_instance(); + + if (uORB::Utils::node_mkpath(orb_name, cur_node->node->get_meta(), &instance)) { + PX4_ERR("Can't construct orb name?"); + break; + } + + if (!strcmp(orb_name, shm->d_name)) { + break; + } + cur_node = cur_node->next; } if (cur_node) { + // already added or there was an error continue; } + void *ptr = nullptr; + uORB::DeviceNode *node = nullptr; + + // open and mmap the shared memory segment + int shm_fd = shm_open(shm->d_name, O_RDWR, 0666); + + if (shm_fd >= 0) { + ptr = px4_mmap(0, sizeof(uORB::DeviceNode), PROT_READ, MAP_SHARED, shm_fd, 0); + } + + if (ptr != MAP_FAILED) { + node = static_cast(ptr); + } + + close(shm_fd); + + if (node == nullptr) { + PX4_ERR("Failed to MMAP an existing node\n"); + continue; + } + + ++num_topics; + if (num_filters > 0 && topic_filter) { bool matched = false; for (int i = 0; i < num_filters; ++i) { - if (strstr(node->get_meta()->o_name, topic_filter[i])) { + if (strstr(node->get_name(), topic_filter[i])) { matched = true; } } if (!matched) { + px4_munmap(node, sizeof(uORB::DeviceNode)); continue; } } @@ -260,6 +202,7 @@ int uORB::DeviceMaster::addNewDeviceNodes(DeviceNodeStatisticsData **first_node, last_node->last_pub_msg_count = last_node->node->updates_available(0); } + closedir(shm_dir); return 0; } @@ -293,12 +236,6 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) lock(); - if (_node_list.empty()) { - unlock(); - PX4_INFO("no active topics"); - return; - } - DeviceNodeStatisticsData *first_node = nullptr; DeviceNodeStatisticsData *cur_node = nullptr; size_t max_topic_name_length = 0; @@ -424,36 +361,8 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) while (cur_node) { DeviceNodeStatisticsData *next_node = cur_node->next; - delete cur_node; + px4_munmap(cur_node->node, sizeof(uORB::DeviceNode)); + delete (cur_node); cur_node = next_node; } } - -#undef CLEAR_LINE - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath) -{ - lock(); - - for (uORB::DeviceNode *node : _node_list) { - if (strcmp(node->get_devname(), nodepath) == 0) { - unlock(); - return node; - } - } - - unlock(); - - return nullptr; -} - -uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance) -{ - for (uORB::DeviceNode *node : _node_list) { - if ((strcmp(node->get_name(), meta->o_name) == 0) && (node->get_instance() == instance)) { - return node; - } - } - - return nullptr; -} diff --git a/platforms/common/uORB/uORBDeviceMaster.hpp b/src/systemcmds/uorb/uORBDeviceMaster.hpp similarity index 70% rename from platforms/common/uORB/uORBDeviceMaster.hpp rename to src/systemcmds/uorb/uORBDeviceMaster.hpp index 94c97e61dfc9..f8f34c56ac8e 100644 --- a/platforms/common/uORB/uORBDeviceMaster.hpp +++ b/src/systemcmds/uorb/uORBDeviceMaster.hpp @@ -35,7 +35,7 @@ #include -#include "uORBCommon.hpp" +#include #include #include @@ -64,42 +64,8 @@ using px4::AtomicBitset; class uORB::DeviceMaster { public: - - int advertise(const struct orb_metadata *meta, bool is_advertiser, int *instance); - - /** - * Public interface for getDeviceNodeLocked(). Takes care of synchronization. - * @return node if exists, nullptr otherwise - */ - uORB::DeviceNode *getDeviceNode(const char *node_name); - uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance) - { - if (meta == nullptr) { - return nullptr; - } - - if (!deviceNodeExists(static_cast(meta->o_id), instance)) { - return nullptr; - } - - lock(); - uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance); - unlock(); - - //We can safely return the node that can be used by any thread, because - //a DeviceNode never gets deleted. - return node; - - } - - bool deviceNodeExists(ORB_ID id, const uint8_t instance) - { - if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) { - return false; - } - - return _node_exists[instance][(uint8_t)id]; - } + DeviceMaster(); + ~DeviceMaster(); /** * Print statistics for each existing topic. @@ -116,9 +82,6 @@ class uORB::DeviceMaster void showTop(char **topic_filter, int num_filters); private: - // Private constructor, uORB::Manager takes care of its creation - DeviceMaster(); - ~DeviceMaster(); struct DeviceNodeStatisticsData { DeviceNode *node; @@ -132,16 +95,6 @@ class uORB::DeviceMaster friend class uORB::Manager; - /** - * Find a node give its name. - * _lock must already be held when calling this. - * @return node if exists, nullptr otherwise - */ - uORB::DeviceNode *getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance); - - IntrusiveSortedList _node_list; - AtomicBitset _node_exists[ORB_MULTI_MAX_INSTANCES]; - px4_sem_t _lock; /**< lock to protect access to all class members (also for derived classes) */ void lock() { do {} while (px4_sem_wait(&_lock) != 0); } diff --git a/src/systemcmds/uorb/uorb.cpp b/src/systemcmds/uorb/uorb.cpp index b9fb565c143b..8a4561c28996 100644 --- a/src/systemcmds/uorb/uorb.cpp +++ b/src/systemcmds/uorb/uorb.cpp @@ -38,10 +38,28 @@ #include #include +#include "uORBDeviceMaster.hpp" + extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); } static void usage(); +static int uorb_status(void) +{ + uORB::DeviceMaster dev; + dev.printStatistics(); + + return OK; +} + +static int uorb_top(char **topic_filter, int num_filters) +{ + uORB::DeviceMaster dev; + dev.showTop(topic_filter, num_filters); + + return OK; +} + int uorb_main(int argc, char *argv[]) { if (argc < 2) { @@ -49,10 +67,7 @@ int uorb_main(int argc, char *argv[]) return -1; } - if (!strcmp(argv[1], "start")) { - return uorb_start(); - - } else if (!strcmp(argv[1], "status")) { + if (!strcmp(argv[1], "status")) { return uorb_status(); } else if (!strcmp(argv[1], "top")) { diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp index ab75009d1ec5..944164163663 100644 --- a/src/systemcmds/ver/ver.cpp +++ b/src/systemcmds/ver/ver.cpp @@ -47,6 +47,10 @@ #include #include +#ifndef PX4_BL_VERSION +#define PX4_BL_VERSION NULL +#endif + /* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[] = "hwcmp"; @@ -177,6 +181,13 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[]) if (git_branch && git_branch[0]) { PX4_INFO_RAW("PX4 git-branch: %s\n", git_branch); } + + const char *bl_version = PX4_BL_VERSION; + + if (bl_version) { + printf("Bootloader version: %s\n", bl_version); + } + } fwver = px4_firmware_vendor_version(); diff --git a/src/systemcmds/work_queue/CMakeLists.txt b/src/systemcmds/work_queue/CMakeLists.txt index 0ef8d88c6078..f4074853d0ef 100644 --- a/src/systemcmds/work_queue/CMakeLists.txt +++ b/src/systemcmds/work_queue/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE systemcmds__work_queue MAIN work_queue + NO_DAEMON SRCS work_queue_main.cpp ) diff --git a/ssrc_config/README.txt b/ssrc_config/README.txt new file mode 100644 index 000000000000..0e1dc530af83 --- /dev/null +++ b/ssrc_config/README.txt @@ -0,0 +1,14 @@ +This folder contains extra parameter sets for different flying environments. +The config_.txt for each environment needs to be copied into sdcard as /etc/config.txt + +config_outdoor.txt +- Empty configuration + +config_indoor.txt +- Contains settings for maximum speeds (very slow), altitudes (very low), disabling GPS, magnetometer etc. + +config_hitl.txt +- Contains settings for configure px4 into hardware-in-the-loop mode and mavlink setup for UART gazebo connection + +config_hitl_eth.txt +- Contains same settings as config_hitl.txt, but the mavlink for gazebo connection is using ethernet diff --git a/ssrc_config/config_hitl.txt b/ssrc_config/config_hitl.txt new file mode 100644 index 000000000000..6d2f78c5dc0b --- /dev/null +++ b/ssrc_config/config_hitl.txt @@ -0,0 +1,57 @@ +# Default parameter set for HITL with UART gazebo connection (partly derived from indoor flying parameters) +# [ type: hitl ] + +##################################### +# HITL configuration +# + +# Set HITL flag +param set SYS_HITL 1 + +# HIL on TELEMETRY 2 +param set SER_TEL2_BAUD 2000000 +param set MAV_1_CONFIG 102 +param set MAV_1_MODE 2 +param set MAV_1_RATE 200000 + +# disable some checks to allow to fly +# - with usb +param set CBRK_USB_CHK 197848 +# - without real battery +param set CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC controller check +param set NAV_RCL_ACT 0 + + +##################################### +# Configs from indoor flight setup +# + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# Cruise speed +param set MPC_XY_CRUISE 0.5 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Increase velocity controller P gain +param set MPC_XY_VEL_P_ACC 2.4 + +# Make it accelerate faster upwards at takeoff +param set MPC_TKO_RAMP_T 1.0 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 diff --git a/ssrc_config/config_hitl_eth.txt b/ssrc_config/config_hitl_eth.txt new file mode 100644 index 000000000000..170cdbfa10d6 --- /dev/null +++ b/ssrc_config/config_hitl_eth.txt @@ -0,0 +1,35 @@ +# Default parameter set for HITL with ethernet gazebo connection (partly derived from indoor flying parameters) +# [ type: hitl_eth ] + +##################################### +# HITL configuration +# + +# Set HITL flag +param set SYS_HITL 1 + +# Start Mavlink for simulator connection +param set MAV_2_CONFIG 1000 +param set MAV_2_REMOTE_IP0 192 +param set MAV_2_REMOTE_IP1 168 +param set MAV_2_REMOTE_IP2 200 +param set MAV_2_REMOTE_IP3 100 +param set MAV_2_REMOTE_PRT 14561 +param set MAV_2_UDP_PRT 14560 +param set MAV_2_RATE 2000000 +param set MAV_2_MODE 2 + +# disable some checks to allow to fly +# - with usb +param set CBRK_USB_CHK 197848 +# - without real battery +param set CBRK_SUPPLY_CHK 894281 +# - without safety switch +param set COM_PREARM_MODE 0 +param set CBRK_IO_SAFETY 22027 + +# Disable RC controller check +param set NAV_RCL_ACT 0 + +# Disable ekf2 multi-imu, because gazebo provides only one +param set EKF2_MULTI_IMU 1 diff --git a/ssrc_config/config_indoor.txt b/ssrc_config/config_indoor.txt new file mode 100644 index 000000000000..98bc409be6a1 --- /dev/null +++ b/ssrc_config/config_indoor.txt @@ -0,0 +1,66 @@ +# Default parameter set for indoor flying with mocap fake GPS +# [ type: indoor ] + +# Disable internal GPS +param set GPS_1_CONFIG 0 + +# Disable magnetometer +param set CAL_MAG0_PRIO 1 +param set CAL_MAG1_PRIO 0 +param set CAL_MAG2_PRIO 0 +param set CAL_MAG3_PRIO 0 +param set EKF2_MAG_TYPE 5 +param set SYS_HAS_MAG 0 + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# Enable LL40LS in i2c and use that as main height source for EKF +param set EKF2_HGT_REF 2 +param set SENS_EN_LL40LS 2 +param set EKF2_RNG_CTRL 2 + +# Enable GPS Yaw fusion +param set EKF2_GPS_CTRL 11 + +# Disable external vision aid +param set EKF2_EV_CTRL 0 + +# RTL altitudes to avoid hitting the roof +param set RTL_DESCEND_ALT 2 +param set RTL_RETURN_ALT 2 + +# Offboard failsafe, set to land immediately even when RC is available +param set COM_OBL_RC_ACT 4 + +# RC failsafe, land immediately +param set NAV_RCL_ACT 3 + +# Cruise speed +param set MPC_XY_CRUISE 2.0 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# LEDS +param set SER_TEL2_BAUD 57600 +param set MAV_1_CONFIG 102 +param set MAV_1_MODE 7 +param set MAV_1_RATE 1000 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/ssrc_config/config_indoor_sim.txt b/ssrc_config/config_indoor_sim.txt new file mode 100644 index 000000000000..4b0777c923e3 --- /dev/null +++ b/ssrc_config/config_indoor_sim.txt @@ -0,0 +1,44 @@ +# Default parameter set for indoor flying in simulation (with regular gps simulation) +# [ type: indoor_sim ] + +# Set waypoint acceptance radius to 0.5m +param set NAV_ACC_RAD 0.5 + +# Set height acceptance radius to 0.3m +param set NAV_MC_ALT_RAD 0.3 + +# RTL altitudes to avoid hitting the roof +param set RTL_DESCEND_ALT 2 +param set RTL_RETURN_ALT 2 + +# Offboard failsafe, set to land immediately even when RC is available +param set COM_OBL_RC_ACT 4 + +# RC failsafe, land immediately +param set NAV_RCL_ACT 3 + +# Cruise speed +param set MPC_XY_CRUISE 2.0 + +# Track trajectory more aggressively (default 0.5) +param set MPC_XY_TRAJ_P 0.7 + +# Increase velocity controller P gain +param set MPC_XY_VEL_P_ACC 1.9 + +# Make it accelerate faster upwards at takeoff +param set MPC_TKO_RAMP_T 1.0 + +# Limit upward movement speed for safety +param set MPC_Z_VEL_MAX_UP 1.0 + +# Smoothing trajectory a bit when using AutoLineSmoothVel +param set MPC_JERK_AUTO 8 +param set MPC_ACC_HOR 3 + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/ssrc_config/config_outdoor.txt b/ssrc_config/config_outdoor.txt new file mode 100644 index 000000000000..16f468f50167 --- /dev/null +++ b/ssrc_config/config_outdoor.txt @@ -0,0 +1,12 @@ +# Default parameter set for indoor flying with lighthouse tracker / libsurvive fake GPS +# [ type: outdoor ] + +# This file should contain only testing/safety related parameters +# and nothing that affects autonomous flight + +# Default Flight Modes and Kill Switch +param set COM_FLTMODE1 0 +param set COM_FLTMODE4 2 +param set COM_FLTMODE6 11 +param set RC_MAP_FLTMODE 5 +param set RC_MAP_KILL_SW 7 diff --git a/tasks.py b/tasks.py new file mode 100644 index 000000000000..5e800bd1bc8d --- /dev/null +++ b/tasks.py @@ -0,0 +1,121 @@ +import glob +import os +from invoke import task, Collection, call + +THISDIR = os.path.dirname(os.path.realpath(__file__)) +MODULE_NAME = os.path.basename(THISDIR) + +def get_submodules(c): + """ + return repository submodule names + """ + submodules = [] + with c.cd(THISDIR): + result = c.run("git submodule status", hide=True) + for line in result.stdout.splitlines(): + submodules.append(line.split()[1]) + return submodules + +def get_iname_tag(image_name): + """ + return tuple with image name and tag + """ + if ":" in image_name: + iname, tag = image_name.split(":") + else: + iname, tag = image_name, "latest" + return iname, tag + + +@task +def init(c): + """ + Init submodules. + """ + print("init submodules") + with c.cd(THISDIR): + c.run("git submodule init", hide=True) + +@task(init) +def clone(c): + """ + Clone this repository submodules. + """ + submodules = get_submodules(c) + with c.cd(THISDIR): + for sub in submodules: + c.run("git submodule update --init --recursive %s" %sub) + +@task( + help={'nocache': "do not use cache when building the image", + 'pull': "always attempt to pull a newer version of the image", + 'ros_distro': "ROS distro to use (Available [humble])"} +) +def build_sitl(c, nocache=False, pull=False, ros_distro="humble", image_name=MODULE_NAME): + """ + Create Docker build environment. + """ + iname, tag = get_iname_tag(image_name) + + args = [] + args.append("--build-arg VERSION=$(git describe --always --tags --dirty | sed 's/^v//')") + args.append("-f packaging/Dockerfile.sitl") + args.append("-t %s_build:%s" % (iname, tag)) + if nocache: + args.append("--no-cache") + elif pull: + args.append("--pull") + with c.cd(THISDIR): + c.run("docker build %s ." % " ".join(args)) + +@task( + help={'reallyclean': "remove & reload all submodules"} +) +def clean(c, reallyclean=False): + """ + Clean workspace. + """ + with c.cd(THISDIR): + if reallyclean: + c.run("git submodule deinit -f --all") + clone(c) + else: + c.run("git submodule foreach git clean -xdf") + c.run("git submodule foreach git checkout .") + c.run("git clean -xdf") + +@task( + help={'out_dir': "output directory for the generated deb files", + 'ros_distro': "ROS distro to use (Available [humble])"} +) +def create_deb_package(c, out_dir="../bin/", ros_distro="humble", image_name=MODULE_NAME): + """ + Build debian package + """ + c.run("./build.sh {0}" + .format(out_dir)) + +@task(help={'nocache': "do not use cache when building the image", + 'pull': "always attempt to pull a newer version of the image", + 'image_name': "name of output docker image"} +) +def build_docker(c, nocache=False, pull=False, image_name=MODULE_NAME): + """ + Build Docker image of this component + """ + col = Collection() + col.add_task(build_sitl) + col['build_sitl'](c, nocache=nocache, pull=pull, image_name=image_name) + + iname, tag = get_iname_tag(image_name) + args = [] + args.append("--build-arg FROM_ARTIFACTS=%s_build:%s" % (iname, tag)) + args.append("-f packaging/Dockerfile.docker") + args.append("-t %s:%s" % (iname, tag)) + if nocache: + args.append("--no-cache") + elif pull: + args.append("--pull") + with c.cd(THISDIR): + print("docker build %s ." % " ".join(args)) + c.run("docker build %s ." % " ".join(args))