diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index 04c8c210b94a..fcf70a5a2fdc 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -35,6 +35,7 @@ pipeline { def nuttx_builds_archive = [ target: [ + "3dr_ctrl-zero-h7-oem-revg_default", "airmind_mindpx-v2_default", "ark_can-flow_canbootloader", "ark_can-flow_default", @@ -46,6 +47,8 @@ pipeline { "ark_cannode_default", "ark_fmu-v6x_bootloader", "ark_fmu-v6x_default", + "ark_pi6x_bootloader", + "ark_pi6x_default", "atl_mantis-edu_default", "av_x-v1_default", "bitcraze_crazyflie21_default", diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index a9ac652d18b9..4e9788b228bd 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -15,6 +15,7 @@ "extensions": [ "chiehyu.vscode-astyle", "dan-c-underwood.arm", + "editorconfig.editorconfig", "fredericbonnet.cmake-test-adapter", "github.vscode-pull-request-github", "marus25.cortex-debug", diff --git a/.editorconfig b/.editorconfig new file mode 100644 index 000000000000..1ac25a4f5860 --- /dev/null +++ b/.editorconfig @@ -0,0 +1,14 @@ +root = true + +[*] +insert_final_newline = true + +[{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}] +indent_style = tab +tab_width = 8 +# Not in the official standard, but supported by many editors +max_line_length = 120 + +[*.yaml, *.yml] +indent_style = space +indent_size = 2 diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml index 60d93ea9b811..ffbb271a900f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.yml +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -20,14 +20,14 @@ body: 3. Took off '....' 4. See error validations: - required: true + required: false - type: textarea attributes: label: Expected behavior description: A clear and concise description of what you expected to happen. validations: - required: true + required: false - type: textarea attributes: @@ -45,7 +45,7 @@ body: placeholder: | # PASTE HERE THE LINK TO THE LOG validations: - required: true + required: false - type: markdown attributes: @@ -60,14 +60,14 @@ body: placeholder: | # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC validations: - required: true + required: false - type: input attributes: label: Flight controller description: Specify your flight controller model (what type is it, where was it bought from, ...). validations: - required: true + required: false - type: dropdown attributes: diff --git a/.github/workflows/build_all_targets.yml b/.github/workflows/build_all_targets.yml new file mode 100644 index 000000000000..119236b3198e --- /dev/null +++ b/.github/workflows/build_all_targets.yml @@ -0,0 +1,83 @@ +name: Build all targets + +on: + push: + branches: + - 'main' + - 'stable' + - 'beta' + - 'release/*' + pull_request: + branches: + - '*' + +jobs: + group_targets: + name: Scan for Board Targets + runs-on: ubuntu-latest + outputs: + matrix: ${{ steps.set-matrix.outputs.matrix }} + timestamp: ${{ steps.set-timestamp.outputs.timestamp }} + steps: + - uses: actions/checkout@v4 + + - name: Install Python Dependencies + uses: py-actions/py-dependency-install@v4 + with: + path: "./Tools/setup/requirements.txt" + + - id: set-matrix + run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py --group)" + + - id: set-timestamp + run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")" + + setup: + name: ${{ matrix.group }} + runs-on: ubuntu-latest + needs: group_targets + strategy: + matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }} + container: + image: ${{ matrix.container }} + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: ownership workaround + run: git config --system --add safe.directory '*' + + - name: ccache setup keys + uses: actions/cache@v4 + with: + path: ~/.ccache + key: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + restore-keys: ${{ matrix.group }}-ccache-${{ needs.group_targets.outputs.timestamp }} + + - name: setup ccache + run: | + mkdir -p ~/.ccache + echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf + echo "compression = true" >> ~/.ccache/ccache.conf + echo "compression_level = 6" >> ~/.ccache/ccache.conf + echo "max_size = 120M" >> ~/.ccache/ccache.conf + echo "hash_dir = false" >> ~/.ccache/ccache.conf + ccache -s + ccache -z + + - name: build target group + run: | + ./Tools/ci_build_all_runner.sh ${{matrix.targets}} + + - name: Upload px4 package + uses: actions/upload-artifact@v4 + with: + name: px4_${{matrix.group}}_build_artifacts + path: | + build/**/*.px4 + build/**/*.bin + compression-level: 0 + + - name: ccache post-run + run: ccache -s diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index f8a893e74729..a2d7aa5f97a8 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -16,6 +16,7 @@ jobs: matrix: check: [ "check_format", + "check_newlines", "tests", "tests_coverage", "px4_fmu-v2_default stack_check", @@ -23,6 +24,7 @@ jobs: "shellcheck_all", "NO_NINJA_BUILD=1 px4_fmu-v5_default", "NO_NINJA_BUILD=1 px4_sitl_default", + "px4_sitl_allyes", "airframe_metadata", "module_documentation", "parameters_metadata", diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml deleted file mode 100644 index 23e46deab7f3..000000000000 --- a/.github/workflows/compile_linux.yml +++ /dev/null @@ -1,58 +0,0 @@ -name: Compile Linux Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2023-06-26 - strategy: - matrix: - config: [ - beaglebone_blue_default, - emlid_navio2_default, - px4_raspberrypi_default, - scumaker_pilotpi_default, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - name: ownership workaround - run: git config --system --add safe.directory '*' - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - 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 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml deleted file mode 100644 index 5c1c318b411e..000000000000 --- a/.github/workflows/compile_linux_arm64.yml +++ /dev/null @@ -1,54 +0,0 @@ -name: Compile Linux ARM64 Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2022-08-12 - strategy: - matrix: - config: [ - scumaker_pilotpi_arm64, - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - 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 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 100M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make ${{matrix.config}} - run: make ${{matrix.config}} - - name: ccache post-run - run: ccache -s diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml deleted file mode 100644 index cf42be021321..000000000000 --- a/.github/workflows/compile_nuttx.yml +++ /dev/null @@ -1,135 +0,0 @@ -name: Compile Nuttx Targets - -on: - push: - branches: - - 'main' - - 'stable' - - 'beta' - - 'release/*' - pull_request: - branches: - - '*' - -jobs: - build: - runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2022-08-12 - strategy: - fail-fast: false - matrix: - config: [ - airmind_mindpx-v2, - ark_can-flow, - ark_can-gps, - ark_can-rtk-gps, - ark_cannode, - ark_fmu-v6x, - ark_septentrio-gps, - atl_mantis-edu, - av_x-v1, - bitcraze_crazyflie, - bitcraze_crazyflie21, - cuav_can-gps-v1, - cuav_nora, - cuav_x7pro, - cubepilot_cubeorange, - cubepilot_cubeorangeplus, - cubepilot_cubeyellow, - diatone_mamba-f405-mk2, - freefly_can-rtk-gps, - holybro_can-gps-v1, - holybro_durandal-v1, - holybro_kakutef7, - holybro_kakuteh7, - holybro_pix32v5, - matek_gnss-m9n-f4, - matek_h743, - matek_h743-mini, - matek_h743-slim, - modalai_fc-v1, - modalai_fc-v2, - mro_ctrl-zero-classic, - mro_ctrl-zero-f7, - mro_ctrl-zero-f7-oem, - mro_ctrl-zero-h7, - mro_ctrl-zero-h7-oem, - mro_pixracerpro, - mro_x21, - mro_x21-777, - nxp_fmuk66-e, - nxp_fmuk66-v3, - nxp_mr-canhubk3, - nxp_ucans32k146, - omnibus_f4sd, - px4_fmu-v2, - px4_fmu-v3, - px4_fmu-v4, - px4_fmu-v4pro, - px4_fmu-v5, - px4_fmu-v5x, - px4_fmu-v6c, - px4_fmu-v6u, - px4_fmu-v6x, - px4_fmu-v6xrt, - raspberrypi_pico, - sky-drones_smartap-airlink, - spracing_h7extreme, - uvify_core, - siyi_n7 - ] - steps: - - uses: actions/checkout@v1 - with: - token: ${{secrets.ACCESS_TOKEN}} - - - name: Prepare ccache timestamp - id: ccache_cache_timestamp - shell: cmake -P {0} - run: | - 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 - with: - path: ~/.ccache - key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}} - restore-keys: ${{matrix.config}}-ccache- - - name: setup ccache - run: | - mkdir -p ~/.ccache - echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf - echo "compression = true" >> ~/.ccache/ccache.conf - echo "compression_level = 6" >> ~/.ccache/ccache.conf - echo "max_size = 120M" >> ~/.ccache/ccache.conf - echo "hash_dir = false" >> ~/.ccache/ccache.conf - ccache -s - ccache -z - - - name: make all_variants_${{matrix.config}} - run: make all_variants_${{matrix.config}} - timeout-minutes: 45 - - name: make ${{matrix.config}} bloaty_compileunits - run: make ${{matrix.config}} bloaty_compileunits || true - - name: make ${{matrix.config}} bloaty_inlines - run: make ${{matrix.config}} bloaty_inlines || true - - name: make ${{matrix.config}} bloaty_segments - run: make ${{matrix.config}} bloaty_segments || true - - name: make ${{matrix.config}} bloaty_symbols - run: make ${{matrix.config}} bloaty_symbols || true - - name: make ${{matrix.config}} bloaty_templates - run: make ${{matrix.config}} bloaty_templates || true - - name: make ${{matrix.config}} bloaty_ram - run: make ${{matrix.config}} bloaty_ram || true - - name: make ${{matrix.config}} bloaty_compare_master - run: make ${{matrix.config}} bloaty_compare_master || true - - name: ccache post-run - run: ccache -s - - - name: Upload px4 package - uses: actions/upload-artifact@v2 - with: - name: px4_package_${{matrix.config}} - path: | - build/**/*.px4 - build/**/*.bin diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml index b4b6ebc814ea..3f1a4c0049b1 100644 --- a/.github/workflows/deploy_all.yml +++ b/.github/workflows/deploy_all.yml @@ -54,4 +54,3 @@ jobs: AWS_REGION: 'us-west-1' SOURCE_DIR: 'build/${{ matrix.target }}/_metadata/' DEST_DIR: 'Firmware/${{ env.version }}/${{ matrix.target }}/' - diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml index 7e95b1fff082..a90d51804ef5 100644 --- a/.github/workflows/mavros_mission_tests.yml +++ b/.github/workflows/mavros_mission_tests.yml @@ -11,6 +11,8 @@ on: jobs: build: runs-on: ubuntu-latest + env: + ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml index 812f63fec406..020caaa2d048 100644 --- a/.github/workflows/mavros_offboard_tests.yml +++ b/.github/workflows/mavros_offboard_tests.yml @@ -11,6 +11,8 @@ on: jobs: build: runs-on: ubuntu-latest + env: + ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true strategy: fail-fast: false matrix: diff --git a/.gitmodules b/.gitmodules index 9d8626404586..012496100adf 100644 --- a/.gitmodules +++ b/.gitmodules @@ -71,7 +71,7 @@ [submodule "src/modules/zenoh/zenoh-pico"] path = src/modules/zenoh/zenoh-pico url = https://github.com/px4/zenoh-pico - branch = pr-zubf-werror-fix + branch = dev/1.0.0-px4 [submodule "src/lib/heatshrink/heatshrink"] path = src/lib/heatshrink/heatshrink url = https://github.com/PX4/heatshrink.git @@ -81,7 +81,7 @@ url = https://github.com/PX4/PX4-gazebo-models.git branch = main [submodule "boards/modalai/voxl2/libfc-sensor-api"] - path = src/modules/muorb/apps/libfc-sensor-api + path = boards/modalai/voxl2/libfc-sensor-api url = https://gitlab.com/voxl-public/voxl-sdk/core-libs/libfc-sensor-api.git [submodule "src/drivers/actuators/vertiq_io/iq-module-communication-cpp"] path = src/drivers/actuators/vertiq_io/iq-module-communication-cpp diff --git a/.vscode/cmake-kits.json b/.vscode/cmake-kits.json index a7d7e14b60ae..517c8c6d5269 100644 --- a/.vscode/cmake-kits.json +++ b/.vscode/cmake-kits.json @@ -2,4 +2,4 @@ { "name": "PX4 detect" } -] \ No newline at end of file +] diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index c2a057d68669..d55b170485e2 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -111,6 +111,11 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: px4_fmu-v6xrt_bootloader + 3dr_ctrl-zero-h7-oem-revg_default: + short: 3dr_ctrl-zero-h7-oem-revg + buildType: MinSizeRel + settings: + CONFIG: 3dr_ctrl-zero-h7-oem-revg_default airmind_mindpx-v2_default: short: airmind_mindpx-v2 buildType: MinSizeRel @@ -151,16 +156,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_can-rtk-gps_canbootloader - ark_septentrio_gps_default: - short: ark_septentrio_gps_default + ark_septentrio-gps_default: + short: ark_septentrio-gps_default buildType: MinSizeRel settings: - CONFIG: ark_septentrio_gps_default - ark_septentrio_gps_canbootloader: - short: ark_septentrio_gps_canbootloader + CONFIG: ark_septentrio-gps_default + ark_septentrio-gps_canbootloader: + short: ark_septentrio-gps_canbootloader buildType: MinSizeRel settings: - CONFIG: ark_septentrio_gps_canbootloader + CONFIG: ark_septentrio-gps_canbootloader ark_cannode_default: short: ark_cannode_default buildType: MinSizeRel @@ -181,6 +186,16 @@ CONFIG: buildType: MinSizeRel settings: CONFIG: ark_fmu-v6x_default + ark_pi6x_bootloader: + short: ark_pi6x_bootloader + buildType: MinSizeRel + settings: + CONFIG: ark_pi6x_bootloader + ark_pi6x_default: + short: ark_pi6x_default + buildType: MinSizeRel + settings: + CONFIG: ark_pi6x_default atl_mantis-edu_default: short: atl_mantis-edu buildType: MinSizeRel diff --git a/.vscode/extensions.json b/.vscode/extensions.json index ec49a0c75fd1..3da9f2675427 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -4,6 +4,7 @@ "recommendations": [ "chiehyu.vscode-astyle", "dan-c-underwood.arm", + "editorconfig.editorconfig", "fredericbonnet.cmake-test-adapter", "github.vscode-pull-request-github", "marus25.cortex-debug", diff --git a/.vscode/settings.json b/.vscode/settings.json index 67016f098380..0192c81e8602 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -15,6 +15,7 @@ "cmake.buildDirectory": "${workspaceFolder}/build/${variant:CONFIG}", "cmake.configureOnOpen": true, "cmake.ctest.parallelJobs": 1, + "cmake.options.statusBarVisibility": "compact", "cmake.skipConfigureIfCachePresent": true, "cmakeExplorer.buildDir": "${workspaceFolder}/build/px4_sitl_test", "cmakeExplorer.parallelJobs": 1, diff --git a/CMakeLists.txt b/CMakeLists.txt index 658c32794584..fc57b95efc79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved. +# Copyright (c) 2017 - 2024 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 @@ -132,7 +132,9 @@ list(GET VERSION_LIST 2 PX4_VERSION_PATCH) string(REPLACE "-" ";" PX4_VERSION_PATCH ${PX4_VERSION_PATCH}) list(GET PX4_VERSION_PATCH 0 PX4_VERSION_PATCH) -message(STATUS "PX4 version: ${PX4_GIT_TAG} (${PX4_VERSION_MAJOR}.${PX4_VERSION_MINOR}.${PX4_VERSION_PATCH})") +# # Capture only the hash part after 'g' +string(REGEX MATCH "g([a-f0-9]+)$" GIT_HASH "${PX4_GIT_TAG}") +set(PX4_GIT_HASH ${CMAKE_MATCH_1}) define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES BRIEF_DOCS "PX4 module libs" @@ -485,7 +487,7 @@ include(package) # install python requirements using configured python add_custom_target(install_python_requirements - COMMAND ${PYTHON_EXECUTABLE} -m pip install --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt + COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt USES_TERMINAL ) diff --git a/Jenkinsfile b/Jenkinsfile index 0b0c8d1870b3..d7f166b5fe4d 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -171,9 +171,9 @@ pipeline { sh('cp airframes.md PX4-user_guide/en/airframes/airframe_reference.md') sh('cp parameters.md PX4-user_guide/en/advanced_config/parameter_reference.md') sh('cp -R modules/*.md PX4-user_guide/en/modules/') - sh('cp -R graph_*.json PX4-user_guide/.vuepress/public/en/middleware/') + sh('cp -R graph_*.json PX4-user_guide/public/middleware/') // vitepress sh('cp -R msg_docs/*.md PX4-user_guide/en/msg_docs/') - sh('cp -R failsafe_sim/* PX4-user_guide/.vuepress/public/en/config/failsafe') + sh('cp -R failsafe_sim/* PX4-user_guide/public/config/failsafe') // vitepress sh('cd PX4-user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true') sh('cd PX4-user_guide; git push origin main || true') sh('rm -rf PX4-user_guide') diff --git a/Kconfig b/Kconfig index 6427534f91f8..f9f2e5e8ec44 100644 --- a/Kconfig +++ b/Kconfig @@ -185,6 +185,17 @@ menu "Serial ports" string "EXT2 tty port" endmenu +menu "File paths" + + config BOARD_ROOT_PATH + string "PX4 Root file path" + default "/fs/microsd" + + config BOARD_PARAM_FILE + string "Parameter file" + default "/fs/mtd_params" +endmenu + menu "drivers" source "src/drivers/Kconfig" endmenu diff --git a/Makefile b/Makefile index 56c373663ffc..5de475d79074 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 - 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2015 - 2024 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 @@ -323,7 +323,33 @@ px4io_update: cp build/cubepilot_io-v2_default/cubepilot_io-v2_default.bin boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin git status -bootloaders_update: ark_fmu-v6x_bootloader cuav_nora_bootloader cuav_x7pro_bootloader cubepilot_cubeorange_bootloader holybro_durandal-v1_bootloader holybro_kakuteh7_bootloader matek_h743_bootloader matek_h743-mini_bootloader matek_h743-slim_bootloader modalai_fc-v2_bootloader mro_ctrl-zero-classic_bootloader mro_ctrl-zero-h7_bootloader mro_ctrl-zero-h7-oem_bootloader mro_pixracerpro_bootloader px4_fmu-v6c_bootloader px4_fmu-v6u_bootloader px4_fmu-v6x_bootloader +bootloaders_update: \ + 3dr_ctrl-zero-h7-oem-revg_bootloader \ + ark_fmu-v6x_bootloader \ + ark_pi6x_bootloader \ + cuav_nora_bootloader \ + cuav_x7pro_bootloader \ + cubepilot_cubeorange_bootloader \ + cubepilot_cubeorangeplus_bootloader \ + hkust_nxt-dual_bootloader \ + hkust_nxt-v1_bootloader \ + holybro_durandal-v1_bootloader \ + holybro_kakuteh7_bootloader \ + holybro_kakuteh7mini_bootloader \ + holybro_kakuteh7v2_bootloader \ + matek_h743_bootloader \ + matek_h743-mini_bootloader \ + matek_h743-slim_bootloader \ + modalai_fc-v2_bootloader \ + mro_ctrl-zero-classic_bootloader \ + mro_ctrl-zero-h7_bootloader \ + mro_ctrl-zero-h7-oem_bootloader \ + mro_pixracerpro_bootloader \ + px4_fmu-v6c_bootloader \ + px4_fmu-v6u_bootloader \ + px4_fmu-v6x_bootloader \ + px4_fmu-v6xrt_bootloader \ + siyi_n7_bootloader git status .PHONY: coverity_scan @@ -354,9 +380,9 @@ doxygen: @$(PX4_MAKE) -C "$(SRC_DIR)"/build/doxygen @touch "$(SRC_DIR)"/build/doxygen/Documentation/.nojekyll -# Astyle +# Style # -------------------------------------------------------------------- -.PHONY: check_format format +.PHONY: check_format format check_newlines check_format: $(call colorecho,'Checking formatting with astyle') @@ -367,6 +393,10 @@ format: $(call colorecho,'Formatting with astyle') @"$(SRC_DIR)"/Tools/astyle/check_code_style_all.sh --fix +check_newlines: + $(call colorecho,'Checking for missing or duplicate newlines at the end of files') + @"$(SRC_DIR)"/Tools/astyle/check_newlines.sh + # Testing # -------------------------------------------------------------------- .PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance @@ -520,14 +550,14 @@ distclean: # All other targets are handled by PX4_MAKE. Add a rule here to avoid printing an error. %: $(if $(filter $(FIRST_ARG),$@), \ - $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) help|list_config_targets' to get a list of all possible [configuration] targets."),@#) + $(error "Make target $@ not found. It either does not exist or $@ cannot be the first argument. Use '$(MAKE) list_config_targets' to get a list of all possible [configuration] targets."),@#) # Print a list of non-config targets (based on http://stackoverflow.com/a/26339924/1487069) help: @echo "Usage: $(MAKE) " @echo "Where is one of:" @$(MAKE) -pRrq -f $(lastword $(MAKEFILE_LIST)) : 2>/dev/null | \ - awk -v RS= -F: '/^# File/,/^# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ + awk -v RS= -F: '/(^|\n)# Files(\n|$$)/,/(^|\n)# Finished Make data base/ {if ($$1 !~ "^[#.]") {print $$1}}' | sort | \ egrep -v -e '^[^[:alnum:]]' -e '^($(subst $(space),|,$(ALL_CONFIG_TARGETS)))$$' -e '_default$$' -e '^(Makefile)' @echo @echo "Or, $(MAKE) []" diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index 532b659b05e0..a520a4d377d1 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -120,6 +120,7 @@ add_custom_command( ${romfs_gen_root_dir}/init.d/rc.serial ${romfs_gen_root_dir}/init.d/rc.autostart ${romfs_gen_root_dir}/init.d/rc.autostart.post + ${romfs_gen_root_dir}/init.d/rc.filepaths ${romfs_copy_stamp} COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/* COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file} @@ -131,6 +132,9 @@ add_custom_command( --rc-dir ${romfs_gen_root_dir}/init.d --serial-ports ${board_serial_ports} ${added_arguments} --config-files ${module_config_files} #--verbose + COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/filepaths/generate_config.py + --rc-dir ${romfs_gen_root_dir}/init.d + --params-file ${CONFIG_BOARD_PARAM_FILE} COMMAND ${CMAKE_COMMAND} -E touch ${romfs_copy_stamp} WORKING_DIRECTORY ${romfs_gen_root_dir} DEPENDS ${romfs_tar_file} @@ -320,6 +324,7 @@ add_custom_target(romfs_gen_files_target DEPENDS ${romfs_copy_stamp} ${romfs_gen_root_dir}/init.d/rc.serial + ${romfs_gen_root_dir}/init.d/rc.filepaths romfs_extras.stamp ) diff --git a/ROMFS/cannode/init.d/rcS b/ROMFS/cannode/init.d/rcS index 0d9da3a10afc..96edd27ead1b 100644 --- a/ROMFS/cannode/init.d/rcS +++ b/ROMFS/cannode/init.d/rcS @@ -21,25 +21,14 @@ set R / # ver all -if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params -then - set PARAM_FILE /fs/mtd_params -fi - -if mft query -q -k MTD -s MTD_PARAMETERS -v /dev/eeeprom0 -then - set PARAM_FILE /dev/eeeprom0 -fi - -if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/qspi/params -then - set PARAM_FILE /mnt/qspi/params -fi +# Load param file location from kconfig +. ${R}etc/init.d/rc.filepaths # # Load parameters. # param select $PARAM_FILE + if ! param load then param reset_all diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris index fbf554091555..4cd88cc111df 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10017_jmavsim_iris @@ -29,4 +29,3 @@ 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 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10018_gazebo-classic_iris_foggy_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/10018_gazebo-classic_iris_foggy_lidar index 161a32492009..c929125c47f7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10018_gazebo-classic_iris_foggy_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10018_gazebo-classic_iris_foggy_lidar @@ -30,4 +30,3 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default EKF2_RNG_A_HMAX 10 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter index 36bed6a03bc6..765d3c9b0c89 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10019_gazebo-classic_omnicopter @@ -94,4 +94,3 @@ param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10030_gazebo-classic_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/10030_gazebo-classic_px4vision index b82481b7c9e5..f2abb94e2333 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10030_gazebo-classic_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10030_gazebo-classic_px4vision @@ -12,4 +12,3 @@ 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 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane index 8580464c4906..d863ad63587d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_sihsim_airplane @@ -24,8 +24,6 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default BAT_N_CELLS 3 - param set-default SIH_T_MAX 6 param set-default SIH_MASS 0.3 param set-default SIH_IXX 0.00402 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert index 536fc6076c39..2ab294ac2476 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_sihsim_xvert @@ -33,8 +33,6 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default BAT_N_CELLS 3 - param set-default SIH_T_MAX 2 param set-default SIH_Q_MAX 0.0165 param set-default SIH_MASS 0.2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow index 38d9019adc70..d334bdb43088 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1010_gazebo-classic_iris_opt_flow @@ -47,5 +47,5 @@ param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 param set-default SENS_FLOW_MINHGT 0.7 -param set-default SENS_FLOW_MAXHGT 3 +param set-default SENS_FLOW_MAXHGT 15 param set-default SENS_FLOW_MAXR 2.5 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1012_gazebo-classic_iris_rplidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1012_gazebo-classic_iris_rplidar index 3fd2ef636f4d..952be680bf4c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1012_gazebo-classic_iris_rplidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1012_gazebo-classic_iris_rplidar @@ -30,4 +30,3 @@ param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 param set-default LPE_FUSION 242 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision index df11976012a3..de17714dff8f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1013_gazebo-classic_iris_vision @@ -18,4 +18,3 @@ param set-default LPE_FUSION 132 # AEQ: External heading set to use vision input param set-default ATT_EXT_HDG_M 1 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus index f6f07d700ef7..59c009bed966 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1021_gazebo-classic_uuv_hippocampus @@ -41,4 +41,3 @@ 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 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy index 0c6c802f8d21..6f742e4fe88c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1022_gazebo-classic_uuv_bluerov2_heavy @@ -61,4 +61,3 @@ param set-default PWM_MAIN_FUNC5 105 param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_FUNC7 107 param set-default PWM_MAIN_FUNC8 108 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane index f203ee14cc48..dc45a34029bf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_gazebo-classic_plane @@ -68,5 +68,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam index 3dc3c9ee1a54..695f851ff08e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1031_gazebo-classic_plane_cam @@ -39,7 +39,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult index f8fd54636649..12e828c08c66 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1032_gazebo-classic_plane_catapult @@ -41,7 +41,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 @@ -71,4 +70,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal index 06a845be30a3..ef9df5af984e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_jsbsim_rascal @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 -param set-default MIS_DIST_1WP 2500 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 @@ -56,4 +55,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric index 7cb6a3bf811e..ef9df5af984e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_flightgear_rascal-electric @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 -param set-default MIS_DIST_1WP 2500 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod index a8b29db64c6b..68bb1420949d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_gazebo-classic_techpod @@ -54,4 +54,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo index 06a845be30a3..ef9df5af984e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_jsbsim_malolo @@ -24,7 +24,6 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 param set-default MIS_TAKEOFF_ALT 20 -param set-default MIS_DIST_1WP 2500 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 @@ -56,4 +55,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer index 83f63cec3842..0a8343a4c889 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_gazebo-classic_believer @@ -62,4 +62,3 @@ param set-default PWM_MAIN_FUNC7 202 param set-default PWM_MAIN_FUNC8 203 param set-default PWM_MAIN_FUNC9 206 param set-default PWM_MAIN_REV 256 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider index d5b59f700792..ef2cd026061e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_gazebo-classic_glider @@ -39,7 +39,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal index 836615e635a2..544fd746e100 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_flightgear_rascal @@ -23,9 +23,7 @@ param set-default FW_RR_P 0.085 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 20 -param set-default MIS_DIST_1WP 2500 param set-default NAV_ACC_RAD 15 param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol index 0e25b5633037..2324f9588571 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_gazebo-classic_standard_vtol @@ -78,4 +78,3 @@ param set-default VT_FWD_THRUST_EN 4 param set-default VT_FWD_THRUST_SC 1 param set-default VT_F_TRANS_THR 0.75 param set-default VT_TYPE 2 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter index 8a3d5d1a4965..a803fcee3cde 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_gazebo-classic_tailsitter @@ -75,4 +75,3 @@ param set-default VT_F_TRANS_DUR 1.5 param set-default VT_TYPE 0 param set-default WV_EN 0 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop index 29f09dc47c9d..eadf0d4e0249 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_gazebo-classic_standard_vtol_drop @@ -88,4 +88,3 @@ param set-default PWM_MAIN_FUNC11 422 param set-default RC_MAP_AUX1 8 param set-default RC_MAP_AUX2 9 param set-default RC_MAP_AUX3 10 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar index 22d7e21119ee..23b7757d4bcd 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1044_gazebo-classic_plane_lidar @@ -39,7 +39,6 @@ param set-default FW_T_SINK_MIN 2.2 param set-default FW_W_EN 1 -param set-default MIS_LTRMIN_ALT 30 param set-default MIS_TAKEOFF_ALT 30 param set-default NAV_ACC_RAD 15 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover index ab8d12109205..920d022f2107 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1060_gazebo-classic_rover @@ -31,4 +31,3 @@ param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 201 param set-default PWM_MAIN_FUNC6 101 param set-default PWM_MAIN_FUNC7 101 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 index f3a6cd1aded5..75cbe5b5dfaf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1062_flightgear_tf-r1 @@ -38,4 +38,3 @@ param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 201 param set-default PWM_MAIN_FUNC6 101 param set-default PWM_MAIN_FUNC7 101 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat index 01ada36f3510..fce37ecde279 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1070_gazebo-classic_boat @@ -41,4 +41,3 @@ param set-default CA_R_REV 3 param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 index 9c9b2c7f6312..dd3b5759bab0 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_flightgear_tf-g1 @@ -64,4 +64,3 @@ param set-default PWM_MAIN_FUNC4 203 param set-default PWM_MAIN_FUNC5 407 param set-default PWM_MAIN_FUNC6 408 param set-default PWM_MAIN_FUNC7 409 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 index 74b6d2a31679..ffb9ef2235e5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17002_flightgear_tf-g2 @@ -69,4 +69,3 @@ param set-default PWM_MAIN_FUNC4 203 param set-default PWM_MAIN_FUNC5 407 param set-default PWM_MAIN_FUNC6 408 param set-default PWM_MAIN_FUNC7 409 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship index 153247203da6..b0eb3bbddc8f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/2507_gazebo-classic_cloudship @@ -36,4 +36,3 @@ param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 201 param set-default PWM_MAIN_FUNC4 103 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bb6bedd386d1..bca3feddfb8b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -52,3 +52,4 @@ param set-default SIM_GZ_EC_MAX3 1000 param set-default SIM_GZ_EC_MAX4 1000 param set-default MPC_THR_HOVER 0.60 +param set-default NAV_DLL_ACT 2 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 3390ee525cc4..96bb25d69a52 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 @@ -101,8 +101,9 @@ param set-default MPC_XY_VEL_I_ACC 4 param set-default MPC_XY_VEL_D_ACC 0.1 param set-default NAV_ACC_RAD 5 +param set-default NAV_DLL_ACT 2 param set-default VT_FWD_THRUST_EN 4 -param set-default VT_F_TRANS_THR 0.75 +param set-default VT_F_TRANS_THR 0.3 param set-default VT_TYPE 2 param set-default FD_ESCS_EN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 92560eb160df..acfcef896069 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -132,3 +132,5 @@ param set-default SIM_GZ_EC_MAX1 1100 param set-default SIM_GZ_EC_MAX2 1100 param set-default SIM_GZ_EC_MAX3 1100 param set-default SIM_GZ_EC_MAX4 1100 + +param set-default NAV_DLL_ACT 2 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 80664cf661c0..7697e23db769 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -6,16 +6,36 @@ . ${R}etc/init.d/rc.rover_differential_defaults PX4_SIMULATOR=${PX4_SIMULATOR:=gz} -PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover} param set-default SIM_GZ_EN 1 # Gazebo bridge +# Rover parameters +param set-default RD_WHEEL_TRACK 0.3 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 6 +param set-default RD_MAX_JERK 30 +param set-default RD_MAX_SPEED 7 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 180 +param set-default RD_MISS_SPD_DEF 7 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 + # Simulated sensors param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 1 +param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 1dcb917a09ad..c0d91e222365 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -15,14 +15,29 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 1 +param set-default SENS_EN_ARSPDSIM 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 -# Set Differential Drive Kinematics Library parameters: -param set RDD_WHEEL_BASE 0.9 -param set RDD_WHEEL_RADIUS 0.22 -param set RDD_WHEEL_SPEED 10.0 # Maximum wheel speed rad/s, approx 8 km/h +# Rover parameters +param set-default RD_WHEEL_TRACK 0.9 +param set-default RD_MAN_YAW_SCALE 0.1 +param set-default RD_YAW_RATE_I 0.1 +param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_ACCEL 1 +param set-default RD_MAX_JERK 3 +param set-default RD_MAX_SPEED 8 +param set-default RD_HEADING_P 5 +param set-default RD_HEADING_I 0.1 +param set-default RD_MAX_YAW_RATE 30 +param set-default RD_MISS_SPD_DEF 8 +param set-default RD_TRANS_DRV_TRN 0.349066 +param set-default RD_TRANS_TRN_DRV 0.174533 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_MAX 30 +param set-default PP_LOOKAHD_MIN 2 +param set-default PP_LOOKAHD_GAIN 1 # Actuator mapping - set SITL motors/servos output parameters: @@ -36,14 +51,14 @@ param set-default SIM_GZ_WH_FUNC1 101 # right wheel param set-default SIM_GZ_WH_FUNC2 102 # left wheel #param set-default SIM_GZ_WH_MIN2 0 #param set-default SIM_GZ_WH_MAX2 200 -#aram set-default SIM_GZ_WH_DIS2 100 +#param set-default SIM_GZ_WH_DIS2 100 #param set-default SIM_GZ_WH_FAIL2 100 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels -# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. -# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate -# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator +# Note: The servo configurations ( SIM_GZ_SV_FUNC*) outlined below are intended for educational purposes in this simulation. +# They do not have physical effects in the simulated environment, except for actuating the joints. Their definitions are meant to demonstrate +# how actuators could be mapped and configured in a real-world application, providing a foundation for understanding and implementing actuator # controls in practical scenarios. # Cutter deck blades clutch, PCA9685 servo channel 3, "RC FLAPS" (406) - leftmost switch, or "Servo 3" (203): diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann new file mode 100644 index 000000000000..6f81b7036aa0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -0,0 +1,49 @@ +#!/bin/sh +# @name Rover Ackermann +# @type Rover +# @class Rover + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_ackermann} + +param set-default SIM_GZ_EN 1 # Gazebo bridge + +# Rover parameters +param set-default NAV_ACC_RAD 0.5 +param set-default RA_ACC_RAD_GAIN 2 +param set-default RA_ACC_RAD_MAX 3 +param set-default RA_MAX_ACCEL 1.5 +param set-default RA_MAX_JERK 15 +param set-default RA_MAX_SPEED 3 +param set-default RA_MAX_STR_ANG 0.5236 +param set-default RA_MAX_STR_RATE 360 +param set-default RA_MISS_VEL_DEF 3 +param set-default RA_MISS_VEL_GAIN 5 +param set-default RA_MISS_VEL_MIN 1 +param set-default RA_SPEED_I 0.01 +param set-default RA_SPEED_P 2 +param set-default RA_WHEEL_BASE 0.321 + +# Pure Pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1 + +# Simulated sensors +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 +param set-default SENS_EN_ARSPDSIM 0 + +# Wheels +param set-default SIM_GZ_WH_FUNC1 101 +param set-default SIM_GZ_WH_MIN1 0 +param set-default SIM_GZ_WH_MAX1 200 +param set-default SIM_GZ_WH_DIS1 100 + +# Steering +param set-default SIM_GZ_SV_FUNC1 201 +param set-default SIM_GZ_SV_REV 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar new file mode 100644 index 000000000000..a316310abe42 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar @@ -0,0 +1,10 @@ +#!/bin/sh +# +# @name Gazebo x500 lidar +# +# @type Quadrotor +# + +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar} + +. ${R}etc/init.d-posix/airframes/4001_gz_x500 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 index 0174411c73a6..f75374b15c64 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_gazebo-classic_typhoon_h480 @@ -63,4 +63,3 @@ param set-default PWM_MAIN_FUNC9 422 # Landing gear param set-default PWM_MAIN_FUNC10 400 param set-default PWM_MAIN_FUNC11 400 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index 707df1bba118..b43b61025c2e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -119,4 +119,3 @@ param set-default CA_METHOD 0 # disable attitude failure detection param set-default FD_FAIL_P 0 param set-default FD_FAIL_R 0 - diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 9277ad303947..9235b2e66340 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -83,6 +83,8 @@ px4_add_romfs_files( 4009_gz_r1_rover 4010_gz_x500_mono_cam 4011_gz_lawnmower + 4012_gz_rover_ackermann + 4013_gz_x500_lidar 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 8121f3914c2a..76be887e84bb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -15,6 +15,9 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" if [ -n "${PX4_HOME_LON}" ]; then param set SIH_LOC_LON0 ${PX4_HOME_LON} fi + if [ -n "${PX4_HOME_ALT}" ]; then + param set SIH_LOC_H0 ${PX4_HOME_ALT} + fi if simulator_sih start; then diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 0084a6701e6d..8902b3664f07 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -157,6 +157,7 @@ param set-default CBRK_SUPPLY_CHK 894281 # disable check, no CPU load reported on posix yet param set-default COM_CPU_MAX -1 +param set-default COM_RAM_MAX -1 # Don't require RC calibration and configuration param set-default COM_RC_IN_MODE 1 @@ -182,10 +183,6 @@ param set-default SYS_FAILURE_EN 1 # does not go below 50% by default, but failure injection can trigger failsafes. param set-default COM_LOW_BAT_ACT 2 -# Allow to override SYS_MC_EST_GROUP via env -if [ -n "$SYS_MC_EST_GROUP" ]; then - param set SYS_MC_EST_GROUP $SYS_MC_EST_GROUP -fi # Adapt timeout parameters if simulation runs faster or slower than realtime. if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then @@ -256,6 +253,23 @@ manual_control start sensors start commander start +# +# state estimator selection +if param compare -s EKF2_EN 1 +then + ekf2 start & +fi + +if param compare -s LPE_EN 1 +then + local_position_estimator start +fi + +if param compare -s ATT_EN 1 +then + attitude_estimator_q start +fi + if ! pwm_out_sim start -m sim then tune_control play error @@ -330,7 +344,7 @@ fi [ -e "$autostart_file".post ] && . "$autostart_file".post # Run script to start logging -if param compare SYS_MC_EST_GROUP 2 +if param compare -s EKF2_EN 1 then set LOGGER_ARGS "-p ekf2_timestamps" else diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt index 34b255e2555f..3319779096c5 100644 --- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_romfs_files( # TODO rc.balloon_apps rc.balloon_defaults + rc.sysinit ) if(CONFIG_MODULES_AIRSHIP_ATT_CONTROL) @@ -77,13 +78,20 @@ if(CONFIG_MODULES_ROVER_POS_CONTROL) ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) px4_add_romfs_files( rc.rover_differential_apps rc.rover_differential_defaults ) endif() +if(CONFIG_MODULES_ROVER_ACKERMANN) + px4_add_romfs_files( + rc.rover_ackermann_apps + rc.rover_ackermann_defaults + ) +endif() + if(CONFIG_MODULES_UUV_ATT_CONTROL) px4_add_romfs_files( rc.uuv_apps diff --git a/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox index 5424cbf9a27e..754d048ef89d 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/11001_hexa_cox @@ -44,4 +44,3 @@ param set-default CA_ROTOR4_KM -0.05 param set-default CA_ROTOR5_PX 0.25 param set-default CA_ROTOR5_PY -0.433 param set-default CA_ROTOR5_PZ 0.05 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil index 07c7d925c238..dacfa2ec7be0 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1101_rc_plane_sih.hil @@ -44,8 +44,6 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default BAT_N_CELLS 3 - param set SIH_T_MAX 6 param set SIH_MASS 0.3 param set SIH_IXX 0.00402 diff --git a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil index 6ac7bea691c4..a9b2f7f03b1b 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil +++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil @@ -62,8 +62,6 @@ param set-default CBRK_SUPPLY_CHK 894281 # - without safety switch param set-default CBRK_IO_SAFETY 22027 -param set-default BAT_N_CELLS 3 - param set SIH_T_MAX 2.0 param set SIH_Q_MAX 0.0165 param set SIH_MASS 0.2 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad index 0b2ea2596637..6456ccc74418 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad +++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad @@ -66,7 +66,6 @@ param set-default MC_PITCHRATE_I 0.05 param set-default MC_YAWRATE_MAX 20 param set-default MC_AIRMODE 1 -param set-default MIS_DIST_1WP 100 param set-default MIS_TAKEOFF_ALT 15 param set-default MPC_XY_P 0.8 diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark index cc8db21732a1..ee98e2c889ea 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark +++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark @@ -119,5 +119,3 @@ param set-default PWM_MAIN_TIM0 50 param set-default PWM_MAIN_DIS1 1500 param set-default PWM_MAIN_DIS2 1500 param set-default PWM_MAIN_DIS4 1500 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt b/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt index 3eb558c46e78..9bd320f85c22 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt +++ b/ROMFS/px4fmu_common/init.d/airframes/14001_generic_mc_with_tilt @@ -32,4 +32,3 @@ param set-default CA_SV_TL0_MAXA 45 param set-default CA_SV_TL0_MINA -45 param set-default CA_SV_TL0_TD 90 param set-default CA_SV_TL_COUNT 1 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter index 36f1a68f1472..de21a8821668 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter +++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter @@ -24,5 +24,3 @@ param set-default MC_PITCHRATE_D 0 param set-default MC_PITCHRATE_FF 0.1 param set-default CA_AIRFRAME 10 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 b/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 index 90965eeb32db..d657af7d57bd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 +++ b/ROMFS/px4fmu_common/init.d/airframes/18001_TF-B1 @@ -23,4 +23,3 @@ param set-default MAV_0_MODE 1 param set-default MAV_0_CONFIG 102 param set-default GPS_UBX_DYNMODEL 8 param set-default SER_TEL2_BAUD 9600 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross index feac59e94ef5..e27d146e0301 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross @@ -72,5 +72,3 @@ param set-default PWM_MAIN_DIS5 1000 param set-default PWM_MAIN_DIS6 1500 param set-default PWM_MAIN_DIS7 1500 param set-default PWM_MAIN_DIS8 1500 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox index 23944e9e0ccf..f894d47d74fd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox +++ b/ROMFS/px4fmu_common/init.d/airframes/24001_dodeca_cox @@ -66,4 +66,3 @@ param set-default CA_ROTOR11_PX -0.344 param set-default CA_ROTOR11_PY -0.25 param set-default CA_ROTOR11_PZ -0.05 param set-default CA_ROTOR11_KM -0.05 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship index 65ecfe25eb0a..82abd7e55d24 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship +++ b/ROMFS/px4fmu_common/init.d/airframes/2507_cloudship @@ -38,4 +38,3 @@ param set-default CA_SV_CS_COUNT 1 param set-default CA_SV_CS0_TRQ_P 1 param set-default CA_R_REV 7 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames index 03adea0bee67..2cc60c5217e8 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames +++ b/ROMFS/px4fmu_common/init.d/airframes/4017_nxp_hovergames @@ -6,6 +6,7 @@ # @class Copter # # @board px4_fmu-v2 exclude +# @board px4_fmu-v5x exclude # @board bitcraze_crazyflie exclude # # @maintainer Iain Galloway diff --git a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu index d3f48bcc0780..e5e5e083437f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu +++ b/ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu @@ -7,6 +7,10 @@ # # @maintainer # @board px4_fmu-v2 exclude +# @board cuav_x7pro exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # . ${R}etc/init.d/rc.mc_defaults @@ -36,7 +40,6 @@ param set-default COM_RC_LOSS_T 3 # ekf2 -param set-default EKF2_TERR_MASK 1 param set-default EKF2_BARO_NOISE 2 param set-default EKF2_BCOEF_X 31.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo index ce1c3f8f0e54..8cfc14ae1ef7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4071_ifo +++ b/ROMFS/px4fmu_common/init.d/airframes/4071_ifo @@ -12,6 +12,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board bitcraze_crazyflie exclude # @board cuav_x7pro exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s index a544a49e0367..276e1b45db27 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s +++ b/ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s @@ -10,6 +10,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board bitcraze_crazyflie exclude # @board cuav_x7pro exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 index 22f8d74462be..885f0239d589 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 +++ b/ROMFS/px4fmu_common/init.d/airframes/4500_clover4 @@ -8,6 +8,7 @@ # @maintainer Oleg Kalachev # # @board px4_fmu-v2 exclude +# @board px4_fmu-v4pro exclude # @board bitcraze_crazyflie exclude # diff --git a/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 new file mode 100644 index 000000000000..d599af5b3868 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/4601_droneblocks_dexi_5 @@ -0,0 +1,80 @@ +#!/bin/sh +# +# @name Droneblocks DEXI 5 +# +# @type Quadrotor x +# @class Copter +# +# @maintainer Dennis Baldwin +# @maintainer Alex klimaj +# +# @board px4_fmu-v2 exclude +# @board px4_fmu-v3 exclude +# @board px4_fmu-v4 exclude +# @board px4_fmu-v4pro exclude +# @board px4_fmu-v5 exclude +# @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude +# @board bitcraze_crazyflie exclude +# @board diatone_mamba-f405-mk2 exclude +# @board ark_fmu-v6x exclude +# + +. ${R}etc/init.d/rc.mc_defaults + +param set-default BAT1_CAPACITY 4000 +param set-default BAT1_N_CELLS 6 +param set-default BAT1_V_EMPTY 3.3 +param set-default BAT_AVRG_CURRENT 13 + +# Square quadrotor X PX4 numbering +param set-default CA_ROTOR_COUNT 4 +param set-default CA_ROTOR0_PX 0.0762 +param set-default CA_ROTOR0_PY 0.09525 +param set-default CA_ROTOR0_KM 0.05 +param set-default CA_ROTOR1_PX -0.0762 +param set-default CA_ROTOR1_PY -0.09525 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR2_PX 0.0762 +param set-default CA_ROTOR2_PY -0.09525 +param set-default CA_ROTOR2_KM -0.05 +param set-default CA_ROTOR3_PX -0.0762 +param set-default CA_ROTOR3_PY 0.09525 +param set-default CA_ROTOR3_KM -0.05 + +param set-default EKF2_MIN_RNG 0.01 +param set-default EKF2_OF_POS_X 0.043 +param set-default EKF2_OF_POS_Y 0.011 +param set-default EKF2_OF_QMIN_GND 1 +param set-default EKF2_RNG_POS_X 0.043 +param set-default EKF2_RNG_POS_Y 0.0 + +param set-default IMU_GYRO_DNF_EN 2 + +param set-default MC_PITCHRATE_D 0.0013 +param set-default MC_PITCHRATE_I 0.185 +param set-default MC_PITCHRATE_P 0.105 +param set-default MC_PITCH_P 7.5 +param set-default MC_ROLLRATE_D 0.0010 +param set-default MC_ROLLRATE_I 0.165 +param set-default MC_ROLLRATE_P 0.095 +param set-default MC_ROLL_P 7.5 +param set-default MC_YAWRATE_I 0.35 +param set-default MC_YAWRATE_P 0.13 + +param set-default MPC_THR_HOVER 0.22 +param set-default MPC_THR_MAX 0.5 +param set-default MPC_THR_MIN 0.025 +param set-default MPC_VEL_MANUAL 5.0 +param set-default MPC_XY_VEL_MAX 8.0 + +param set-default RC_CRSF_PRT_CFG 300 +param set-default RC_CRSF_TEL_EN 1 + +param set-default RTL_RETURN_ALT 15 + +param set-default SENS_FLOW_MINHGT 0.0 + +param set-default SER_TEL2_BAUD 3000000 + +param set-default UXRCE_DDS_CFG 102 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 37dfc34def96..4833d17af23c 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -13,11 +13,12 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board px4_fmu-v6x exclude # @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults -param set-default SYS_MC_EST_GROUP 2 + param set-default SYS_HAS_MAG 0 param set-default EKF2_OF_CTRL 1 param set-default EKF2_GPS_CTRL 0 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential new file mode 100644 index 000000000000..8c490d497be2 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/50000_generic_rover_differential @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Generic Rover Differential +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_differential_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover similarity index 85% rename from ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover rename to ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 20a16428b2ed..c06b158cf857 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -15,9 +15,6 @@ param set-default BAT1_N_CELLS 4 -param set-default EKF2_GBIAS_INIT 0.01 -param set-default EKF2_ANGERR_INIT 0.01 - # Set geometry & output configration param set-default RBCLW_ADDRESS 128 param set-default RBCLW_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann new file mode 100644 index 000000000000..43620a50ddcd --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/51000_generic_rover_ackermann @@ -0,0 +1,12 @@ +#!/bin/sh +# +# @name Generic Rover Ackermann +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_ackermann_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho new file mode 100644 index 000000000000..6f50c7f4a1e0 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho @@ -0,0 +1,37 @@ +#!/bin/sh +# +# @name Axial SCX10 2 Trail Honcho +# +# @url https://www.axialadventure.com/product/1-10-scx10-ii-trail-honcho-4wd-rock-crawler-brushed-rtr/AXID9059.html +# +# @type Rover +# @class Rover +# +# @board px4_fmu-v2 exclude +# @board bitcraze_crazyflie exclude +# + +. ${R}etc/init.d/rc.rover_ackermann_defaults + +param set-default BAT1_N_CELLS 3 + +# Rover parameters +param set-default NAV_ACC_RAD 0.5 +param set-default RA_ACC_RAD_GAIN 2 +param set-default RA_ACC_RAD_MAX 3 +param set-default RA_MAX_ACCEL 0.5 +param set-default RA_MAX_JERK 10 +param set-default RA_MAX_SPEED 2.7 +param set-default RA_MAX_STR_ANG 0.5236 +param set-default RA_MAX_STR_RATE 270 +param set-default RA_MISS_VEL_DEF 2.7 +param set-default RA_MISS_VEL_GAIN 3.5 +param set-default RA_MISS_VEL_MIN 1 +param set-default RA_SPEED_I 0.1 +param set-default RA_SPEED_P 0.5 +param set-default RA_WHEEL_BASE 0.321 + +# Pure pursuit parameters +param set-default PP_LOOKAHD_GAIN 1 +param set-default PP_LOOKAHD_MAX 10 +param set-default PP_LOOKAHD_MIN 1.5 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle similarity index 96% rename from ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle rename to ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle index ba4598ce832d..a773c1158d44 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50000_generic_ground_vehicle +++ b/ROMFS/px4fmu_common/init.d/airframes/59000_generic_ground_vehicle @@ -1,6 +1,6 @@ #!/bin/sh # -# @name Generic Ground Vehicle (Ackermann) +# @name Generic Ground Vehicle (Deprecated) # # @type Rover # @class Rover @@ -48,5 +48,3 @@ param set-default PWM_MAIN_FUNC1 201 param set-default PWM_MAIN_FUNC2 201 param set-default PWM_MAIN_FUNC6 101 param set-default PWM_MAIN_FUNC7 101 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx similarity index 97% rename from ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx rename to ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx index 67620ddc0375..9f08553d970f 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50004_nxpcup_car_dfrobot_gpx +++ b/ROMFS/px4fmu_common/init.d/airframes/59001_nxpcup_car_dfrobot_gpx @@ -1,6 +1,6 @@ #!/bin/sh # -# @name NXP Cup car: DF Robot GPX +# @name NXP Cup car: DF Robot GPX (Deprecated) # # @type Rover diff --git a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic index 3a9ceaa5d95f..aecf862113d7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic +++ b/ROMFS/px4fmu_common/init.d/airframes/60000_uuv_generic @@ -42,4 +42,3 @@ param set-default CA_ROTOR3_KM 0 param set-default CA_ROTOR3_PX 0 param set-default CA_ROTOR3_PY -0.3 param set-default CA_ROTOR3_PZ 0.3 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x index ec8720d1dc34..016671a5f570 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x +++ b/ROMFS/px4fmu_common/init.d/airframes/6001_hexa_x @@ -31,5 +31,3 @@ param set-default CA_ROTOR4_PY 0.25 param set-default CA_ROTOR5_PX -0.43 param set-default CA_ROTOR5_PY -0.25 param set-default CA_ROTOR5_KM -0.05 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r index e0eefa351bd8..7d641c272dac 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r +++ b/ROMFS/px4fmu_common/init.d/airframes/6002_draco_r @@ -123,4 +123,3 @@ param set-default PWM_MAIN_FUNC6 106 param set-default PWM_MAIN_TIM0 -1 param set-default PWM_MAIN_TIM1 -1 param set-default PWM_MAIN_TIM2 -1 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ index b22afec00ea1..872e0bb941f4 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/7001_hexa_+ @@ -31,5 +31,3 @@ param set-default CA_ROTOR4_PY -0.43 param set-default CA_ROTOR5_PX -0.25 param set-default CA_ROTOR5_PY 0.43 param set-default CA_ROTOR5_KM -0.05 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x b/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x index 19c112b3066a..4f2d13b24cca 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x +++ b/ROMFS/px4fmu_common/init.d/airframes/8001_octo_x @@ -36,5 +36,3 @@ param set-default CA_ROTOR6_PY -0.46 param set-default CA_ROTOR7_KM -0.05 param set-default CA_ROTOR7_PX -0.19 param set-default CA_ROTOR7_PY 0.46 - - diff --git a/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ b/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ index 53dba8b799d4..98e45f9bef50 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ +++ b/ROMFS/px4fmu_common/init.d/airframes/9001_octo_+ @@ -36,4 +36,3 @@ param set-default CA_ROTOR6_PY -0.5 param set-default CA_ROTOR7_KM -0.05 param set-default CA_ROTOR7_PX 0 param set-default CA_ROTOR7_PY 0.5 - diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt index 66e29e32b4dd..634eb3e1b882 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt @@ -69,6 +69,7 @@ if(CONFIG_MODULES_MC_RATE_CONTROL) 4071_ifo 4073_ifo-s 4500_clover4 + 4601_droneblocks_dexi_5 4901_crazyflie21 # [5000, 5999] Quadrotor + @@ -135,16 +136,27 @@ if(CONFIG_MODULES_VTOL_ATT_CONTROL) ) endif() -if(CONFIG_MODULES_ROVER_POS_CONTROL) +if(CONFIG_MODULES_ROVER_DIFFERENTIAL) + px4_add_romfs_files( + # [50000, 50999] Differential rovers + 50000_generic_rover_differential + 50001_aion_robotics_r1_rover + ) +endif() + +if(CONFIG_MODULES_ROVER_ACKERMANN) px4_add_romfs_files( - 50000_generic_ground_vehicle - 50004_nxpcup_car_dfrobot_gpx + # [51000, 51999] Ackermann rovers + 51000_generic_rover_ackermann + 51001_axial_scx10_2_trail_honcho ) endif() -if(CONFIG_MODULES_DIFFERENTIAL_DRIVE) +if(CONFIG_MODULES_ROVER_POS_CONTROL) px4_add_romfs_files( - 50003_aion_robotics_r1_rover + # [59000, 59999] Rover position control (deprecated) + 59000_generic_ground_vehicle + 59001_nxpcup_car_dfrobot_gpx ) endif() diff --git a/ROMFS/px4fmu_common/init.d/rc.airship_apps b/ROMFS/px4fmu_common/init.d/rc.airship_apps index 837a3a21613d..090c16b2231d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.airship_apps +++ b/ROMFS/px4fmu_common/init.d/rc.airship_apps @@ -5,49 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - -# -# LPE -# -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_apps b/ROMFS/px4fmu_common/init.d/rc.balloon_apps index 2adb216ec145..7f42a3b27ead 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_apps +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_apps @@ -4,39 +4,3 @@ # # NOTE: Script variables are declared/initialized/unset in the rcS script. # - -# -# Start the attitude and position estimator. -# - -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi diff --git a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults index 6f7d925e0c42..70161f3fa3d5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.balloon_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.balloon_defaults @@ -13,4 +13,6 @@ param set-default MAV_TYPE 8 # # Default parameters for balloon UAVs. # -param set-default SYS_MC_EST_GROUP 1 +param set-default LPE_EN 1 +param set-default ATT_EN 1 +param set-default EKF2_EN 0 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index f3c559298e8c..81af5e1acd7a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -5,11 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -# -# Start the attitude and position estimator. -# -ekf2 start & - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index 8158ab8b03cf..d5d9989c8666 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,49 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - -# -# LPE -# -if param compare SYS_MC_EST_GROUP 1 -then - # - # Try to start LPE. If it fails, start EKF2 as a default. - # Unfortunately we do not build it on px4_fmu-v2 due to a limited flash. - # - if attitude_estimator_q start - then - echo "WARN [init] Estimator LPE unsupported, EKF2 recommended." - local_position_estimator start - else - echo "ERROR [init] Estimator LPE not available. Using EKF2" - param set SYS_MC_EST_GROUP 2 - param save - reboot - fi -else - # - # Q estimator (attitude estimation only) - # - if param compare SYS_MC_EST_GROUP 3 - then - attitude_estimator_q start - else - # - # EKF2 - # - param set SYS_MC_EST_GROUP 2 - ekf2 start & - fi -fi - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps new file mode 100644 index 000000000000..181233babe26 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_apps @@ -0,0 +1,8 @@ +#!/bin/sh +# Standard apps for an ackermann rover. + +# Start rover ackermann module. +rover_ackermann start + +# Start Land Detector. +land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults new file mode 100644 index 000000000000..fe0ae7aa9bb8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.rover_ackermann_defaults @@ -0,0 +1,11 @@ +#!/bin/sh +# Ackermann rover parameters. + +set VEHICLE_TYPE rover_ackermann +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 5 # Rover (Ackermann) +param set-default CA_R_REV 1 # Motor is assumed to be reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_apps b/ROMFS/px4fmu_common/init.d/rc.rover_apps index 1db046ace4a1..2decfbb5ed15 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_apps @@ -5,11 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -# -# Start the attitude and position estimator. -# -ekf2 start & - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps index 848527a9e975..e1a7ecd9ccd2 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_apps @@ -1,11 +1,8 @@ #!/bin/sh -# Standard apps for a differential drive rover. +# Standard apps for a differential rover. -# Start the attitude and position estimator. -ekf2 start & - -# Start rover differential drive controller. -differential_drive start +# Start rover differential module. +rover_differential start # Start Land Detector. land_detector start rover diff --git a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults index e0cace3a3238..7c29ba364f3e 100644 --- a/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.rover_differential_defaults @@ -2,10 +2,10 @@ # Differential rover parameters. set VEHICLE_TYPE rover_differential - -param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER - -param set-default CA_AIRFRAME 6 # Rover (Differential) -param set-default CA_R_REV 3 # Right and left motors reversible - -param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying +param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER +param set-default CA_AIRFRAME 6 # Rover (Differential) +param set-default CA_R_REV 3 # Right and left motors reversible +param set-default EKF2_MAG_TYPE 1 # Make sure magnetometer is fused even when not flying +param set-default NAV_ACC_RAD 0.5 # Waypoint acceptance radius +param set-default EKF2_GBIAS_INIT 0.01 +param set-default EKF2_ANGERR_INIT 0.01 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 490038c608fe..5410d28beeb1 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -138,6 +138,12 @@ then adis16507 -S start fi +# SCH16T spi external IMU +if param compare -s SENS_EN_SCH16T 1 +then + sch16t -S start +fi + # Eagle Tree airspeed sensor external I2C if param compare -s SENS_EN_ETSASPD 1 then diff --git a/ROMFS/px4fmu_common/init.d/rc.sysinit b/ROMFS/px4fmu_common/init.d/rc.sysinit new file mode 100644 index 000000000000..9f878a1387bb --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.sysinit @@ -0,0 +1,4 @@ +#!/bin/sh +# +# Standard system init script +# diff --git a/ROMFS/px4fmu_common/init.d/rc.uuv_apps b/ROMFS/px4fmu_common/init.d/rc.uuv_apps index d2eb13208c25..f604b644525c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.uuv_apps +++ b/ROMFS/px4fmu_common/init.d/rc.uuv_apps @@ -5,16 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator Group Selection # -############################################################################### - -ekf2 start & - -############################################################################### -# End Estimator Group Selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup index 1b7ddee5a569..ef455c2ff759 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vehicle_setup +++ b/ROMFS/px4fmu_common/init.d/rc.vehicle_setup @@ -41,6 +41,15 @@ then . ${R}etc/init.d/rc.rover_differential_apps fi +# +# Ackermann Rover setup. +# +if [ $VEHICLE_TYPE = rover_ackermann ] +then + # Start ackermann drive rover apps. + . ${R}etc/init.d/rc.rover_ackermann_apps +fi + # # VTOL setup. # @@ -76,5 +85,4 @@ fi if [ $VEHICLE_TYPE = none ] then echo "No autostart ID found" - ekf2 start & fi diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_apps b/ROMFS/px4fmu_common/init.d/rc.vtol_apps index fdc16af83dc5..c28adef56ae9 100644 --- a/ROMFS/px4fmu_common/init.d/rc.vtol_apps +++ b/ROMFS/px4fmu_common/init.d/rc.vtol_apps @@ -5,16 +5,6 @@ # NOTE: Script variables are declared/initialized/unset in the rcS script. # -############################################################################### -# Begin Estimator group selection # -############################################################################### - -ekf2 start & - -############################################################################### -# End Estimator group selection # -############################################################################### - # # Start Control Allocator # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3f2360187f00..dc50233b00c4 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -30,12 +30,18 @@ set LOGGER_BUF 8 set PARAM_FILE "" set PARAM_BACKUP_FILE "" set RC_INPUT_ARGS "" -set SDCARD_AVAILABLE no +set STORAGE_AVAILABLE no set SDCARD_EXT_PATH /fs/microsd/ext_autostart set SDCARD_FORMAT no set STARTUP_TUNE 1 set VEHICLE_TYPE none +# Airframe parameter versioning +# Value set to 1 by default but can optionally be overridden in the airframe configuration startup script. +# Airframe maintainers can ensure a reset to the airframe defaults during an update by increasing by one. +# e.g. add line "set PARAM_DEFAULTS_VER 2" in your airframe file to build the first update that enfoces a reset. +set PARAM_DEFAULTS_VER 1 + # # Print full system version. # @@ -56,11 +62,11 @@ then umount /fs/microsd else - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes fi fi - if [ $SDCARD_AVAILABLE = no -o $SDCARD_FORMAT = yes ] + if [ $STORAGE_AVAILABLE = no -o $SDCARD_FORMAT = yes ] then echo "INFO [init] formatting /dev/mmcsd0" set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds) @@ -71,7 +77,7 @@ then if mount -t vfat /dev/mmcsd0 /fs/microsd then - set SDCARD_AVAILABLE yes + set STORAGE_AVAILABLE yes set STARTUP_TUNE 14 # tune 14 = SD_INIT else echo "ERROR [init] card mount failed" @@ -80,16 +86,22 @@ then echo "ERROR [init] format failed" fi fi +else + # Is there a device mounted for storage + if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/microsd + then + set STORAGE_AVAILABLE yes + fi +fi - if [ $SDCARD_AVAILABLE = yes ] +if [ $STORAGE_AVAILABLE = yes ] +then + if hardfault_log check then - if hardfault_log check + set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE + if hardfault_log commit then - set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE - if hardfault_log commit - then - hardfault_log reset - fi + hardfault_log reset fi fi @@ -114,13 +126,14 @@ then . $FRC else - # - # Set the parameter file the board supports params on - # MTD device. - # - if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params + # Load param file location from kconfig + . ${R}etc/init.d/rc.filepaths + + # Check if /fs/mtd_params is a valid BSON file + if ! bsondump docsize /fs/mtd_caldata then - set PARAM_FILE /fs/mtd_params + echo "New /fs/mtd_caldata size is:" + bsondump docsize /fs/mtd_caldata fi # @@ -165,7 +178,7 @@ else fi fi - if [ $SDCARD_AVAILABLE = yes ] + if [ $STORAGE_AVAILABLE = yes ] then param select-backup $PARAM_BACKUP_FILE fi @@ -175,13 +188,11 @@ else netman update -i eth0 fi - # - # If the airframe has been previously reset SYS_AUTCONFIG will have been set to 1 and other params will be reset on the next boot. - # + # To trigger a parameter reset during boot SYS_AUTCONFIG was set to 1 before if param greater SYS_AUTOCONFIG 0 then - # Reset params except Airframe, RC calibration, sensor calibration, flight modes, total flight time, and next flight UUID. - param reset_all SYS_AUTOSTART RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT* + # Reset parameters except airframe, parameter version, RC calibration, sensor calibration, flight modes, total flight time, flight UUID + param reset_all SYS_AUTOSTART SYS_PARAM_VER RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT* fi # @@ -206,25 +217,42 @@ else fi unset BOARD_RC_DEFAULTS - # - # Set parameters and env variables for selected SYS_AUTOSTART. - # - set AUTOSTART_PATH etc/init.d/rc.autostart + # Load airframe configuration based on SYS_AUTOSTART parameter if ! param compare SYS_AUTOSTART 0 then - if param greater SYS_AUTOSTART 1000000 + # rc.autostart directly run the right airframe script which sets the VEHICLE_TYPE + # Look for airframe in ROMFS + . ${R}etc/init.d/rc.autostart + + if [ ${VEHICLE_TYPE} == none ] then # Use external startup file - if [ $SDCARD_AVAILABLE = yes ] + if [ $STORAGE_AVAILABLE = yes ] then - set AUTOSTART_PATH etc/init.d/rc.autostart_ext + . ${R}etc/init.d/rc.autostart_ext else - echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS" + echo "ERROR [init] SD card not mounted - can't load external airframe" fi fi - . ${R}$AUTOSTART_PATH + + if [ ${VEHICLE_TYPE} == none ] + then + echo "ERROR [init] No airframe file found for SYS_AUTOSTART value" + param set SYS_AUTOSTART 0 + tune_control play error + fi + fi + + # Check parameter version and reset upon airframe configuration version mismatch. + # Reboot required because "param reset_all" would reset all "param set" lines from airframe. + if ! param compare SYS_PARAM_VER ${PARAM_DEFAULTS_VER} + then + echo "Switched to different parameter version. Resetting parameters." + param set SYS_PARAM_VER ${PARAM_DEFAULTS_VER} + param set SYS_AUTOCONFIG 1 + param save + reboot fi - unset AUTOSTART_PATH # # Start the tone_alarm driver. @@ -391,6 +419,23 @@ else pwm_out start fi + # + # state estimator selection + if param compare -s EKF2_EN 1 + then + ekf2 start & + fi + + if param compare -s LPE_EN 1 + then + local_position_estimator start + fi + + if param compare -s ATT_EN 1 + then + attitude_estimator_q start + fi + # # Configure vehicle type specific parameters. # Note: rc.vehicle_setup is the entry point for all vehicle type specific setup. @@ -422,6 +467,14 @@ else # Must be started after the serial config is read rc_input start $RC_INPUT_ARGS + # Manages USB interface + if ! cdcacm_autostart start + then + sercon + echo "Starting MAVLink on /dev/ttyACM0" + mavlink start -d /dev/ttyACM0 + fi + # # Play the startup tune (if not disabled or there is an error) # @@ -573,8 +626,9 @@ unset LOGGER_ARGS unset LOGGER_BUF unset PARAM_FILE unset PARAM_BACKUP_FILE +unset PARAM_DEFAULTS_VER unset RC_INPUT_ARGS -unset SDCARD_AVAILABLE +unset STORAGE_AVAILABLE unset SDCARD_EXT_PATH unset SDCARD_FORMAT unset STARTUP_TUNE diff --git a/Tools/Matlab/ellipsoid_fit.m b/Tools/Matlab/ellipsoid_fit.m index d288aa382136..ead7e6d7c355 100644 --- a/Tools/Matlab/ellipsoid_fit.m +++ b/Tools/Matlab/ellipsoid_fit.m @@ -45,7 +45,7 @@ % * center - ellispoid center coordinates [xc; yc; zc] % * ax - ellipsoid radii [a; b; c] % * evecs - ellipsoid radii directions as columns of the 3x3 matrix -% * v - the 9 parameters describing the ellipsoid algebraically: +% * v - the 9 parameters describing the ellipsoid algebraically: % Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 % % Author: @@ -59,7 +59,7 @@ if flag == 2 && nargin == 2 equals = 'xy'; end - + if size( X, 2 ) ~= 3 error( 'Input data must have three columns!' ); else @@ -69,7 +69,7 @@ end % need nine or more data points -if length( x ) < 9 && flag == 0 +if length( x ) < 9 && flag == 0 error( 'Must have at least 9 points to fit a unique ellipsoid' ); end if length( x ) < 6 && flag == 1 @@ -91,7 +91,7 @@ 2 * x .* z, ... 2 * y .* z, ... 2 * x, ... - 2 * y, ... + 2 * y, ... 2 * z ]; % ndatapoints x 9 ellipsoid parameters elseif flag == 1 % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 @@ -99,7 +99,7 @@ y .* y, ... z .* z, ... 2 * x, ... - 2 * y, ... + 2 * y, ... 2 * z ]; % ndatapoints x 6 ellipsoid parameters elseif flag == 2 % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1, @@ -127,7 +127,7 @@ % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1 D = [ x .* x + y .* y + z .* z, ... 2 * x, ... - 2 * y, ... + 2 * y, ... 2 * z ]; % ndatapoints x 4 sphere parameters end @@ -170,5 +170,3 @@ radii = ( sqrt( gam ./ v( 1:3 ) ) )'; evecs = eye( 3 ); end - - diff --git a/Tools/astyle/check_code_style.sh b/Tools/astyle/check_code_style.sh index 3a2fcef35be8..c68b32a9a2bc 100755 --- a/Tools/astyle/check_code_style.sh +++ b/Tools/astyle/check_code_style.sh @@ -3,22 +3,20 @@ FILE=$1 DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -if [ -f "$FILE" ]; then - CHECK_FAILED=$(${DIR}/fix_code_style.sh --dry-run --formatted $FILE) - if [ -n "$CHECK_FAILED" ]; then - ${DIR}/fix_code_style.sh --quiet < $FILE > $FILE.pretty +CHECK_FAILED=$(${DIR}/fix_code_style.sh --dry-run --formatted $FILE) +if [ -n "$CHECK_FAILED" ]; then + ${DIR}/fix_code_style.sh --quiet < $FILE > $FILE.pretty - echo -e 'Formatting issue found in' $FILE - echo - git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty | grep -vE -e "^.{,4}diff.*\.pretty.{,3}$" -e "^.{,4}--- a/.*$" -e "^.{,4}\+\+\+ b/.*$" -e "^.{,5}@@ .* @@.*$" -e "^.{,4}index .{10}\.\." - rm -f $FILE.pretty - echo + echo -e 'Formatting issue found in' $FILE + echo + git --no-pager diff --no-index --minimal --histogram --color=always $FILE $FILE.pretty | grep -vE -e "^.{,4}diff.*\.pretty.{,3}$" -e "^.{,4}--- a/.*$" -e "^.{,4}\+\+\+ b/.*$" -e "^.{,5}@@ .* @@.*$" -e "^.{,4}index .{10}\.\." + rm -f $FILE.pretty + echo - if [[ $PX4_ASTYLE_FIX -eq 1 ]]; then - ${DIR}/fix_code_style.sh $FILE - else - echo 'to fix automatically run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"' - exit 1 - fi + if [[ $PX4_ASTYLE_FIX -eq 1 ]]; then + ${DIR}/fix_code_style.sh $FILE + else + echo 'to fix automatically run "make format" or "./Tools/astyle/fix_code_style.sh' $FILE'"' + exit 1 fi fi diff --git a/Tools/astyle/check_newlines.sh b/Tools/astyle/check_newlines.sh new file mode 100755 index 000000000000..516047693576 --- /dev/null +++ b/Tools/astyle/check_newlines.sh @@ -0,0 +1,19 @@ +#!/usr/bin/env bash + +return_value=0 + +# Check if there are files checked in that don't end in a newline (POSIX requirement) +git grep --cached -Il '' | xargs -L1 bash -c 'if test "$(tail -c 1 "$0")"; then echo "No new line at end of $0"; exit 1; fi' + +if [ $? -ne 0 ]; then + return_value=1 +fi + +# Check if there are files checked in that have duplicate newlines at the end (fail trailing whitespace checks) +git grep --cached -Il '' | xargs -L1 bash -c 'if tail -c 2 "$0" | ( read x && read y && [ x"$x" = x ] && [ x"$y" = x ]); then echo "Multiple newlines at the end of $0"; exit 1; fi' + +if [ $? -ne 0 ]; then + return_value=1 +fi + +exit $return_value diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index e0cc5693edb3..c94132867ee1 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -18,7 +18,8 @@ exec find boards msg src platforms test \ -path src/lib/events/libevents -prune -o \ -path src/lib/parameters/uthash -prune -o \ -path src/lib/wind_estimator/python/generated -prune -o \ - -path src/modules/ekf2/EKF -prune -o \ + -path src/modules/ekf2/EKF/python/ekf_derivation/generated -prune -o \ + -path src/modules/ekf2/EKF/yaw_estimator/derivation/generated -prune -o \ -path src/modules/gyro_fft/CMSIS_5 -prune -o \ -path src/modules/mavlink/mavlink -prune -o \ -path test/mavsdk_tests/catch2 -prune -o \ @@ -30,5 +31,5 @@ exec find boards msg src platforms test \ -path src/lib/cdrstream/cyclonedds -prune -o \ -path src/lib/cdrstream/rosidl -prune -o \ -path src/modules/zenoh/zenoh-pico -prune -o \ - -path src/modules/muorb/apps/libfc-sensor-api -prune -o \ - -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN + -path boards/modalai/voxl2/libfc-sensor-api -prune -o \ + \( -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) -print \) | grep $PATTERN diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index 8082491ee204..1931deb9a31d 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -6,7 +6,7 @@ function check_git_submodule { if [[ -f $1"/.git" || -d $1"/.git" ]]; then # always update within CI environment or configuring withing VSCode CMake where you can't interact - if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then + if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ] || [ -n "${CLION_IDE+set}" ]; then git submodule --quiet sync --recursive -- $1 git submodule --quiet update --init --recursive --jobs=8 -- $1 || true git submodule --quiet sync --recursive -- $1 diff --git a/Tools/ci_build_all_runner.sh b/Tools/ci_build_all_runner.sh new file mode 100755 index 000000000000..bfb77e2623a9 --- /dev/null +++ b/Tools/ci_build_all_runner.sh @@ -0,0 +1,19 @@ +#!/bin/bash +# This script is meant to be used by the build_all.yml workflow in a github runner +# Please only modify if you know what you are doing +set -e + +echo "### :clock1: Build Times" >> $GITHUB_STEP_SUMMARY +targets=$1 +for target in ${targets//,/ } +do + echo "::group::Building: [${target}]" + start=$(date +%s) + make $target + stop=$(date +%s) + diff=$(($stop-$start)) + build_time="$(($diff /60/60))h $(($diff /60))m $(($diff % 60))s elapsed" + echo -e "\033[0;32mBuild Time: [$build_time]" + echo "* **$target** - $build_time" >> $GITHUB_STEP_SUMMARY + echo "::endgroup::" +done diff --git a/Tools/cryptotools.py b/Tools/cryptotools.py index 6e4bd42d63e5..f452bea444f5 100755 --- a/Tools/cryptotools.py +++ b/Tools/cryptotools.py @@ -170,4 +170,3 @@ def generate_key(key_file): fs.write(f.read()) except: pass - diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index f0c33cfc8e41..98e7c23b9304 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -139,7 +139,3 @@ def find_checks_that_apply( innov_fail_checks.append('ofy') return sensor_checks, innov_fail_checks - - - - diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index d435f88af9f7..47a8bb412e4e 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -54,7 +54,7 @@ def calculate_sensor_metrics( # calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for # estimator status variables for signal, result_id in [('hgt_test_ratio', 'hgt'), - ('mag_test_ratio', 'mag'), + ('hdg_test_ratio', 'mag'), ('vel_test_ratio', 'vel'), ('pos_test_ratio', 'pos'), ('tas_test_ratio', 'tas'), diff --git a/Tools/ecl_ekf/batch_process_logdata_ekf.py b/Tools/ecl_ekf/batch_process_logdata_ekf.py index 9d41431a104d..d118c655eb76 100755 --- a/Tools/ecl_ekf/batch_process_logdata_ekf.py +++ b/Tools/ecl_ekf/batch_process_logdata_ekf.py @@ -84,4 +84,4 @@ def main() -> None: if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index db5d4fdd508a..35821fbc4697 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -160,7 +160,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str # plot normalised innovation test levels # define variables to plot - variables = [['mag_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']] + variables = [['hdg_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'], ['hgt_test_ratio']] y_labels = ['mag', 'vel, pos', 'hgt'] legend = [['mag'], ['vel', 'pos'], ['hgt']] if np.amax(estimator_status['hagl_test_ratio']) > 0.0: # plot hagl test ratio, if applicable @@ -223,7 +223,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str data_plot.save() data_plot.close() - # plot innovation_check_flags summary + # plot innovation flags summary data_plot = CheckFlagsPlot( status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos', 'reject_hagl'], diff --git a/Tools/filepaths/generate_config.py b/Tools/filepaths/generate_config.py new file mode 100755 index 000000000000..d4aa8b6d2625 --- /dev/null +++ b/Tools/filepaths/generate_config.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +""" Script to generate Serial rc.filepaths for the ROMFS startup script """ + +import argparse +import os +import sys + +try: + from jinja2 import Environment, FileSystemLoader +except ImportError as e: + print("Failed to import jinja2: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user jinja2") + print("") + sys.exit(1) + +try: + import yaml +except ImportError as e: + print("Failed to import yaml: " + str(e)) + print("") + print("You may need to install it using:") + print(" pip3 install --user pyyaml") + print("") + sys.exit(1) + +parser = argparse.ArgumentParser(description='Generate PX4 ROMFS filepaths') + +parser.add_argument('--config-files', type=str, nargs='*', default=[], + help='YAML module config file(s)') +parser.add_argument('--constrained-flash', action='store_true', + help='Reduce verbosity in ROMFS scripts to reduce flash size') +parser.add_argument('--rc-dir', type=str, action='store', + help='ROMFS output directory', default=None) +parser.add_argument('--params-file', type=str, action='store', + help='Parameter output file', default=None) +parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', + help='Verbose Output') + +args = parser.parse_args() + +verbose = args.verbose +constrained_flash = args.constrained_flash +rc_filepaths_output_dir = args.rc_dir +rc_filepaths_template = 'rc.filepaths.jinja' + + +jinja_env = Environment(loader=FileSystemLoader( + os.path.dirname(os.path.realpath(__file__)))) + +# generate the ROMFS script using a jinja template +if rc_filepaths_output_dir is not None: + rc_filepath_output_file = os.path.join(rc_filepaths_output_dir, "rc.filepaths") + + if verbose: print("Generating {:}".format(rc_filepath_output_file)) + template = jinja_env.get_template(rc_filepaths_template) + with open(rc_filepath_output_file, 'w') as fid: + fid.write(template.render(constrained_flash=constrained_flash, params_file=args.params_file)) +else: + raise Exception("--rc-dir needs to be specified") diff --git a/Tools/filepaths/rc.filepaths.jinja b/Tools/filepaths/rc.filepaths.jinja new file mode 100644 index 000000000000..30a2c2cce643 --- /dev/null +++ b/Tools/filepaths/rc.filepaths.jinja @@ -0,0 +1,6 @@ +{# jinja template to generate the serial autostart script. #} + +# serial autostart script generated with generate_serial_config.py + + +set PARAM_FILE {{ params_file }} diff --git a/Tools/generate_board_targets_json.py b/Tools/generate_board_targets_json.py index 931d5c14776f..649b3fceefa6 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/generate_board_targets_json.py @@ -23,11 +23,14 @@ help='Verbose Output') parser.add_argument('-p', '--pretty', dest='pretty', action='store_true', help='Pretty output instead of a single line') +parser.add_argument('-g', '--groups', dest='group', action='store_true', + help='Groups targets') args = parser.parse_args() verbose = args.verbose build_configs = [] +grouped_targets = {} excluded_boards = ['modalai_voxl2', 'px4_ros2'] # TODO: fix and enable excluded_manufacturers = ['atlflight'] excluded_platforms = ['qurt'] @@ -37,10 +40,27 @@ 'uavcanv1', # TODO: fix and enable ] +github_action_config = { 'include': build_configs } +extra_args = {} +if args.pretty: + extra_args['indent'] = 2 + +def chunks(arr, size): + # splits array into parts + for i in range(0, len(arr), size): + yield arr[i:i + size] + +def comma_targets(targets): + # turns array of targets into a comma split string + return ",".join(targets) + def process_target(px4board_file, target_name): + # reads through the board file and grabs + # useful information for building ret = None platform = None toolchain = None + group = None if px4board_file.endswith("default.px4board") or \ px4board_file.endswith("recovery.px4board") or \ @@ -63,22 +83,34 @@ def process_target(px4board_file, target_name): # get the container based on the platform and toolchain if platform == 'posix': container = 'px4io/px4-dev-base-focal:2021-09-08' + group = 'base' if toolchain: if toolchain.startswith('aarch64'): container = 'px4io/px4-dev-aarch64:2022-08-12' + group = 'aarch64' elif toolchain == 'arm-linux-gnueabihf': container = 'px4io/px4-dev-armhf:2023-06-26' + group = 'armhf' else: if verbose: print(f'unmatched toolchain: {toolchain}') elif platform == 'nuttx': container = 'px4io/px4-dev-nuttx-focal:2022-08-12' + group = 'nuttx' else: if verbose: print(f'unmatched platform: {platform}') ret = {'target': target_name, 'container': container} + if(args.group): + ret['arch'] = group return ret +# Look for board targets in the ./boards directory +if(verbose): + print("=======================") + print("= scanning for boards =") + print("=======================") + for manufacturer in os.scandir(os.path.join(source_dir, 'boards')): if not manufacturer.is_dir(): continue @@ -105,12 +137,140 @@ def process_target(px4board_file, target_name): if verbose: print(f'excluding label {label} ({target_name})') continue target = process_target(files.path, target_name) + if (args.group and target is not None): + if (target['arch'] not in grouped_targets): + grouped_targets[target['arch']] = {} + grouped_targets[target['arch']]['container'] = target['container'] + grouped_targets[target['arch']]['manufacturers'] = {} + if(manufacturer.name not in grouped_targets[target['arch']]['manufacturers']): + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = {} + grouped_targets[target['arch']]['manufacturers'][manufacturer.name] = [] + grouped_targets[target['arch']]['manufacturers'][manufacturer.name].append(target_name) if target is not None: build_configs.append(target) +if(verbose): + import pprint + print("============================") + print("= Boards found in ./boards =") + print("============================") + pprint.pp(grouped_targets) -github_action_config = { 'include': build_configs } -extra_args = {} -if args.pretty: - extra_args['indent'] = 2 -print(json.dumps(github_action_config, **extra_args)) +if (args.group): + # if we are using this script for grouping builds + # we loop trough the manufacturers list and split their targets + # if a manufacturer has more than a LIMIT of boards then we split that + # into sub groups such as "arch-manufacturer name-index" + # example: + # nuttx-px4-0 + # nuttx-px4-1 + # nuttx-px4-2 + # nuttx-ark-0 + # nuttx-ark-1 + # if the manufacturer doesn't have more targets than LIMIT then we add + # them to a generic group with the following structure "arch-index" + # example: + # nuttx-0 + # nuttx-1 + final_groups = [] + temp_group = [] + group_number = {} + last_man = '' + last_arch = '' + SPLIT_LIMIT = 10 + LOWER_LIMIT = 5 + for arch in grouped_targets: + if(last_arch == ''): + last_arch = arch + if(arch not in group_number): + group_number[arch] = 0 + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + for man in grouped_targets[arch]['manufacturers']: + for tar in grouped_targets[arch]['manufacturers'][man]: + if(last_man != man): + man_len = len(grouped_targets[arch]['manufacturers'][man]) + if(man_len > LOWER_LIMIT and man_len < (SPLIT_LIMIT + 1)): + # Manufacturers can have their own group + group_name = arch + "-" + man + targets = comma_targets(grouped_targets[arch]['manufacturers'][man]) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(grouped_targets[arch]['manufacturers'][man]) + }) + elif(man_len >= (SPLIT_LIMIT + 1)): + # Split big man groups into subgroups + # example: Pixhawk + chunk_limit = SPLIT_LIMIT + chunk_counter = 0 + for chunk in chunks(grouped_targets[arch]['manufacturers'][man], chunk_limit): + group_name = arch + "-" + man + "-" + str(chunk_counter) + targets = comma_targets(chunk) + last_man = man + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(chunk), + }) + chunk_counter += 1 + else: + temp_group.append(tar) + + if(last_arch != arch and len(temp_group) > 0): + group_name = last_arch + "-" + str(group_number[last_arch]) + group_number[last_arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[last_arch]['container'], + "targets": targets, + "arch": last_arch, + "group": group_name, + "len": len(temp_group) + }) + last_arch = arch + temp_group = [] + if(len(temp_group) > (LOWER_LIMIT - 1)): + group_name = arch + "-" + str(group_number[arch]) + last_arch = arch + group_number[arch] += 1 + targets = comma_targets(temp_group) + final_groups.append({ + "container": grouped_targets[arch]['container'], + "targets": targets, + "arch": arch, + "group": group_name, + "len": len(temp_group) + }) + temp_group = [] + if(verbose): + import pprint + print("================") + print("= final_groups =") + print("================") + pprint.pp(final_groups) + + print("===============") + print("= JSON output =") + print("===============") + + print(json.dumps({ "include": final_groups }, **extra_args)) +else: + print(json.dumps(github_action_config, **extra_args)) diff --git a/Tools/kconfig/allyesconfig.py b/Tools/kconfig/allyesconfig.py new file mode 100644 index 000000000000..889b659195ed --- /dev/null +++ b/Tools/kconfig/allyesconfig.py @@ -0,0 +1,126 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018-2019, Ulf Magnusson +# SPDX-License-Identifier: ISC + +""" +Writes a configuration file where as many symbols as possible are set to 'y'. + +The default output filename is '.config'. A different filename can be passed +in the KCONFIG_CONFIG environment variable. + +Usage for the Linux kernel: + + $ make [ARCH=] scriptconfig SCRIPT=Kconfiglib/allyesconfig.py +""" +import kconfiglib +import os + + +exception_list = [ + 'DRIVERS_BOOTLOADERS', # Only used for bootloader target + 'MODULES_PX4IOFIRMWARE', # Only needed for PX4IO firmware itself, maybe fix through dependencies + 'BOARD_LTO', # Experimental + 'BOARD_TESTING', # Don't build test suite + 'BOARD_CONSTRAINED_FLASH', # only used to reduce flash size + 'BOARD_NO_HELP', # only used to reduce flash size + 'BOARD_CONSTRAINED_MEMORY', # only used to reduce flash size + 'BOARD_EXTERNAL_METADATA', # only used to reduce flash size + 'BOARD_CRYPTO', # Specialized use + 'BOARD_PROTECTED', # Experimental for MPU use + 'DRIVERS_LIGHTS_RGBLED_PWM', # Only on specific boards, needs dependency fixing + 'DRIVERS_LIGHTS_NEOPIXEL', # Only on specific boards, needs dependency fixing + 'DRIVERS_DISTANCE_SENSOR_LIGHTWARE_SF45_SERIAL', # Only on specific boards, needs dependency fixing + 'PARAM_PRIMARY', # Plainly broken + 'PARAM_REMOTE', # Plainly broken + 'DRIVERS_ACTUATORS_VOXL_ESC', # Dependency need fixing, requires VOXL_ESC_DEFAULT_XXX + 'DRIVERS_VOXL2_IO', # Dependency need fixing, requires VOXL2_IO_DEFAULT_XX + 'DRIVERS_BAROMETER_TCBP001TA', # Requires hardcoded PX4_SPI_BUS_BARO mapping + 'DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50', # Requires hardcoded PX4_SPI_BUS_BARO mapping + 'DRIVERS_DISTANCE_SENSOR_SRF05', # Requires hardcoded GPIO_ULTRASOUND + 'DRIVERS_PPS_CAPTURE', # Requires PPS GPIO config + 'DRIVERS_PWM_INPUT', # Requires PWM config + 'DRIVERS_TEST_PPM', # PIN config not portable + 'DRIVERS_TATTU_CAN', # Broken needs fixing + 'MODULES_REPLAY', # Fails on NuttX targets maybe force POSIX dependency? + 'SYSTEMCMDS_HIST', # This module can only be used on boards that enable BOARD_ENABLE_LOG_HISTORY + 'SYSTEMCMDS_GPIO', # PIN config not portable + 'SYSTEMCMDS_SHUTDOWN', # Needs dependency checking + 'EXAMPLES_DYN_HELLO', # NuttX doesn't support dynamic linking + 'SYSTEMCMDS_DYN', # NuttX doesn't support dynamic linking + 'DRIVERS_RPI_RC_IN', # RPI specific driver + 'SYSTEMCMDS_I2C_LAUNCHER', # undefined reference to `system', + 'MODULES_MUORB_APPS', # Weird QURT/Posix package doesn't work on x86 px4 sitl + 'MODULES_SIMULATION_SIMULATOR_SIH', # Causes compile errors +] + +exception_list_sitl = [ + 'DRIVERS_BAROMETER', # Fails I2C dependencies + 'COMMON_BAROMETERS', # Fails I2C dependencies + 'DRIVERS_ADC_BOARD_ADC', # Fails HW dependencies, I think this only works on NuttX + 'DRIVERS_CAMERA_CAPTURE', # GPIO config failure + 'DRIVERS_DSHOT', # No Posix driver, I think this only works on NuttX + 'DRIVERS_PWM_OUT', # No Posix driver, I think this only works on NuttX + 'COMMON', # Fails I2C dependencies + 'DRIVERS', # Fails I2C dependencies + 'SYSTEMCMDS_REBOOT', # Sitl can't reboot + 'MODULES_BATTERY_STATUS', # Sitl doesn't provide a power brick + 'SYSTEMCMDS_SERIAL_PASSTHRU', # Not supported in SITL + 'SYSTEMCMDS_SERIAL_TEST', # Not supported in SITL + 'SYSTEMCMDS_SD_STRESS', # Not supported in SITL + 'SYSTEMCMDS_SD_BENCH', # Not supported in SITL + 'SYSTEMCMDS_I2CDETECT', # Not supported in SITL + 'SYSTEMCMDS_DMESG', # Not supported in SITL + 'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL +] + +def main(): + kconf = kconfiglib.standard_kconfig(__doc__) + + + if 'BASE_DEFCONFIG' in os.environ: + kconf.load_config(os.environ['BASE_DEFCONFIG']) + + if 'MODEL' in os.environ: + if os.environ['MODEL'] == 'sitl': + for sym in kconf.unique_defined_syms: + if sym.name.startswith(tuple(exception_list_sitl)): + exception_list.append(sym.name) + + + # See allnoconfig.py + kconf.warn = False + + # Try to set all symbols to 'y'. Dependencies might truncate the value down + # later, but this will at least give the highest possible value. + # + # Assigning 0/1/2 to non-bool/tristate symbols has no effect (int/hex + # symbols still take a string, because they preserve formatting). + for sym in kconf.unique_defined_syms: + # Set choice symbols to 'm'. This value will be ignored for choices in + # 'y' mode (the "normal" mode), which will instead just get their + # default selection, but will set all symbols in m-mode choices to 'm', + # which is as high as they can go. + # + # Here's a convoluted example of how you might get an m-mode choice + # even during allyesconfig: + # + # choice + # tristate "weird choice" + # depends on m + if sym.name not in exception_list: + sym.set_value(1 if sym.choice else 2) + + # Set all choices to the highest possible mode + for choice in kconf.unique_choices: + choice.set_value(2) + + kconf.warn = True + + kconf.load_allconfig("allyes.config") + + print(kconf.write_config()) + + +if __name__ == "__main__": + main() diff --git a/Tools/mavlink_shell.py b/Tools/mavlink_shell.py index 35c8d130ed5a..7dd3675bc1b1 100755 --- a/Tools/mavlink_shell.py +++ b/Tools/mavlink_shell.py @@ -243,4 +243,3 @@ def erase_last_n_chars(N): if __name__ == '__main__': main() - diff --git a/Tools/mavlink_ulog_streaming.py b/Tools/mavlink_ulog_streaming.py index c0ff6fe15dc3..2b21f1032741 100755 --- a/Tools/mavlink_ulog_streaming.py +++ b/Tools/mavlink_ulog_streaming.py @@ -282,4 +282,3 @@ def main(): if __name__ == '__main__': main() - diff --git a/Tools/migrate_c_params.py b/Tools/migrate_c_params.py new file mode 100755 index 000000000000..042bdfb543f9 --- /dev/null +++ b/Tools/migrate_c_params.py @@ -0,0 +1,450 @@ +#!/usr/bin/python3 + +""" +Migrate c parameter definitions to yaml module definitions. + +This script is used to transition legacy c parameter definitions to +yaml module definitions. It parses each specified legacy c file +and produces a corresponding yaml definition in the same directory. +For modules with multiple parameter c files and/or existing module.yaml files, +the resulting list of yaml files must be merged separately, either manually +or with a tool like yq4: + + yq eval-all '. as $item ireduce ({}; . *+ $item)' *.yaml + +The legacy files and temporary yaml files can then be manually deleted +and the CMakeLists.txt updated after proofreading. +""" + +import argparse +import ast +import sys +import re +import math +import logging +import os +from dataclasses import dataclass, field +from typing import Any + +import yaml + +global default_var +default_var = {} + +@dataclass +class Parameter: + """ + Single parameter + """ + + name: str + type: str + fields: dict[str, str] = field(init=False, default_factory=dict) + default: str = "" + category: str = "" + enum: dict[str, str] = field(init=False, default_factory=dict) + bitmask: dict[int, str] = field(init=False, default_factory=dict) + volatile: bool = False + boolean: bool = False + + def __lt__(self, other): + return self.name < other.name + +@dataclass +class ParameterGroup: + """ + Single parameter group + """ + + name: str + parameters: list[Parameter] = field(init=False, default_factory=list) + no_code_generation: bool = False + + +class SourceParser: + """ + Parses provided data and stores all found parameters internally. + """ + + re_split_lines = re.compile(r'[\r\n]+') + re_comment_start = re.compile(r'^\/\*\*') + re_comment_content = re.compile(r'^\*\s*(.*)') + re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)') + re_comment_end = re.compile(r'(.*?)\s*\*\/') + re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;') + re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;') + re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*') + re_cut_type_specifier = re.compile(r'[a-z]+$') + re_is_a_number = re.compile(r'^-?[0-9\.]') + re_remove_dots = re.compile(r'\.+$') + re_remove_carriage_return = re.compile('\n+') + + valid_tags = set(["group", "board", "min", "max", "unit", "decimal", + "increment", "reboot_required", "value", "boolean", + "bit", "category", "volatile"]) + + # Order of parameter groups + priority = { + # All other groups = 0 (sort alphabetically) + "Miscellaneous": -10 + } + + def __init__(self): + self.param_groups = {} + + def parse(self, contents): + """ + Incrementally parse program contents and append all found parameters + to the list. + """ + # This code is essentially a comment-parsing grammar. "state" + # represents parser state. It contains human-readable state + # names. + state = None + for line in self.re_split_lines.split(contents): + line = line.strip() + # Ignore empty lines + if line == "": + continue + if self.re_comment_start.match(line): + state = "wait-short" + short_desc = None + long_desc = None + tags = {} + def_values = {} + def_bitmask = {} + elif state is not None and state != "comment-processed": + m = self.re_comment_end.search(line) + if m: + line = m.group(1) + last_comment_line = True + else: + last_comment_line = False + m = self.re_comment_content.match(line) + if m: + comment_content = m.group(1) + if comment_content == "": + # When short comment ends with empty comment line, + # start waiting for the next part - long comment. + if state == "wait-short-end": + state = "wait-long" + else: + m = self.re_comment_tag.match(comment_content) + if m: + tag, desc = m.group(1, 2) + if (tag == "value"): + # Take the meta info string and split + # the code and description + metainfo = desc.split(" ", 1) + def_values[metainfo[0]] = metainfo[1] + elif (tag == "bit"): + # Take the meta info string and split + # the code and description + metainfo = desc.split(" ", 1) + def_bitmask[metainfo[0]] = metainfo[1] + else: + tags[tag] = desc + current_tag = tag + state = "wait-tag-end" + elif state == "wait-short": + # Store first line of the short description + short_desc = comment_content + state = "wait-short-end" + elif state == "wait-short-end": + # Append comment line to the short description + short_desc += "\n" + comment_content + elif state == "wait-long": + # Store first line of the long description + long_desc = comment_content + state = "wait-long-end" + elif state == "wait-long-end": + # Append comment line to the long description + long_desc += "\n" + comment_content + elif state == "wait-tag-end": + # Append comment line to the tag text + tags[current_tag] += "\n" + comment_content + else: + raise AssertionError( + "Invalid parser state: %s" % state) + elif not last_comment_line: + # Invalid comment line (inside comment, but not + # startin with "*" or "*/". Reset parsed content. + state = None + if last_comment_line: + state = "comment-processed" + else: + tp = None + name = None + defval = "" + # Non-empty line outside the comment + m = self.re_px4_param_default_definition.match(line) + # Default value handling + if m: + name_m, defval_m = m.group(1, 2) + default_var[name_m] = defval_m + m = self.re_parameter_definition.match(line) + if m: + tp, name, defval = m.group(1, 2, 3) + else: + m = self.re_px4_parameter_definition.match(line) + if m: + tp, name = m.group(1, 2) + if (name+'_DEFAULT') in default_var: + defval = default_var[name+'_DEFAULT'] + if tp is not None: + # Remove trailing type specifier from numbers: 0.1f => 0.1 + if defval != "" and self.re_is_a_number.match(defval): + defval = self.re_cut_type_specifier.sub('', defval) + param = Parameter(name=name, type=tp, default=defval) + param.fields["short_desc"] = name + # If comment was found before the parameter declaration, + # inject its data into the newly created parameter. + group = "Miscellaneous" + if state == "comment-processed": + if short_desc is not None: + if '\n' in short_desc: + raise Exception('short description must be a single line (parameter: {:})'.format(name)) + if len(short_desc) > 150: + raise Exception('short description too long (150 max, is {:}, parameter: {:})'.format(len(short_desc), name)) + param.fields["short_desc"] = self.re_remove_dots.sub('', short_desc) + if long_desc is not None: + long_desc = self.re_remove_carriage_return.sub(' ', long_desc) + param.fields["long_desc"] = long_desc + for tag in tags: + if tag == "group": + group = tags[tag] + elif tag == "volatile": + param.volatile = True + elif tag == "category": + param.category = tags[tag] + elif tag == "boolean": + param.boolean = True + elif tag not in self.valid_tags: + sys.stderr.write("Skipping invalid documentation tag: '%s'\n" % tag) + return False + else: + param.fields[tag] = tags[tag] + for def_value in def_values: + param.enum[def_value] = def_values[def_value] + for def_bit in def_bitmask: + param.bitmask[def_bit] = def_bitmask[def_bit] + # Store the parameter + if group not in self.param_groups: + self.param_groups[group] = ParameterGroup(group) + self.param_groups[group].parameters.append(param) + state = None + return True + + def validate(self): + """ + Validates the parameter meta data. + """ + seenParamNames = [] + + for group in self.parameter_groups: + for param in sorted(group.parameters): + name = param.name + if len(name) > 16: + sys.stderr.write("Parameter Name {0} is too long (Limit is 16)\n".format(name)) + return False + board = param.fields.get("board", "") + # Check for duplicates + name_plus_board = name + "+" + board + for seenParamName in seenParamNames: + if seenParamName == name_plus_board: + sys.stderr.write("Duplicate parameter definition: {0}\n".format(name_plus_board)) + return False + seenParamNames.append(name_plus_board) + # Validate values + default = param.default + min = param.fields.get("min", "") + max = param.fields.get("max", "") + units = param.fields.get("unit", "") + if default != "" and not self.is_number(default): + sys.stderr.write("Default value not number: {0} {1}\n".format(name, default)) + return False + if min != "": + if not self.is_number(default): + sys.stderr.write("Min value not number: {0} {1}\n".format(name, min)) + return False + if default != "" and float(default) < float(min): + sys.stderr.write("Default value is smaller than min: {0} default:{1} min:{2}\n".format(name, default, min)) + return False + if max != "": + if not self.is_number(max): + sys.stderr.write("Max value not number: {0} {1}\n".format(name, max)) + return False + if default != "" and float(default) > float(max): + sys.stderr.write("Default value is larger than max: {0} default:{1} max:{2}\n".format(name, default, max)) + return False + for code in sorted(param.enum, key=float): + if not self.is_number(code): + sys.stderr.write("Min value not number: {0} {1}\n".format(name, code)) + return False + if param.enum.get(code, "") == "": + sys.stderr.write("Description for enum value is empty: {0} {1}\n".format(name, code)) + return False + for index in sorted(param.bitmask.keys(), key=float): + if not self.is_number(index): + sys.stderr.write("bit value not number: {0} {1}\n".format(name, index)) + return False + if not int(min) <= math.pow(2, int(index)) <= int(max): + sys.stderr.write("Bitmask bit must be between {0} and {1}: {2} {3}\n".format(min, max, name, math.pow(2, int(index)))) + return False + if param.bitmask.get(index, "") == "": + sys.stderr.write("Description for bitmask bit is empty: {0} {1}\n".format(name, index)) + return False + return True + + def is_number(self, num): + try: + float(num) + return True + except ValueError: + return False + + @property + def parameter_groups(self) -> list[ParameterGroup]: + """ + Returns the parsed list of parameters. Every parameter is a Parameter + object. Note that returned object is not a copy. Modifications affect + state of the parser. + """ + groups = self.param_groups.values() + groups = sorted(groups, key=lambda x: x.name) + groups = sorted(groups, key=lambda x: self.priority.get(x.name, 0), + reverse=True) + + return groups + + +def parse(filename: str) -> list[ParameterGroup]: + with open(filename) as f: + cdata = f.read() + + parser = SourceParser() + if not parser.parse(cdata): + logging.error(f"Error parsing c parameter file {filename}") + sys.exit(1) + if not parser.validate(): + logging.error(f"Error while validating c parameter file {filename}") + sys.exit(1) + + return parser.parameter_groups + + +def cast(val: str) -> Any: + if val == "true": + return True + elif val == "false": + return False + try: + return ast.literal_eval(val) + except ValueError: + return val + + +def generate_yaml(filename: str, groups: list[ParameterGroup]) -> str: + data = dict() + + module_name = os.path.dirname(os.path.realpath(filename)).split(os.sep)[-1] + + data["module_name"] = module_name + data["parameters"] = list() + + for group in groups: + g = dict() + g["group"] = group.name + g["definitions"] = dict() + + for parameter in group.parameters: + p = dict() + + p["description"] = dict() + p["description"]["short"] = parameter.fields["short_desc"] + del parameter.fields["short_desc"] + if "long_desc" in parameter.fields: + p["description"]["long"] = parameter.fields["long_desc"] + del parameter.fields["long_desc"] + + if parameter.category != "": + p["category"] = parameter.category.title() + # the enum check has to happen first + # since some parameters are both boolean and enum at the same time + # (even with more than 0 and 1 as options for some reason) so let's assume + # they are enums not booleans + if len(parameter.enum) > 0: + p["type"] = "enum" + p["values"] = dict() + for key, val in parameter.enum.items(): + try: + p["values"][int(key)] = val + except ValueError: + p["values"][float(key)] = val + p["type"] = "float" + elif parameter.boolean: + p["type"] = "boolean" + elif len(parameter.bitmask) > 0: + p["type"] = "bitmask" + p["bit"] = dict() + for key, val in parameter.bitmask.items(): + p["bit"][int(key)] = val.strip() + elif parameter.type == "FLOAT": + p["type"] = "float" + else: + p["type"] = "int32" + p["default"] = cast(parameter.default) + + if parameter.volatile: + p["volatile"] = bool(parameter.volatile) + + for key, val in parameter.fields.items(): + try: + p[key] = cast(val) + except SyntaxError: + p[key] = val + + g["definitions"][parameter.name] = p + data["parameters"].append(g) + + return yaml.dump(data, sort_keys=False) + + +def main(): + parser = argparse.ArgumentParser(description="Migrate legacy c parameter definitions to yaml") + parser.add_argument('input_file', nargs='+', help='input file(s)') + parser.add_argument('--update-cmake', action=argparse.BooleanOptionalAction) + args = parser.parse_args() + + input_files = args.input_file + update_cmake = args.update_cmake + + logging.basicConfig(level=logging.INFO) + + for filename in input_files: + logging.info(f"Migrating c parameter file {filename}") + + parameter_groups = parse(filename) + output = generate_yaml(filename, parameter_groups) + + dirname, fname = os.path.split(os.path.realpath(filename)) + fname = fname.split(".")[0] + with open(os.path.join(dirname, f"module_{fname}.yaml"), "w") as f: + f.write(output) + if update_cmake: + with open(os.path.join(dirname, "CMakeLists.txt"), "r+") as f: + content = f.read() + if "MODULE_CONFIG" not in content: + try: + index = content.index("DEPENDS") + except ValueError: + index = content.index("px4_add_module") + index += content[index:].index(")") + content = content[:index] + "MODULE_CONFIG\n\t\tmodule.yaml\n\t" + content[index:] + f.seek(0) + f.write(content) + f.truncate() + + +if __name__ == "__main__": + main() diff --git a/Tools/models/sdp3x_pitot_model.py b/Tools/models/sdp3x_pitot_model.py index 44c013fdc88d..864823d6634d 100644 --- a/Tools/models/sdp3x_pitot_model.py +++ b/Tools/models/sdp3x_pitot_model.py @@ -107,4 +107,4 @@ ##plt.plot(airspeed_corrected,(airspeed_corrected-airspeed_raw)/airspeed_corrected) ##plt.xlabel('airspeed [m/s]') ##plt.ylabel('relative error [-]') -##plt.show() \ No newline at end of file +##plt.show() diff --git a/Tools/module_config/output_groups_from_timer_config.py b/Tools/module_config/output_groups_from_timer_config.py index ce16545a7d3b..8a932ec9dea7 100755 --- a/Tools/module_config/output_groups_from_timer_config.py +++ b/Tools/module_config/output_groups_from_timer_config.py @@ -34,33 +34,41 @@ def extract_timer(line): if search: return search.group(1), 'generic' - # nxp rt1062 format: initIOPWM(PWM::FlexPWM2), - search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE) + # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2), + search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE) if search: - return search.group(1), 'imxrt' + return (search.group(1) + '_' + search.group(2)), 'imxrt' return None, 'unknown' -def extract_timer_from_channel(line, num_channels_already_found): +def extract_timer_from_channel(line, timer_names): # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE) if search: return search.group(1) - # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06), - search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE) + # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06), + search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE) if search: - # imxrt uses a 1:1 timer group to channel association - return str(num_channels_already_found) + return str(timer_names.index((search.group(1) + '_' + search.group(2)))) return None +def imxrt_is_dshot(line): + + # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2), + search = re.search('(initIOPWMDshot)', line, re.IGNORECASE) + if search: + return True + + return False + def get_timer_groups(timer_config_file, verbose=False): with open(timer_config_file, 'r') as f: timer_config = f.read() # timers - dshot_support = {} # key: timer + dshot_support = {str(i): False for i in range(16)} timers_start_marker = 'io_timers_t io_timers' timers_start = timer_config.find(timers_start_marker) if timers_start == -1: @@ -69,6 +77,7 @@ def get_timer_groups(timer_config_file, verbose=False): open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose) timers_str = timer_config[open_idx:close_idx] timers = [] + timer_names = [] for line in timers_str.splitlines(): line = line.strip() if len(line) == 0 or line.startswith('//'): @@ -77,14 +86,11 @@ def get_timer_groups(timer_config_file, verbose=False): if timer_type == 'imxrt': if verbose: print('imxrt timer found') - max_num_channels = 16 # Just add a fixed number of timers - timers = [str(i) for i in range(max_num_channels)] - dshot_support = {str(i): False for i in range(max_num_channels)} - for i in range(8): # First 8 channels support dshot - dshot_support[str(i)] = True - break - - if timer: + timer_names.append(timer) + if imxrt_is_dshot(line): + dshot_support[str(len(timers))] = True + timers.append(str(len(timers))) + elif timer: if verbose: print('found timer def: {:}'.format(timer)) dshot_support[timer] = 'DMA' in line timers.append(timer) @@ -111,7 +117,7 @@ def get_timer_groups(timer_config_file, verbose=False): continue if verbose: print('--'+line+'--') - timer = extract_timer_from_channel(line, len(channel_timers)) + timer = extract_timer_from_channel(line, timer_names) if timer: if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line)) diff --git a/Tools/msg/generate_msg_docs.py b/Tools/msg/generate_msg_docs.py index 81a998fbf393..a44a317b5279 100755 --- a/Tools/msg/generate_msg_docs.py +++ b/Tools/msg/generate_msg_docs.py @@ -81,15 +81,15 @@ def get_msgs_list(msgdir): with open(output_file, 'w') as content_file: content_file.write(markdown_output) - readme_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) + index_markdown_file_link='- [%s](%s.md)' % (msg_name,msg_name) if summary_description: - readme_markdown_file_link+=" — %s" % summary_description - filelist_in_markdown+=readme_markdown_file_link+"\n" + index_markdown_file_link+=" — %s" % summary_description + filelist_in_markdown+=index_markdown_file_link+"\n" - # Write out the README.md file - readme_text="""# uORB Message Reference + # Write out the index.md file + index_text="""# uORB Message Reference -:::note +::: info This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code. ::: @@ -98,6 +98,6 @@ def get_msgs_list(msgdir): %s """ % (filelist_in_markdown) - readme_file = os.path.join(output_dir, 'README.md') - with open(readme_file, 'w') as content_file: - content_file.write(readme_text) + index_file = os.path.join(output_dir, 'index.md') + with open(index_file, 'w') as content_file: + content_file.write(index_text) diff --git a/Tools/msg/templates/uorb/msg.json.em b/Tools/msg/templates/uorb/msg.json.em index 31be5d61aa7e..81d6bbbc4f58 100644 --- a/Tools/msg/templates/uorb/msg.json.em +++ b/Tools/msg/templates/uorb/msg.json.em @@ -47,4 +47,4 @@ for field in spec.parsed_fields(): "main_orb_id": @( all_topics.index(name_snake_case) if name_snake_case in all_topics else -1 ), "dependencies": @( json.dumps(list(set(dependencies))) ), "name": "@( spec.full_name )" -} \ No newline at end of file +} diff --git a/Tools/px4airframes/__init__.py b/Tools/px4airframes/__init__.py index 43e82d26433b..be02bd2190b3 100644 --- a/Tools/px4airframes/__init__.py +++ b/Tools/px4airframes/__init__.py @@ -1 +1 @@ -__all__ = ["srcscanner", "srcparser", "xmlout", "rcout"] \ No newline at end of file +__all__ = ["srcscanner", "srcparser", "xmlout", "rcout"] diff --git a/Tools/px4airframes/markdownout.py b/Tools/px4airframes/markdownout.py index e6b69ce31ae2..bf1a605723f7 100644 --- a/Tools/px4airframes/markdownout.py +++ b/Tools/px4airframes/markdownout.py @@ -7,7 +7,7 @@ class MarkdownTablesOutput(): def __init__(self, groups, board, image_path): result = """# Airframes Reference -:::note +::: info **This list is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/px4airframes/markdownout.py) from the source code** using the build command: `make airframe_metadata`. ::: diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py index 9bc8747185a2..a6bdc9e94410 100644 --- a/Tools/px4airframes/rcout.py +++ b/Tools/px4airframes/rcout.py @@ -79,12 +79,6 @@ def __init__(self, groups, board, post_start=False): result += "then\n" result += "\techo \"Loading airframe: /etc/init.d/airframes/${AIRFRAME}\"\n" result += "\t. /etc/init.d/airframes/${AIRFRAME}\n" - if not post_start: - result += "else\n" - result += "\techo \"ERROR [init] No file matches SYS_AUTOSTART value found in : /etc/init.d/airframes\"\n" - # Reset the configuration - result += "\tparam set SYS_AUTOSTART 0\n" - result += "\ttone_alarm ${TUNE_ERR}\n" result += "fi\n" result += "unset AIRFRAME" self.output = result diff --git a/Tools/px4moduledoc/markdownout.py b/Tools/px4moduledoc/markdownout.py index 290618446ebd..469e031a6380 100644 --- a/Tools/px4moduledoc/markdownout.py +++ b/Tools/px4moduledoc/markdownout.py @@ -19,7 +19,7 @@ def __init__(self, module_groups): They describe the provided functionality, high-level implementation overview and how to use the command-line interface. -:::note +::: info **This is auto-generated from the source code** and contains the most recent modules documentation. ::: diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py index b9b348252013..3eacbf432548 100644 --- a/Tools/px4moduledoc/srcparser.py +++ b/Tools/px4moduledoc/srcparser.py @@ -15,7 +15,7 @@ class ModuleDocumentation(object): # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md valid_categories = ['driver', 'estimator', 'controller', 'system', 'communication', 'command', 'template', 'simulation', 'autotune'] - valid_subcategories = ['', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor', + valid_subcategories = ['', 'camera', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor', 'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder'] max_line_length = 80 # wrap lines that are longer than this diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d479be314837..0ab3a6ff258d 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 ############################################################################ # -# Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2024 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 @@ -50,9 +50,6 @@ # Currently only used for informational purposes. # -# for python2.7 compatibility -from __future__ import print_function - import sys import argparse import binascii @@ -70,35 +67,32 @@ try: import serial except ImportError as e: - print("Failed to import serial: " + str(e)) + print(f"Failed to import serial: {e}") print("") print("You may need to install it using:") - print(" pip3 install --user pyserial") + print(" python -m pip install pyserial") print("") sys.exit(1) -# Define time to use time.time() by default -def _time(): - return time.time() - # Detect python version if sys.version_info[0] < 3: - runningPython3 = False -else: - runningPython3 = True - if sys.version_info[1] >=3: - # redefine to use monotonic time when available - def _time(): - try: - return time.monotonic() - except Exception: - return time.time() + raise RuntimeError("Python 2 is not supported. Please try again using Python 3.") + sys.exit(1) + + +# Use monotonic time where available +def _time(): + try: + return time.monotonic() + except Exception: + return time.time() class FirmwareNotSuitableException(Exception): def __init__(self, message): super(FirmwareNotSuitableException, self).__init__(message) + class firmware(object): '''Loads a firmware file''' @@ -163,13 +157,13 @@ def __crc32(self, bytes, state): def crc(self, padlen): state = self.__crc32(self.image, int(0)) - for i in range(len(self.image), (padlen - 1), 4): + for _ in range(len(self.image), (padlen - 1), 4): state = self.__crc32(self.crcpad, state) return state -class uploader(object): - '''Uploads a firmware file to the PX FMU bootloader''' +class uploader: + '''Uploads a firmware file to the PX4 bootloader''' # protocol bytes INSYNC = b'\x12' @@ -195,6 +189,8 @@ class uploader(object): GET_CHIP = b'\x2c' # rev5+ , get chip version SET_BOOT_DELAY = b'\x2d' # rev5+ , set boot delay GET_CHIP_DES = b'\x2e' # rev5+ , get chip description in ASCII + GET_VERSION = b'\x2f' # rev5+ , get chip description in ASCII + CHIP_FULL_ERASE = b'\x40' # full erase of flash, rev6+ MAX_DES_LENGTH = 20 REBOOT = b'\x30' @@ -205,6 +201,7 @@ class uploader(object): INFO_BOARD_ID = b'\x02' # board type INFO_BOARD_REV = b'\x03' # board revision INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes + BL_VERSION = b'\x07' # get bootloader version, e.g. major.minor.patch.githash (up to 20 chars) PROG_MULTI_MAX = 252 # protocol max is 255, must be multiple of 4 READ_MULTI_MAX = 252 # protocol max is 255 @@ -235,6 +232,7 @@ def __init__(self, portname, baudrate_bootloader, baudrate_flightstack): self.baudrate_bootloader = baudrate_bootloader self.baudrate_flightstack = baudrate_flightstack self.baudrate_flightstack_idx = -1 + self.force_erase = False def close(self): if self.port is not None: @@ -357,19 +355,22 @@ def __determineInterface(self): self.port.baudrate = self.baudrate_bootloader * 2.33 except NotImplementedError as e: # This error can occur because pySerial on Windows does not support odd baudrates - print(str(e) + " -> could not check for FTDI device, assuming USB connection") + print(f"{e} -> could not check for FTDI device, assuming USB connection") return self.__send(uploader.GET_SYNC + uploader.EOC) try: self.__getSync(False) - except: + except RuntimeError: # if it fails we are on a real serial port - only leave this enabled on Windows if sys.platform.startswith('win'): self.ackWindowedMode = True finally: - self.port.baudrate = self.baudrate_bootloader + try: + self.port.baudrate = self.baudrate_bootloader + except Exception: + pass # send the GET_DEVICE command and wait for an info parameter def __getInfo(self, param): @@ -410,6 +411,17 @@ def __getCHIPDes(self): pieces = value.split(b",") return pieces + def __getVersion(self): + self.__send(uploader.GET_VERSION + uploader.EOC) + try: + length = self.__recv_int() + value = self.__recv(length) + self.__getSync() + except RuntimeError: + # Bootloader doesn't support version call + return "unknown" + return value.decode() + def __drawProgressBar(self, label, progress, maxVal): if maxVal < progress: progress = maxVal @@ -421,10 +433,16 @@ def __drawProgressBar(self, label, progress, maxVal): # send the CHIP_ERASE command and wait for the bootloader to become ready def __erase(self, label): - print("Windowed mode: %s" % self.ackWindowedMode) + print(f"Windowed mode: {self.ackWindowedMode}") print("\n", end='') - self.__send(uploader.CHIP_ERASE + - uploader.EOC) + + if self.force_erase: + print("Trying force erase of full chip...\n") + self.__send(uploader.CHIP_FULL_ERASE + + uploader.EOC) + else: + self.__send(uploader.CHIP_ERASE + + uploader.EOC) # erase is very slow, give it 30s deadline = _time() + 30.0 @@ -441,9 +459,14 @@ def __erase(self, label): if self.__trySync(): self.__drawProgressBar(label, 10.0, 10.0) + if self.force_erase: + print("\nForce erase done.\n") return - raise RuntimeError("timed out waiting for erase") + if self.force_erase: + raise RuntimeError("timed out waiting for erase, force erase is likely not supported by bootloader!") + else: + raise RuntimeError("timed out waiting for erase") # send a PROG_MULTI command to write a collection of bytes def __program_multi(self, data, windowMode): @@ -581,8 +604,11 @@ def identify(self): self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV) self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE) + self.version = self.__getVersion() + # upload the firmware - def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): + def upload(self, fw_list, force=False, boot_delay=None, boot_check=False, force_erase=False): + self.force_erase = force_erase # select correct binary found_suitable_firmware = False for file in fw_list: @@ -611,6 +637,8 @@ def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent)) print() + print(f"Bootloader version: {self.version}") + # Make sure we are doing the right thing start = _time() if self.board_type != fw.property('board_id'): @@ -640,14 +668,14 @@ def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): self.otp_coa = self.otp[32:160] # show user: try: - print("sn: ", end='') + print("Sn: ", end='') for byte in range(0, 12, 4): x = self.__getSN(byte) x = x[::-1] # reverse the bytes self.sn = self.sn + x print(binascii.hexlify(x).decode('Latin-1'), end='') # show user print('') - print("chip: %08x" % self.__getCHIP()) + print("Chip: %08x" % self.__getCHIP()) otp_id = self.otp_id.decode('Latin-1') if ("PX4" in otp_id): @@ -657,17 +685,19 @@ def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): print("OTP pid: " + binascii.hexlify(self.otp_pid).decode('Latin-1')) print("OTP coa: " + binascii.b2a_base64(self.otp_coa).decode('Latin-1')) - except Exception: + except Exception as e: # ignore bad character encodings + print(f"Exception ignored: {e}") pass # Silicon errata check was added in v5 if (self.bl_rev >= 5): des = self.__getCHIPDes() if (len(des) == 2): - print("family: %s" % des[0]) - print("revision: %s" % des[1]) - print("flash: %d bytes" % self.fw_maxsize) + family, revision = des + print(f"Family: {family.decode()}") + print(f"Revision: {revision.decode()}") + print(f"Flash: {self.fw_maxsize} bytes") # Prevent uploads where the maximum image size of the board config is smaller than the flash # of the board. This is a hint the user chose the wrong config and will lack features @@ -678,8 +708,7 @@ def upload(self, fw_list, force=False, boot_delay=None, boot_check=False): # https://github.com/PX4/Firmware/blob/master/src/drivers/boards/common/stm32/board_mcu_version.c#L125-L144 if self.fw_maxsize > fw.property('image_maxsize') and not force: - raise RuntimeError("Board can accept larger flash images (%u bytes) than board config (%u bytes). Please use the correct board configuration to avoid lacking critical functionality." - % (self.fw_maxsize, fw.property('image_maxsize'))) + raise RuntimeError(f"Board can accept larger flash images ({self.fw_maxsize} bytes) than board config ({fw.property('image_maxsize')} bytes). Please use the correct board configuration to avoid lacking critical functionality.") else: # If we're still on bootloader v4 on a Pixhawk, we don't know if we # have the silicon errata and therefore need to flash px4_fmu-v2 @@ -780,16 +809,13 @@ def send_reboot(self, use_protocol_splitter_format=False): def main(): - # Python2 is EOL - if not runningPython3: - raise RuntimeError("Python 2 is not supported. Please try again using Python 3.") - # Parse commandline arguments parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.") 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.") parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading') + parser.add_argument('--force-erase', action="store_true", help="Do not perform the blank check, always erase every sector of the application space") parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash') parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot') parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)") @@ -867,9 +893,10 @@ def main(): # Windows, don't open POSIX ports if "/" not in port: up = uploader(port, args.baud_bootloader, baud_flightstack) - except Exception: + except Exception as e: # open failed, rate-limit our attempts time.sleep(0.05) + print(f"Exception ignored: {e}") # and loop to the next port continue @@ -884,10 +911,10 @@ def main(): up.identify() found_bootloader = True print() - print("Found board id: %s,%s bootloader version: %s on %s" % (up.board_type, up.board_rev, up.bl_rev, port)) + print(f"Found board id: {up.board_type},{up.board_rev} bootloader protocol revision {up.bl_rev} on {port}") break - except Exception: + except (RuntimeError, serial.SerialException): if not up.send_reboot(args.use_protocol_splitter_format): break @@ -907,14 +934,14 @@ def main(): try: # ok, we have a bootloader, try flashing it - up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay) + up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay, force_erase=args.force_erase) # if we made this far without raising exceptions, the upload was successful successful = True - except RuntimeError as ex: + except RuntimeError as e: # print the error - print("\nERROR: %s" % ex.args) + print(f"\n\nError: {e}") except FirmwareNotSuitableException: unsuitable_board = True diff --git a/Tools/serial/generate_config.py b/Tools/serial/generate_config.py index 099091dfe470..dc9c2762f9e5 100755 --- a/Tools/serial/generate_config.py +++ b/Tools/serial/generate_config.py @@ -330,4 +330,3 @@ def parse_yaml_serial_config(yaml_config): fid.write(template.render(serial_devices=serial_devices, ethernet_configuration=ethernet_configuration, commands=commands, serial_ports=serial_ports)) - diff --git a/Tools/serial/rc.serial.jinja b/Tools/serial/rc.serial.jinja index 753f2af75023..3ff8d62b811c 100644 --- a/Tools/serial/rc.serial.jinja +++ b/Tools/serial/rc.serial.jinja @@ -30,4 +30,3 @@ unset PRT unset PRT_F unset BAUD_PARAM unset MAV_ARGS - diff --git a/Tools/serial/serial_params.c.jinja b/Tools/serial/serial_params.c.jinja index 13cee1e896ea..cdc6e999a51a 100644 --- a/Tools/serial/serial_params.c.jinja +++ b/Tools/serial/serial_params.c.jinja @@ -70,4 +70,3 @@ PARAM_DEFINE_INT32({{ command.port_param_name }}, {{ command.default_port }}); {% endfor -%} {% endif %} - diff --git a/Tools/setup/arch.sh b/Tools/setup/arch.sh index 8e19ee51015d..1ab164b2aaf4 100755 --- a/Tools/setup/arch.sh +++ b/Tools/setup/arch.sh @@ -95,7 +95,7 @@ if [[ $INSTALL_SIM == "true" ]]; then # java (jmavsim) sudo pacman -S --noconfirm --needed \ - ant + ant \ ; # Gazebo setup diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index 2db827d34e3b..0202963fd773 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -1,5 +1,4 @@ argcomplete -argparse>=1.2 cerberus coverage empy==3.3.4 @@ -8,7 +7,7 @@ jinja2>=2.8 jsonschema kconfiglib lxml -matplotlib>=3.0.* +matplotlib>=3.0 numpy>=1.13 nunavut>=1.1.0 packaging diff --git a/Tools/setup/ubuntu.sh b/Tools/setup/ubuntu.sh index f509ced4eb1a..077cfda56bf1 100755 --- a/Tools/setup/ubuntu.sh +++ b/Tools/setup/ubuntu.sh @@ -66,6 +66,8 @@ elif [[ "${UBUNTU_RELEASE}" == "20.04" ]]; then echo "Ubuntu 20.04" elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then echo "Ubuntu 22.04" +elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then + echo "Linux Mint 21.3" fi @@ -146,7 +148,7 @@ if [[ $INSTALL_NUTTX == "true" ]]; then util-linux \ vim-common \ ; - if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" ]]; then + if [[ "${UBUNTU_RELEASE}" == "20.04" || "${UBUNTU_RELEASE}" == "22.04" || "${UBUNTU_RELEASE}" == "21.3" ]]; then sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \ kconfig-frontends \ ; @@ -205,6 +207,8 @@ if [[ $INSTALL_SIM == "true" ]]; then java_version=13 elif [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then java_version=11 + elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then + java_version=11 else java_version=14 fi @@ -228,6 +232,17 @@ if [[ $INSTALL_SIM == "true" ]]; then echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null sudo apt-get update -y --quiet + # Install Gazebo + gazebo_packages="gz-garden" + elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then + echo "Gazebo (Garden) will be installed" + echo "Earlier versions will be removed" + # Add Gazebo binary repository + sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + + sudo apt-get update -y --quiet + # Install Gazebo gazebo_packages="gz-garden" else diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index da7206e05770..f1d11a612699 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit da7206e057703cc645770f02437013358b71e1c0 +Subproject commit f1d11a6126990d487d4aa8ff68c23ff370516510 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 6b4ed09d1b49..536305adee09 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 6b4ed09d1b495fbff663f098979cc046df013abd +Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 diff --git a/Tools/test_keys/rsa2048.pem b/Tools/test_keys/rsa2048.pem index e2ee0e61bfde..21445d8d6466 100644 --- a/Tools/test_keys/rsa2048.pem +++ b/Tools/test_keys/rsa2048.pem @@ -24,4 +24,4 @@ q8jVArPNQT4R2erSmKmIGTRkLMG7CzUmwk1taHdSvzcmUyL4uYc5QBSubxat6gWh +a85AoGBAKgfnjjVoAWWvqEDLpfGPmE8lW+RaS7i5ff6QsSBx7uTEnHq6RNHuVnN et4pR87yIENeG8jMBiDCj8AGDtUNt9Ps9vWCPrf9HSOYoBUk+gZagU/9N+RBpuCM egoxtxIHM7HI+XIer+ZnZpVpgr+EoCaL7avx6k/susLQb7tqSBt1 ------END RSA PRIVATE KEY----- \ No newline at end of file +-----END RSA PRIVATE KEY----- diff --git a/Tools/test_keys/test_keys.json b/Tools/test_keys/test_keys.json index 127a5be598d5..0421279c87b9 100644 --- a/Tools/test_keys/test_keys.json +++ b/Tools/test_keys/test_keys.json @@ -1 +1 @@ -{"date": "Tue Nov 3 13:02:09 2020", "public": "4db0c20105552a3cd7fbaf5cba7ab0811b3663db28525edb1436f2578d02b7fd", "private": "734d597e7d8ab0a1d0d64b95083aa6bee34b46b9e6e76dac1e363af114f12d15"} \ No newline at end of file +{"date": "Tue Nov 3 13:02:09 2020", "public": "4db0c20105552a3cd7fbaf5cba7ab0811b3663db28525edb1436f2578d02b7fd", "private": "734d597e7d8ab0a1d0d64b95083aa6bee34b46b9e6e76dac1e363af114f12d15"} diff --git a/Tools/upload.sh b/Tools/upload.sh index 25d41e389f76..2a6416ed374d 100755 --- a/Tools/upload.sh +++ b/Tools/upload.sh @@ -15,7 +15,7 @@ SERIAL_PORTS="/dev/tty.usbmodemPX*,/dev/tty.usbmodem*" fi if [ $SYSTYPE = "Linux" ]; then -SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/ARK*," +SERIAL_PORTS="/dev/serial/by-id/*_PX4_*,/dev/serial/by-id/usb-3D_Robotics*,/dev/serial/by-id/usb-The_Autopilot*,/dev/serial/by-id/usb-Bitcraze*,/dev/serial/by-id/pci-Bitcraze*,/dev/serial/by-id/usb-Gumstix*,/dev/serial/by-id/usb-UVify*,/dev/serial/by-id/usb-ArduPilot*,/dev/serial/by-id/usb-ARK*," fi if [[ $SYSTYPE = *"CYGWIN"* ]]; then diff --git a/Tools/upload_log.py b/Tools/upload_log.py index d39a792cea87..a13f1b14b07c 100755 --- a/Tools/upload_log.py +++ b/Tools/upload_log.py @@ -125,4 +125,3 @@ def main(): if __name__ == '__main__': main() - diff --git a/Tools/validate_json.py b/Tools/validate_json.py index b80dad3198be..2c53ef815baa 100755 --- a/Tools/validate_json.py +++ b/Tools/validate_json.py @@ -50,4 +50,3 @@ except: print("JSON validation for {:} failed (schema: {:})".format(json_file, schema_file)) raise - diff --git a/Tools/validate_yaml.py b/Tools/validate_yaml.py index 3a2d022bfc88..e7b9a25fd838 100755 --- a/Tools/validate_yaml.py +++ b/Tools/validate_yaml.py @@ -65,4 +65,3 @@ def load_yaml_file(file_name): print(validator.errors) print("") raise Exception("Validation of {:} failed".format(yaml_file)) - diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board new file mode 100755 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board new file mode 100644 index 000000000000..549ba99b7331 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/default.px4board @@ -0,0 +1,96 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS4" +CONFIG_DRIVERS_ADC_ADS1115=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PCA9685_PWM_OUT=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SMART_BATTERY_BATMON=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=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_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y +CONFIG_MODULES_TEMPERATURE_COMPENSATION=y +CONFIG_MODULES_UUV_ATT_CONTROL=y +CONFIG_MODULES_UUV_POS_CONTROL=y +CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_REFLECT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=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_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin new file mode 100755 index 000000000000..86dc50de7b9c Binary files /dev/null and b/boards/3dr/ctrl-zero-h7-oem-revg/extras/3dr_ctrl-zero-h7-oem-revg_bootloader.bin differ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype b/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype new file mode 100755 index 000000000000..d11458c1a3fe --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1124, + "magic": "3dr-ctrl-zero-h7-oem-revg", + "description": "Firmware for the 3dr-ctrl-zero-h7-oem-revg board", + "image": "", + "build_time": 0, + "summary": "3dr-ctrl-zero-h7-oem-revg", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults new file mode 100755 index 000000000000..32117f4a5886 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_defaults @@ -0,0 +1,9 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_A_PER_V 17 + +safety_button start diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors new file mode 100755 index 000000000000..563c24bdf4fe --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/init/rc.board_sensors @@ -0,0 +1,19 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# Internal ICM-20602 +icm20602 -s -b 1 -R 8 start + +# Internal SPI bus BMI088 accel & gyro +bmi088 -A -s -b 5 -R 8 start +bmi088 -G -s -b 5 -R 8 start + +# Internal ICM-20948 (with magnetometer) +icm20948 -s -b 1 -R 8 -M start + +# Interal DPS310 (barometer) +dps310 -s -b 2 start diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/bootloader/defconfig new file mode 100755 index 000000000000..796a2853edef --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/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_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL 3DR ControlZeroH7 OEM" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=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_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h new file mode 100755 index 000000000000..698e8835fc12 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board.h @@ -0,0 +1,288 @@ +/************************************************************************************ + * nuttx-config/include/board.h + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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. + * + ************************************************************************************/ +#pragma once + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/* Clocking *************************************************************************/ +/* The board provides the following clock sources: + * + * X1: 24 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed internal oscillator + * HSE: 24 MHz crystal for HSE + */ +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 0 + +#define STM32_HSEBYP_ENABLE 1 + +/* Main PLL Configuration. + * + * PLL source is HSE = 24,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (24,000,000 / 2) * 80 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE|RCC_PLLCFGR_PLL1RGE_4_8_MHZ|RCC_PLLCFGR_DIVP1EN|RCC_PLLCFGR_DIVQ1EN|RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(2) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(80) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 80) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE|RCC_PLLCFGR_PLL2RGE_4_8_MHZ|RCC_PLLCFGR_DIVP2EN|RCC_PLLCFGR_DIVQ2EN|RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(32) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE|RCC_PLLCFGR_PLL3RGE_4_8_MHZ|RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(32) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 32) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * Note: look at Table 54 in ST Manual + */ +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI /* I2C123 clock source */ +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI /* I2C4 clock source */ +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 /* SPI123 clock source */ +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 /* SPI45 clock source */ +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 /* SPI6 clock source */ +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 /* USB 1 and 2 clock source */ +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + +/* FLASH wait states */ +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + + +/* UART/USART */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ + +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_CTS GPIO_USART3_CTS_NSS_2 /* PD11 */ +#define GPIO_USART3_RTS GPIO_USART3_RTS_2 /* PD12 */ + +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ + +#define GPIO_USART6_TX 0 /* USART6 is RX-only */ +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ + +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ + +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ + + +/* CAN */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +#define GPIO_CAN2_RX GPIO_CAN2_RX_1 /* PB12 */ +#define GPIO_CAN2_TX GPIO_CAN2_TX_1 /* PB13 */ + + +/* SPI */ +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ + +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_3) /* PB10 */ +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 /* PB15 */ + +#define GPIO_SPI5_SCK ADJ_SLEW_RATE(GPIO_SPI5_SCK_1) /* PF7 */ +#define GPIO_SPI5_MISO GPIO_SPI5_MISO_1 /* PF8 */ +#define GPIO_SPI5_MOSI GPIO_SPI5_MOSI_2 /* PF9 */ + + +/* I2C */ +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_2 /* PH7 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_4 /* PB6 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_4 /* PB7 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h new file mode 100755 index 000000000000..99a2fe3ba9b9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/include/board_dma_map.h @@ -0,0 +1,44 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +// DMAMUX1 +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_1 /* DMA1:71 */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_1 /* DMA1:72 */ + +#define DMAMAP_SPI5_RX DMAMAP_DMA12_SPI5RX_0 /* DMA1:83 */ +#define DMAMAP_SPI5_TX DMAMAP_DMA12_SPI5TX_0 /* DMA1:84 */ diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..c9ec46ed878e --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/nsh/defconfig @@ -0,0 +1,260 @@ +# +# 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_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_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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1124 +CONFIG_CDCACM_PRODUCTSTR="3DRControlZeroH7OEM_revG" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x26ac +CONFIG_CDCACM_VENDORSTR="3DR" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_STM32H7_PWR_EXTERNAL_SOURCE_SUPPLY=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +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_EXTERNAL=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_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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI5=y +CONFIG_STM32H7_SPI5_DMA=y +CONFIG_STM32H7_SPI5_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_SERIAL_CONSOLE=y +CONFIG_UART8_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld new file mode 100755 index 000000000000..3fb4cc1f33ce --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,221 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 128K + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > AXI_SRAM AT > FLASH + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* 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) } + + .ramfunc : { + _sramfuncs = .; + *(.ramfunc .ramfunc.*) + . = ALIGN(4); + _eramfuncs = .; + } > ITCM_RAM AT > FLASH + + _framfuncs = LOADADDR(.ramfunc); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld new file mode 100755 index 000000000000..02e763a790fb --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2021 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743XIH6 and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* 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/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt new file mode 100755 index 000000000000..6a7d2ce306f7 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/CMakeLists.txt @@ -0,0 +1,65 @@ +############################################################################ +# +# 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + spi.cpp + timer_config.cpp + usb.c + ) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h new file mode 100755 index 000000000000..bcc8bba1a619 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/board_config.h @@ -0,0 +1,187 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +#include +#include +#include +#include + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ +#define GPIO_nLED_RED /* PB11 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) +#define GPIO_nLED_GREEN /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PB3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN3) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ +#define PX4_ADC_GPIO \ + /* PA2 */ GPIO_ADC12_INP14, \ + /* PA3 */ GPIO_ADC12_INP15, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PC1 */ GPIO_ADC123_INP11 + +/* Define Channel numbers must match above GPIO pins */ +#define ADC_BATTERY_VOLTAGE_CHANNEL 14 /* PA2 BATT_VOLT_SENS */ +#define ADC_BATTERY_CURRENT_CHANNEL 15 /* PA3 BATT_CURRENT_SENS */ +#define ADC_SCALED_V5_CHANNEL 18 /* PA4 VDD_5V_SENS */ +#define ADC_RC_RSSI_CHANNEL 11 /* PC1 */ + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_RC_RSSI_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* CAN Silence: Silent mode control */ +#define GPIO_CAN1_SILENT_S0 /* PF5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN5) + +/* PWM */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +/* Power supply control and monitoring GPIOs */ +#define GPIO_nPOWER_IN_A /* PB5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) + +#define GPIO_VDD_BRICK1_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 + +#define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_VDD_1V2_CORE_POWER_EN /* PH5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN5) + +/* Define True logic Power Control in arch agnostic form */ +#define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (!on_true)) +#define READ_VDD_3V3_SPEKTRUM_POWER_EN() (px4_arch_gpioread(GPIO_VDD_3V3_SPEKTRUM_POWER_EN) == 0) + +/* Tone alarm output */ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* PA15 TIM2_CH1 */ + +#define GPIO_BUZZER_1 /* PA15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN15) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM2_CH1OUT_2 + +/* USB OTG FS */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +/* High-resolution timer */ +#define HRT_TIMER 3 /* use timer3 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +#define HRT_PPM_CHANNEL /* T3C3 */ 3 /* use capture/compare channel 3 */ +#define GPIO_PPM_IN /* PB0 T3C3 */ GPIO_TIM3_CH3IN_1 + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS3" + +#define GPIO_RSSI_IN /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) + +/* Safety Switch: Enable the FMU to control it if there is no px4io fixme:This should be BOARD_SAFETY_LED(__ontrue) */ +#define GPIO_SAFETY_SWITCH_IN /* PC4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN4) +/* Enable the FMU to use the switch it if there is no px4io fixme:This should be BOARD_SAFTY_BUTTON() */ +#define GPIO_BTN_SAFETY GPIO_SAFETY_SWITCH_IN /* Enable the FMU to control it if there is no px4io */ + +/* Power switch controls ******************************************************/ +#define SPEKTRUM_POWER(_on_true) VDD_3V3_SPEKTRUM_POWER_EN(_on_true) + +/* + * Board has a separate RC_IN + * + * GPIO PPM_IN on PB0 T3CH3 + * SPEKTRUM_RX (it's TX or RX in Bind) on UART6 PC7 + * Inversion is possible in the UART and can drive GPIO_PPM_IN as an output + */ +#define GPIO_PPM_IN_AS_OUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) +#define SPEKTRUM_RX_AS_GPIO_OUTPUT() px4_arch_configgpio(GPIO_PPM_IN_AS_OUT) +#define SPEKTRUM_RX_AS_UART() /* Can be left as uart */ +#define SPEKTRUM_OUT(_one_true) px4_arch_gpiowrite(GPIO_PPM_IN_AS_OUT, (_one_true)) + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) +#define BOARD_ADC_USB_VALID BOARD_ADC_USB_CONNECTED +#define BOARD_ADC_SERVO_VALID (1) /* never powers off the Servo rail */ +#define BOARD_ADC_BRICK_VALID (px4_arch_gpioread(GPIO_VDD_BRICK1_VALID)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define BOARD_HAS_STATIC_MANIFEST 1 + + +#define BOARD_NUM_IO_TIMERS 3 + + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN2_TX, \ + GPIO_CAN2_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_nPOWER_IN_A, \ + GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_1V2_CORE_POWER_EN, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SAFETY_SWITCH_IN, \ + GPIO_OTGFS_VBUS, \ + } + +__BEGIN_DECLS +#ifndef __ASSEMBLY__ + +extern void stm32_spiinitialize(void); +extern void board_peripheral_reset(int ms); + +#include +#endif /* __ASSEMBLY__ */ +__END_DECLS diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c new file mode 100755 index 000000000000..bb6b4dc23aad --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (c) 2020, 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_configgpio(GPIO_OTGFS_VBUS); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h new file mode 100755 index 000000000000..b212ccd60273 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 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 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1124 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 24 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_RED +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#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 diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp new file mode 100755 index 000000000000..1b8927c69939 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c new file mode 100755 index 000000000000..daaace48fb2f --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/init.c @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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 init.c + * + * board-specific early startup code. This file implements the + * board_app_initialize() 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. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +__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) +{ + bool last = READ_VDD_3V3_SPEKTRUM_POWER_EN(); + /* Keep Spektum on to discharge rail*/ + VDD_3V3_SPEKTRUM_POWER_EN(false); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + VDD_3V3_SPEKTRUM_POWER_EN(last); +} + +/************************************************************************************ + * 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) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. 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 stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + board_control_spi_sensors_power_configgpio(); + + /* configure LEDs */ + board_autoled_initialize(); +} + +/**************************************************************************** + * 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; + * + * 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) +{ + /* Power on Interfaces */ + board_control_spi_sensors_power(true, 0xffff); + VDD_3V3_SPEKTRUM_POWER_EN(true); + + px4_platform_init(); + + stm32_spiinitialize(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + + /* 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 + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + struct sdio_dev_s *sdio_dev = sdio_initialize(0); // SDIO_SLOTNO 0 Only one slot + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] Failed to initialize SDIO slot %d\n", 0); + } + + if (mmcsd_slotinitialize(0, sdio_dev) != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver\n"); + } + + /* Assume that the SD card is inserted. What choice do we have? */ + sdio_mediachange(sdio_dev, true); +#endif /* CONFIG_MMCSD */ + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + return OK; +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c new file mode 100755 index 000000000000..0e7beb3a9e3b --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/led.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(led, true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(led, false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(led, !phy_get_led(led)); +} diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp new file mode 100755 index 000000000000..4a4c3502bbd9 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), + initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), + }), + initSPIBus(SPI::Bus::SPI5, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp new file mode 100755 index 000000000000..e23880491444 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/timer_config.cpp @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer8, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel1}, {GPIO::PortI, GPIO::Pin5}), + initIOTimerChannel(io_timers, {Timer::Timer8, Timer::Channel2}, {GPIO::PortI, GPIO::Pin6}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c new file mode 100755 index 000000000000..fe32ce71ac63 --- /dev/null +++ b/boards/3dr/ctrl-zero-h7-oem-revg/src/usb.c @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + + + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/airmind/mindpx-v2/default.px4board b/boards/airmind/mindpx-v2/default.px4board index cb56388c36fc..33ad75b6957b 100644 --- a/boards/airmind/mindpx-v2/default.px4board +++ b/boards/airmind/mindpx-v2/default.px4board @@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_GPS=y diff --git a/boards/airmind/mindpx-v2/src/i2c.cpp b/boards/airmind/mindpx-v2/src/i2c.cpp index 5802883bfa13..2b708954d559 100644 --- a/boards/airmind/mindpx-v2/src/i2c.cpp +++ b/boards/airmind/mindpx-v2/src/i2c.cpp @@ -37,4 +37,3 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusInternal(1), initI2CBusExternal(2), }; - diff --git a/boards/airmind/mindpx-v2/src/usb.c b/boards/airmind/mindpx-v2/src/usb.c index 93a008cf0f48..a714f2f6f1c1 100644 --- a/boards/airmind/mindpx-v2/src/usb.c +++ b/boards/airmind/mindpx-v2/src/usb.c @@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/boards/ark/can-rtk-gps/init/rc.board_defaults b/boards/ark/can-rtk-gps/init/rc.board_defaults index c50d81602629..99b30c747f8a 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_defaults +++ b/boards/ark/can-rtk-gps/init/rc.board_defaults @@ -6,6 +6,7 @@ param set-default CBRK_IO_SAFETY 0 param set-default CANNODE_SUB_MBD 1 param set-default CANNODE_SUB_RTCM 1 +param set-default GPS_1_GNSS 63 param set-default MBE_ENABLE 1 param set-default SENS_IMU_CLPNOTI 0 diff --git a/boards/ark/can-rtk-gps/src/board_config.h b/boards/ark/can-rtk-gps/src/board_config.h index 2263956bae0f..2d5f582075f1 100644 --- a/boards/ark/can-rtk-gps/src/board_config.h +++ b/boards/ark/can-rtk-gps/src/board_config.h @@ -47,7 +47,7 @@ #define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) /* Safety LED */ -#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) +#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) /* Tone alarm output. */ #define TONE_ALARM_TIMER 2 /* timer 2 */ diff --git a/boards/ark/cannode/default.px4board b/boards/ark/cannode/default.px4board index 3141d40b3089..d875f6b8211f 100644 --- a/boards/ark/cannode/default.px4board +++ b/boards/ark/cannode/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_COMMON_HYGROMETERS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IRLOCK=y diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 4018e493a2ad..92bf7cb23895 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -7,11 +7,13 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -21,6 +23,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16507=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y +CONFIG_DRIVERS_IMU_MURATA_SCH16T=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y @@ -29,7 +32,7 @@ CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y -CONFIG_DRIVERS_RC_INPUT=y +CONFIG_COMMON_RC=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y @@ -57,6 +60,7 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin b/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin index f4ec9666cc17..ea510ab36e38 100755 Binary files a/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin and b/boards/ark/fmu-v6x/extras/ark_fmu-v6x_bootloader.bin differ diff --git a/boards/ark/fmu-v6x/extras/px4_io-v2_default.bin b/boards/ark/fmu-v6x/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/ark/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/ark/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index 17da75424e9f..f5e59d2c3781 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -147,8 +147,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/ark/pi6x/bootloader.px4board b/boards/ark/pi6x/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/ark/pi6x/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/ark/pi6x/default.px4board b/boards/ark/pi6x/default.px4board new file mode 100644 index 000000000000..84c6fd79f2b2 --- /dev/null +++ b/boards/ark/pi6x/default.px4board @@ -0,0 +1,74 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS5" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS2" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_DRIVERS_DISTANCE_SENSOR_BROADCOM_AFBRS50=y +CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_COMMON_LIGHT=y +CONFIG_DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA=y +CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y +CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_COMMON_RC=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 +CONFIG_MODULES_AIRSPEED_SELECTOR=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_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y +CONFIG_MODULES_LOAD_MON=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" +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_UXRCE_DDS_CLIENT=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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 diff --git a/boards/ark/pi6x/extras/ark_pi6x_bootloader.bin b/boards/ark/pi6x/extras/ark_pi6x_bootloader.bin new file mode 100755 index 000000000000..fce9348a7408 Binary files /dev/null and b/boards/ark/pi6x/extras/ark_pi6x_bootloader.bin differ diff --git a/boards/ark/pi6x/firmware.prototype b/boards/ark/pi6x/firmware.prototype new file mode 100644 index 000000000000..6c21025a6bc2 --- /dev/null +++ b/boards/ark/pi6x/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 58, + "magic": "ARKPi6XFWv1", + "description": "Firmware for the ARKPi6X board", + "image": "", + "build_time": 0, + "summary": "ARKPi6X", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/ark/pi6x/init/rc.board_defaults b/boards/ark/pi6x/init/rc.board_defaults new file mode 100644 index 000000000000..717632839be8 --- /dev/null +++ b/boards/ark/pi6x/init/rc.board_defaults @@ -0,0 +1,41 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ + +# transision from params file to flash-based params (2022-08) +if [ -f $PARAM_FILE ] +then + param load $PARAM_FILE + param save + # create a backup + mv $PARAM_FILE ${PARAM_FILE}.bak + reboot +fi + +param set-default SENS_EN_INA226 1 + +# TODO: Set params to start UXRCE DDS on Telem2 + +# TODO: Start Mavlink on USB by default + +# TODO: Tune the following parameters +param set-default SENS_EN_THERMAL 1 +param set-default SENS_IMU_TEMP 10.0 +#param set-default SENS_IMU_TEMP_FF 0.0 +#param set-default SENS_IMU_TEMP_I 0.025 +#param set-default SENS_IMU_TEMP_P 1.0 + +if ver hwtypecmp ARKPI6X000 +then + # TODO: Add the correct sensor ID + param set-default SENS_TEMP_ID 2490378 +fi + +param set-default EKF2_MULTI_IMU 2 +param set-default EKF2_OF_CTRL 1 +param set-default EKF2_OF_N_MIN 0.05 +param set-default EKF2_RNG_A_HMAX 25 +param set-default EKF2_RNG_QLTY_T 0.1 + +param set-default SENS_FLOW_RATE 150 diff --git a/boards/ark/pi6x/init/rc.board_sensors b/boards/ark/pi6x/init/rc.board_sensors new file mode 100644 index 000000000000..cc97b1d601fd --- /dev/null +++ b/boards/ark/pi6x/init/rc.board_sensors @@ -0,0 +1,36 @@ +#!/bin/sh +# +# ARKPI6X specific board sensors init +#------------------------------------------------------------------------------ + +board_adc start + +if param compare SENS_EN_INA226 1 +then + # Start Digital power monitors + ina226 -I -b 3 -t 1 -k start +fi + +if ver hwtypecmp ARKPI6X000 +then + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 3 -s -b 1 -C 32051 start + + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 1 -s -b 2 -C 32051 start +fi + +# Internal magnetometer on I2C +if ! iis2mdc -R 4 -I -b 4 start +then + mmc5983ma -I -b 4 start +fi + +# Internal Baro on I2C +bmp388 -I -b 4 start + +# Internal optical flow +paw3902 -s -b 3 start -Y 90 + +# Internal distance sensor +afbrs50 start diff --git a/boards/ark/pi6x/nuttx-config/Kconfig b/boards/ark/pi6x/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/ark/pi6x/nuttx-config/bootloader/defconfig b/boards/ark/pi6x/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..745251793bd0 --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/bootloader/defconfig @@ -0,0 +1,95 @@ +# +# 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_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/pi6x/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="ark" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTSTR="ARK BL Pi6X.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="ARK" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=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_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_UART7=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_RXDMA=y +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_TXDMA=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/ark/pi6x/nuttx-config/include/board.h b/boards/ark/pi6x/nuttx-config/include/board.h new file mode 100644 index 000000000000..2b716936cebd --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/include/board.h @@ -0,0 +1,512 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6x/include/board.h + * + * Copyright (C) 2016-2024 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The px4_fmu-v6X board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(60) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 60) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 2 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The ARKV6X board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +/* LED definitions ******************************************************************/ +/* The px4_fmu-v6x board has three, LED_GREEN a Green LED, LED_BLUE a Blue LED and + * LED_RED a Red LED, that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_3 /* PB7 */ +#define GPIO_USART1_TX GPIO_USART1_TX_3 /* PB6 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_6 /* PH14 */ +#define GPIO_UART4_TX GPIO_UART4_TX_6 /* PH13 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_3 /* PD2 */ +#define GPIO_UART5_TX GPIO_UART5_TX_3 /* PC12 */ +// GPIO_UART5_RTS No remap /* PC8 */ +#define GPIO_UART5_CTS (GPIO_ALT|GPIO_AF8|GPIO_PORTC|GPIO_PIN9|GPIO_PULLDOWN) /* PC9 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_4 /* PF6 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_2 /* PF8 */ +#define GPIO_UART7_CTS (GPIO_UART7_CTS_1 | GPIO_PULLDOWN) /* PE10 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + */ +#define GPIO_CAN1_RX GPIO_CAN1_RX_3 /* PD0 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_3 /* PD1 */ + +/* SPI + * SPI1 is sensors1 + * SPI2 is sensors2 + * SPI3 is sensors3 + * SPI4 is Not Used + * SPI5 is Not Used + * SPI6 is EXTERNAL1 + * + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_3 /* PG9 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_2 /* PB5 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_6) /* PI1 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 /* PC11 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_3 /* PB2 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_2) /* PC10 */ + +#define GPIO_SPI6_MISO GPIO_SPI6_MISO_2 /* PA6 */ +#define GPIO_SPI6_MOSI GPIO_SPI6_MOSI_1 /* PG14 */ +#define GPIO_SPI6_SCK ADJ_SLEW_RATE(GPIO_SPI6_SCK_3) /* PB3 */ + +/* I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_2 /* PB9 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN9) + +#define GPIO_I2C3_SCL GPIO_I2C3_SCL_1 /* PA8 */ +#define GPIO_I2C3_SDA GPIO_I2C3_SDA_2 /* PH8 */ + +#define GPIO_I2C3_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN8) +#define GPIO_I2C3_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTH | GPIO_PIN8) + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN14) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTF | GPIO_PIN15) + +/* SDMMC2 + * + * VDD 3.3 + * GND + * SDMMC2_CK PD6 + * SDMMC2_CMD PD7 + * SDMMC2_D0 PB14 + * SDMMC2_D1 PB15 + * SDMMC2_D2 PG11 + * SDMMC2_D3 PB4 + */ + +#define GPIO_SDMMC2_CK GPIO_SDMMC2_CK_1 /* PD6 */ +#define GPIO_SDMMC2_CMD GPIO_SDMMC2_CMD_1 /* PD7 */ +// GPIO_SDMMC2_D0 No Remap /* PB14 */ +// GPIO_SDMMC2_D1 No Remap /* PB15 */ +#define GPIO_SDMMC2_D2 GPIO_SDMMC2_D2_1 /* PG11 */ +// GPIO_SDMMC2_D3 No Remap /* PB4 */ + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# include "stm32_gpio.h" +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) /* PI0 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN12) /* PH12 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN11) /* PH11 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN10) /* PH10 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) /* PD13 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) /* PH6 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) /* PH9 AUX8 */ + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ + if ((mask)& PROBE_N(9)) { stm32_configgpio(PROBE_9); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +#endif /*__NUTTX_CONFIG_PX4_FMU_V6X_INCLUDE_BOARD_H */ diff --git a/boards/ark/pi6x/nuttx-config/include/board_dma_map.h b/boards/ark/pi6x/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..62e278597243 --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/include/board_dma_map.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// DMAMUX1 Using at most 8 Channels on DMA1 -------- Assigned +// V + +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* 1 DMA1:37 ICM-20649 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* 2 DMA1:38 ICM-20649 */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* 3 DMA1:39 ICM-42688-P */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 ICM-42688-P */ + +//#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_0 /* DMA1:41 GPS1 */ +//#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_0 /* DMA1:42 GPS1 */ + +//#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_0 /* DMA1:45 DEBUG */ +//#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_0 /* DMA1:46 DEBUG */ + +//#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 EXT2 */ +//#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 EXT2 */ + +#define DMAMAP_USART6_RX DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 PX4IO */ +#define DMAMAP_USART6_TX DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 PX4IO */ + +// Assigned in timer_config.cpp + +// Timer 4 /* 7 DMA1:32 TIM4UP */ +// Timer 5 /* 8 DMA1:50 TIM5UP */ + +// DMAMUX2 Using at most 8 Channels on DMA2 -------- Assigned +// V + +#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_1 /* 1 DMA2:61 BMI088 */ +#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_1 /* 2 DMA2:62 BMI088 */ + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* 3 DMA2:45 DEBUG */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* 4 DMA2:46 DEBUG */ + +#define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_1 /* 5 DMA2:65 TELEM2 */ +#define DMAMAP_UART5_TX DMAMAP_DMA12_UART5TX_1 /* 6 DMA2:66 TELEM2 */ + +#define DMAMAP_UART7_RX DMAMAP_DMA12_UART7RX_1 /* 7 DMA1:79 TELEM1 */ +#define DMAMAP_UART7_TX DMAMAP_DMA12_UART7TX_1 /* 8 DMA1:80 TELEM1 */ + +// DMAMUX2 Using at most 8 Channels on BDMA -------- Assigned +// V + +#define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* 1 BDMA:11 SPI J11 */ +#define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* 2 BDMA:12 SPI J11 */ diff --git a/boards/ark/pi6x/nuttx-config/nsh/defconfig b/boards/ark/pi6x/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..0e944e53c999 --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/nsh/defconfig @@ -0,0 +1,270 @@ +# +# 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_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_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_TELNETD 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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/ark/pi6x/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="ark" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743II=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0039 +CONFIG_CDCACM_PRODUCTSTR="ARK Pi6X.x" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="ARK" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +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=3000 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C3=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI3_DMA=y +CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI6=y +CONFIG_STM32H7_SPI6_DMA=y +CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=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_RXDMA=y +CONFIG_UART5_TXBUFSIZE=10000 +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_UART7_TXDMA=y +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_RXDMA=y +CONFIG_USART3_SERIAL_CONSOLE=y +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART3_TXDMA=y +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/ark/pi6x/nuttx-config/scripts/bootloader_script.ld b/boards/ark/pi6x/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..b0515c91c7f8 --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2024 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The ARKV6X uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The ARKV6X has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The bootloader uses the first sector of the flash, which is 128K in length. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 128K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* 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/ark/pi6x/nuttx-config/scripts/script.ld b/boards/ark/pi6x/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..8e6dca3e4941 --- /dev/null +++ b/boards/ark/pi6x/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2024 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The ARKV6X uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The ARKV6X has a test point on board, the BOOT0 pin is at ground so by + * default, the STM32 will boot to address 0x0800:0000 in FLASH unless the test + * point is pulled to 3.3V.then the boot will be from 0x1FF0:0000 + * + * The STM32H743II also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K /* params in last sector */ + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) +EXTERN(board_get_manifest) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* 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/ark/pi6x/src/CMakeLists.txt b/boards/ark/pi6x/src/CMakeLists.txt new file mode 100644 index 000000000000..78b8222f19d8 --- /dev/null +++ b/boards/ark/pi6x/src/CMakeLists.txt @@ -0,0 +1,77 @@ +############################################################################ +# +# Copyright (c) 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 +# 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_compile_definitions(BOOTLOADER) + add_library(drivers_board + bootloader_main.c + init.c + usb.c + timer_config.cpp + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer #gpio + arch_io_pins # iotimer + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + can.c + i2c.cpp + init.c + led.c + mtd.cpp + sdio.c + spi.cpp + spix_sync.c + spix_sync.h + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/ark/pi6x/src/board_config.h b/boards/ark/pi6x/src/board_config.h new file mode 100644 index 000000000000..5e123db35124 --- /dev/null +++ b/boards/ark/pi6x/src/board_config.h @@ -0,0 +1,402 @@ +/**************************************************************************** + * + * Copyright (c) 2016-2024 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 board_config.h + * + * ARK Pi6X internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#undef TRACE_PINS + +/* Configuration ************************************************************************************/ + +#define BOARD_HAS_NBAT_V 1d // 2 Digital Voltage +#define BOARD_HAS_NBAT_I 1d // 2 Digital Current + +/* PX4FMU GPIOs ***********************************************************************************/ + +/* Trace Clock and D0-D3 are available on the trace connector + * + * TRACECLK PE2 - Dedicated - Trace Connector Pin 1 + * TRACED0 PE3 - nLED_RED - Trace Connector Pin 3 + * TRACED1 PE4 - nLED_GREEN - Trace Connector Pin 5 + * TRACED2 PE5 - nLED_BLUE - Trace Connector Pin 7 + * TRACED3 PE6 - nARMED - Trace Connector Pin 8 + + */ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V or used as TRACE0-2 */ + +#if !defined(TRACE_PINS) +# define GPIO_nLED_RED /* PE3 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +# define GPIO_nLED_GREEN /* PE4 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +# define GPIO_nLED_BLUE /* PE5 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + +#else + +# define GPIO_TRACECLK1 (GPIO_TRACECLK |GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN2) +# define GPIO_TRACED0 (GPIO_TRACED0_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN3) +# define GPIO_TRACED1 (GPIO_TRACED1_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN4) +# define GPIO_TRACED2 (GPIO_TRACED2_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN5) +# define GPIO_TRACED3 (GPIO_TRACED3_2|GPIO_PULLUP|GPIO_SPEED_100MHz|GPIO_PUSHPULL) //(GPIO_ALT|GPIO_AF0|GPIO_PORTE|GPIO_PIN6) +//#define GPIO_TRACESWO //(GPIO_ALT|GPIO_AF0|GPIO_PORTB|GPIO_PIN3) + +# undef BOARD_HAS_CONTROL_STATUS_LEDS +# undef BOARD_OVERLOAD_LED +# undef BOARD_ARMED_STATE_LED + +# define GPIO_nLED_RED GPIO_TRACED0 +# define GPIO_nLED_GREEN GPIO_TRACED1 +# define GPIO_nLED_BLUE GPIO_TRACED2 +# define GPIO_nARMED GPIO_TRACED3 +# define GPIO_nARMED_INIT GPIO_TRACED3 +#endif + +/* I2C busses */ + +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +#define PX4_I2C_OBDEV_SE050 0x48 + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC1_CH(n) (n) + +/* N.B. there is no offset mapping needed for ADC3 because */ +#define ADC3_CH(n) (n) + +/* We are only use ADC3 for REV/VER. + * ADC3_6V6 and ADC3_3V3 are mapped back to ADC1 + * To do this We are relying on PC2_C, PC3_C being connected to PC2, PC3 + * respectively by the SYSCFG_PMCR default of setting for PC3SO PC2SO PA1SO + * PA0SO of 0. + * + * 0 Analog switch closed (pads are connected through the analog switch) + * + * So ADC3_INP0 is GPIO_ADC123_INP12 + * ADC3_INP1 is GPIO_ADC12_INP13 + */ + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ GPIO_ADC1_INP16, \ + /* PA4 */ GPIO_ADC12_INP18, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PB1 */ GPIO_ADC12_INP5, \ + /* PC2 */ GPIO_ADC123_INP12, \ + /* PC3 */ GPIO_ADC12_INP13, \ + /* PF12 */ GPIO_ADC1_INP6, \ + /* PH3 */ GPIO_ADC3_INP14, \ + /* PH4 */ GPIO_ADC3_INP15 + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL /* PA0 */ ADC1_CH(16) +#define ADC_SCALED_V5_CHANNEL /* PB1 */ ADC1_CH(5) +#define ADC_HW_VER_SENSE_CHANNEL /* PH3 */ ADC3_CH(14) +#define ADC_HW_REV_SENSE_CHANNEL /* PH4 */ ADC3_CH(15) + +#define ADC_CHANNELS \ + ((1 << ADC_SCALED_VDD_3V3_SENSORS1_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL)) + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ + +#define HW_REV_VER_ADC_BASE STM32_ADC3_BASE + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +/* HW has to large of R termination on ADC todo:change when HW value is chosen */ +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + +/* HW Version and Revision drive signals Default to 1 to detect */ +#define BOARD_HAS_HW_SPLIT_VERSIONING + +#define GPIO_HW_VER_REV_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) +#define GPIO_HW_REV_SENSE /* PH4 */ GPIO_ADC3_INP15 +#define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 +#define HW_INFO_INIT_PREFIX "ARKPI6X" + +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 +// Base/FMUM +#define ARKPI6X_0 HW_FMUM_ID(0x0) // ARKPI6X, Sensor Set Rev 0 +#define ARKPI6X_1 HW_FMUM_ID(0x1) // ARKPI6X, Sensor Set Rev 1 + +#define UAVCAN_NUM_IFACES_RUNTIME 1 + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PB10 T2CH3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PE6 is nARMED + * The GPIO will be set as input while not armed HW will have external HW Pull UP. + * While armed it shall be configured at a GPIO OUT set LOW + */ +#if !defined(TRACE_PINS) +#define GPIO_nARMED_INIT /* PE6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTE|GPIO_PIN6) +#define GPIO_nARMED /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN6) +#define BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE(enabled) px4_arch_configgpio((enabled) ? GPIO_nARMED : GPIO_nARMED_INIT) +#define BOARD_GET_EXTERNAL_LOCKOUT_STATE() px4_arch_gpioread(GPIO_nARMED) +#endif + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define GPIO_FMU_CH1 /* PI0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTI|GPIO_PIN0) +#define GPIO_FMU_CH2 /* PH12 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN12) +#define GPIO_FMU_CH3 /* PH11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN11) +#define GPIO_FMU_CH4 /* PH10 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN10) +#define GPIO_FMU_CH5 /* PD13 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN13) +#define GPIO_FMU_CH6 /* PD14 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTD|GPIO_PIN14) +#define GPIO_FMU_CH7 /* PH6 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN6) +#define GPIO_FMU_CH8 /* PH9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTH|GPIO_PIN9) + +#define GPIO_FMU_CAP /* PE11 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN11) +#define GPIO_SPIX_SYNC /* PE9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN9) + +#define BROADCOM_AFBR_S50_S2PI_SPI_BUS 6 +#define BROADCOM_AFBR_S50_S2PI_CS /* PI10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTI|GPIO_PIN10) +#define BROADCOM_AFBR_S50_S2PI_IRQ /* PD11 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN11|GPIO_EXTI) +#define BROADCOM_AFBR_S50_S2PI_CLK /* PB3 */ GPIO_SPI6_SCK_3 +#define BROADCOM_AFBR_S50_S2PI_MOSI /* PG14 */ GPIO_SPI6_MOSI_1 +#define BROADCOM_AFBR_S50_S2PI_MISO /* PA6 */ GPIO_SPI6_MISO_2 + +/* Power supply control and monitoring GPIOs */ + +#define BOARD_NUMBER_BRICKS 0 +#define BOARD_NUMBER_DIGITAL_BRICKS 1 + +#define GPIO_VDD_5V_HIPOWER_nEN /* PG10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN10) +#define GPIO_VDD_5V_HIPOWER_nOC /* PF13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +/* Spare GPIO */ + +#define GPIO_PD15 /* PD15 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15) + +/* NFC GPIO */ + +#define GPIO_NFC_GPIO /* PC0 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTC|GPIO_PIN0) + + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +#define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 14 /* Timer 14 */ +#define TONE_ALARM_CHANNEL 1 /* PF9 GPIO_TIM14_CH1OUT_2 */ + +#define GPIO_BUZZER_1 /* PF9 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN9) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM14_CH1OUT_2 + +/* USB OTG FS + * + * PA9 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PA9 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN9) + +#define FLASH_BASED_PARAMS + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* RC Serial port */ + +#define RC_SERIAL_PORT "/dev/ttyS4" + +/* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */ +#define PWMIN_TIMER 4 +#define PWMIN_TIMER_CHANNEL /* T4C2 */ 2 +#define GPIO_PWM_IN /* PD13 */ GPIO_TIM4_CH2IN_2 + +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ + +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* By Providing BOARD_ADC_USB_CONNECTED (using the px4_arch abstraction) + * this board support the ADC system_power interface, and therefore + * provides the true logic GPIO BOARD_ADC_xxxx macros. + */ +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* ARKPI6X never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_HIPOWER_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_HIPOWER_nOC)) + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ + +#define BOARD_HAS_ON_RESET 1 + +#if defined(TRACE_PINS) +#define GPIO_TRACE \ + GPIO_TRACECLK1, \ + GPIO_TRACED0, \ + GPIO_TRACED1, \ + GPIO_TRACED2, \ + GPIO_TRACED3 +#else +#define GPIO_TRACE (GPIO_OUTPUT|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#endif + +#define PX4_GPIO_INIT_LIST { \ + GPIO_TRACE, \ + PX4_ADC_GPIO, \ + GPIO_HW_VER_REV_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_HEATER_OUTPUT, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_PD15, \ + GPIO_NFC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_nARMED_INIT, \ + GPIO_FMU_CH1, \ + GPIO_FMU_CH2, \ + GPIO_FMU_CH3, \ + GPIO_FMU_CH4, \ + GPIO_FMU_CH5, \ + GPIO_FMU_CH6, \ + GPIO_FMU_CH7, \ + GPIO_FMU_CH8, \ + GPIO_FMU_CAP, \ + GPIO_SPIX_SYNC \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 3 +#define BOARD_SPIX_SYNC_FREQ 32000 + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/ark/pi6x/src/bootloader_main.c b/boards/ark/pi6x/src/bootloader_main.c new file mode 100644 index 000000000000..7a3ef5e01932 --- /dev/null +++ b/boards/ark/pi6x/src/bootloader_main.c @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure pins */ + const uint32_t list[] = PX4_GPIO_INIT_LIST; + + for (size_t gpio = 0; gpio < arraySize(list); gpio++) { + if (list[gpio] != 0) { + px4_arch_configgpio(list[gpio]); + } + } + + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/ark/pi6x/src/can.c b/boards/ark/pi6x/src/can.c new file mode 100644 index 000000000000..cdebe7a3ad61 --- /dev/null +++ b/boards/ark/pi6x/src/can.c @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 can.c + * + * Board-specific CAN functions. + */ + +#if !defined(CONFIG_CAN) + +#include + +#include "board_config.h" + + +__EXPORT +uint16_t board_get_can_interfaces(void) +{ + uint16_t enabled_interfaces = 0x3; + + if (!PX4_MFT_HW_SUPPORTED(PX4_MFT_CAN2)) { + enabled_interfaces &= ~(1 << 1); + } + + return enabled_interfaces; +} + +#else + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2) +# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1." +# undef CONFIG_STM32_CAN2 +#endif + +#ifdef CONFIG_STM32_CAN1 +# define CAN_PORT 1 +#else +# define CAN_PORT 2 +#endif + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ +int can_devinit(void); + +/************************************************************************************ + * Name: can_devinit + * + * Description: + * All STM32 architectures must provide the following interface to work with + * examples/can. + * + ************************************************************************************/ + +int can_devinit(void) +{ + static bool initialized = false; + struct can_dev_s *can; + int ret; + + /* Check if we have already initialized */ + + if (!initialized) { + /* Call stm32_caninitialize() to get an instance of the CAN interface */ + + can = stm32_caninitialize(CAN_PORT); + + if (can == NULL) { + canerr("ERROR: Failed to get CAN interface\n"); + return -ENODEV; + } + + /* Register the CAN driver at "/dev/can0" */ + + ret = can_register("/dev/can0", can); + + if (ret < 0) { + canerr("ERROR: can_register failed: %d\n", ret); + return ret; + } + + /* Now we are initialized */ + + initialized = true; + } + + return OK; +} +#endif /* CONFIG_CAN */ diff --git a/boards/ark/pi6x/src/hw_config.h b/boards/ark/pi6x/src/hw_config.h new file mode 100644 index 000000000000..52c70ec748da --- /dev/null +++ b/boards/ark/pi6x/src/hw_config.h @@ -0,0 +1,129 @@ +/* + * 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 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS5,921600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 58 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#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) + * +*/ + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#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 + +#endif /* HW_CONFIG_H_ */ diff --git a/boards/ark/pi6x/src/i2c.cpp b/boards/ark/pi6x/src/i2c.cpp new file mode 100644 index 000000000000..86c060404934 --- /dev/null +++ b/boards/ark/pi6x/src/i2c.cpp @@ -0,0 +1,40 @@ +/**************************************************************************** + * + * Copyright (C) 2022 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 + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(3), + initI2CBusInternal(4), +}; diff --git a/boards/ark/pi6x/src/init.c b/boards/ark/pi6x/src/init.c new file mode 100644 index 000000000000..90bb4fc128be --- /dev/null +++ b/boards/ark/pi6x/src/init.c @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 init.c + * + * ARKFMU-specific early startup code. This file implements the + * board_app_initialize() 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 initialization. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" +#include "spix_sync.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +/**************************************************************************** + * 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) +{ + /* set the peripheral rails off */ + + VDD_5V_HIPOWER_EN(false); + board_control_spi_sensors_power(false, 0xffff); + + /* wait for the peripheral rail to reach GND */ + usleep(ms * 1000); + syslog(LOG_DEBUG, "reset done, %d ms\n", ms); + + /* re-enable power */ + + /* switch the peripheral rail back on */ + board_control_spi_sensors_power(true, 0xffff); + VDD_5V_HIPOWER_EN(true); +} + +/************************************************************************************ + * 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) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. 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 +stm32_boardinitialize(void) +{ + board_on_reset(-1); /* Reset PWM first thing */ + + /* configure LEDs */ + + board_autoled_initialize(); + + /* configure pins */ + + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure USB interfaces */ + + stm32_usbinitialize(); + +} + +/**************************************************************************** + * 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) +{ + /* Power on Interfaces */ + VDD_5V_HIPOWER_EN(true); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + // Use the default HW_VER_REV(0x0,0x0) for Ramtron + + stm32_spiinitialize(); + +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_AMBER); + } + +#endif // FLASH_BASED_PARAMS + + /* Configure the HW based on the manifest */ + + px4_platform_configure(); + + 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 the Actual SPI interfaces (after we determined the HW version) */ + + stm32_spiinitialize(); + + board_spi_reset(10, 0xffff); + + /* Configure the DMA allocator */ + + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + +#if defined(SERIAL_HAVE_RXDMA) + // set up the serial DMA polling at 1ms intervals for received bytes that have not triggered a DMA event. + static struct hrt_call serial_dma_call; + hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_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); + } + + // Ensure Power is off for > 10 mS + usleep(15 * 1000); + VDD_3V3_SD_CARD_EN(true); + usleep(500 * 1000); + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + +#endif /* CONFIG_MMCSD */ + + /* Configure the SPIX_SYNC output */ + spix_sync_servo_init(BOARD_SPIX_SYNC_FREQ); + spix_sync_servo_set(0, 150); + + return OK; +} diff --git a/boards/ark/pi6x/src/led.c b/boards/ark/pi6x/src/led.c new file mode 100644 index 000000000000..b629ade32c36 --- /dev/null +++ b/boards/ark/pi6x/src/led.c @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 led.c + * + * ARKFMU LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +#ifdef CONFIG_ARCH_LEDS +static bool nuttx_owns_leds = true; +// B R S G +// 0 1 2 3 +static const uint8_t xlatpx4[] = {1, 2, 4, 0}; +# define xlat(p) xlatpx4[(p)] +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +#else + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_BLUE, // Indexed by LED_BLUE + GPIO_nLED_RED, // Indexed by LED_RED, LED_AMBER + 0, // Indexed by LED_SAFETY (defaulted to an input) + GPIO_nLED_GREEN, // Indexed by LED_GREEN +}; + +#endif + +__EXPORT void led_init(void) +{ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} + +#ifdef CONFIG_ARCH_LEDS +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: board_autoled_initialize + ****************************************************************************/ + +void board_autoled_initialize(void) +{ + led_init(); +} + +/**************************************************************************** + * Name: board_autoled_on + ****************************************************************************/ + +void board_autoled_on(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_HEAPALLOCATE: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_IRQSENABLED: + phy_set_led(BOARD_LED_BLUE, false); + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_STACKCREATED: + phy_set_led(BOARD_LED_GREEN, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, true); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, true); + phy_set_led(BOARD_LED_BLUE, true); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, true); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, true); + break; + } +} + +/**************************************************************************** + * Name: board_autoled_off + ****************************************************************************/ + +void board_autoled_off(int led) +{ + if (!nuttx_owns_leds) { + return; + } + + switch (led) { + default: + break; + + case LED_SIGNAL: + phy_set_led(BOARD_LED_GREEN, false); + break; + + case LED_INIRQ: + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_ASSERTION: + phy_set_led(BOARD_LED_RED, false); + phy_set_led(BOARD_LED_BLUE, false); + break; + + case LED_PANIC: + phy_set_led(BOARD_LED_RED, false); + break; + + case LED_IDLE : /* IDLE */ + phy_set_led(BOARD_LED_RED, false); + break; + } +} + +#endif /* CONFIG_ARCH_LEDS */ diff --git a/boards/ark/pi6x/src/mtd.cpp b/boards/ark/pi6x/src/mtd.cpp new file mode 100644 index 000000000000..bd74d551ee4c --- /dev/null +++ b/boards/ark/pi6x/src/mtd.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 + +static const px4_mft_entry_s mft_mft = { + .type = MFT, + .pmft = (void *) system_query_manifest, +}; + +static const px4_mft_s mft = { + .nmft = 1, + .mfts = { + &mft_mft, + } +}; + +const px4_mft_s *board_get_manifest(void) +{ + return &mft; +} diff --git a/boards/ark/pi6x/src/sdio.c b/boards/ark/pi6x/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/ark/pi6x/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] 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, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/ark/pi6x/src/spi.cpp b/boards/ark/pi6x/src/spi.cpp new file mode 100644 index 000000000000..bae9af7622a3 --- /dev/null +++ b/boards/ark/pi6x/src/spi.cpp @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 + +constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = { + initSPIFmumID(ARKPI6X_0, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_FLOW_DEVTYPE_PAW3902, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + // initSPIBus(SPI::Bus::SPI5, { + // initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + // }), + initSPIBus(SPI::Bus::SPI6, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + }), + }), + initSPIFmumID(ARKPI6X_1, { // Placeholder + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_FLOW_DEVTYPE_PAW3902, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }), + // initSPIBus(SPI::Bus::SPI4, { + // // no devices + // TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h + // }, {GPIO::PortG, GPIO::Pin8}), + // initSPIBus(SPI::Bus::SPI5, { + // initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7}) + // }), + initSPIBus(SPI::Bus::SPI6, { + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}) + }), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/ark/pi6x/src/spix_sync.c b/boards/ark/pi6x/src/spix_sync.c new file mode 100644 index 000000000000..056e38e75f74 --- /dev/null +++ b/boards/ark/pi6x/src/spix_sync.c @@ -0,0 +1,309 @@ +/**************************************************************************** + * + * 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 Airmind 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 spix_sync.c +* +* +*/ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include + +#include "spix_sync.h" + +#define REG(_tmr, _reg) (*(volatile uint32_t *)(spix_sync_timers[_tmr].base + _reg)) + +#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) +#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) +#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) +#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) +#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) +#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) +#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) +#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) +#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) +#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) +#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) +#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) +#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) +#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) +#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) +#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) +#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) +#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) +#define rBDTR(_tmr) REG(_tmr, STM32_ATIM_BDTR_OFFSET) + +#define BOARD_SPIX_SYNC_PWM_FREQ 1024000 + +unsigned +spix_sync_timer_get_period(unsigned timer) +{ + return (rARR(timer)); +} + +static void spix_sync_timer_init_timer(unsigned timer, unsigned rate) +{ + if (spix_sync_timers[timer].base) { + + irqstate_t flags = px4_enter_critical_section(); + + /* enable the timer clock before we try to talk to it */ + + modifyreg32(spix_sync_timers[timer].clock_register, 0, spix_sync_timers[timer].clock_bit); + + /* disable and configure the timer */ + rCR1(timer) = 0; + rCR2(timer) = 0; + rSMCR(timer) = 0; + rDIER(timer) = 0; + rCCER(timer) = 0; + rCCMR1(timer) = 0; + rCCMR2(timer) = 0; + rCCR1(timer) = 0; + rCCR2(timer) = 0; + rCCR3(timer) = 0; + rCCR4(timer) = 0; + rCCER(timer) = 0; + rDCR(timer) = 0; + + if ((spix_sync_timers[timer].base == STM32_TIM1_BASE) || (spix_sync_timers[timer].base == STM32_TIM8_BASE)) { + + /* master output enable = on */ + + rBDTR(timer) = ATIM_BDTR_MOE; + } + + /* If the timer clock source provided as clock_freq is the STM32_APBx_TIMx_CLKIN + * then configure the timer to free-run at 1MHz. + * Otherwise, other frequencies are attainable by adjusting .clock_freq accordingly. + */ + + rPSC(timer) = (spix_sync_timers[timer].clock_freq / BOARD_SPIX_SYNC_PWM_FREQ) - 1; + + /* configure the timer to update at the desired rate */ + + rARR(timer) = (BOARD_SPIX_SYNC_PWM_FREQ / rate) - 1; + + /* generate an update event; reloads the counter and all registers */ + rEGR(timer) = GTIM_EGR_UG; + + px4_leave_critical_section(flags); + } + +} + +void spix_sync_channel_init(unsigned channel) +{ + /* Only initialize used channels */ + + if (spix_sync_channels[channel].timer_channel) { + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* configure the GPIO first */ + px4_arch_configgpio(spix_sync_channels[channel].gpio_out); + + uint16_t polarity = spix_sync_channels[channel].masks; + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC1M_SHIFT) | GTIM_CCMR1_OC1PE; + rCCER(timer) |= polarity | GTIM_CCER_CC1E; + break; + + case 2: + rCCMR1(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR1_OC2M_SHIFT) | GTIM_CCMR1_OC2PE; + rCCER(timer) |= polarity | GTIM_CCER_CC2E; + break; + + case 3: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC3M_SHIFT) | GTIM_CCMR2_OC3PE; + rCCER(timer) |= polarity | GTIM_CCER_CC3E; + break; + + case 4: + rCCMR2(timer) |= (GTIM_CCMR_MODE_PWM1 << GTIM_CCMR2_OC4M_SHIFT) | GTIM_CCMR2_OC4PE; + rCCER(timer) |= polarity | GTIM_CCER_CC4E; + break; + } + } +} + +int +spix_sync_servo_set(unsigned channel, uint8_t cvalue) +{ + if (channel >= arraySize(spix_sync_channels)) { + return -1; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].gpio_out == 0)) { + return -1; + } + + unsigned period = spix_sync_timer_get_period(timer); + + unsigned value = (unsigned)cvalue * period / 255; + + /* configure the channel */ + if (value > 0) { + value--; + } + + + switch (spix_sync_channels[channel].timer_channel) { + case 1: + rCCR1(timer) = value; + break; + + case 2: + rCCR2(timer) = value; + break; + + case 3: + rCCR3(timer) = value; + break; + + case 4: + rCCR4(timer) = value; + break; + + default: + return -1; + } + + return 0; +} + +unsigned spix_sync_servo_get(unsigned channel) +{ + if (channel >= 3) { + return 0; + } + + unsigned timer = spix_sync_channels[channel].timer_index; + uint16_t value = 0; + + /* test timer for validity */ + if ((spix_sync_timers[timer].base == 0) || + (spix_sync_channels[channel].timer_channel == 0)) { + return 0; + } + + /* configure the channel */ + switch (spix_sync_channels[channel].timer_channel) { + case 1: + value = rCCR1(timer); + break; + + case 2: + value = rCCR2(timer); + break; + + case 3: + value = rCCR3(timer); + break; + + case 4: + value = rCCR4(timer); + break; + } + + unsigned period = spix_sync_timer_get_period(timer); + return ((value + 1) * 255 / period); +} + +int spix_sync_servo_init(unsigned rate) +{ + /* do basic timer initialisation first */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + spix_sync_timer_init_timer(i, rate); + } + + /* now init channels */ + for (unsigned i = 0; i < arraySize(spix_sync_channels); i++) { + spix_sync_channel_init(i); + } + + spix_sync_servo_arm(true); + return OK; +} + +void +spix_sync_servo_deinit(void) +{ + /* disable the timers */ + spix_sync_servo_arm(false); +} +void +spix_sync_servo_arm(bool armed) +{ + /* iterate timers and arm/disarm appropriately */ + for (unsigned i = 0; i < arraySize(spix_sync_timers); i++) { + if (spix_sync_timers[i].base != 0) { + if (armed) { + /* force an update to preload all registers */ + rEGR(i) = GTIM_EGR_UG; + + /* arm requires the timer be enabled */ + rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; + + } else { + rCR1(i) = 0; + } + } + } +} diff --git a/boards/ark/pi6x/src/spix_sync.h b/boards/ark/pi6x/src/spix_sync.h new file mode 100644 index 000000000000..2e37c8908613 --- /dev/null +++ b/boards/ark/pi6x/src/spix_sync.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +__BEGIN_DECLS +void spix_sync_channel_init(unsigned channel); +int spix_sync_servo_set(unsigned channel, uint8_t value); +unsigned spix_sync_servo_get(unsigned channel); +int spix_sync_servo_init(unsigned rate); +void spix_sync_servo_deinit(void); +void spix_sync_servo_arm(bool armed); +unsigned spix_sync_timer_get_period(unsigned timer); +__END_DECLS diff --git a/boards/ark/pi6x/src/timer_config.cpp b/boards/ark/pi6x/src/timer_config.cpp new file mode 100644 index 000000000000..bee77b999337 --- /dev/null +++ b/boards/ark/pi6x/src/timer_config.cpp @@ -0,0 +1,88 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 + +/* Timer allocation + * + * TIM5_CH4 T FMU_CH1 + * TIM5_CH3 T FMU_CH2 + * TIM5_CH2 T FMU_CH3 + * TIM5_CH1 T FMU_CH4 + * + * TIM4_CH2 T FMU_CH5 + * TIM4_CH3 T FMU_CH6 + * + * TIM12_CH1 T FMU_CH7 + * TIM12_CH2 T FMU_CH8 + * + * TIM1_CH3 T SPI2_DRDY2_ISM330_INT2 < Capture or GPIO INT + * TIM1_CH1 T SPIX_SYNC > Pulse or GPIO strobe + * + * TIM2_CH3 T HEATER > PWM OUT or GPIO + * + * TIM14_CH1 T BUZZER_1 - Driven by other driver + */ + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer12), + //initIOTimer(Timer::Timer1), + //initIOTimer(Timer::Timer2), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortI, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortH, GPIO::Pin12}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortH, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortH, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel1}, {GPIO::PortH, GPIO::Pin6}), + initIOTimerChannel(io_timers, {Timer::Timer12, Timer::Channel2}, {GPIO::PortH, GPIO::Pin9}), + //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + //initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); + + +constexpr io_timers_t spix_sync_timers[MAX_SPIX_SYNC_TIMERS] = { + initIOTimer(Timer::Timer1), +}; + +constexpr timer_io_channels_t spix_sync_channels[MAX_SPIX_SYNC_TIMERS] = { + initIOTimerChannel(spix_sync_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), +}; diff --git a/boards/ark/pi6x/src/usb.c b/boards/ark/pi6x/src/usb.c new file mode 100644 index 000000000000..1c64e94ba104 --- /dev/null +++ b/boards/ark/pi6x/src/usb.c @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * 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 usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include "arm_internal.h" +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/************************************************************************************ + * Private Functions + ************************************************************************************/ + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the ARKFMU board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ + +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/ark/septentrio-gps/default.px4board b/boards/ark/septentrio-gps/default.px4board index 1480a0b4da05..7600200577e1 100644 --- a/boards/ark/septentrio-gps/default.px4board +++ b/boards/ark/septentrio-gps/default.px4board @@ -1,13 +1,13 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" -CONFIG_BOARD_NO_HELP=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y +CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_BOARD_UAVCAN_INTERFACES=1 @@ -20,6 +20,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y CONFIG_UAVCANNODE_SAFETY_BUTTON=y CONFIG_UAVCANNODE_STATIC_PRESSURE=y CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y +CONFIG_MODULES_EKF2=y CONFIG_MODULES_GYRO_CALIBRATION=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_SENSORS=y diff --git a/boards/ark/septentrio-gps/init/rc.board_defaults b/boards/ark/septentrio-gps/init/rc.board_defaults index c50d81602629..0db1234ce96d 100644 --- a/boards/ark/septentrio-gps/init/rc.board_defaults +++ b/boards/ark/septentrio-gps/init/rc.board_defaults @@ -11,3 +11,5 @@ param set-default SENS_IMU_CLPNOTI 0 safety_button start tone_alarm start + +ekf2 start diff --git a/boards/ark/septentrio-gps/init/rc.board_sensors b/boards/ark/septentrio-gps/init/rc.board_sensors index 43e4e6c506cc..a41dd7b070ec 100644 --- a/boards/ark/septentrio-gps/init/rc.board_sensors +++ b/boards/ark/septentrio-gps/init/rc.board_sensors @@ -8,4 +8,7 @@ icm42688p -R 0 -s start bmp388 -I -b 1 start -bmm150 -I -b 1 start +if ! iis2mdc -R 4 -I -b 1 start +then + bmm150 -I -b 1 start +fi diff --git a/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig b/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig index 6b3c2c951842..0208af2cac25 100644 --- a/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig +++ b/boards/ark/septentrio-gps/nuttx-config/canbootloader/defconfig @@ -50,6 +50,7 @@ CONFIG_STACK_COLORATION=y CONFIG_START_DAY=30 CONFIG_START_MONTH=11 CONFIG_STDIO_DISABLE_BUFFERING=y +CONFIG_STM32_FLASH_CONFIG_G=y CONFIG_STM32_NOEXT_VECTORS=y CONFIG_STM32_TIM8=y CONFIG_TASK_NAME_SIZE=0 diff --git a/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig b/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig index 81007d30a44f..176ae3eccc5b 100644 --- a/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig +++ b/boards/ark/septentrio-gps/nuttx-config/nsh/defconfig @@ -123,6 +123,7 @@ CONFIG_STM32_ADC1=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y CONFIG_STM32_DMA1=y CONFIG_STM32_DMA2=y +CONFIG_STM32_FLASH_CONFIG_G=y CONFIG_STM32_FLASH_PREFETCH=y CONFIG_STM32_FLOWCONTROL_BROKEN=y CONFIG_STM32_I2C1=y diff --git a/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld b/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld index cbf9fddc32dc..48a59fe92d0e 100644 --- a/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld +++ b/boards/ark/septentrio-gps/nuttx-config/scripts/canbootloader_script.ld @@ -33,7 +33,7 @@ * ****************************************************************************/ -/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and * 256Kb of SRAM. SRAM is split up into three blocks: * * 1) 112Kb of SRAM beginning at address 0x2000:0000 diff --git a/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld b/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld index d8573d2bcdde..2f4769b8f5b4 100644 --- a/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld +++ b/boards/ark/septentrio-gps/nuttx-config/scripts/script.ld @@ -33,7 +33,7 @@ * ****************************************************************************/ -/* The STM32F412 has 512Kb of FLASH beginning at address 0x0800:0000 and +/* The STM32F412 has 1M of FLASH beginning at address 0x0800:0000 and * 256Kb of SRAM. SRAM is split up into three blocks: * * 1) 112Kb of SRAM beginning at address 0x2000:0000 diff --git a/boards/ark/septentrio-gps/src/board_config.h b/boards/ark/septentrio-gps/src/board_config.h index 2263956bae0f..2d5f582075f1 100644 --- a/boards/ark/septentrio-gps/src/board_config.h +++ b/boards/ark/septentrio-gps/src/board_config.h @@ -47,7 +47,7 @@ #define GPIO_BTN_SAFETY /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15) /* Safety LED */ -#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) +#define GPIO_LED_SAFETY /* PA1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1) /* Tone alarm output. */ #define TONE_ALARM_TIMER 2 /* timer 2 */ diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board index 58fcec76d7e8..fd76e4d6ca56 100644 --- a/boards/atl/mantis-edu/default.px4board +++ b/boards/atl/mantis-edu/default.px4board @@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_MAIERTEK_MPC2520=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y diff --git a/boards/atl/mantis-edu/upload.sh b/boards/atl/mantis-edu/upload.sh index 98e2a0686ae5..d1df5cc5fd22 100755 --- a/boards/atl/mantis-edu/upload.sh +++ b/boards/atl/mantis-edu/upload.sh @@ -8,4 +8,3 @@ echo "uploading: $PX4_BINARY_FILE" PX4_BINARY_FILE_SIZE=$(stat -c%s "$PX4_BINARY_FILE") curl -v -F "image=@$PX4_BINARY_FILE" -H "Expect:" -H "File-Size:$PX4_BINARY_FILE_SIZE" http://192.168.42.1/cgi-bin/upload - diff --git a/boards/av/x-v1/nuttx-config/include/board.h b/boards/av/x-v1/nuttx-config/include/board.h index a1cdc1635def..b9a18e9a3b88 100644 --- a/boards/av/x-v1/nuttx-config/include/board.h +++ b/boards/av/x-v1/nuttx-config/include/board.h @@ -374,4 +374,3 @@ * SDMMC1_D2 PC10 * SDMMC1_D3 PC11 */ - diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index d5e403904764..1a2237008684 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -139,8 +139,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 CONFIG_NETUTILS_TELNETD=y diff --git a/boards/beaglebone/blue/default.px4board b/boards/beaglebone/blue/default.px4board index 34459e580634..79e0d80b9276 100644 --- a/boards/beaglebone/blue/default.px4board +++ b/boards/beaglebone/blue/default.px4board @@ -14,7 +14,6 @@ CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y -CONFIG_DRIVERS_LINUX_PWM_OUT=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SMART_BATTERY_BATMON=y diff --git a/boards/beaglebone/blue/src/board_pwm_out.cpp b/boards/beaglebone/blue/src/board_pwm_out.cpp index 5b728ddfbcdc..d65bea2a76f7 100644 --- a/boards/beaglebone/blue/src/board_pwm_out.cpp +++ b/boards/beaglebone/blue/src/board_pwm_out.cpp @@ -81,4 +81,3 @@ int BBBlueRcPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs) return ret; } - diff --git a/boards/bitcraze/crazyflie/default.px4board b/boards/bitcraze/crazyflie/default.px4board index e7e9705f2dc1..9c90e55655c5 100644 --- a/boards/bitcraze/crazyflie/default.px4board +++ b/boards/bitcraze/crazyflie/default.px4board @@ -4,6 +4,7 @@ CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing" CONFIG_DRIVERS_BAROMETER_LPS25H=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y diff --git a/boards/bitcraze/crazyflie/src/usb.c b/boards/bitcraze/crazyflie/src/usb.c index 29bae3dcf9c5..fcd52fd07530 100644 --- a/boards/bitcraze/crazyflie/src/usb.c +++ b/boards/bitcraze/crazyflie/src/usb.c @@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index aa55b50621e8..0c268b6983a6 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -4,6 +4,7 @@ CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_COMPILE_DEFINITIONS="-Wno-narrowing" CONFIG_DRIVERS_BAROMETER_BMP388=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI088_I2C=y diff --git a/boards/bitcraze/crazyflie21/src/usb.c b/boards/bitcraze/crazyflie21/src/usb.c index 29bae3dcf9c5..fcd52fd07530 100644 --- a/boards/bitcraze/crazyflie21/src/usb.c +++ b/boards/bitcraze/crazyflie21/src/usb.c @@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/boards/cuav/nora/default.px4board b/boards/cuav/nora/default.px4board index f312504ee5e4..675ffa54cb3e 100644 --- a/boards/cuav/nora/default.px4board +++ b/boards/cuav/nora/default.px4board @@ -10,9 +10,11 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y @@ -37,7 +39,6 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y -CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y @@ -56,7 +57,6 @@ 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 @@ -68,7 +68,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y @@ -96,4 +95,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/cuav/nora/extras/cuav_nora_bootloader.bin b/boards/cuav/nora/extras/cuav_nora_bootloader.bin index 015f8e3c8523..789124a74935 100755 Binary files a/boards/cuav/nora/extras/cuav_nora_bootloader.bin and b/boards/cuav/nora/extras/cuav_nora_bootloader.bin differ diff --git a/boards/cuav/x7pro/default.px4board b/boards/cuav/x7pro/default.px4board index 99e831c12938..f72610fc65cf 100644 --- a/boards/cuav/x7pro/default.px4board +++ b/boards/cuav/x7pro/default.px4board @@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin b/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin index 29926616d41a..1093b162f810 100755 Binary files a/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin and b/boards/cuav/x7pro/extras/cuav_x7pro_bootloader.bin differ diff --git a/boards/cubepilot/cubeorange/console.px4board b/boards/cubepilot/cubeorange/console.px4board new file mode 100644 index 000000000000..89afc484a172 --- /dev/null +++ b/boards/cubepilot/cubeorange/console.px4board @@ -0,0 +1 @@ +# Same as default, only defconfig is different diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 91ddcda596bc..480f896d67cf 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -11,9 +11,11 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin index e8553b4e508e..a2b7b74c1cc1 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_cubeorange_bootloader.bin differ diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin index 4d29f3a04155..fee2b669933d 100755 Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeorange/init/rc.board_sensors b/boards/cubepilot/cubeorange/init/rc.board_sensors index e84edf7e2ff6..42ba25df2153 100644 --- a/boards/cubepilot/cubeorange/init/rc.board_sensors +++ b/boards/cubepilot/cubeorange/init/rc.board_sensors @@ -12,4 +12,3 @@ icm20948 -s -b 4 -R 10 -M start # SPI1 ms5611 -s -b 1 start icm20649 -s -b 1 start - diff --git a/boards/cubepilot/cubeorange/nuttx-config/console/defconfig b/boards/cubepilot/cubeorange/nuttx-config/console/defconfig new file mode 100644 index 000000000000..e8e0422f4386 --- /dev/null +++ b/boards/cubepilot/cubeorange/nuttx-config/console/defconfig @@ -0,0 +1,257 @@ +# +# 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_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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorange/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743ZI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=79954 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1016 +CONFIG_CDCACM_PRODUCTSTR="CubeOrange" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x2DAE +CONFIG_CDCACM_VENDORSTR="CubePilot" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +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_EXTERNAL=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_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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +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_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/include/board.h b/boards/cubepilot/cubeorange/nuttx-config/include/board.h index 5827b1bc81d1..62605f98d13a 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorange/nuttx-config/include/board.h @@ -194,6 +194,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/cubepilot/cubeorangeplus/console.px4board b/boards/cubepilot/cubeorangeplus/console.px4board new file mode 100644 index 000000000000..89afc484a172 --- /dev/null +++ b/boards/cubepilot/cubeorangeplus/console.px4board @@ -0,0 +1 @@ +# Same as default, only defconfig is different diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index 837876ee7d41..e3fc787417c6 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -11,9 +11,11 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y diff --git a/boards/cubepilot/cubeorangeplus/extras/cubepilot_cubeorangeplus_bootloader.bin b/boards/cubepilot/cubeorangeplus/extras/cubepilot_cubeorangeplus_bootloader.bin index fef54cd6d8a7..7715cf2d8433 100755 Binary files a/boards/cubepilot/cubeorangeplus/extras/cubepilot_cubeorangeplus_bootloader.bin and b/boards/cubepilot/cubeorangeplus/extras/cubepilot_cubeorangeplus_bootloader.bin differ diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors index 2b08f2976417..7782e73d6aea 100644 --- a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors +++ b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors @@ -37,4 +37,3 @@ else fi ms5611 -s -b 1 start - diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig new file mode 100644 index 000000000000..bb6a7cee41b7 --- /dev/null +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/console/defconfig @@ -0,0 +1,259 @@ +# +# 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_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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/cubepilot/cubeorangeplus/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H747XI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=79954 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x1058 +CONFIG_CDCACM_PRODUCTSTR="CubeOrange+" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x2DAE +CONFIG_CDCACM_VENDORSTR="CubePilot" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_DEV_FIFO_SIZE=0 +CONFIG_DEV_PIPE_MAXSIZE=1024 +CONFIG_DEV_PIPE_SIZE=70 +CONFIG_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAMTRON_EMULATE_PAGE_SHIFT=5 +CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT=5 +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_EXTERNAL=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_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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_PWR_DIRECT_SMPS_SUPPLY=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI4_DMA=y +CONFIG_STM32H7_SPI4_DMA_BUFFER=1024 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +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_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_IFLOWCONTROL=y +CONFIG_USART3_OFLOWCONTROL=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=3000 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h index d650c6169118..babaf587e450 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/include/board.h @@ -195,6 +195,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 /* ADC 1 2 3 clock source */ +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/cubepilot/cubeyellow/default.px4board b/boards/cubepilot/cubeyellow/default.px4board index 45cf93a38d95..62cf898af96c 100644 --- a/boards/cubepilot/cubeyellow/default.px4board +++ b/boards/cubepilot/cubeyellow/default.px4board @@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin index 4d29f3a04155..fee2b669933d 100755 Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ diff --git a/boards/cubepilot/cubeyellow/init/rc.board_sensors b/boards/cubepilot/cubeyellow/init/rc.board_sensors index e84edf7e2ff6..42ba25df2153 100644 --- a/boards/cubepilot/cubeyellow/init/rc.board_sensors +++ b/boards/cubepilot/cubeyellow/init/rc.board_sensors @@ -12,4 +12,3 @@ icm20948 -s -b 4 -R 10 -M start # SPI1 ms5611 -s -b 1 start icm20649 -s -b 1 start - diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 32ec4fd47b45..e8ee5ec656c8 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -8,6 +8,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/diatone/mamba-f405-mk2/init/rc.board_defaults b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults index 61c5953c4147..f5e37010631a 100644 --- a/boards/diatone/mamba-f405-mk2/init/rc.board_defaults +++ b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults @@ -9,9 +9,4 @@ param set-default CBRK_SUPPLY_CHK 894281 # Disable safety switch by default param set-default CBRK_IO_SAFETY 22027 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - param set-default SYS_HAS_MAG 0 diff --git a/boards/diatone/mamba-f405-mk2/src/board_config.h b/boards/diatone/mamba-f405-mk2/src/board_config.h index 7c2b17d84db4..3c5f0e9830b9 100644 --- a/boards/diatone/mamba-f405-mk2/src/board_config.h +++ b/boards/diatone/mamba-f405-mk2/src/board_config.h @@ -74,8 +74,7 @@ #define ADC_BATTERY_CURRENT_CHANNEL 13 #define ADC_RC_RSSI_CHANNEL 12 -/* Define Battery 1 Voltage Divider and A per V - */ +/* Define Battery Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (11.12f) #define BOARD_BATTERY1_A_PER_V (31.f) diff --git a/boards/emlid/navio2/default.px4board b/boards/emlid/navio2/default.px4board index bc912f507ab9..722f42b410d0 100644 --- a/boards/emlid/navio2/default.px4board +++ b/boards/emlid/navio2/default.px4board @@ -15,7 +15,6 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y CONFIG_DRIVERS_IMU_ST_LSM9DS1=y -CONFIG_DRIVERS_LINUX_PWM_OUT=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_LSM9DS1_MAG=y CONFIG_DRIVERS_RC_INPUT=y diff --git a/boards/emlid/navio2/init/rc.board_defaults b/boards/emlid/navio2/init/rc.board_defaults index 65348c4f846e..5d576dd74711 100644 --- a/boards/emlid/navio2/init/rc.board_defaults +++ b/boards/emlid/navio2/init/rc.board_defaults @@ -5,4 +5,3 @@ param set-default BAT1_V_DIV 10.177939394 param set-default BAT1_A_PER_V 15.391030303 - diff --git a/boards/emlid/navio2/src/board_pwm_out.cpp b/boards/emlid/navio2/src/board_pwm_out.cpp index bd6d7a6dc758..c93c6186655e 100644 --- a/boards/emlid/navio2/src/board_pwm_out.cpp +++ b/boards/emlid/navio2/src/board_pwm_out.cpp @@ -153,4 +153,3 @@ int NavioSysfsPWMOut::pwm_write_sysfs(char *path, int value) return 0; } - diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index b016b0db9a6a..54d17325bc92 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y diff --git a/boards/flywoo/gn-f405/init/rc.board_defaults b/boards/flywoo/gn-f405/init/rc.board_defaults index c1fc72c526f1..51fd2a6fbcfd 100644 --- a/boards/flywoo/gn-f405/init/rc.board_defaults +++ b/boards/flywoo/gn-f405/init/rc.board_defaults @@ -12,14 +12,9 @@ param set-default CBRK_SUPPLY_CHK 894281 # Disable safety switch by default param set-default CBRK_IO_SAFETY 22027 -# EKF2 can be enabled when baro is avaialble and EKF2_MAG_TYPE is set to 5 -param set-default SYS_MC_EST_GROUP 2 param set-default EKF2_MAG_TYPE 5 param set-default SENS_BOARD_ROT 6 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - param set-default SYS_HAS_MAG 0 # GPS is on Uart6 diff --git a/boards/flywoo/gn-f405/init/rc.board_extras b/boards/flywoo/gn-f405/init/rc.board_extras index 159a0008e9d3..911b4b4c61b0 100644 --- a/boards/flywoo/gn-f405/init/rc.board_extras +++ b/boards/flywoo/gn-f405/init/rc.board_extras @@ -7,5 +7,3 @@ if ! param compare OSD_ATXXXX_CFG 0 then atxxxx start -s fi - - diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h index 0dbed5a97966..73f22444d1b3 100644 --- a/boards/freefly/can-rtk-gps/nuttx-config/include/board.h +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board.h @@ -264,4 +264,3 @@ #define GPIO_SPI3_MISO GPIO_SPI3_MISO_2 /* PC11 */ #define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_3 /* PC12 */ #define GPIO_SPI3_SCK GPIO_SPI3_SCK_1 /* PB3 */ - diff --git a/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h index ca0d10189fe8..7e9aa79709b4 100644 --- a/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h +++ b/boards/freefly/can-rtk-gps/nuttx-config/include/board_dma_map.h @@ -99,4 +99,3 @@ // AVAILABLE // DMA1, Stream 5 // AVAILABLE // DMA1, Stream 6 // DMAMAP_USART1_TX USART1_TX // DMA1, Stream 7, Channel 5 - diff --git a/boards/hkust/nxt-dual/bootloader.px4board b/boards/hkust/nxt-dual/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/hkust/nxt-dual/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/hkust/nxt-dual/default.px4board b/boards/hkust/nxt-dual/default.px4board new file mode 100644 index 000000000000..cab44e781293 --- /dev/null +++ b/boards/hkust/nxt-dual/default.px4board @@ -0,0 +1,90 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS6" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS7" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=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_GYRO_FFT=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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=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_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/hkust/nxt-dual/extras/hkust_nxt-dual_bootloader.bin b/boards/hkust/nxt-dual/extras/hkust_nxt-dual_bootloader.bin new file mode 100755 index 000000000000..fc7f6a73fc44 Binary files /dev/null and b/boards/hkust/nxt-dual/extras/hkust_nxt-dual_bootloader.bin differ diff --git a/boards/hkust/nxt-dual/firmware.prototype b/boards/hkust/nxt-dual/firmware.prototype new file mode 100644 index 000000000000..e236ff1bcf5b --- /dev/null +++ b/boards/hkust/nxt-dual/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1013, + "magic": "PX4FWv1", + "description": "Firmware for the MatekH743-slim board", + "image": "", + "build_time": 0, + "summary": "MatekH743-mini", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/hkust/nxt-dual/init/rc.board_defaults b/boards/hkust/nxt-dual/init/rc.board_defaults new file mode 100644 index 000000000000..a4afdd873eeb --- /dev/null +++ b/boards/hkust/nxt-dual/init/rc.board_defaults @@ -0,0 +1,24 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default BAT1_A_PER_V 17 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_DIV 10.1 +param set-default BAT1_V_EMPTY 3.2 + +param set-default SYS_HAS_MAG 0 +param set-default PWM_MAIN_TIM0 -4 +param set-default RC_INPUT_PROTO -1 + +param set-default IMU_GYRO_RATEMAX 2000 +param set-default SYS_AUTOSTART 4001 +param set-default MC_PITCHRATE_K 0.4 +param set-default MC_ROLLRATE_K 0.35 +param set-default MC_YAWRATE_K 1.2 +param set-default MC_YAWRATE_MAX 360 +param set-default MAV_TYPE 2 +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 4 +param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/hkust/nxt-dual/init/rc.board_extras b/boards/hkust/nxt-dual/init/rc.board_extras new file mode 100644 index 000000000000..780b13dc96b3 --- /dev/null +++ b/boards/hkust/nxt-dual/init/rc.board_extras @@ -0,0 +1,13 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# NxtV1 does not have OSD +# if ! param compare OSD_ATXXXX_CFG 0 +# then +# atxxxx start -s +# fi + +# DShot telemetry is always on UART7 +# dshot telemetry /dev/ttyS5 diff --git a/boards/hkust/nxt-dual/init/rc.board_sensors b/boards/hkust/nxt-dual/init/rc.board_sensors new file mode 100644 index 000000000000..3091824e8b20 --- /dev/null +++ b/boards/hkust/nxt-dual/init/rc.board_sensors @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# # Internal SPI bus BMI088 accel/gyro +bmi088 -s -b 1 -A -R 2 start +bmi088 -s -b 1 -G -R 2 start + +bmi088 -s -b 4 -A -R 2 start +bmi088 -s -b 4 -G -R 2 start + +# internal baro +spl06 -X -a 0x77 start diff --git a/boards/hkust/nxt-dual/nuttx-config/Kconfig b/boards/hkust/nxt-dual/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/hkust/nxt-dual/nuttx-config/bootloader/defconfig b/boards/hkust/nxt-dual/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..ea894db106e4 --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/bootloader/defconfig @@ -0,0 +1,90 @@ +# +# 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_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/hkust/nxt-dual/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="HKUST UAV NxtPX4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=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_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART6=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/hkust/nxt-dual/nuttx-config/include/board.h b/boards/hkust/nxt-dual/nuttx-config/include/board.h new file mode 100644 index 000000000000..436b172b1ff9 --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/include/board.h @@ -0,0 +1,487 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6u/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MatekH743-Slim board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(60) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 60) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 clock source */ + +// #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The board has two, LED_GREEN a Green LED and LED_BLUE a Blue LED, + * that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PBA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ +#define GPIO_USART1_CK GPIO_USART1_CK /* PB8 NC */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_CK GPIO_USART2_CK_2 /* PD7 NC */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_CK GPIO_USART3_CK_3 /* PD10 NC */ + +#define GPIO_UART4_RX GPIO_UART4_RX_3 /* PB8 */ +#define GPIO_UART4_TX GPIO_UART4_TX_3 /* PB9 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_1 /* PB12 */ +#define GPIO_UART5_TX GPIO_UART5_TX_1 /* PB13 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 NC */ + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + +/* SPI + * + + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 /* PB14 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_3 /* PB2 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PB3 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE5 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_2) /* PE2 */ + +/* I2C + * + + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C4_SCL GPIO_I2C4_SCL_1 /* PD12 */ +#define GPIO_I2C4_SDA GPIO_I2C4_SDA_1 /* PD13 */ + +#define GPIO_I2C4_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN12) +#define GPIO_I2C4_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTD | GPIO_PIN13) + + +/* SDMMC1 + * + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + */ + +// #define GPIO_SDMMC1_D0 GPIO_SDMMC1_D0 /* PC8 */ +// #define GPIO_SDMMC1_D1 GPIO_SDMMC1_D1 /* PC9 */ +// #define GPIO_SDMMC1_D2 GPIO_SDMMC1_D2 /* PC10 */ +// #define GPIO_SDMMC1_D3 GPIO_SDMMC1_D3 /* PC11 */ +// #define GPIO_SDMMC1_CK GPIO_SDMMC1_CK /* PC12 */ +// #define GPIO_SDMMC1_CMD GPIO_SDMMC1_CMD /* PD2 */ + + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +// #if defined(CONFIG_BOARD_USE_PROBES) +// # include "stm32_gpio.h" +// # define PROBE_N(n) (1<<((n)-1)) +// # define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) /* PI0 AUX1 */ +// # define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN12) /* PH12 AUX2 */ +// # define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN11) /* PH11 AUX3 */ +// # define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN10) /* PH10 AUX4 */ +// # define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) /* PD13 AUX5 */ +// # define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX6 */ +// # define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) /* PH6 AUX7 */ +// # define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) /* PH9 AUX8 */ +// # define PROBE_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +// # define PROBE_INIT(mask) \ +// do { \ +// if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ +// if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ +// if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ +// if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ +// if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ +// if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ +// if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ +// if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ +// if ((mask)& PROBE_N(9)) { stm32_configgpio(PROBE_9); } \ +// } while(0) + +// # define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +// # define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +// #else +// # define PROBE_INIT(mask) +// # define PROBE(n,s) +// # define PROBE_MARK(n) +// #endif + +#endif /*__NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H */ diff --git a/boards/hkust/nxt-dual/nuttx-config/include/board_dma_map.h b/boards/hkust/nxt-dual/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..a27735b3541e --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/include/board_dma_map.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once +// #define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +// #define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ + +// DMAMUX2 +// #define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_0 /* DMA1:61 */ +// #define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_0 /* DMA1:62 */ + +// #define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* BDMA:11 */ +// #define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* BDMA:12 */ + +//TODO: UART DMA test +// #define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_1 /*DMA2:41*/ +// #define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_1 /*DMA2:42*/ + +// #define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2:43 */ +// #define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_1 /* DMA2:44 */ + +// #define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* DMA2:45 */ +// #define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* DMA2:46 */ + +#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_1 /* DMA1:63 */ +#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_1 /* DMA1:64 */ + +// #define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_0 /* DMA1:65 */ +// #define DMAMAP_UART5_TX DMAMAP_DMA12_UART5RX_0 /* DMA1:66 */ diff --git a/boards/hkust/nxt-dual/nuttx-config/nsh/defconfig b/boards/hkust/nxt-dual/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..e1e35f96def1 --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/nsh/defconfig @@ -0,0 +1,269 @@ +# +# 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_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_PRINTF 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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/hkust/nxt-dual/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="HKUST UAV NxtPX4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=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_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_MTD=y +CONFIG_MTD_BYTE_WRITE=y +CONFIG_MTD_PARTITION=y +CONFIG_MTD_PROGMEM=y +CONFIG_MTD_RAMTRON=y +CONFIG_MTD_W25=y +CONFIG_MTD_W25QXXXJV=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +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_EXTERNAL=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_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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC2=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI4=y +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM2=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=921600 +CONFIG_UART4_RXBUFSIZE=3000 +CONFIG_UART4_RXDMA=y +CONFIG_UART4_TXBUFSIZE=3000 +CONFIG_UART4_TXDMA=y +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_UART8_BAUD=57600 +CONFIG_UART8_RXBUFSIZE=600 +CONFIG_UART8_TXBUFSIZE=3000 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=3000 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=180 +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=180 +CONFIG_USART6_SERIAL_CONSOLE=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/hkust/nxt-dual/nuttx-config/nsh/old_defconfig.txt b/boards/hkust/nxt-dual/nuttx-config/nsh/old_defconfig.txt new file mode 100644 index 000000000000..cbe89638ad1b --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/nsh/old_defconfig.txt @@ -0,0 +1,271 @@ +# +# 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_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_DF is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_ITEF is not set +# CONFIG_NSH_DISABLE_LOOPS is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_SEMICOLON is not set +# CONFIG_NSH_DISABLE_TIME is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/hkust/nxt/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=512 +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_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95751 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_C99_BOOL8=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="NxtPX4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="Gumstix" +CONFIG_CLOCK_MONOTONIC=y +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=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_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +# CONFIG_MTD=y +# CONFIG_MTD_BYTE_WRITE=y +# CONFIG_MTD_PARTITION=y +# CONFIG_MTD_PROGMEM=y +# CONFIG_MTD_RAMTRON=y +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +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_ATEXIT=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKSTACKSIZE=1632 +CONFIG_SCHED_WAITPID=y +CONFIG_SDCLONE_DISABLE=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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +# CONFIG_STM32H7_I2C3=y +# CONFIG_STM32H7_I2C4=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SDMMC1_DMA=y +CONFIG_STM32H7_SDMMC1_DMA_BUFFER=1024 +# CONFIG_STM32H7_SDMMC2=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI3_DMA=y +CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 +# CONFIG_STM32H7_SPI5=y +# CONFIG_STM32H7_SPI6=y +# CONFIG_STM32H7_SPI6_DMA=y +# CONFIG_STM32H7_SPI6_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMA=y +# CONFIG_STM32H7_TIM12=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y + + +CONFIG_STM32H7_USART1=y #ttyS0 +CONFIG_STM32H7_USART2=y #ttyS1 +CONFIG_STM32H7_USART3=y #ttyS2 +CONFIG_STM32H7_UART4=y #ttyS3 +CONFIG_STM32H7_UART5=y #ttyS4 +CONFIG_STM32H7_UART7=y #ttyS5 + +# CONFIG_STM32H7_USART_BREAKS=y +# CONFIG_STM32H7_USART_INVERT=y +# CONFIG_STM32H7_USART_SINGLEWIRE=y +# CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 + + +# CONFIG_UART5_IFLOWCONTROL=y +# CONFIG_UART5_OFLOWCONTROL=y + +# 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_UART4_BAUD=921600 +CONFIG_UART4_RXBUFSIZE=3000 +CONFIG_UART4_TXBUFSIZE=1200 +# CONFIG_UART4_RXDMA=y +# CONFIG_UART4_TXDMA=y + +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_TXBUFSIZE=1500 + + +# CONFIG_USART6_BAUD=57600 +# CONFIG_USART6_RXBUFSIZE=600 +# CONFIG_USART6_TXBUFSIZE=1500 + +CONFIG_UART7_BAUD=57600 +# CONFIG_UART7_IFLOWCONTROL=y +# CONFIG_UART7_OFLOWCONTROL=y +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=3000 + + +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_USERMAIN_STACKSIZE=2944 +CONFIG_USER_ENTRYPOINT="nsh_main" +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/hkust/nxt-dual/nuttx-config/scripts/bootloader_script.ld b/boards/hkust/nxt-dual/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,213 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The Durandal-v1 uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* 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/hkust/nxt-dual/nuttx-config/scripts/script.ld b/boards/hkust/nxt-dual/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..1dc1a0ef97eb --- /dev/null +++ b/boards/hkust/nxt-dual/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* 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/hkust/nxt-dual/src/CMakeLists.txt b/boards/hkust/nxt-dual/src/CMakeLists.txt new file mode 100644 index 000000000000..798c5243474e --- /dev/null +++ b/boards/hkust/nxt-dual/src/CMakeLists.txt @@ -0,0 +1,69 @@ +############################################################################ +# +# 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + mtd.cpp + ) + # add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/hkust/nxt-dual/src/board_config.h b/boards/hkust/nxt-dual/src/board_config.h new file mode 100644 index 000000000000..2b8aba1296db --- /dev/null +++ b/boards/hkust/nxt-dual/src/board_config.h @@ -0,0 +1,230 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +#define FLASH_BASED_PARAMS + + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +# define GPIO_nLED_RED /* PD15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN15) +# define GPIO_nLED_GREEN /* PD11 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN11) +# define GPIO_nLED_BLUE /* PB15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + +/* I2C busses */ +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +// #define PX4_I2C_OBDEV_SE050 0x48 + +#define GPIO_SPL_ADDR_SET /* PB5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN5) + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +#define ADC12_CH(n) (n) + +#define PX4_ADC_GPIO \ + /* PC4 */ GPIO_ADC12_INP4, \ + /* PC5 */ GPIO_ADC12_INP8 + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL ADC12_CH(4) +#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(5) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + +#define BOARD_ADC_OPEN_CIRCUIT_V (1.6f) + + + +/* Define Battery 1 Voltage Divider and A per V + */ + +// #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ +// #define BOARD_BATTERY1_A_PER_V (40.0f) +// #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* Spare GPIO */ +#define GPIO_PA4 /* PA4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN4) +#define GPIO_PC0 /* PC0 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN0) +#define GPIO_PC1 /* PC1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN1) +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 4 /* Timer 4 */ +#define TONE_ALARM_CHANNEL 3 /* PD14 GPIO_TIM4_CH3 NC */ +/*NC can be modified with Spare GPIO then connected with hardware */ +#define GPIO_BUZZER_1 /* PA4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN4) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_BUZZER_1 + +/* USB OTG FS + * + * PD0 OTG_FS_VBUS VBUS sensing + */ +#define GPIO_OTGFS_VBUS /* PD0 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTD|GPIO_PIN0) +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS4" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +// #define GPIO_SBUS_INV (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +// #define RC_INVERT_INPUT(_invert_true) px4_arch_gpiowrite(GPIO_SBUS_INV, _invert_true); + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + GPIO_SPL_ADDR_SET, \ + GPIO_PC0, \ + GPIO_PC1, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 5 + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/hkust/nxt-dual/src/bootloader_main.c b/boards/hkust/nxt-dual/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/hkust/nxt-dual/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/hkust/nxt-dual/src/flash_w25q128.c b/boards/hkust/nxt-dual/src/flash_w25q128.c new file mode 100644 index 000000000000..3a3e2cee1fcc --- /dev/null +++ b/boards/hkust/nxt-dual/src/flash_w25q128.c @@ -0,0 +1,505 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file flash_w25q128.c + * + * Board-specific external flash W25Q128 functions. + */ + + +#include "board_config.h" +#include "qspi.h" +#include "arm_internal.h" +#include +#include + + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ +/* Configuration ********************************************************************/ + +#define N25Q128_SECTOR_SIZE (4*1024) +#define N25Q128_SECTOR_SHIFT (12) +#define N25Q128_SECTOR_COUNT (4096) +#define N25Q128_PAGE_SIZE (256) +#define N25Q128_PAGE_SHIFT (8) + +#define W25Q_DUMMY_CYCLES_FAST_READ_QUAD 6 +#define W25Q_INSTR_FAST_READ_QUAD 0xEB +#define W25Q_ADDRESS_SIZE 3 // 3 bytes -> 24 bits + +#define N25QXXX_READ_STATUS 0x05 /* Read status register: * + * 0x05 | SR */ +#define N25QXXX_PAGE_PROGRAM 0x02 /* Page Program: * + * 0x02 | ADDR(MS) | ADDR(MID) | * + * ADDR(LS) | data */ +#define N25QXXX_WRITE_ENABLE 0x06 /* Write enable: * + * 0x06 */ +#define N25QXXX_WRITE_DISABLE 0x04 /* Write disable command code: * + * 0x04 */ +#define N25QXXX_SUBSECTOR_ERASE 0x20 /* Sub-sector Erase (4 kB) * + * 0x20 | ADDR(MS) | ADDR(MID) | * + * ADDR(LS) */ + + +/* N25QXXX Registers ****************************************************************/ +/* Status register bit definitions */ + +#define STATUS_BUSY_MASK (1 << 0) /* Bit 0: Device ready/busy status */ +# define STATUS_READY (0 << 0) /* 0 = Not Busy */ +# define STATUS_BUSY (1 << 0) /* 1 = Busy */ +#define STATUS_WEL_MASK (1 << 1) /* Bit 1: Write enable latch status */ +# define STATUS_WEL_DISABLED (0 << 1) /* 0 = Not Write Enabled */ +# define STATUS_WEL_ENABLED (1 << 1) /* 1 = Write Enabled */ +#define STATUS_BP_SHIFT (2) /* Bits 2-4: Block protect bits */ +#define STATUS_BP_MASK (7 << STATUS_BP_SHIFT) +# define STATUS_BP_NONE (0 << STATUS_BP_SHIFT) +# define STATUS_BP_ALL (7 << STATUS_BP_SHIFT) +#define STATUS_TB_MASK (1 << 5) /* Bit 5: Top / Bottom Protect */ +# define STATUS_TB_TOP (0 << 5) /* 0 = BP2-BP0 protect Top down */ +# define STATUS_TB_BOTTOM (1 << 5) /* 1 = BP2-BP0 protect Bottom up */ +#define STATUS_BP3_MASK (1 << 5) /* Bit 6: BP3 */ +#define STATUS_SRP0_MASK (1 << 7) /* Bit 7: Status register protect 0 */ +# define STATUS_SRP0_UNLOCKED (0 << 7) /* 0 = WP# no effect / PS Lock Down */ +# define STATUS_SRP0_LOCKED (1 << 7) /* 1 = WP# protect / OTP Lock Down */ + +/************************************************************************************ + * Private Types + ************************************************************************************/ + +/* This type represents the state of the MTD device. The struct mtd_dev_s must + * appear at the beginning of the definition so that you can freely cast between + * pointers to struct mtd_dev_s and struct n25qxxx_dev_s. + */ + +struct n25qxxx_dev_s { + //struct mtd_dev_s mtd; /* MTD interface */ + FAR struct qspi_dev_s *qspi; /* Saved QuadSPI interface instance */ + uint16_t nsectors; /* Number of erase sectors */ + uint8_t sectorshift; /* Log2 of sector size */ + uint8_t pageshift; /* Log2 of page size */ + FAR uint8_t *cmdbuf; /* Allocated command buffer */ + FAR uint8_t *readbuf; /* Allocated status read buffer */ +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +struct qspi_dev_s *ptr_qspi_dev; +struct qspi_meminfo_s qspi_meminfo = { + .flags = QSPIMEM_QUADIO, + .addrlen = W25Q_ADDRESS_SIZE, + .dummies = W25Q_DUMMY_CYCLES_FAST_READ_QUAD, + .cmd = W25Q_INSTR_FAST_READ_QUAD +}; + +struct n25qxxx_dev_s n25qxxx_dev; +uint8_t cmdbuf[4] = {0u}; +uint8_t readbuf[1] = {0u}; + +/************************************************************************************ + * Private Functions + ************************************************************************************/ +__ramfunc__ int n25qxxx_command(FAR struct qspi_dev_s *qspi, uint8_t cmd); +__ramfunc__ uint8_t n25qxxx_read_status(FAR struct n25qxxx_dev_s *priv); +__ramfunc__ int n25qxxx_command_read(FAR struct qspi_dev_s *qspi, uint8_t cmd, + FAR void *buffer, size_t buflen); +__ramfunc__ void n25qxxx_write_enable(FAR struct n25qxxx_dev_s *priv); +__ramfunc__ void n25qxxx_write_disable(FAR struct n25qxxx_dev_s *priv); + +__ramfunc__ int n25qxxx_write_page(struct n25qxxx_dev_s *priv, FAR const uint8_t *buffer, + off_t address, size_t buflen); + +__ramfunc__ int n25qxxx_write_one_page(struct n25qxxx_dev_s *priv, struct qspi_meminfo_s *meminfo); + +__ramfunc__ int n25qxxx_erase_sector(struct n25qxxx_dev_s *priv, off_t sector); + +__ramfunc__ bool n25qxxx_isprotected(FAR struct n25qxxx_dev_s *priv, uint8_t status, + off_t address); + +__ramfunc__ int n25qxxx_command_address(FAR struct qspi_dev_s *qspi, uint8_t cmd, + off_t addr, uint8_t addrlen); + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +void flash_w25q128_init(void) +{ + int qspi_interface_number = 0; + ptr_qspi_dev = stm32h7_qspi_initialize(qspi_interface_number); + n25qxxx_dev.qspi = ptr_qspi_dev; + n25qxxx_dev.cmdbuf = cmdbuf; + n25qxxx_dev.readbuf = readbuf; + n25qxxx_dev.sectorshift = N25Q128_SECTOR_SHIFT; + n25qxxx_dev.pageshift = N25Q128_PAGE_SHIFT; + n25qxxx_dev.nsectors = N25Q128_SECTOR_COUNT; +} + +__ramfunc__ ssize_t up_progmem_ext_getpage(size_t addr) +{ + ssize_t page_address = (addr - STM32_FMC_BANK4) / N25Q128_SECTOR_COUNT; + + return page_address; +} + +__ramfunc__ ssize_t up_progmem_ext_eraseblock(size_t block) +{ + ssize_t size = N25Q128_SECTOR_COUNT; + + irqstate_t irqstate = px4_enter_critical_section(); + stm32h7_qspi_exit_memorymapped(ptr_qspi_dev); + + n25qxxx_erase_sector(&n25qxxx_dev, block); + + stm32h7_qspi_enter_memorymapped(ptr_qspi_dev, &qspi_meminfo, 0); + px4_leave_critical_section(irqstate); + return size; +} + +__ramfunc__ ssize_t up_progmem_ext_write(size_t addr, FAR const void *buf, size_t count) +{ + ssize_t ret_val = 0; + + irqstate_t irqstate = px4_enter_critical_section(); + stm32h7_qspi_exit_memorymapped(ptr_qspi_dev); + + addr &= 0xFFFFFF; + n25qxxx_write_page(&n25qxxx_dev, buf, (off_t)addr, count); + + stm32h7_qspi_enter_memorymapped(ptr_qspi_dev, &qspi_meminfo, 0); + px4_leave_critical_section(irqstate); + + return ret_val; +} + +/************************************************************************************ + * Name: n25qxxx_command + ************************************************************************************/ + +__ramfunc__ int n25qxxx_command(FAR struct qspi_dev_s *qspi, uint8_t cmd) +{ + struct qspi_cmdinfo_s cmdinfo; + + finfo("CMD: %02" PRIx8 "\n", cmd); + + cmdinfo.flags = 0; + cmdinfo.addrlen = 0; + cmdinfo.cmd = cmd; + cmdinfo.buflen = 0; + cmdinfo.addr = 0; + cmdinfo.buffer = NULL; + + int rv; + rv = qspi_command(qspi, &cmdinfo); + return rv; +} + +/************************************************************************************ + * Name: n25qxxx_read_status + ************************************************************************************/ + +__ramfunc__ uint8_t n25qxxx_read_status(FAR struct n25qxxx_dev_s *priv) +{ + DEBUGVERIFY(n25qxxx_command_read(priv->qspi, N25QXXX_READ_STATUS, + (FAR void *)&priv->readbuf[0], 1)); + return priv->readbuf[0]; +} + +/************************************************************************************ + * Name: n25qxxx_command_read + ************************************************************************************/ + +__ramfunc__ int n25qxxx_command_read(FAR struct qspi_dev_s *qspi, uint8_t cmd, + FAR void *buffer, size_t buflen) +{ + struct qspi_cmdinfo_s cmdinfo; + + finfo("CMD: %02" PRIx8 " buflen: %zu\n", cmd, buflen); + + cmdinfo.flags = QSPICMD_READDATA; + cmdinfo.addrlen = 0; + cmdinfo.cmd = cmd; + cmdinfo.buflen = buflen; + cmdinfo.addr = 0; + cmdinfo.buffer = buffer; + + int rv; + rv = qspi_command(qspi, &cmdinfo); + return rv; +} + + +/************************************************************************************ + * Name: n25qxxx_write_enable + ************************************************************************************/ + +__ramfunc__ void n25qxxx_write_enable(FAR struct n25qxxx_dev_s *priv) +{ + uint8_t status; + + do { + n25qxxx_command(priv->qspi, N25QXXX_WRITE_ENABLE); + status = n25qxxx_read_status(priv); + } while ((status & STATUS_WEL_MASK) != STATUS_WEL_ENABLED); +} + +/************************************************************************************ + * Name: n25qxxx_write_disable + ************************************************************************************/ + +__ramfunc__ void n25qxxx_write_disable(FAR struct n25qxxx_dev_s *priv) +{ + uint8_t status; + + do { + n25qxxx_command(priv->qspi, N25QXXX_WRITE_DISABLE); + status = n25qxxx_read_status(priv); + } while ((status & STATUS_WEL_MASK) != STATUS_WEL_DISABLED); +} + +/************************************************************************************ + * Name: n25qxxx_write_page + ************************************************************************************/ + +__ramfunc__ int n25qxxx_write_page(struct n25qxxx_dev_s *priv, FAR const uint8_t *buffer, + off_t address, size_t buflen) +{ + struct qspi_meminfo_s meminfo; + unsigned int pagesize; + unsigned int npages; + unsigned int firstpagesize = 0; + int ret = OK; + unsigned int i; + + finfo("address: %08jx buflen: %zu\n", (intmax_t)address, buflen); + + pagesize = (1 << priv->pageshift); + + /* Set up non-varying parts of transfer description */ + + meminfo.flags = QSPIMEM_WRITE; + meminfo.cmd = N25QXXX_PAGE_PROGRAM; + meminfo.addrlen = 3; + meminfo.dummies = 0; + meminfo.buffer = (void *)buffer; + + if (0 != (address % pagesize)) { + firstpagesize = pagesize - (address % pagesize); + } + + if (buflen <= firstpagesize) { + meminfo.addr = address; + meminfo.buflen = buflen; + ret = n25qxxx_write_one_page(priv, &meminfo); + + } else { + + if (firstpagesize > 0) { + meminfo.addr = address; + meminfo.buflen = firstpagesize; + ret = n25qxxx_write_one_page(priv, &meminfo); + + buffer += firstpagesize; + address += firstpagesize; + buflen -= firstpagesize; + } + + npages = (buflen >> priv->pageshift); + + meminfo.buflen = pagesize; + + /* Then write each page */ + + for (i = 0; (i < npages) && (ret == OK); i++) { + /* Set up varying parts of the transfer description */ + + meminfo.addr = address; + meminfo.buffer = (void *)buffer; + + ret = n25qxxx_write_one_page(priv, &meminfo); + + /* Update for the next time through the loop */ + + buffer += pagesize; + address += pagesize; + buflen -= pagesize; + } + + if ((ret == OK) && (buflen > 0)) { + meminfo.addr = address; + meminfo.buffer = (void *)buffer; + meminfo.buflen = buflen; + + ret = n25qxxx_write_one_page(priv, &meminfo); + } + } + + return ret; +} + +__ramfunc__ int n25qxxx_write_one_page(struct n25qxxx_dev_s *priv, struct qspi_meminfo_s *meminfo) +{ + int ret; + + n25qxxx_write_enable(priv); + ret = qspi_memory(priv->qspi, meminfo); + n25qxxx_write_disable(priv); + + if (ret < 0) { + ferr("ERROR: QSPI_MEMORY failed writing address=%06" PRIx32 "\n", + meminfo->addr); + } + + return ret; +} + +/************************************************************************************ + * Name: n25qxxx_erase_sector + ************************************************************************************/ + +__ramfunc__ int n25qxxx_erase_sector(struct n25qxxx_dev_s *priv, off_t sector) +{ + off_t address; + uint8_t status; + + finfo("sector: %08jx\n", (intmax_t) sector); + + /* Check that the flash is ready and unprotected */ + + status = n25qxxx_read_status(priv); + + if ((status & STATUS_BUSY_MASK) != STATUS_READY) { + ferr("ERROR: Flash busy: %02" PRIx8, status); + return -EBUSY; + } + + /* Get the address associated with the sector */ + + address = (off_t)sector << priv->sectorshift; + + if ((status & (STATUS_BP3_MASK | STATUS_BP_MASK)) != 0 && + n25qxxx_isprotected(priv, status, address)) { + ferr("ERROR: Flash protected: %02" PRIx8, status); + return -EACCES; + } + + /* Send the sector erase command */ + + n25qxxx_write_enable(priv); + n25qxxx_command_address(priv->qspi, N25QXXX_SUBSECTOR_ERASE, address, 3); + + /* Wait for erasure to finish */ + + while ((n25qxxx_read_status(priv) & STATUS_BUSY_MASK) != 0); + + return OK; +} + +/************************************************************************************ + * Name: n25qxxx_isprotected + ************************************************************************************/ + +__ramfunc__ bool n25qxxx_isprotected(FAR struct n25qxxx_dev_s *priv, uint8_t status, + off_t address) +{ + off_t protstart; + off_t protend; + off_t protsize; + unsigned int bp; + + /* The BP field is spread across non-contiguous bits */ + + bp = (status & STATUS_BP_MASK) >> STATUS_BP_SHIFT; + + if (status & STATUS_BP3_MASK) { + bp |= 8; + } + + /* the BP field is essentially the power-of-two of the number of 64k sectors, + * saturated to the device size. + */ + + if (0 == bp) { + return false; + } + + protsize = 0x00010000; + protsize <<= (protsize << (bp - 1)); + protend = (1 << priv->sectorshift) * priv->nsectors; + + if (protsize > protend) { + protsize = protend; + } + + /* The final protection range then depends on if the protection region is + * configured top-down or bottom up (assuming CMP=0). + */ + + if ((status & STATUS_TB_MASK) != 0) { + protstart = 0x00000000; + protend = protstart + protsize; + + } else { + protstart = protend - protsize; + /* protend already computed above */ + } + + return (address >= protstart && address < protend); +} + +/************************************************************************************ + * Name: n25qxxx_command_address + ************************************************************************************/ + +__ramfunc__ int n25qxxx_command_address(FAR struct qspi_dev_s *qspi, uint8_t cmd, + off_t addr, uint8_t addrlen) +{ + struct qspi_cmdinfo_s cmdinfo; + + finfo("CMD: %02" PRIx8 " Address: %04jx addrlen=%" PRIx8 "\n", cmd, (intmax_t) addr, addrlen); + + cmdinfo.flags = QSPICMD_ADDRESS; + cmdinfo.addrlen = addrlen; + cmdinfo.cmd = cmd; + cmdinfo.buflen = 0; + cmdinfo.addr = addr; + cmdinfo.buffer = NULL; + + int rv; + rv = qspi_command(qspi, &cmdinfo); + return rv; +} diff --git a/boards/hkust/nxt-dual/src/hw_config.h b/boards/hkust/nxt-dual/src/hw_config.h new file mode 100644 index 000000000000..352f29436a06 --- /dev/null +++ b/boards/hkust/nxt-dual/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 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 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 6 +#define INTERFACE_USART_CONFIG "/dev/ttyS5,57600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1013 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // RED +#define BOARD_LED_ON 1 +#define BOARD_LED_OFF 0 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#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 diff --git a/boards/hkust/nxt-dual/src/i2c.cpp b/boards/hkust/nxt-dual/src/i2c.cpp new file mode 100644 index 000000000000..72ba2d6c0b57 --- /dev/null +++ b/boards/hkust/nxt-dual/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusExternal(4), +}; diff --git a/boards/hkust/nxt-dual/src/init.c b/boards/hkust/nxt-dual/src/init.c new file mode 100644 index 000000000000..657c0080c019 --- /dev/null +++ b/boards/hkust/nxt-dual/src/init.c @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 init.c + * + * FMU-specific early startup code. This file implements the + * board_app_initialize() 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. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__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) +{ + UNUSED(ms); +} + +/************************************************************************************ + * 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) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. 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 stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure USB interfaces */ + stm32_usbinitialize(); + +} + +/**************************************************************************** + * 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; + * + * 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) +{ + /* Need hrt running before using the ADC */ + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_BLUE); + return ret; + } + +#endif + +// TODO：internal flash store parameters +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_RED); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/hkust/nxt-dual/src/led.c b/boards/hkust/nxt-dual/src/led.c new file mode 100644 index 000000000000..0420c1da2e79 --- /dev/null +++ b/boards/hkust/nxt-dual/src/led.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/hkust/nxt-dual/src/manifest.c b/boards/hkust/nxt-dual/src/manifest.c new file mode 100644 index 000000000000..e13f3d08607f --- /dev/null +++ b/boards/hkust/nxt-dual/src/manifest.c @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 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_v0600[] = { + { + .present = 0, + .mandatory = 0, + .connection = px4_hw_con_unknown, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { + {V6U00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, +}; + +/************************************************************************************ + * 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() << 16; + 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 %08" 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/hkust/nxt-dual/src/mtd.cpp b/boards/hkust/nxt-dual/src/mtd.cpp new file mode 100644 index 000000000000..e374a9be7e68 --- /dev/null +++ b/boards/hkust/nxt-dual/src/mtd.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +//TODO:Prepare for NxtDual + +#include +#include +// KiB BS nB +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi5, + .npart = 1, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &fmum_fram + } +}; + +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/hkust/nxt-dual/src/sdio.c b/boards/hkust/nxt-dual/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/hkust/nxt-dual/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] 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, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/hkust/nxt-dual/src/spi.cpp b/boards/hkust/nxt-dual/src/spi.cpp new file mode 100644 index 000000000000..058c48982636 --- /dev/null +++ b/boards/hkust/nxt-dual/src/spi.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin3}, SPI::DRDY{GPIO::PortA, GPIO::Pin1}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortA, GPIO::Pin2}, SPI::DRDY{GPIO::PortA, GPIO::Pin0}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin4}) + }), + initSPIBus(SPI::Bus::SPI3, { + // not in use, future development + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortE, GPIO::Pin3}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortC, GPIO::Pin13}, SPI::DRDY{GPIO::PortE, GPIO::Pin4}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/hkust/nxt-dual/src/timer_config.cpp b/boards/hkust/nxt-dual/src/timer_config.cpp new file mode 100644 index 000000000000..27ad0f247617 --- /dev/null +++ b/boards/hkust/nxt-dual/src/timer_config.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer2, DMA{DMA::Index1}), + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + // initIOTimer(Timer::Timer2), + // initIOTimer(Timer::Timer3), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel3}, {GPIO::PortB, GPIO::Pin10}), + initIOTimerChannel(io_timers, {Timer::Timer2, Timer::Channel4}, {GPIO::PortB, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/hkust/nxt-dual/src/usb.c b/boards/hkust/nxt-dual/src/usb.c new file mode 100644 index 000000000000..9591784866a6 --- /dev/null +++ b/boards/hkust/nxt-dual/src/usb.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +#include "board_config.h" +#include +#include +#include +#include + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/hkust/nxt-v1/bootloader.px4board b/boards/hkust/nxt-v1/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/hkust/nxt-v1/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/hkust/nxt-v1/default.px4board b/boards/hkust/nxt-v1/default.px4board new file mode 100644 index 000000000000..b72160775850 --- /dev/null +++ b/boards/hkust/nxt-v1/default.px4board @@ -0,0 +1,92 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_COMMON_BAROMETERS=y +CONFIG_DRIVERS_BATT_SMBUS=y +CONFIG_DRIVERS_CAMERA_CAPTURE=y +CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_MPU6500=y +CONFIG_DRIVERS_IRLOCK=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_ROBOCLAW=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_TONE_ALARM=y +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y +CONFIG_MODULES_BATTERY_STATUS=y +CONFIG_MODULES_CAMERA_FEEDBACK=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_ESC_BATTERY=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_GYRO_FFT=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_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=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_BL_UPDATE=y +CONFIG_SYSTEMCMDS_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_LED_CONTROL=y +CONFIG_SYSTEMCMDS_MFT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=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_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y +CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/hkust/nxt-v1/extras/hkust_nxt-v1_bootloader.bin b/boards/hkust/nxt-v1/extras/hkust_nxt-v1_bootloader.bin new file mode 100755 index 000000000000..a940993d6c77 Binary files /dev/null and b/boards/hkust/nxt-v1/extras/hkust_nxt-v1_bootloader.bin differ diff --git a/boards/hkust/nxt-v1/firmware.prototype b/boards/hkust/nxt-v1/firmware.prototype new file mode 100644 index 000000000000..d715678889a3 --- /dev/null +++ b/boards/hkust/nxt-v1/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1013, + "magic": "PX4FWv1", + "description": "Firmware for the MatekH743-slim board", + "image": "", + "build_time": 0, + "summary": "MatekH743-mini", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/hkust/nxt-v1/init/rc.board_defaults b/boards/hkust/nxt-v1/init/rc.board_defaults new file mode 100644 index 000000000000..d3504a657e8f --- /dev/null +++ b/boards/hkust/nxt-v1/init/rc.board_defaults @@ -0,0 +1,6 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default SYS_HAS_MAG 0 +param set-default IMU_GYRO_RATEMAX 2000 diff --git a/boards/hkust/nxt-v1/init/rc.board_extras b/boards/hkust/nxt-v1/init/rc.board_extras new file mode 100644 index 000000000000..780b13dc96b3 --- /dev/null +++ b/boards/hkust/nxt-v1/init/rc.board_extras @@ -0,0 +1,13 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# NxtV1 does not have OSD +# if ! param compare OSD_ATXXXX_CFG 0 +# then +# atxxxx start -s +# fi + +# DShot telemetry is always on UART7 +# dshot telemetry /dev/ttyS5 diff --git a/boards/hkust/nxt-v1/init/rc.board_sensors b/boards/hkust/nxt-v1/init/rc.board_sensors new file mode 100644 index 000000000000..5c51e7642170 --- /dev/null +++ b/boards/hkust/nxt-v1/init/rc.board_sensors @@ -0,0 +1,16 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# # Internal SPI bus ICM24688P +icm42688p -R 0 -s start + +# # Internal SPI bus BMI088 accel/gyro +bmi088 -A -R 0 -s start +bmi088 -G -R 0 -s start + +# internal baro +bmp388 -I -a 0x76 start diff --git a/boards/hkust/nxt-v1/nuttx-config/Kconfig b/boards/hkust/nxt-v1/nuttx-config/Kconfig new file mode 100644 index 000000000000..bb33d3cfda4d --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/Kconfig @@ -0,0 +1,17 @@ +# +# For a description of the syntax of this configuration file, +# see misc/tools/kconfig-language.txt. +# +config BOARD_HAS_PROBES + bool "Board provides GPIO or other Hardware for signaling to timing analyze." + default y + ---help--- + This board provides GPIO FMU-CH1-5, CAP1-6 as PROBE_1-11 to provide timing signals from selected drivers. + +config BOARD_USE_PROBES + bool "Enable the use the board provided FMU-CH1-5, CAP1-6 as PROBE_1-11" + default n + depends on BOARD_HAS_PROBES + + ---help--- + Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. diff --git a/boards/hkust/nxt-v1/nuttx-config/bootloader/defconfig b/boards/hkust/nxt-v1/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..6cf4a40708fa --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/bootloader/defconfig @@ -0,0 +1,90 @@ +# +# 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_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/hkust/nxt-v1/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="HKUST UAV NxtPX4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=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_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_USART3=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=300 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/hkust/nxt-v1/nuttx-config/include/board.h b/boards/hkust/nxt-v1/nuttx-config/include/board.h new file mode 100644 index 000000000000..003922a0ef44 --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/include/board.h @@ -0,0 +1,484 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6u/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MatekH743-Slim board provides the following clock sources: + * + * X1: 16 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 16 MHz RC factory-trimmed + * HSE: 16 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 16000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 16,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (16,000,000 / 1) * 60 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(60) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 60) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(4) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(4) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 4) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 clock source */ + +// #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 400kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div = PLL1Q/(2*freq) + * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The board has two, LED_GREEN a Green LED and LED_BLUE a Blue LED, + * that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PBA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ +#define GPIO_USART1_CK GPIO_USART1_CK /* PB6 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_CK GPIO_USART2_CK_2 /* PD7 */ + + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ +#define GPIO_USART3_CK GPIO_USART3_CK_3 /* PD10 */ + + +#define GPIO_UART4_RX GPIO_UART4_RX_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + +#define GPIO_UART5_RX GPIO_UART5_RX_1 /* PB12 */ +#define GPIO_UART5_TX GPIO_UART5_TX_1 /* PB13 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + + +/* SPI + * + + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_2 /* PC1 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + +#define GPIO_SPI3_MISO GPIO_SPI3_MISO_1 /* PB4 */ +#define GPIO_SPI3_MOSI GPIO_SPI3_MOSI_4 /* PB5 */ +#define GPIO_SPI3_SCK ADJ_SLEW_RATE(GPIO_SPI3_SCK_1) /* PC10 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_2 /* PE5 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PB6 */ +#define GPIO_SPI4_SCK ADJ_SLEW_RATE(GPIO_SPI4_SCK_1) /* PE12 */ + +/* I2C + * + + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_2 /* PB8 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN8) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11 */ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN |GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + + +/* SDMMC1 + * + * SDMMC1_D0 PC8 + * SDMMC1_D1 PC9 + * SDMMC1_D2 PC10 + * SDMMC1_D3 PC11 + * SDMMC1_CK PC12 + * SDMMC1_CMD PD2 + */ + +// #define GPIO_SDMMC1_D0 GPIO_SDMMC1_D0 /* PC8 */ +// #define GPIO_SDMMC1_D1 GPIO_SDMMC1_D1 /* PC9 */ +// #define GPIO_SDMMC1_D2 GPIO_SDMMC1_D2 /* PC10 */ +// #define GPIO_SDMMC1_D3 GPIO_SDMMC1_D3 /* PC11 */ +// #define GPIO_SDMMC1_CK GPIO_SDMMC1_CK /* PC12 */ +// #define GPIO_SDMMC1_CMD GPIO_SDMMC1_CMD /* PD2 */ + + +/* USB + * + * OTG_FS_DM PA11 + * OTG_FS_DP PA12 + * VBUS PA9 + */ + + +/* Board provides GPIO or other Hardware for signaling to timing analyzer */ + +#if defined(CONFIG_BOARD_USE_PROBES) +# include "stm32_gpio.h" +# define PROBE_N(n) (1<<((n)-1)) +# define PROBE_1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTI|GPIO_PIN0) /* PI0 AUX1 */ +# define PROBE_2 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN12) /* PH12 AUX2 */ +# define PROBE_3 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN11) /* PH11 AUX3 */ +# define PROBE_4 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN10) /* PH10 AUX4 */ +# define PROBE_5 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) /* PD13 AUX5 */ +# define PROBE_6 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) /* PD14 AUX6 */ +# define PROBE_7 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN6) /* PH6 AUX7 */ +# define PROBE_8 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN9) /* PH9 AUX8 */ +# define PROBE_9 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN11) /* PE11 CAP1 */ + +# define PROBE_INIT(mask) \ + do { \ + if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \ + if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \ + if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \ + if ((mask)& PROBE_N(4)) { stm32_configgpio(PROBE_4); } \ + if ((mask)& PROBE_N(5)) { stm32_configgpio(PROBE_5); } \ + if ((mask)& PROBE_N(6)) { stm32_configgpio(PROBE_6); } \ + if ((mask)& PROBE_N(7)) { stm32_configgpio(PROBE_7); } \ + if ((mask)& PROBE_N(8)) { stm32_configgpio(PROBE_8); } \ + if ((mask)& PROBE_N(9)) { stm32_configgpio(PROBE_9); } \ + } while(0) + +# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0) +# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true) +#else +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) +#endif + +#endif /*__NUTTX_CONFIG_MATEKH743SLIM_INCLUDE_BOARD_H */ diff --git a/boards/hkust/nxt-v1/nuttx-config/include/board_dma_map.h b/boards/hkust/nxt-v1/nuttx-config/include/board_dma_map.h new file mode 100644 index 000000000000..ff89fb6ae9db --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/include/board_dma_map.h @@ -0,0 +1,62 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once +#define DMAMAP_SPI1_RX DMAMAP_DMA12_SPI1RX_0 /* DMA1:37 */ +#define DMAMAP_SPI1_TX DMAMAP_DMA12_SPI1TX_0 /* DMA1:38 */ + +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ + +// DMAMUX2 +#define DMAMAP_SPI3_RX DMAMAP_DMA12_SPI3RX_0 /* DMA1:61 */ +#define DMAMAP_SPI3_TX DMAMAP_DMA12_SPI3TX_0 /* DMA1:62 */ + +#define DMAMAP_SPI6_RX DMAMAP_BDMA_SPI6_RX /* BDMA:11 */ +#define DMAMAP_SPI6_TX DMAMAP_BDMA_SPI6_TX /* BDMA:12 */ + +//TODO: UART DMA test +#define DMAMAP_USART1_RX DMAMAP_DMA12_USART1RX_1 /*DMA2:41*/ +#define DMAMAP_USART1_TX DMAMAP_DMA12_USART1TX_1 /*DMA2:42*/ + +#define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2:43 */ +#define DMAMAP_USART2_TX DMAMAP_DMA12_USART2TX_1 /* DMA2:44 */ + +#define DMAMAP_USART3_RX DMAMAP_DMA12_USART3RX_1 /* DMA2:45 */ +#define DMAMAP_USART3_TX DMAMAP_DMA12_USART3TX_1 /* DMA2:46 */ + +#define DMAMAP_UART4_RX DMAMAP_DMA12_UART4RX_0 /* DMA1:63 */ +#define DMAMAP_UART4_TX DMAMAP_DMA12_UART4TX_0 /* DMA1:64 */ + +#define DMAMAP_UART5_RX DMAMAP_DMA12_UART5RX_0 /* DMA1:65 */ +#define DMAMAP_UART5_TX DMAMAP_DMA12_UART5RX_0 /* DMA1:66 */ diff --git a/boards/hkust/nxt-v1/nuttx-config/nsh/defconfig b/boards/hkust/nxt-v1/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..21c65a81ee65 --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/nsh/defconfig @@ -0,0 +1,255 @@ +# +# 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_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_PRINTF 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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/hkust/nxt-v1/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="HKUST UAV NxtPX4" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="Matek" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=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_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +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_EXTERNAL=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_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_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC2=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_FLOWCONTROL_BROKEN=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI1_DMA=y +CONFIG_STM32H7_SPI1_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI3=y +CONFIG_STM32H7_SPI3_DMA=y +CONFIG_STM32H7_SPI3_DMA_BUFFER=1024 +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM5=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART5=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=921600 +CONFIG_UART4_RXBUFSIZE=3000 +CONFIG_UART4_TXBUFSIZE=1200 +CONFIG_UART5_BAUD=57600 +CONFIG_UART5_RXBUFSIZE=600 +CONFIG_UART5_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=3000 +CONFIG_USART1_BAUD=57600 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=1500 +CONFIG_USART2_BAUD=57600 +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_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y +CONFIG_WQUEUE_NOTIFIER=y diff --git a/boards/hkust/nxt-v1/nuttx-config/scripts/bootloader_script.ld b/boards/hkust/nxt-v1/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,213 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The Durandal-v1 uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* 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/hkust/nxt-v1/nuttx-config/scripts/script.ld b/boards/hkust/nxt-v1/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..85f4990724d5 --- /dev/null +++ b/boards/hkust/nxt-v1/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1920K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* 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/hkust/nxt-v1/src/CMakeLists.txt b/boards/hkust/nxt-v1/src/CMakeLists.txt new file mode 100644 index 000000000000..45d0650aaea8 --- /dev/null +++ b/boards/hkust/nxt-v1/src/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + # add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/hkust/nxt-v1/src/board_config.h b/boards/hkust/nxt-v1/src/board_config.h new file mode 100644 index 000000000000..34dff55da531 --- /dev/null +++ b/boards/hkust/nxt-v1/src/board_config.h @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +// #define FLASH_BASED_PARAMS + + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +# define GPIO_nLED_RED /* PC6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN6) +# define GPIO_nLED_GREEN /* PB14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN14) +# define GPIO_nLED_BLUE /* PB15 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN15) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + +/* I2C busses */ +/* Devices on the onboard buses. + * + * Note that these are unshifted addresses. + */ +// #define PX4_I2C_OBDEV_SE050 0x48 + +#define GPIO_I2C1_DRDY1_BMP388 /* PB9 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTB|GPIO_PIN9) + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define SYSTEM_ADC_BASE STM32_ADC1_BASE + +#define ADC12_CH(n) (n) + +#define PX4_ADC_GPIO \ + /* PB0 */ GPIO_ADC12_INP5, \ + /* PB1 */ GPIO_ADC12_INP9 + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL ADC12_CH(9) +#define ADC_BATTERY_CURRENT_CHANNEL ADC12_CH(5) + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + +#define BOARD_ADC_OPEN_CIRCUIT_V (5.6f) + + + +/* Define Battery 1 Voltage Divider and A per V + */ + +// #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ +// #define BOARD_BATTERY1_A_PER_V (40.0f) +// #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 8 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* Spare GPIO */ +#define GPIO_PD4 /* PD4 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN4) +#define GPIO_PC13 /* PC13 */ (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN13) +#define GPIO_PH1 /* PH1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTH|GPIO_PIN1) + +/* Tone alarm output */ + +#define TONE_ALARM_TIMER 1 /* Timer 3 */ +#define TONE_ALARM_CHANNEL 1 /* PC7 GPIO_TIM3_CH2 */ +/*NC can be modified with Spare GPIO then connected with hardware */ +#define GPIO_BUZZER_1 /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_BUZZER_1 + +/* USB OTG FS + * + * PE2 OTG_FS_VBUS VBUS sensing + */ + + +#define GPIO_OTGFS_VBUS /* PA15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN15) +#define BOARD_ADC_USB_CONNECTED (px4_arch_gpioread(GPIO_OTGFS_VBUS)) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer1 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 1 */ + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS5" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* SD card bringup does not work if performed on the IDLE thread because it + * will cause waiting. Use either: + * + * CONFIG_LIB_BOARDCTL=y, OR + * CONFIG_BOARD_INITIALIZE=y && CONFIG_BOARD_INITTHREAD=y + */ +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 +#if defined(CONFIG_BOARD_INITIALIZE) && !defined(CONFIG_LIB_BOARDCTL) && \ + !defined(CONFIG_BOARD_INITTHREAD) +# warning SDIO initialization cannot be perfomed on the IDLE thread +#endif + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define BOARD_NUM_IO_TIMERS 5 + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/hkust/nxt-v1/src/bootloader_main.c b/boards/hkust/nxt-v1/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/hkust/nxt-v1/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/hkust/nxt-v1/src/hw_config.h b/boards/hkust/nxt-v1/src/hw_config.h new file mode 100644 index 000000000000..1e91b651457e --- /dev/null +++ b/boards/hkust/nxt-v1/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 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 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,57600" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1013 +#define _FLASH_KBYTES (*(uint32_t *)0x1FF1E880) +#define BOARD_FLASH_SECTORS (15) +#define BOARD_FLASH_SIZE (_FLASH_KBYTES * 1024) + +#define OSC_FREQ 16 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_RED // RED +#define BOARD_LED_ON 1 +#define BOARD_LED_OFF 0 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#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 diff --git a/boards/hkust/nxt-v1/src/i2c.cpp b/boards/hkust/nxt-v1/src/i2c.cpp new file mode 100644 index 000000000000..6700d8c8f244 --- /dev/null +++ b/boards/hkust/nxt-v1/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusInternal(1), + initI2CBusExternal(2), +}; diff --git a/boards/hkust/nxt-v1/src/init.c b/boards/hkust/nxt-v1/src/init.c new file mode 100644 index 000000000000..657c0080c019 --- /dev/null +++ b/boards/hkust/nxt-v1/src/init.c @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * 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 init.c + * + * FMU-specific early startup code. This file implements the + * board_app_initialize() 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. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__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) +{ + UNUSED(ms); +} + +/************************************************************************************ + * 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) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. 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 stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure USB interfaces */ + stm32_usbinitialize(); + +} + +/**************************************************************************** + * 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; + * + * 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) +{ + /* Need hrt running before using the ADC */ + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_BLUE); + return ret; + } + +#endif + +// TODO：internal flash store parameters +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_RED); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/hkust/nxt-v1/src/led.c b/boards/hkust/nxt-v1/src/led.c new file mode 100644 index 000000000000..0420c1da2e79 --- /dev/null +++ b/boards/hkust/nxt-v1/src/led.c @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/hkust/nxt-v1/src/mtd.cpp b/boards/hkust/nxt-v1/src/mtd.cpp new file mode 100644 index 000000000000..51c6dec3cc93 --- /dev/null +++ b/boards/hkust/nxt-v1/src/mtd.cpp @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ +//TODO:Prepare for NxtDual + +#include +#include +// KiB BS nB +static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +static const px4_mtd_entry_t fmum_fram = { + .device = &spi5, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + +static const px4_mtd_manifest_t board_mtd_config = { + .nconfigs = 1, + .entries = { + &fmum_fram + } +}; + +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/hkust/nxt-v1/src/sdio.c b/boards/hkust/nxt-v1/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/hkust/nxt-v1/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] 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, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/hkust/nxt-v1/src/spi.cpp b/boards/hkust/nxt-v1/src/spi.cpp new file mode 100644 index 000000000000..54a338d94ce6 --- /dev/null +++ b/boards/hkust/nxt-v1/src/spi.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_MPU6500, SPI::CS{GPIO::PortC, GPIO::Pin4}, SPI::DRDY{GPIO::PortC, GPIO::Pin5}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortC, GPIO::Pin3}, SPI::DRDY{GPIO::PortA, GPIO::Pin4}), + }), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortE, GPIO::Pin3}, SPI::DRDY{GPIO::PortE, GPIO::Pin0}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/hkust/nxt-v1/src/timer_config.cpp b/boards/hkust/nxt-v1/src/timer_config.cpp new file mode 100644 index 000000000000..e3373ffeed96 --- /dev/null +++ b/boards/hkust/nxt-v1/src/timer_config.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer5, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), + initIOTimer(Timer::Timer1), + initIOTimer(Timer::Timer3), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel2}, {GPIO::PortA, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel3}, {GPIO::PortA, GPIO::Pin2}), + initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel2}, {GPIO::PortC, GPIO::Pin7}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}) +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/hkust/nxt-v1/src/usb.c b/boards/hkust/nxt-v1/src/usb.c new file mode 100644 index 000000000000..9591784866a6 --- /dev/null +++ b/boards/hkust/nxt-v1/src/usb.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +#include "board_config.h" +#include +#include +#include +#include + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/holybro/durandal-v1/default.px4board b/boards/holybro/durandal-v1/default.px4board index 362eaff74250..ecc1bc81013b 100644 --- a/boards/holybro/durandal-v1/default.px4board +++ b/boards/holybro/durandal-v1/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin b/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin index 388ee7ac7afb..c24d00a61bf4 100755 Binary files a/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin and b/boards/holybro/durandal-v1/extras/holybro_durandal-v1_bootloader.bin differ diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index bcaedd9cf281..8aaa037cc55a 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -9,6 +9,7 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y diff --git a/boards/holybro/kakutef7/init/rc.board_defaults b/boards/holybro/kakutef7/init/rc.board_defaults index 7a3bc92e844a..2ad7443124e6 100644 --- a/boards/holybro/kakutef7/init/rc.board_defaults +++ b/boards/holybro/kakutef7/init/rc.board_defaults @@ -15,7 +15,7 @@ param set-default SYS_AUTOSTART 4050 param set-default SYS_HAS_MAG 0 # enable gravity fusion -param set-default EKF2_IMU_CONTROL 7 +param set-default EKF2_IMU_CTRL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakutef7/init/rc.board_extras b/boards/holybro/kakutef7/init/rc.board_extras index 309ed17768f9..5822cd29458d 100644 --- a/boards/holybro/kakutef7/init/rc.board_extras +++ b/boards/holybro/kakutef7/init/rc.board_extras @@ -10,4 +10,3 @@ fi # DShot telemetry is always on UART7 dshot telemetry /dev/ttyS5 - diff --git a/boards/holybro/kakutef7/nuttx-config/include/board.h b/boards/holybro/kakutef7/nuttx-config/include/board.h index aae28ec8ebc7..e622f1590516 100644 --- a/boards/holybro/kakutef7/nuttx-config/include/board.h +++ b/boards/holybro/kakutef7/nuttx-config/include/board.h @@ -304,4 +304,3 @@ * OTG_FS_DP PA12 * VBUS PA8 */ - diff --git a/boards/holybro/kakutef7/nuttx-config/include/board_dma_map.h b/boards/holybro/kakutef7/nuttx-config/include/board_dma_map.h index c095f53105c9..5ef86fa68548 100644 --- a/boards/holybro/kakutef7/nuttx-config/include/board_dma_map.h +++ b/boards/holybro/kakutef7/nuttx-config/include/board_dma_map.h @@ -99,4 +99,3 @@ // DMAMAP_TIM1_UP // DMA2, Stream 5, Channel 6 (DSHOT) // AVAILABLE // DMA1, Stream 6 // AVAILABLE // DMA1, Stream 7 - diff --git a/boards/holybro/kakutef7/src/led.c b/boards/holybro/kakutef7/src/led.c index 5169b86d2288..05f02573f116 100644 --- a/boards/holybro/kakutef7/src/led.c +++ b/boards/holybro/kakutef7/src/led.c @@ -105,4 +105,3 @@ __EXPORT void led_toggle(int led) { phy_set_led(led, !phy_get_led(led)); } - diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 270caa7b6510..dbf8aeb7f217 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin b/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin index 093514667d82..7f5d2b31afa5 100755 Binary files a/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin and b/boards/holybro/kakuteh7/extras/holybro_kakuteh7_bootloader.bin differ diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults index 47dea71be617..f9f3ca7d5004 100644 --- a/boards/holybro/kakuteh7/init/rc.board_defaults +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -25,10 +25,9 @@ param set-default SYS_AUTOSTART 4050 # use EKF2 without mag param set-default SYS_HAS_MAG 0 # and enable gravity fusion -param set-default EKF2_IMU_CONTROL 7 +param set-default EKF2_IMU_CTRL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 param set-default IMU_GYRO_RATEMAX 2000 - diff --git a/boards/holybro/kakuteh7/init/rc.board_extras b/boards/holybro/kakuteh7/init/rc.board_extras index 256633f33c9d..3423728dab9f 100644 --- a/boards/holybro/kakuteh7/init/rc.board_extras +++ b/boards/holybro/kakuteh7/init/rc.board_extras @@ -10,4 +10,3 @@ fi # DShot telemetry is always on UART7 dshot telemetry /dev/ttyS5 - diff --git a/boards/holybro/kakuteh7/nuttx-config/include/board.h b/boards/holybro/kakuteh7/nuttx-config/include/board.h index 4d3ed5eb4766..0c96869f01d1 100644 --- a/boards/holybro/kakuteh7/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7/nuttx-config/include/board.h @@ -416,4 +416,3 @@ # define PROBE_INIT(mask) # define PROBE(n,s) # define PROBE_MARK(n) - diff --git a/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h index 1954ce3e6806..d23e0d412072 100644 --- a/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h +++ b/boards/holybro/kakuteh7/nuttx-config/include/board_dma_map.h @@ -37,4 +37,3 @@ #define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ #define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ - diff --git a/boards/holybro/kakuteh7/src/spi.cpp b/boards/holybro/kakuteh7/src/spi.cpp index 1ff5b7f35c1c..e0f4db0edd6e 100644 --- a/boards/holybro/kakuteh7/src/spi.cpp +++ b/boards/holybro/kakuteh7/src/spi.cpp @@ -43,7 +43,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), }), initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortE, GPIO::Pin4}, SPI::DRDY{GPIO::PortE, GPIO::Pin1}), }), diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index b49f1cfa012f..eb465e00275c 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin b/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin index fca97ea49d7c..81108119f3eb 100755 Binary files a/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin and b/boards/holybro/kakuteh7mini/extras/holybro_kakuteh7mini_bootloader.bin differ diff --git a/boards/holybro/kakuteh7mini/init/rc.board_defaults b/boards/holybro/kakuteh7mini/init/rc.board_defaults index 4503905262e8..a0ccc7e101e5 100644 --- a/boards/holybro/kakuteh7mini/init/rc.board_defaults +++ b/boards/holybro/kakuteh7mini/init/rc.board_defaults @@ -25,7 +25,7 @@ param set-default SYS_AUTOSTART 4050 # use EKF2 without mag param set-default SYS_HAS_MAG 0 # and enable gravity fusion -param set-default EKF2_IMU_CONTROL 7 +param set-default EKF2_IMU_CTRL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakuteh7mini/init/rc.board_extras b/boards/holybro/kakuteh7mini/init/rc.board_extras index 256633f33c9d..3423728dab9f 100644 --- a/boards/holybro/kakuteh7mini/init/rc.board_extras +++ b/boards/holybro/kakuteh7mini/init/rc.board_extras @@ -10,4 +10,3 @@ fi # DShot telemetry is always on UART7 dshot telemetry /dev/ttyS5 - diff --git a/boards/holybro/kakuteh7mini/nuttx-config/include/board.h b/boards/holybro/kakuteh7mini/nuttx-config/include/board.h index 4d3ed5eb4766..0c96869f01d1 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7mini/nuttx-config/include/board.h @@ -416,4 +416,3 @@ # define PROBE_INIT(mask) # define PROBE(n,s) # define PROBE_MARK(n) - diff --git a/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h index 1954ce3e6806..d23e0d412072 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h +++ b/boards/holybro/kakuteh7mini/nuttx-config/include/board_dma_map.h @@ -37,4 +37,3 @@ #define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ #define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ - diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index d9d432beec13..45fd6e11eb79 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin b/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin index 7ed8c65c2f53..c441ef2a2317 100755 Binary files a/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin and b/boards/holybro/kakuteh7v2/extras/holybro_kakuteh7v2_bootloader.bin differ diff --git a/boards/holybro/kakuteh7v2/init/rc.board_defaults b/boards/holybro/kakuteh7v2/init/rc.board_defaults index 4503905262e8..a0ccc7e101e5 100644 --- a/boards/holybro/kakuteh7v2/init/rc.board_defaults +++ b/boards/holybro/kakuteh7v2/init/rc.board_defaults @@ -25,7 +25,7 @@ param set-default SYS_AUTOSTART 4050 # use EKF2 without mag param set-default SYS_HAS_MAG 0 # and enable gravity fusion -param set-default EKF2_IMU_CONTROL 7 +param set-default EKF2_IMU_CTRL 7 # the startup tune is not great on a binary output buzzer, so disable it param set-default CBRK_BUZZER 782090 diff --git a/boards/holybro/kakuteh7v2/init/rc.board_extras b/boards/holybro/kakuteh7v2/init/rc.board_extras index 256633f33c9d..3423728dab9f 100644 --- a/boards/holybro/kakuteh7v2/init/rc.board_extras +++ b/boards/holybro/kakuteh7v2/init/rc.board_extras @@ -10,4 +10,3 @@ fi # DShot telemetry is always on UART7 dshot telemetry /dev/ttyS5 - diff --git a/boards/holybro/kakuteh7v2/nuttx-config/include/board.h b/boards/holybro/kakuteh7v2/nuttx-config/include/board.h index 4d3ed5eb4766..0c96869f01d1 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/include/board.h +++ b/boards/holybro/kakuteh7v2/nuttx-config/include/board.h @@ -416,4 +416,3 @@ # define PROBE_INIT(mask) # define PROBE(n,s) # define PROBE_MARK(n) - diff --git a/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h b/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h index 1954ce3e6806..d23e0d412072 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h +++ b/boards/holybro/kakuteh7v2/nuttx-config/include/board_dma_map.h @@ -37,4 +37,3 @@ #define DMAMAP_SPI4_TX DMAMAP_DMA12_SPI4TX_1 /* DMA2 */ #define DMAMAP_USART2_RX DMAMAP_DMA12_USART2RX_1 /* DMA2 */ - diff --git a/boards/holybro/pix32v5/default.px4board b/boards/holybro/pix32v5/default.px4board index 45bd43ee4f4d..0093ee2dcfd0 100644 --- a/boards/holybro/pix32v5/default.px4board +++ b/boards/holybro/pix32v5/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ diff --git a/boards/matek/h743-mini/default.px4board b/boards/matek/h743-mini/default.px4board index 64e472d77048..22efafcfdb38 100644 --- a/boards/matek/h743-mini/default.px4board +++ b/boards/matek/h743-mini/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin b/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin index 121b7eae3591..aa47d3e32116 100755 Binary files a/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin and b/boards/matek/h743-mini/extras/matek_h743-mini_bootloader.bin differ diff --git a/boards/matek/h743-mini/init/rc.board_extras b/boards/matek/h743-mini/init/rc.board_extras index a39b81bdc223..66e293646417 100644 --- a/boards/matek/h743-mini/init/rc.board_extras +++ b/boards/matek/h743-mini/init/rc.board_extras @@ -10,7 +10,5 @@ atxxxx start -s - # DShot telemetry is always on UART7 # dshot telemetry /dev/ttyS5 - diff --git a/boards/matek/h743-mini/src/board_config.h b/boards/matek/h743-mini/src/board_config.h index 151d6b59c5e5..e47444085a4e 100644 --- a/boards/matek/h743-mini/src/board_config.h +++ b/boards/matek/h743-mini/src/board_config.h @@ -102,9 +102,7 @@ (1 << ADC_RSSI_IN_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ #define BOARD_BATTERY1_A_PER_V (40.0f) #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ diff --git a/boards/matek/h743-slim/default.px4board b/boards/matek/h743-slim/default.px4board index ce5f2222296e..71024fea5ceb 100644 --- a/boards/matek/h743-slim/default.px4board +++ b/boards/matek/h743-slim/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y diff --git a/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin b/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin index 2a5c57d308b2..954f7f612547 100755 Binary files a/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin and b/boards/matek/h743-slim/extras/matek_h743-slim_bootloader.bin differ diff --git a/boards/matek/h743-slim/init/rc.board_extras b/boards/matek/h743-slim/init/rc.board_extras index a39b81bdc223..bba363769ce9 100644 --- a/boards/matek/h743-slim/init/rc.board_extras +++ b/boards/matek/h743-slim/init/rc.board_extras @@ -13,4 +13,3 @@ atxxxx start -s # DShot telemetry is always on UART7 # dshot telemetry /dev/ttyS5 - diff --git a/boards/matek/h743/default.px4board b/boards/matek/h743/default.px4board index ce5f2222296e..71024fea5ceb 100644 --- a/boards/matek/h743/default.px4board +++ b/boards/matek/h743/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y diff --git a/boards/matek/h743/extras/matek_h743_bootloader.bin b/boards/matek/h743/extras/matek_h743_bootloader.bin index 605024f3e277..93591c17c925 100755 Binary files a/boards/matek/h743/extras/matek_h743_bootloader.bin and b/boards/matek/h743/extras/matek_h743_bootloader.bin differ diff --git a/boards/matek/h743/init/rc.board_extras b/boards/matek/h743/init/rc.board_extras index a39b81bdc223..bba363769ce9 100644 --- a/boards/matek/h743/init/rc.board_extras +++ b/boards/matek/h743/init/rc.board_extras @@ -13,4 +13,3 @@ atxxxx start -s # DShot telemetry is always on UART7 # dshot telemetry /dev/ttyS5 - diff --git a/boards/matek/h743/src/board_config.h b/boards/matek/h743/src/board_config.h index 151d6b59c5e5..e47444085a4e 100644 --- a/boards/matek/h743/src/board_config.h +++ b/boards/matek/h743/src/board_config.h @@ -102,9 +102,7 @@ (1 << ADC_RSSI_IN_CHANNEL)) -/* Define Battery 1 Voltage Divider and A per V - */ - +/* Define Battery Voltage Divider and A per V */ #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ #define BOARD_BATTERY1_A_PER_V (40.0f) #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ diff --git a/boards/micoair/h743/bootloader.px4board b/boards/micoair/h743/bootloader.px4board new file mode 100644 index 000000000000..19b6e662be69 --- /dev/null +++ b/boards/micoair/h743/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/micoair/h743/default.px4board b/boards/micoair/h743/default.px4board new file mode 100644 index 000000000000..98d8f08aaf05 --- /dev/null +++ b/boards/micoair/h743/default.px4board @@ -0,0 +1,85 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_URT6="/dev/ttyS5" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS3" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" +CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" +CONFIG_DRIVERS_ADC_BOARD_ADC=y +CONFIG_DRIVERS_BAROMETER_DPS310=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_BOSCH_BMI270=y +CONFIG_COMMON_INS=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_DRIVERS_OSD_ATXXXX=y +CONFIG_DRIVERS_PPS_CAPTURE=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_COMMON_RC=y +CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_TAP_ESC=y +CONFIG_COMMON_TELEMETRY=y +CONFIG_DRIVERS_UAVCAN=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=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_ESC_BATTERY=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_GYRO_CALIBRATION=y +CONFIG_MODULES_GYRO_FFT=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_PAYLOAD_DELIVERER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_ROVER_POS_CONTROL=y +CONFIG_MODULES_SENSORS=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_DMESG=y +CONFIG_SYSTEMCMDS_DUMPFILE=y +CONFIG_SYSTEMCMDS_FAILURE=y +CONFIG_SYSTEMCMDS_GPIO=y +CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2CDETECT=y +CONFIG_SYSTEMCMDS_NSHTERM=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_REBOOT=y +CONFIG_SYSTEMCMDS_SD_BENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=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_USB_CONNECTED=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/micoair/h743/extras/micoair_h743_bootloader.bin b/boards/micoair/h743/extras/micoair_h743_bootloader.bin new file mode 100644 index 000000000000..ca8c51127966 Binary files /dev/null and b/boards/micoair/h743/extras/micoair_h743_bootloader.bin differ diff --git a/boards/micoair/h743/firmware.prototype b/boards/micoair/h743/firmware.prototype new file mode 100644 index 000000000000..92f78470517d --- /dev/null +++ b/boards/micoair/h743/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1166, + "magic": "PX4FWv1", + "description": "Firmware for the MicoAir743 board", + "image": "", + "build_time": 0, + "summary": "MicoAir743", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1835008, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/micoair/h743/init/rc.board_defaults b/boards/micoair/h743/init/rc.board_defaults new file mode 100644 index 000000000000..c915812e0065 --- /dev/null +++ b/boards/micoair/h743/init/rc.board_defaults @@ -0,0 +1,24 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default BAT1_A_PER_V 40 +param set-default BAT1_N_CELLS 4 +param set-default BAT1_V_CHARGED 4.2 +param set-default BAT1_V_DIV 21.2 +param set-default BAT1_V_EMPTY 3.2 + +param set-default SYS_HAS_MAG 1 +param set-default PWM_MAIN_TIM0 -4 +param set-default RC_INPUT_PROTO -1 + +param set-default IMU_GYRO_RATEMAX 2000 +param set-default SYS_AUTOSTART 4001 +param set-default MC_PITCHRATE_K 0.4 +param set-default MC_ROLLRATE_K 0.35 +param set-default MC_YAWRATE_K 1.2 +param set-default MC_YAWRATE_MAX 360 +param set-default MAV_TYPE 2 +param set-default CA_AIRFRAME 0 +param set-default CA_ROTOR_COUNT 4 +param set-default CBRK_SUPPLY_CHK 894281 diff --git a/boards/micoair/h743/init/rc.board_extras b/boards/micoair/h743/init/rc.board_extras new file mode 100644 index 000000000000..549676dd7df5 --- /dev/null +++ b/boards/micoair/h743/init/rc.board_extras @@ -0,0 +1,7 @@ +#!/bin/sh +# +# board specific extras init +#------------------------------------------------------------------------------ + +# enable onboard OSD chip +atxxxx start -s diff --git a/boards/micoair/h743/init/rc.board_sensors b/boards/micoair/h743/init/rc.board_sensors new file mode 100644 index 000000000000..96273de2da8b --- /dev/null +++ b/boards/micoair/h743/init/rc.board_sensors @@ -0,0 +1,18 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ + +board_adc start + +# # Internal SPI bus BMI088 accel/gyro +bmi088 -s -b 2 -A -R 6 start +bmi088 -s -b 2 -G -R 6 start + +# # Internal SPI bus BMI270 accel/gyro +bmi270 -s -b 2 -R 0 start + +# Internal baro +dps310 -I start -a 118 + +ist8310 -I -R 2 start diff --git a/boards/micoair/h743/nuttx-config/bootloader/defconfig b/boards/micoair/h743/nuttx-config/bootloader/defconfig new file mode 100644 index 000000000000..81e907784ecf --- /dev/null +++ b/boards/micoair/h743/nuttx-config/bootloader/defconfig @@ -0,0 +1,85 @@ +# +# 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_DEV_CONSOLE is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_SPI_EXCHANGE is not set +# CONFIG_STM32H7_SYSCFG is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +CONFIG_ARMV7M_BASEPRI_WAR=y +CONFIG_ARMV7M_ICACHE=y +CONFIG_ARMV7M_MEMCPY=y +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_BOARDCTL=y +CONFIG_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_INITTHREAD_PRIORITY=254 +CONFIG_BOARD_LATE_INITIALIZE=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x004b +CONFIG_CDCACM_PRODUCTSTR="MicoAir743" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3162 +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_SYMBOLS=y +CONFIG_DEBUG_TCBINFO=y +CONFIG_DEFAULT_SMALL=y +CONFIG_EXPERIMENTAL=y +CONFIG_FDCLONE_DISABLE=y +CONFIG_FDCLONE_STDIO=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +CONFIG_IDLETHREAD_STACKSIZE=750 +CONFIG_INIT_ENTRYPOINT="bootloader_main" +CONFIG_INIT_STACKSIZE=3194 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_PREALLOC_TIMERS=50 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SPI=y +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_TIM1=y +CONFIG_SYSTEMTICK_HOOK=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_TTY_SIGINT=y +CONFIG_TTY_SIGINT_CHAR=0x03 +CONFIG_TTY_SIGTSTP=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/micoair/h743/nuttx-config/include/board.h b/boards/micoair/h743/nuttx-config/include/board.h new file mode 100644 index 000000000000..9c30129d3889 --- /dev/null +++ b/boards/micoair/h743/nuttx-config/include/board.h @@ -0,0 +1,426 @@ +/************************************************************************************ + * nuttx-configs/px4_fmu-v6u/include/board.h + * + * Copyright (C) 2016-2019 Gregory Nutt. All rights reserved. + * Authors: David Sidrane + * + * 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. + * + ************************************************************************************/ +#ifndef __NUTTX_CONFIG_MICOAIR743_INCLUDE_BOARD_H +#define __NUTTX_CONFIG_MICOAIR743_INCLUDE_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include "board_dma_map.h" + +#include + +#ifndef __ASSEMBLY__ +# include +#endif + +#include "stm32_rcc.h" +#include "stm32_sdmmc.h" + +/************************************************************************************ + * Pre-processor Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The MicoAir743 board provides the following clock sources: + * + * X1: 8 MHz crystal for HSE + * + * So we have these clock source available within the STM32 + * + * HSI: 64 MHz RC factory-trimmed + * HSE: 8 MHz crystal for HSE + */ + +#define STM32_BOARD_XTAL 8000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL +#define STM32_LSE_FREQUENCY 32768 + +/* Main PLL Configuration. + * + * PLL source is HSE = 8,000,000 + * + * PLL_VCOx = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * Subject to: + * + * 1 <= PLLM <= 63 + * 4 <= PLLN <= 512 + * 150 MHz <= PLL_VCOL <= 420MHz + * 192 MHz <= PLL_VCOH <= 836MHz + * + * SYSCLK = PLL_VCO / PLLP + * CPUCLK = SYSCLK / D1CPRE + * Subject to + * + * PLLP1 = {2, 4, 6, 8, ..., 128} + * PLLP2,3 = {2, 3, 4, ..., 128} + * CPUCLK <= 480 MHz + */ + +#define STM32_BOARD_USEHSE + +#define STM32_PLLCFG_PLLSRC RCC_PLLCKSELR_PLLSRC_HSE + +/* PLL1, wide 4 - 8 MHz input, enable DIVP, DIVQ, DIVR + * + * PLL1_VCO = (8,000,000 / 1) * 120 = 960 MHz + * + * PLL1P = PLL1_VCO/2 = 960 MHz / 2 = 480 MHz + * PLL1Q = PLL1_VCO/4 = 960 MHz / 4 = 240 MHz + * PLL1R = PLL1_VCO/8 = 960 MHz / 8 = 120 MHz + */ + +#define STM32_PLLCFG_PLL1CFG (RCC_PLLCFGR_PLL1VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL1RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP1EN | \ + RCC_PLLCFGR_DIVQ1EN | \ + RCC_PLLCFGR_DIVR1EN) +#define STM32_PLLCFG_PLL1M RCC_PLLCKSELR_DIVM1(1) +#define STM32_PLLCFG_PLL1N RCC_PLL1DIVR_N1(120) +#define STM32_PLLCFG_PLL1P RCC_PLL1DIVR_P1(2) +#define STM32_PLLCFG_PLL1Q RCC_PLL1DIVR_Q1(4) +#define STM32_PLLCFG_PLL1R RCC_PLL1DIVR_R1(8) + +#define STM32_VCO1_FREQUENCY ((STM32_HSE_FREQUENCY / 1) * 120) +#define STM32_PLL1P_FREQUENCY (STM32_VCO1_FREQUENCY / 2) +#define STM32_PLL1Q_FREQUENCY (STM32_VCO1_FREQUENCY / 4) +#define STM32_PLL1R_FREQUENCY (STM32_VCO1_FREQUENCY / 8) + +/* PLL2 */ + +#define STM32_PLLCFG_PLL2CFG (RCC_PLLCFGR_PLL2VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL2RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVP2EN | \ + RCC_PLLCFGR_DIVQ2EN | \ + RCC_PLLCFGR_DIVR2EN) +#define STM32_PLLCFG_PLL2M RCC_PLLCKSELR_DIVM2(2) +#define STM32_PLLCFG_PLL2N RCC_PLL2DIVR_N2(48) +#define STM32_PLLCFG_PLL2P RCC_PLL2DIVR_P2(2) +#define STM32_PLLCFG_PLL2Q RCC_PLL2DIVR_Q2(2) +#define STM32_PLLCFG_PLL2R RCC_PLL2DIVR_R2(2) + +#define STM32_VCO2_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL2P_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2Q_FREQUENCY (STM32_VCO2_FREQUENCY / 2) +#define STM32_PLL2R_FREQUENCY (STM32_VCO2_FREQUENCY / 2) + +/* PLL3 */ + +#define STM32_PLLCFG_PLL3CFG (RCC_PLLCFGR_PLL3VCOSEL_WIDE | \ + RCC_PLLCFGR_PLL3RGE_4_8_MHZ | \ + RCC_PLLCFGR_DIVQ3EN) +#define STM32_PLLCFG_PLL3M RCC_PLLCKSELR_DIVM3(2) +#define STM32_PLLCFG_PLL3N RCC_PLL3DIVR_N3(48) +#define STM32_PLLCFG_PLL3P RCC_PLL3DIVR_P3(2) +#define STM32_PLLCFG_PLL3Q RCC_PLL3DIVR_Q3(4) +#define STM32_PLLCFG_PLL3R RCC_PLL3DIVR_R3(2) + +#define STM32_VCO3_FREQUENCY ((STM32_HSE_FREQUENCY / 2) * 48) +#define STM32_PLL3P_FREQUENCY (STM32_VCO3_FREQUENCY / 2) +#define STM32_PLL3Q_FREQUENCY (STM32_VCO3_FREQUENCY / 4) +#define STM32_PLL3R_FREQUENCY (STM32_VCO3_FREQUENCY / 2) + +/* SYSCLK = PLL1P = 480MHz + * CPUCLK = SYSCLK / 1 = 480 MHz + */ + +#define STM32_RCC_D1CFGR_D1CPRE (RCC_D1CFGR_D1CPRE_SYSCLK) +#define STM32_SYSCLK_FREQUENCY (STM32_PLL1P_FREQUENCY) +#define STM32_CPUCLK_FREQUENCY (STM32_SYSCLK_FREQUENCY / 1) + +/* Configure Clock Assignments */ + +/* AHB clock (HCLK) is SYSCLK/2 (240 MHz max) + * HCLK1 = HCLK2 = HCLK3 = HCLK4 = 240 + */ + +#define STM32_RCC_D1CFGR_HPRE RCC_D1CFGR_HPRE_SYSCLKd2 /* HCLK = SYSCLK / 2 */ +#define STM32_ACLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* ACLK in D1, HCLK3 in D1 */ +#define STM32_HCLK_FREQUENCY (STM32_CPUCLK_FREQUENCY / 2) /* HCLK in D2, HCLK4 in D3 */ +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE1 RCC_D2CFGR_D2PPRE1_HCLKd2 /* PCLK1 = HCLK / 2 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB2 clock (PCLK2) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D2CFGR_D2PPRE2 RCC_D2CFGR_D2PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB3 clock (PCLK3) is HCLK/2 (120 MHz) */ + +#define STM32_RCC_D1CFGR_D1PPRE RCC_D1CFGR_D1PPRE_HCLKd2 /* PCLK3 = HCLK / 2 */ +#define STM32_PCLK3_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* APB4 clock (PCLK4) is HCLK/4 (120 MHz) */ + +#define STM32_RCC_D3CFGR_D3PPRE RCC_D3CFGR_D3PPRE_HCLKd2 /* PCLK4 = HCLK / 2 */ +#define STM32_PCLK4_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timer clock frequencies */ + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM15_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM16_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM17_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Kernel Clock Configuration + * + * Note: look at Table 54 in ST Manual + */ + +/* I2C123 clock source */ + +#define STM32_RCC_D2CCIP2R_I2C123SRC RCC_D2CCIP2R_I2C123SEL_HSI + +/* I2C4 clock source */ + +#define STM32_RCC_D3CCIPR_I2C4SRC RCC_D3CCIPR_I2C4SEL_HSI + +/* SPI123 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI123SRC RCC_D2CCIP1R_SPI123SEL_PLL2 + +/* SPI45 clock source */ + +#define STM32_RCC_D2CCIP1R_SPI45SRC RCC_D2CCIP1R_SPI45SEL_PLL2 + +/* SPI6 clock source */ + +#define STM32_RCC_D3CCIPR_SPI6SRC RCC_D3CCIPR_SPI6SEL_PLL2 + +/* USB 1 and 2 clock source */ + +#define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 + +/* ADC 1 2 3 clock source */ + +#define STM32_RCC_D3CCIPR_ADCSEL RCC_D3CCIPR_ADCSEL_PLL2 + +/* FDCAN 1 clock source */ + +#define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ + +#define STM32_FDCANCLK STM32_HSE_FREQUENCY + +/* FLASH wait states + * + * ------------ ---------- ----------- + * Vcore MAX ACLK WAIT STATES + * ------------ ---------- ----------- + * 1.15-1.26 V 70 MHz 0 + * (VOS1 level) 140 MHz 1 + * 210 MHz 2 + * 1.05-1.15 V 55 MHz 0 + * (VOS2 level) 110 MHz 1 + * 165 MHz 2 + * 220 MHz 3 + * 0.95-1.05 V 45 MHz 0 + * (VOS3 level) 90 MHz 1 + * 135 MHz 2 + * 180 MHz 3 + * 225 MHz 4 + * ------------ ---------- ----------- + */ + +#define BOARD_FLASH_WAITSTATES 2 + +/* SDMMC definitions ********************************************************/ + +/* Init 480kHz, freq = PLL1Q/(2*div) div = PLL1Q/(2*freq) */ + +#define STM32_SDMMC_INIT_CLKDIV (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) + +/* 20 MHz Max for now - more reliable on some boards than 25 MHz + * 20 MHz = PLL1Q/(2*div), div = PLL1Q/(2*freq), div = 6 = 240 / 40 + */ + +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_MMCXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_MMCXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif +#if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA) +# define STM32_SDMMC_SDXFR_CLKDIV (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#else +# define STM32_SDMMC_SDXFR_CLKDIV (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT) +#endif + +#define STM32_SDMMC_CLKCR_EDGE STM32_SDMMC_CLKCR_NEGEDGE + +/* LED definitions ******************************************************************/ +/* The board has two, LED_GREEN a Green LED and LED_BLUE a Blue LED, + * that can be controlled by software. + * + * If CONFIG_ARCH_LEDS is not defined, then the user can control the LEDs in any way. + * The following definitions are used to access individual LEDs. + */ + +/* LED index values for use with board_userled() */ + +#define BOARD_LED1 0 +#define BOARD_LED2 1 +#define BOARD_LED3 2 +#define BOARD_NLEDS 3 + +#define BOARD_LED_RED BOARD_LED1 +#define BOARD_LED_GREEN BOARD_LED2 +#define BOARD_LED_BLUE BOARD_LED3 + +/* LED bits for use with board_userled_all() */ + +#define BOARD_LED1_BIT (1 << BOARD_LED1) +#define BOARD_LED2_BIT (1 << BOARD_LED2) +#define BOARD_LED3_BIT (1 << BOARD_LED3) + +/* If CONFIG_ARCH_LEDS is defined, the usage by the board port is defined in + * include/board.h and src/stm32_leds.c. The LEDs are used to encode OS-related + * events as follows: + * + * + * SYMBOL Meaning LED state + * Red Green Blue + * ---------------------- -------------------------- ------ ------ ----*/ + +#define LED_STARTED 0 /* NuttX has been started OFF OFF OFF */ +#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF ON */ +#define LED_IRQSENABLED 2 /* Interrupts enabled OFF ON OFF */ +#define LED_STACKCREATED 3 /* Idle stack created OFF ON ON */ +#define LED_INIRQ 4 /* In an interrupt N/C N/C GLOW */ +#define LED_SIGNAL 5 /* In a signal handler N/C GLOW N/C */ +#define LED_ASSERTION 6 /* An assertion failed GLOW N/C GLOW */ +#define LED_PANIC 7 /* The system has crashed Blink OFF N/C */ +#define LED_IDLE 8 /* MCU is is sleep mode ON OFF OFF */ + +/* Thus if the Green LED is statically on, NuttX has successfully booted and + * is, apparently, running normally. If the Red LED is flashing at + * approximately 2Hz, then a fatal error has been detected and the system + * has halted. + */ + +/* Alternate function pin selections ************************************************/ + +#define GPIO_USART1_RX GPIO_USART1_RX_2 /* PA10 */ +#define GPIO_USART1_TX GPIO_USART1_TX_2 /* PA9 */ + +#define GPIO_USART2_RX GPIO_USART2_RX_1 /* PA3 */ +#define GPIO_USART2_TX GPIO_USART2_TX_1 /* PA2 */ + +#define GPIO_USART3_RX GPIO_USART3_RX_3 /* PD9 */ +#define GPIO_USART3_TX GPIO_USART3_TX_3 /* PD8 */ + +#define GPIO_UART4_RX GPIO_UART4_RX_2 /* PA1 */ +#define GPIO_UART4_TX GPIO_UART4_TX_2 /* PA0 */ + +#define GPIO_USART6_RX GPIO_USART6_RX_1 /* PC7 */ +#define GPIO_USART6_TX GPIO_USART6_TX_1 /* PC6 */ + +#define GPIO_UART7_RX GPIO_UART7_RX_3 /* PE7 */ +#define GPIO_UART7_TX GPIO_UART7_TX_3 /* PE8 */ + + +#define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ +#define GPIO_UART8_TX GPIO_UART8_TX_1 /* PE1 */ + + +/* CAN + * + * CAN1 is routed to transceiver. + */ + +#define GPIO_CAN1_RX GPIO_CAN1_RX_2 /* PB8 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_2 /* PB9 */ + +/* SPI + * + + */ + +#define ADJ_SLEW_RATE(p) (((p) & ~GPIO_SPEED_MASK) | (GPIO_SPEED_2MHz)) + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_1 /* PA7 */ +#define GPIO_SPI1_SCK ADJ_SLEW_RATE(GPIO_SPI1_SCK_1) /* PA5 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_2 /* PC2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_3 /* PC3 */ +#define GPIO_SPI2_SCK ADJ_SLEW_RATE(GPIO_SPI2_SCK_5) /* PD3 */ + + + +/* I2C + * + + * + */ + +#define GPIO_I2C1_SCL GPIO_I2C1_SCL_1 /* PB6 */ +#define GPIO_I2C1_SDA GPIO_I2C1_SDA_1 /* PB7 */ + +#define GPIO_I2C1_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN6) +#define GPIO_I2C1_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN7) + +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 /* PB10 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 /* PB11 */ + +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT | GPIO_OPENDRAIN | GPIO_SPEED_50MHz | GPIO_OUTPUT_SET | GPIO_PORTB | GPIO_PIN11) + +# define PROBE_INIT(mask) +# define PROBE(n,s) +# define PROBE_MARK(n) + +#endif /*__NUTTX_CONFIG_MICOAIR743_INCLUDE_BOARD_H */ diff --git a/src/modules/ekf2/ekf2_params_volatile.c b/boards/micoair/h743/nuttx-config/include/board_dma_map.h similarity index 88% rename from src/modules/ekf2/ekf2_params_volatile.c rename to boards/micoair/h743/nuttx-config/include/board_dma_map.h index 5b947a5cf818..ba3f15eb9701 100644 --- a/src/modules/ekf2/ekf2_params_volatile.c +++ b/boards/micoair/h743/nuttx-config/include/board_dma_map.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * 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 @@ -31,13 +31,6 @@ * ****************************************************************************/ -/** - * Magnetic declination - * - * @group EKF2 - * @volatile - * @category system - * @unit deg - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); +#pragma once +#define DMAMAP_SPI2_RX DMAMAP_DMA12_SPI2RX_0 /* DMA1:39 */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* DMA1:40 */ diff --git a/boards/micoair/h743/nuttx-config/nsh/defconfig b/boards/micoair/h743/nuttx-config/nsh/defconfig new file mode 100644 index 000000000000..9e4db72b9044 --- /dev/null +++ b/boards/micoair/h743/nuttx-config/nsh/defconfig @@ -0,0 +1,245 @@ +# +# 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_DEV_CONSOLE is not set +# CONFIG_DISABLE_ENVIRON is not set +# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set +# CONFIG_DISABLE_PTHREAD 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_PRINTF 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_USLEEP is not set +CONFIG_ARCH="arm" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/micoair/h743/nuttx-config" +CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y +CONFIG_ARCH_BOARD_CUSTOM_NAME="px4" +CONFIG_ARCH_CHIP="stm32h7" +CONFIG_ARCH_CHIP_STM32H743VI=y +CONFIG_ARCH_CHIP_STM32H7=y +CONFIG_ARCH_INTERRUPTSTACK=768 +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_BOARDCTL_RESET=y +CONFIG_BOARD_ASSERT_RESET_VALUE=0 +CONFIG_BOARD_CRASHDUMP=y +CONFIG_BOARD_LOOPSPERMSEC=95150 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_BUILTIN=y +CONFIG_CDCACM=y +CONFIG_CDCACM_PRODUCTID=0x0036 +CONFIG_CDCACM_PRODUCTSTR="MicoAir743" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x1B8C +CONFIG_CDCACM_VENDORSTR="MicoAir" +CONFIG_DEBUG_FULLOPT=y +CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=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_EXPERIMENTAL=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_LFN_ALIAS_HASH=y +CONFIG_FDCLONE_STDIO=y +CONFIG_FS_BINFS=y +CONFIG_FS_CROMFS=y +CONFIG_FS_FAT=y +CONFIG_FS_FATTIME=y +CONFIG_FS_PROCFS=y +CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y +CONFIG_FS_PROCFS_MAX_TASKS=64 +CONFIG_FS_PROCFS_REGISTER=y +CONFIG_FS_ROMFS=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=3194 +CONFIG_IOB_NBUFFERS=24 +CONFIG_IOB_NCHAINS=24 +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIBC_LONG_LONG=y +CONFIG_LIBC_MAX_EXITFUNS=1 +CONFIG_LIBC_STRERROR=y +CONFIG_MEMSET_64BIT=y +CONFIG_MEMSET_OPTSPEED=y +CONFIG_MMCSD=y +CONFIG_MMCSD_SDIO=y +CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y +CONFIG_MM_IOB=y +CONFIG_MM_REGIONS=4 +CONFIG_NAME_MAX=40 +CONFIG_NSH_ARCHINIT=y +CONFIG_NSH_ARGCAT=y +CONFIG_NSH_BUILTIN_APPS=y +CONFIG_NSH_CMDPARMS=y +CONFIG_NSH_CROMFSETC=y +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_PREALLOC_TIMERS=50 +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_PTHREAD_MUTEX_ROBUST=y +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_RAM_SIZE=245760 +CONFIG_RAM_START=0x20010000 +CONFIG_RAW_BINARY=y +CONFIG_RTC_DATETIME=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_HPWORKPRIORITY=249 +CONFIG_SCHED_HPWORKSTACKSIZE=1280 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=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_PULLUP=y +CONFIG_SEM_PREALLOCHOLDERS=32 +CONFIG_SERIAL_TERMIOS=y +CONFIG_SIG_DEFAULT=y +CONFIG_SIG_SIGALRM_ACTION=y +CONFIG_SIG_SIGUSR1_ACTION=y +CONFIG_SIG_SIGUSR2_ACTION=y +CONFIG_SIG_SIGWORK=4 +CONFIG_STACK_COLORATION=y +CONFIG_START_DAY=30 +CONFIG_START_MONTH=11 +CONFIG_STDIO_BUFFER_SIZE=256 +CONFIG_STM32H7_ADC1=y +CONFIG_STM32H7_ADC3=y +CONFIG_STM32H7_BBSRAM=y +CONFIG_STM32H7_BBSRAM_FILES=5 +CONFIG_STM32H7_BDMA=y +CONFIG_STM32H7_BKPSRAM=y +CONFIG_STM32H7_DMA1=y +CONFIG_STM32H7_DMA2=y +CONFIG_STM32H7_DMACAPABLE=y +CONFIG_STM32H7_FLASH_OVERRIDE_I=y +CONFIG_STM32H7_I2C1=y +CONFIG_STM32H7_I2C2=y +CONFIG_STM32H7_I2C_DYNTIMEO=y +CONFIG_STM32H7_I2C_DYNTIMEO_STARTSTOP=10 +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_RTC=y +CONFIG_STM32H7_RTC_HSECLOCK=y +CONFIG_STM32H7_RTC_MAGIC_REG=1 +CONFIG_STM32H7_SAVE_CRASHDUMP=y +CONFIG_STM32H7_SDMMC1=y +CONFIG_STM32H7_SERIALBRK_BSDCOMPAT=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_SPI1=y +CONFIG_STM32H7_SPI2=y +CONFIG_STM32H7_SPI2_DMA=y +CONFIG_STM32H7_SPI2_DMA_BUFFER=4096 +CONFIG_STM32H7_SPI_DMATHRESHOLD=8 +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_TIM8=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y +CONFIG_STM32H7_USART6=y +CONFIG_STM32H7_USART_BREAKS=y +CONFIG_STM32H7_USART_INVERT=y +CONFIG_STM32H7_USART_SINGLEWIRE=y +CONFIG_STM32H7_USART_SWAP=y +CONFIG_SYSTEM_CDCACM=y +CONFIG_SYSTEM_NSH=y +CONFIG_TASK_NAME_SIZE=24 +CONFIG_UART4_BAUD=57600 +CONFIG_UART4_RXBUFSIZE=600 +CONFIG_UART4_TXBUFSIZE=1500 +CONFIG_UART7_BAUD=57600 +CONFIG_UART7_RXBUFSIZE=600 +CONFIG_UART7_TXBUFSIZE=1500 +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_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=1500 +CONFIG_USART3_BAUD=57600 +CONFIG_USART3_RXBUFSIZE=600 +CONFIG_USART3_TXBUFSIZE=1500 +CONFIG_USART6_BAUD=57600 +CONFIG_USART6_RXBUFSIZE=600 +CONFIG_USART6_TXBUFSIZE=1500 +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 +CONFIG_WATCHDOG=y diff --git a/boards/micoair/h743/nuttx-config/scripts/bootloader_script.ld b/boards/micoair/h743/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 000000000000..511ef2624248 --- /dev/null +++ b/boards/micoair/h743/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,213 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2016, 2019 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The Durandal-v1 uses an STM32H743II has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * The Durandal has a Swtich on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the swiutch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + flash (rx) : ORIGIN = 0x08000000, LENGTH = 2048K + dtcm1 (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + dtcm2 (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + sram (rwx) : ORIGIN = 0x24000000, LENGTH = 512K + sram1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K + sram2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K + sram3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K + sram4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K + bbram (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* 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/micoair/h743/nuttx-config/scripts/script.ld b/boards/micoair/h743/nuttx-config/scripts/script.ld new file mode 100644 index 000000000000..1dc1a0ef97eb --- /dev/null +++ b/boards/micoair/h743/nuttx-config/scripts/script.ld @@ -0,0 +1,228 @@ +/**************************************************************************** + * scripts/script.ld + * + * Copyright (C) 2020 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * + * 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. + * + ****************************************************************************/ + +/* The board uses an STM32H743II and has 2048Kb of main FLASH memory. + * The flash memory is partitioned into a User Flash memory and a System + * Flash memory. Each of these memories has two banks: + * + * 1) User Flash memory: + * + * Bank 1: Start address 0x0800:0000 to 0x080F:FFFF with 8 sectors, 128Kb each + * Bank 2: Start address 0x0810:0000 to 0x081F:FFFF with 8 sectors, 128Kb each + * + * 2) System Flash memory: + * + * Bank 1: Start address 0x1FF0:0000 to 0x1FF1:FFFF with 1 x 128Kb sector + * Bank 1: Start address 0x1FF4:0000 to 0x1FF5:FFFF with 1 x 128Kb sector + * + * 3) User option bytes for user configuration, only in Bank 1. + * + * In the STM32H743II, two different boot spaces can be selected through + * the BOOT pin and the boot base address programmed in the BOOT_ADD0 and + * BOOT_ADD1 option bytes: + * + * 1) BOOT=0: Boot address defined by user option byte BOOT_ADD0[15:0]. + * ST programmed value: Flash memory at 0x0800:0000 + * 2) BOOT=1: Boot address defined by user option byte BOOT_ADD1[15:0]. + * ST programmed value: System bootloader at 0x1FF0:0000 + * + * There's a switch on board, the BOOT0 pin is at ground so by default, + * the STM32 will boot to address 0x0800:0000 in FLASH unless the switch is + * drepresed, then the boot will be from 0x1FF0:0000 + * + * The STM32H743ZI also has 1024Kb of data SRAM. + * SRAM is split up into several blocks and into three power domains: + * + * 1) TCM SRAMs are dedicated to the Cortex-M7 and are accessible with + * 0 wait states by the Cortex-M7 and by MDMA through AHBS slave bus + * + * 1.1) 128Kb of DTCM-RAM beginning at address 0x2000:0000 + * + * The DTCM-RAM is organized as 2 x 64Kb DTCM-RAMs on 2 x 32 bit + * DTCM ports. The DTCM-RAM could be used for critical real-time + * data, such as interrupt service routines or stack / heap memory. + * Both DTCM-RAMs can be used in parallel (for load/store operations) + * thanks to the Cortex-M7 dual issue capability. + * + * 1.2) 64Kb of ITCM-RAM beginning at address 0x0000:0000 + * + * This RAM is connected to ITCM 64-bit interface designed for + * execution of critical real-times routines by the CPU. + * + * 2) AXI SRAM (D1 domain) accessible by all system masters except BDMA + * through D1 domain AXI bus matrix + * + * 2.1) 512Kb of SRAM beginning at address 0x2400:0000 + * + * 3) AHB SRAM (D2 domain) accessible by all system masters except BDMA + * through D2 domain AHB bus matrix + * + * 3.1) 128Kb of SRAM1 beginning at address 0x3000:0000 + * 3.2) 128Kb of SRAM2 beginning at address 0x3002:0000 + * 3.3) 32Kb of SRAM3 beginning at address 0x3004:0000 + * + * SRAM1 - SRAM3 are one contiguous block: 288Kb at address 0x3000:0000 + * + * 4) AHB SRAM (D3 domain) accessible by most of system masters + * through D3 domain AHB bus matrix + * + * 4.1) 64Kb of SRAM4 beginning at address 0x3800:0000 + * 4.1) 4Kb of backup RAM beginning at address 0x3880:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + */ + +MEMORY +{ + ITCM_RAM (rwx) : ORIGIN = 0x00000000, LENGTH = 64K + FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 1792K + + DTCM1_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 64K + DTCM2_RAM (rwx) : ORIGIN = 0x20010000, LENGTH = 64K + AXI_SRAM (rwx) : ORIGIN = 0x24000000, LENGTH = 512K /* D1 domain AXI bus */ + SRAM1 (rwx) : ORIGIN = 0x30000000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM2 (rwx) : ORIGIN = 0x30020000, LENGTH = 128K /* D2 domain AHB bus */ + SRAM3 (rwx) : ORIGIN = 0x30040000, LENGTH = 32K /* D2 domain AHB bus */ + SRAM4 (rwx) : ORIGIN = 0x38000000, LENGTH = 64K /* D3 domain */ + BKPRAM (rwx) : ORIGIN = 0x38800000, LENGTH = 4K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +ENTRY(_stext) + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) +EXTERN(_bootdelay_signature) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + } > FLASH + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > FLASH + + + .ARM.extab : { + *(.ARM.extab*) + } > FLASH + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > FLASH + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + + /* Pad out last section as the STM32H7 Flash write size is 256 bits. 32 bytes */ + . = ALIGN(16); + FILL(0xffff) + . += 16; + } > AXI_SRAM AT > FLASH = 0xffff + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > AXI_SRAM + + /* Emit the the D3 power domain section for locating BDMA data */ + + .sram4_reserve (NOLOAD) : + { + *(.sram4) + . = ALIGN(4); + _sram4_heap_start = ABSOLUTE(.); + } > SRAM4 + + /* 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/micoair/h743/src/CMakeLists.txt b/boards/micoair/h743/src/CMakeLists.txt new file mode 100644 index 000000000000..c47215375d08 --- /dev/null +++ b/boards/micoair/h743/src/CMakeLists.txt @@ -0,0 +1,68 @@ +############################################################################ +# +# 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. +# +############################################################################ +if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") + add_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch + nuttx_drivers + bootloader + ) + target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common) + +else() + add_library(drivers_board + i2c.cpp + init.c + led.c + sdio.c + spi.cpp + timer_config.cpp + usb.c + ) + add_dependencies(drivers_board arch_board_hw_info) + + target_link_libraries(drivers_board + PRIVATE + arch_io_pins + arch_spi + arch_board_hw_info + drivers__led + nuttx_arch + nuttx_drivers + px4_layer + ) +endif() diff --git a/boards/micoair/h743/src/board_config.h b/boards/micoair/h743/src/board_config.h new file mode 100644 index 000000000000..48ff3ff203ee --- /dev/null +++ b/boards/micoair/h743/src/board_config.h @@ -0,0 +1,197 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +// #define FLASH_BASED_PARAMS + + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +# define GPIO_nLED_RED /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN5) +# define GPIO_nLED_GREEN /* PE6 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN6) +# define GPIO_nLED_BLUE /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +# define BOARD_HAS_CONTROL_STATUS_LEDS 1 +# define BOARD_OVERLOAD_LED LED_RED +# define BOARD_ARMED_STATE_LED LED_BLUE + + +/* + * ADC channels + * + * These are the channel numbers of the ADCs of the microcontroller that + * can be used by the Px4 Firmware in the adc driver + */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ +#define SYSTEM_ADC_BASE STM32_ADC1_BASE +#define ADC1_CH(n) (n) + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ +#define PX4_ADC_GPIO \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11 + + +/* Define Channel numbers must match above GPIO pin IN(n)*/ +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PC0 */ ADC1_CH(10) +#define ADC_BATTERY_CURRENT_CHANNEL /* PC1 */ ADC1_CH(11) + + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL)) + + +/* Define Battery 1 Voltage Divider and A per V + */ + +// #define BOARD_BATTERY1_V_DIV (11.0f) /* measured with the provided PM board */ +// #define BOARD_BATTERY1_A_PER_V (40.0f) +// #define BOARD_BATTERY2_V_DIV (11.0f) /* measured with the provided PM board */ + + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 10 +#define DIRECT_INPUT_TIMER_CHANNELS 10 + +#define BOARD_HAS_PWM DIRECT_PWM_OUTPUT_CHANNELS + + +/* USB OTG FS + * + * PA8 OTG_FS_VBUS VBUS sensing + */ + +#define GPIO_OTGFS_VBUS /* PA8 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_SPEED_100MHz|GPIO_PORTA|GPIO_PIN8) + + +/* High-resolution timer */ +#define HRT_TIMER 2 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel 3 */ + + +/* RC Serial port */ +#define RC_SERIAL_PORT "/dev/ttyS4" +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT + +/* SD Card */ +#define SDIO_SLOTNO 0 /* Only one slot */ +#define SDIO_MINOR 0 + +/* This board provides a DMA pool and APIs */ +#define BOARD_DMA_ALLOC_POOL_SIZE 5120 + +/* This board provides the board_on_reset interface */ +#define BOARD_HAS_ON_RESET 1 + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +#define FLASH_BASED_PARAMS + +#define BOARD_NUM_IO_TIMERS 4 + + +__BEGIN_DECLS + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void); + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +extern void stm32_usbinitialize(void); + +extern void board_peripheral_reset(int ms); + +#include + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/boards/micoair/h743/src/bootloader_main.c b/boards/micoair/h743/src/bootloader_main.c new file mode 100644 index 000000000000..5670308a29d8 --- /dev/null +++ b/boards/micoair/h743/src/bootloader_main.c @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * 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 bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ + +#include "board_config.h" +#include "bl.h" + +#include +#include +#include +#include +#include +#include "arm_internal.h" +#include + +extern int sercon_main(int c, char **argv); + +__EXPORT void board_on_reset(int status) {} + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure USB interfaces */ + stm32_usbinitialize(); +} + +__EXPORT int board_app_initialize(uintptr_t arg) +{ + return 0; +} + +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} + +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/micoair/h743/src/hw_config.h b/boards/micoair/h743/src/hw_config.h new file mode 100644 index 000000000000..a428dd53592d --- /dev/null +++ b/boards/micoair/h743/src/hw_config.h @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +/**************************************************************************** + * 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 0x08020000 +#define BOOTLOADER_DELAY 5000 +#define INTERFACE_USB 1 +#define INTERFACE_USB_CONFIG "/dev/ttyACM0" +#define BOARD_VBUS MK_GPIO_INPUT(GPIO_OTGFS_VBUS) + +//#define USE_VBUS_PULL_DOWN +#define INTERFACE_USART 1 +#define INTERFACE_USART_CONFIG "/dev/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1166 +#define BOARD_FLASH_SECTORS (14) +#define BOARD_FLASH_SIZE (16 * 128 * 1024) +#define APP_RESERVATION_SIZE (1 * 128 * 1024) + +#define OSC_FREQ 8 + +#define BOARD_PIN_LED_ACTIVITY GPIO_nLED_BLUE // BLUE +#define BOARD_PIN_LED_BOOTLOADER GPIO_nLED_GREEN // GREEN +#define BOARD_LED_ON 0 +#define BOARD_LED_OFF 1 + +#define SERIAL_BREAK_DETECT_DISABLED 1 + +#if !defined(ARCH_SN_MAX_LENGTH) +# define ARCH_SN_MAX_LENGTH 12 +#endif + +#if !defined(APP_RESERVATION_SIZE) +# define APP_RESERVATION_SIZE 0 +#endif + +#if !defined(BOARD_FIRST_FLASH_SECTOR_TO_ERASE) +# define BOARD_FIRST_FLASH_SECTOR_TO_ERASE 1 +#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 diff --git a/boards/micoair/h743/src/i2c.cpp b/boards/micoair/h743/src/i2c.cpp new file mode 100644 index 000000000000..1444ea117204 --- /dev/null +++ b/boards/micoair/h743/src/i2c.cpp @@ -0,0 +1,39 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { + initI2CBusExternal(1), + initI2CBusInternal(2), +}; diff --git a/boards/micoair/h743/src/init.c b/boards/micoair/h743/src/init.c new file mode 100644 index 000000000000..43f86f902d24 --- /dev/null +++ b/boards/micoair/h743/src/init.c @@ -0,0 +1,203 @@ +/**************************************************************************** + * + * 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 init.c + * + * FMU-specific early startup code. This file implements the + * board_app_initialize() 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. + */ + +#include "board_config.h" + +#include + +#include +#include +#include +#include +#include +#include "arm_internal.h" + +#include +#include +#include +#include +#include +#include +#include + +# if defined(FLASH_BASED_PARAMS) +# include +#endif + +__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) +{ + UNUSED(ms); +} + +/************************************************************************************ + * 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) +{ + for (int i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + px4_arch_configgpio(PX4_MAKE_GPIO_INPUT(io_timer_channel_get_as_pwm_input(i))); + } + + if (status >= 0) { + up_mdelay(6); + } +} + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. 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 stm32_boardinitialize(void) +{ + /* Reset PWM first thing */ + board_on_reset(-1); + + /* configure LEDs */ + board_autoled_initialize(); + + /* configure pins */ + const uint32_t gpio[] = PX4_GPIO_INIT_LIST; + px4_gpio_init(gpio, arraySize(gpio)); + + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure USB interfaces */ + stm32_usbinitialize(); + +} + +/**************************************************************************** + * 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; + * + * 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) +{ + /* Need hrt running before using the ADC */ + px4_platform_init(); + + /* configure the DMA allocator */ + if (board_dma_alloc_init() < 0) { + syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); + } + + /* initial LED state */ + drv_led_start(); + led_off(LED_RED); + led_off(LED_BLUE); + + if (board_hardfault_init(2, true) != 0) { + led_on(LED_BLUE); + } + +#ifdef CONFIG_MMCSD + int ret = stm32_sdio_initialize(); + + if (ret != OK) { + led_on(LED_BLUE); + return ret; + } + +#endif + +// TODO：internal flash store parameters +#if defined(FLASH_BASED_PARAMS) + static sector_descriptor_t params_sector_map[] = { + {15, 128 * 1024, 0x081E0000}, + {0, 0, 0}, + }; + + /* Initialize the flashfs layer to use heap allocated memory */ + int result = parameter_flashfs_init(params_sector_map, NULL, 0); + + if (result != OK) { + syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result); + led_on(LED_RED); + } + +#endif + + /* Configure the HW based on the manifest */ + px4_platform_configure(); + + return OK; +} diff --git a/boards/micoair/h743/src/led.c b/boards/micoair/h743/src/led.c new file mode 100644 index 000000000000..d7794392dbb3 --- /dev/null +++ b/boards/micoair/h743/src/led.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 led.c + * + * LED backend. + */ + +#include + +#include + +#include "chip.h" +#include "stm32_gpio.h" +#include "board_config.h" + +#include +#include + +/* + * 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); +extern void led_toggle(int led); +__END_DECLS + +# define xlat(p) (p) +static uint32_t g_ledmap[] = { + GPIO_nLED_GREEN, // Indexed by BOARD_LED_GREEN + GPIO_nLED_BLUE, // Indexed by BOARD_LED_BLUE + GPIO_nLED_RED, // Indexed by BOARD_LED_RED +}; + +__EXPORT void led_init(void) +{ + /* Configure LED GPIOs for output */ + for (size_t l = 0; l < (sizeof(g_ledmap) / sizeof(g_ledmap[0])); l++) { + if (g_ledmap[l] != 0) { + stm32_configgpio(g_ledmap[l]); + } + } +} + +static void phy_set_led(int led, bool state) +{ + /* Drive Low to switch on */ + if (g_ledmap[led] != 0) { + stm32_gpiowrite(g_ledmap[led], !state); + } +} + +static bool phy_get_led(int led) +{ + /* If Low it is on */ + if (g_ledmap[led] != 0) { + return !stm32_gpioread(g_ledmap[led]); + } + + return false; +} + + +__EXPORT void led_on(int led) +{ + phy_set_led(xlat(led), true); +} + +__EXPORT void led_off(int led) +{ + phy_set_led(xlat(led), false); +} + +__EXPORT void led_toggle(int led) +{ + phy_set_led(xlat(led), !phy_get_led(xlat(led))); +} diff --git a/boards/micoair/h743/src/sdio.c b/boards/micoair/h743/src/sdio.c new file mode 100644 index 000000000000..869d757756a0 --- /dev/null +++ b/boards/micoair/h743/src/sdio.c @@ -0,0 +1,177 @@ +/**************************************************************************** + * + * Copyright (C) 2014, 2016 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 + +#include +#include +#include +#include + +#include +#include + +#include "chip.h" +#include "board_config.h" +#include "stm32_gpio.h" +#include "stm32_sdmmc.h" + +#ifdef CONFIG_MMCSD + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/* Card detections requires card support and a card detection GPIO */ + +#define HAVE_NCD 1 +#if !defined(GPIO_SDMMC1_NCD) +# undef HAVE_NCD +#endif + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static FAR struct sdio_dev_s *sdio_dev; +#ifdef HAVE_NCD +static bool g_sd_inserted = 0xff; /* Impossible value */ +#endif + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_ncd_interrupt + * + * Description: + * Card detect interrupt handler. + * + ****************************************************************************/ + +#ifdef HAVE_NCD +static int stm32_ncd_interrupt(int irq, FAR void *context) +{ + bool present; + + present = !stm32_gpioread(GPIO_SDMMC1_NCD); + + if (sdio_dev && present != g_sd_inserted) { + sdio_mediachange(sdio_dev, present); + g_sd_inserted = present; + } + + return OK; +} +#endif + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: stm32_sdio_initialize + * + * Description: + * Initialize SDIO-based MMC/SD card support + * + ****************************************************************************/ + +int stm32_sdio_initialize(void) +{ + int ret; + +#ifdef HAVE_NCD + /* Card detect */ + + bool cd_status; + + /* Configure the card detect GPIO */ + + stm32_configgpio(GPIO_SDMMC1_NCD); + + /* Register an interrupt handler for the card detect pin */ + + stm32_gpiosetevent(GPIO_SDMMC1_NCD, true, true, true, stm32_ncd_interrupt); +#endif + + /* Mount the SDIO-based MMC/SD block driver */ + /* First, get an instance of the SDIO interface */ + + finfo("Initializing SDIO slot %d\n", SDIO_SLOTNO); + + sdio_dev = sdio_initialize(SDIO_SLOTNO); + + if (!sdio_dev) { + syslog(LOG_ERR, "[boot] 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, sdio_dev); + + if (ret != OK) { + syslog(LOG_ERR, "[boot] Failed to bind SDIO to the MMC/SD driver: %d\n", ret); + return ret; + } + + finfo("Successfully bound SDIO to the MMC/SD driver\n"); + +#ifdef HAVE_NCD + /* Use SD card detect pin to check if a card is g_sd_inserted */ + + cd_status = !stm32_gpioread(GPIO_SDMMC1_NCD); + finfo("Card detect : %d\n", cd_status); + + sdio_mediachange(sdio_dev, cd_status); +#else + /* Assume that the SD card is inserted. What choice do we have? */ + + sdio_mediachange(sdio_dev, true); +#endif + + return OK; +} + +#endif /* CONFIG_MMCSD */ diff --git a/boards/micoair/h743/src/spi.cpp b/boards/micoair/h743/src/spi.cpp new file mode 100644 index 000000000000..81fdd03f282d --- /dev/null +++ b/boards/micoair/h743/src/spi.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), + }), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin5}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortD, GPIO::Pin4}), + initSPIDevice(DRV_IMU_DEVTYPE_BMI270, SPI::CS{GPIO::PortA, GPIO::Pin15}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/micoair/h743/src/timer_config.cpp b/boards/micoair/h743/src/timer_config.cpp new file mode 100644 index 000000000000..515a56928cad --- /dev/null +++ b/boards/micoair/h743/src/timer_config.cpp @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, DMA{DMA::Index1}), + initIOTimer(Timer::Timer3, DMA{DMA::Index1}), + initIOTimer(Timer::Timer4, DMA{DMA::Index1}), +}; + +constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel4}, {GPIO::PortE, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel3}, {GPIO::PortE, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel2}, {GPIO::PortE, GPIO::Pin11}), + initIOTimerChannel(io_timers, {Timer::Timer1, Timer::Channel1}, {GPIO::PortE, GPIO::Pin9}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel4}, {GPIO::PortB, GPIO::Pin1}), + initIOTimerChannel(io_timers, {Timer::Timer3, Timer::Channel3}, {GPIO::PortB, GPIO::Pin0}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}), + initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/micoair/h743/src/usb.c b/boards/micoair/h743/src/usb.c new file mode 100644 index 000000000000..9591784866a6 --- /dev/null +++ b/boards/micoair/h743/src/usb.c @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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 usb.c + * + * Board-specific USB functions. + */ + +#include "board_config.h" +#include +#include +#include +#include + +/************************************************************************************ + * Name: stm32_usbinitialize + * + * Description: + * Called to setup USB-related GPIO pins for the board. + * + ************************************************************************************/ + +__EXPORT void stm32_usbinitialize(void) +{ + /* The OTG FS has an internal soft pull-up */ + + /* Configure the OTG FS VBUS sensing GPIO, Power On, and Overcurrent GPIOs */ + +#ifdef CONFIG_STM32H7_OTGFS + stm32_configgpio(GPIO_OTGFS_VBUS); +#endif +} + +/************************************************************************************ + * Name: stm32_usbsuspend + * + * Description: + * Board logic must provide the stm32_usbsuspend logic if the USBDEV driver is + * used. This function is called whenever the USB enters or leaves suspend mode. + * This is an opportunity for the board logic to shutdown clocks, power, etc. + * while the USB is suspended. + * + ************************************************************************************/ +__EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) +{ + uinfo("resume: %d\n", resume); +} diff --git a/boards/modalai/fc-v1/default.px4board b/boards/modalai/fc-v1/default.px4board index 2a33d1ddb875..e9104cfaeec1 100644 --- a/boards/modalai/fc-v1/default.px4board +++ b/boards/modalai/fc-v1/default.px4board @@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index be01cfa93415..f95c859e8c52 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -24,6 +25,7 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_RC_CRSF_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_COMMON_TELEMETRY=y CONFIG_MODULES_AIRSPEED_SELECTOR=y @@ -60,7 +62,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y -CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y diff --git a/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin b/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin index c80dba5421af..95b62a8d68f9 100755 Binary files a/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin and b/boards/modalai/fc-v2/extras/modalai_fc-v2_bootloader.bin differ diff --git a/boards/modalai/fc-v2/scripts/run_docker.sh b/boards/modalai/fc-v2/scripts/run_docker.sh new file mode 100755 index 000000000000..4e42146ca6cb --- /dev/null +++ b/boards/modalai/fc-v2/scripts/run_docker.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +# Run this from the px4 project top level directory +docker run -it --rm --privileged -v `pwd`:/usr/local/workspace px4io/px4-dev-nuttx-focal:2022-08-12 diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 73e75dd7bcb0..db58d4e28a55 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -4,6 +4,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L0X=y CONFIG_DRIVERS_DISTANCE_SENSOR_VL53L1X=y +CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y @@ -11,6 +12,7 @@ CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_QSHELL_QURT=y +CONFIG_DRIVERS_RC_CRSF_RC=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_EKF2=y @@ -27,7 +29,7 @@ CONFIG_MODULES_MUORB_SLPI=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_PWM_OUT_SIM=y -CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y CONFIG_PARAM_REMOTE=y diff --git a/boards/modalai/voxl2-slpi/src/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/CMakeLists.txt index bb35ef068a33..21a1e6090e3e 100644 --- a/boards/modalai/voxl2-slpi/src/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/CMakeLists.txt @@ -46,8 +46,8 @@ add_library(drivers_board # Add custom drivers for SLPI add_subdirectory(${PX4_BOARD_DIR}/src/drivers/rc_controller) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/mavlink_rc_in) -# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) -# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/spektrum_rc) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/ghst_rc) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_hitl) -# add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus) +add_subdirectory(${PX4_BOARD_DIR}/src/drivers/dsp_sbus) add_subdirectory(${PX4_BOARD_DIR}/src/drivers/elrs_led) diff --git a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp index b26f56508216..380ebc04d004 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/dsp_hitl/dsp_hitl.cpp @@ -1148,7 +1148,6 @@ handle_message_hil_sensor_dsp(mavlink_message_t *msg) hil_battery_status.timestamp = gyro_accel_time; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp index a40bd8eda219..93ad304a9719 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.cpp @@ -125,7 +125,7 @@ void elrs_led_task() orb_copy(ORB_ID(manual_control_input), manual_control_input_fd, &setpoint_req); - _cmd = (ControllerInput)setpoint_req.aux1; + _cmd = (ControllerInput)setpoint_req.buttons; // skip duplicate cmds if (_cmd == _prev_cmd) { @@ -314,4 +314,4 @@ int elrs_led_main(int argc, char *argv[]) } return 0; -} \ No newline at end of file +} diff --git a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h index 25de041d4b8a..1c71e5e81b0e 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h +++ b/boards/modalai/voxl2-slpi/src/drivers/elrs_led/elrs_led.h @@ -108,6 +108,3 @@ ControllerInput getKey(const std::map &map, const return ControllerInput::DEFAULT; } - - - diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp index 004fa020fb43..c9fe697e62cf 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/ghst_rc.hpp @@ -123,4 +123,3 @@ class GhstRc : public ModuleBase, public ModuleParams, public px4::Sched // (ParamBool) _param_rc_ghst_tel_en // ) }; - diff --git a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml index 21be68b97ebe..8f3998c2b133 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml +++ b/boards/modalai/voxl2-slpi/src/drivers/ghst_rc/module.yaml @@ -8,4 +8,3 @@ serial_config: #depends_on_port: RC description_extended: | Ghost RC (GHST) driver. - diff --git a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt index 6d8087695f0a..05097e59f28c 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt +++ b/boards/modalai/voxl2-slpi/src/drivers/spektrum_rc/CMakeLists.txt @@ -41,4 +41,3 @@ px4_add_module( DEPENDS rc ) - 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 0cf350edeb78..18067b1756eb 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 @@ -110,16 +110,18 @@ void task_main(int argc, char *argv[]) } } - int uart_fd = dsm_init(device_path); + int uart_fd = qurt_uart_open(device_path, 115200); if (uart_fd < 0) { - PX4_ERR("dsm init failed"); + PX4_ERR("uart open failed"); return; } else if (verbose) { - PX4_INFO("Spektrum RC: dsm_init succeeded"); + PX4_INFO("Spektrum RC: uart open succeeded"); } + dsm_proto_init(); + orb_advert_t rc_pub = nullptr; // Use a buffer size of the double of the minimum, just to be safe. diff --git a/boards/modalai/voxl2/README.md b/boards/modalai/voxl2/README.md index 02d7f6fc5ab4..ac6bd1f5f15c 100644 --- a/boards/modalai/voxl2/README.md +++ b/boards/modalai/voxl2/README.md @@ -9,7 +9,7 @@ When running PX4 directly on the QRB5165 SoC it runs partially on the Sensor Low The portion running on the DSP hosts the flight critical portions of PX4 such as the IMU, barometer, magnetometer, GPS, ESC, and power management drivers, and the state estimation. The DSP acts as the real time portion of the system. Non flight -critical applications such as Mavlink, logging, and commander are running on the +critical applications such as Mavlink, and logging are running on the ARM CPU cluster (aka apps proc). The DSP and ARM CPU cluster communicate via a Qualcomm proprietary shared memory interface. @@ -27,6 +27,7 @@ The full instructions are available here: ``` px4$ boards/modalai/voxl2/scripts/run-docker.sh root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh +root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-deps.sh root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh root@9373fa1401b8:/usr/local/workspace# exit @@ -69,16 +70,15 @@ pxh> ## Notes You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have -to power cycle the board and restart everything. +to power cycle the board and restart everything. Starting with SDK 1.3.0 it is possible +to cleanly shutdown PX4 on VOXL 2. ## Tips -Start with a VOXL 2 that only has the system image installed, not the SDK - -Run the command ```voxl-px4 -s``` on target to run the self-test +Always use the latest SDK release In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK -can be used: +can be used (Most messages are passed to the apps proc but certain low level messages are not): ``` modalai@modalai-XPS-15-9570:/local/mnt/workspace/Qualcomm/Hexagon_SDK/4.1.0.4/tools/debug/mini-dm/Ubuntu18$ sudo ./mini-dm [sudo] password for modalai: diff --git a/boards/modalai/voxl2/cmake/init.cmake b/boards/modalai/voxl2/cmake/init.cmake new file mode 100644 index 000000000000..357d1d45ec1d --- /dev/null +++ b/boards/modalai/voxl2/cmake/init.cmake @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2024 ModalAI, Inc. 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_directories(${PX4_BOARD_DIR}/libfc-sensor-api/inc) diff --git a/boards/modalai/voxl2/cmake/link_libraries.cmake b/boards/modalai/voxl2/cmake/link_libraries.cmake index f250f52e211b..e0b09beb7038 100644 --- a/boards/modalai/voxl2/cmake/link_libraries.cmake +++ b/boards/modalai/voxl2/cmake/link_libraries.cmake @@ -1,7 +1,7 @@ # Link against the public stub version of the proprietary fc sensor library target_link_libraries(px4 PRIVATE - ${PX4_SOURCE_DIR}/src//modules/muorb/apps/libfc-sensor-api/build/libfc_sensor.so + ${PX4_BOARD_DIR}/libfc-sensor-api/build/libfc_sensor.so px4_layer ${module_libraries} ) diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index 8ac31695723f..20860e3fce47 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -6,6 +6,7 @@ CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_QSHELL_POSIX=y +CONFIG_DRIVERS_RC_CRSF_RC=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_VOXL2_IO=y CONFIG_MODULES_COMMANDER=y diff --git a/src/modules/muorb/apps/libfc-sensor-api b/boards/modalai/voxl2/libfc-sensor-api similarity index 100% rename from src/modules/muorb/apps/libfc-sensor-api rename to boards/modalai/voxl2/libfc-sensor-api diff --git a/boards/modalai/voxl2/scripts/build-deps.sh b/boards/modalai/voxl2/scripts/build-deps.sh index b76cd0d7fa79..fe7f85486888 100755 --- a/boards/modalai/voxl2/scripts/build-deps.sh +++ b/boards/modalai/voxl2/scripts/build-deps.sh @@ -1,10 +1,9 @@ #!/bin/bash -cd src/modules/muorb/apps/libfc-sensor-api +cd boards/modalai/voxl2/libfc-sensor-api rm -fR build mkdir build cd build CC=/home/4.1.0.4/tools/linaro64/bin/aarch64-linux-gnu-gcc cmake .. make -cd ../../../../../.. - +cd ../../../../.. diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index 6bf21f1e564c..a908b0a83576 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -20,7 +20,6 @@ adb shell chmod a+x /usr/bin/voxl-px4-hitl-start # Push configuration file adb shell mkdir -p /etc/modalai -adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai @@ -128,6 +127,7 @@ adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uart_esc_driver" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server" adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-apps_sbus" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-voxl_save_cal_params" # Make sure any required directories exist adb shell "/bin/mkdir -p /data/px4/param" diff --git a/boards/modalai/voxl2/scripts/patch-gazebo.sh b/boards/modalai/voxl2/scripts/patch-gazebo.sh index 9e8b22cf60d9..bb31b99b1241 100755 --- a/boards/modalai/voxl2/scripts/patch-gazebo.sh +++ b/boards/modalai/voxl2/scripts/patch-gazebo.sh @@ -10,4 +10,4 @@ cd - cd Tools/simulation/gazebo-classic/sitl_gazebo-classic/models/iris_vision patch < ../../../../../../boards/modalai/voxl2/gazebo-docker/patch/iris_vision.patch -cd - \ No newline at end of file +cd - diff --git a/boards/modalai/voxl2/src/CMakeLists.txt b/boards/modalai/voxl2/src/CMakeLists.txt index 97cc043a1483..f783825128f1 100644 --- a/boards/modalai/voxl2/src/CMakeLists.txt +++ b/boards/modalai/voxl2/src/CMakeLists.txt @@ -44,3 +44,6 @@ add_library(drivers_board # Add custom drivers add_subdirectory(${PX4_BOARD_DIR}/src/drivers/apps_sbus) + +# Add custom modules +add_subdirectory(${PX4_BOARD_DIR}/src/modules/voxl_save_cal_params) diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index a83f28966bfe..9bc7247e9d61 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -52,4 +52,4 @@ #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2 #define VOXL_ESC_DEFAULT_PORT "2" -#define VOXL2_IO_DEFAULT_PORT "2" \ No newline at end of file +#define VOXL2_IO_DEFAULT_PORT "2" diff --git a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/CMakeLists.txt b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/CMakeLists.txt new file mode 100644 index 000000000000..f645fd3c53b2 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2024 ModalAI, Inc. 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__voxl_save_cal_params + MAIN voxl_save_cal_params + COMPILE_FLAGS + SRCS + VoxlSaveCalParams.cpp + VoxlSaveCalParams.hpp + DEPENDS + px4_work_queue + ) diff --git a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp new file mode 100644 index 000000000000..5c8717db267e --- /dev/null +++ b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.cpp @@ -0,0 +1,234 @@ +/**************************************************************************** + * + * Copyright (c) 2024 ModalAI, Inc. 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 "VoxlSaveCalParams.hpp" + +#include +#include +#include +#include +#include + +using namespace std; + +static bool debug = false; + +VoxlSaveCalParams::VoxlSaveCalParams() : + ModuleParams(nullptr), + WorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{ +} + +bool +VoxlSaveCalParams::init() +{ + if (!_parameter_primary_set_value_request_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; + } + + return true; +} + +void +VoxlSaveCalParams::save_calibration_parameter_to_file(const char *name, param_type_t type, + param_value_u value) +{ + // If the parameter being set is a calibration parameter then save it out to + // a separate calibration file so that they can be preserved and reloaded + // after system updates + string cal_file_name = param_get_default_file(); + string cal_file_append; + string param_name(name); + string cal_strings[] = {"CAL_GYRO", "CAL_MAG", "CAL_BARO", "CAL_ACC"}; + + for (auto i : cal_strings) { + // Check to see if the parameter is one of the desired calibration parameters + if (param_name.substr(0, i.size()) == i) { + // We want the filename to be the standard parameters file name with + // the calibration type appended to it. + cal_file_append = i.substr(3, i.size()); + // Make sure it is lowercase + transform(cal_file_append.begin(), cal_file_append.end(), cal_file_append.begin(), ::tolower); + // And add a cal file extension + cal_file_append += ".cal"; + break; + } + } + + // Check for level horizon calibration parameters + if (cal_file_append.empty() && + (param_name == "SENS_BOARD_X_OFF" || param_name == "SENS_BOARD_Y_OFF")) { + cal_file_append = "_level.cal"; + } + + // Check for RC calibration parameters + if (cal_file_append.empty() && name[0] == 'R' && name[1] == 'C' && isdigit(name[2])) { + cal_file_append = "_rc.cal"; + } + + if (! cal_file_append.empty()) { + cal_file_name += cal_file_append; + + stringstream param_data_stream; + + switch (type) { + case PARAM_TYPE_INT32: + param_data_stream << value.i; + param_data_stream << "\t" << 6; + break; + + case PARAM_TYPE_FLOAT: + param_data_stream << value.f; + param_data_stream << "\t" << 9; + break; + + default: + PX4_ERR("Calibration parameter must be either int or float"); + break; + } + + string param_data; + param_data += "1\t1\t"; + param_data += param_name; + param_data += "\t"; + param_data += param_data_stream.str(); + + if (debug) { PX4_INFO("Writing %s to file %s", param_data.c_str(), cal_file_name.c_str()); } + + // open a file in write (append) mode. + ofstream cal_file; + cal_file.open(cal_file_name, ios_base::app); + + if (cal_file) { + cal_file << param_data << endl; + cal_file.close(); + + } else { + PX4_ERR("Couldn't open %s for writing calibration value", cal_file_name.c_str()); + } + } +} + +void +VoxlSaveCalParams::Run() +{ + if (should_exit()) { + _parameter_primary_set_value_request_sub.unregisterCallback(); + exit_and_cleanup(); + return; + } + + parameter_set_value_request_s req; + + if (_parameter_primary_set_value_request_sub.copy(&req)) { + // PX4_INFO("Got set value request in autosave module"); + + // debug_counters.set_value_received++; + + param_t param = req.parameter_index; + param_value_u value; + value.i = 0; + value.f = 0.0f; + + switch (param_type(param)) { + case PARAM_TYPE_INT32: + param_set_no_remote_update(param, (const void *) &req.int_value, true); + value.i = req.int_value; + break; + + case PARAM_TYPE_FLOAT: + param_set_no_remote_update(param, (const void *) &req.float_value, true); + value.f = req.float_value; + break; + + default: + PX4_ERR("Parameter must be either int or float"); + break; + } + + save_calibration_parameter_to_file(param_name(param), param_type(param), value); + } +} + +int VoxlSaveCalParams::task_spawn(int argc, char *argv[]) +{ + VoxlSaveCalParams *instance = new VoxlSaveCalParams(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int VoxlSaveCalParams::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int VoxlSaveCalParams::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This implements autosaving of calibration parameters on VOXL2 platform. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int voxl_save_cal_params_main(int argc, char *argv[]) +{ + return VoxlSaveCalParams::main(argc, argv); +} diff --git a/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp new file mode 100644 index 000000000000..791bbf5073f8 --- /dev/null +++ b/boards/modalai/voxl2/src/modules/voxl_save_cal_params/VoxlSaveCalParams.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2024 ModalAI, Inc. 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 +#include +#include + +using namespace time_literals; + +class VoxlSaveCalParams : public ModuleBase, public ModuleParams, + public px4::WorkItem +{ +public: + VoxlSaveCalParams(); + ~VoxlSaveCalParams() = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + void save_calibration_parameter_to_file(const char *name, param_type_t type, param_value_u value); + + uORB::SubscriptionCallbackWorkItem _parameter_primary_set_value_request_sub{this, ORB_ID(parameter_primary_set_value_request)}; +}; diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 index 3ce4309ee2ba..bf1b62e981aa 100755 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -129,12 +129,6 @@ else DAEMON="-d" fi -if [ ! -f /data/px4/param/parameters ]; then - echo "[INFO] Setting default parameters for PX4 on voxl" - px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config - /bin/sync -fi - if [ $SENSOR_CAL == "FAKE" ]; then /bin/echo "[INFO] Setting up fake sensor calibration values" px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config diff --git a/boards/modalai/voxl2/target/voxl-px4-hitl-start b/boards/modalai/voxl2/target/voxl-px4-hitl-start index 45ef0b2ddf53..9dcc4bb4f03f 100755 --- a/boards/modalai/voxl2/target/voxl-px4-hitl-start +++ b/boards/modalai/voxl2/target/voxl-px4-hitl-start @@ -43,13 +43,6 @@ else param load fi -# Start logging and use timestamps for log files when possible. -# Add the "-e" option to start logging immediately. Default is -# to log only when armed. Caution must be used with the "-e" option -# because if power is removed without stopping the logger gracefully then -# the log file may be corrupted. -logger start -e -t -b 200 - /bin/sleep 1 # Start all of the processing modules on DSP @@ -97,6 +90,9 @@ mavlink stream -u 14556 -s GLOBAL_POSITION_INT -r 30 # start the slow normal mode for voxl-mavlink-server to forward to GCS mavlink start -x -u 14558 -o 14559 -r 100000 -n lo +# Start logging and use timestamps for log files when possible. +logger start -t -b 256 + /bin/sleep 1 mavlink boot_complete diff --git a/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config b/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config deleted file mode 100755 index bb4045b39734..000000000000 --- a/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config +++ /dev/null @@ -1,192 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -param select /data/px4/param/parameters - -# Make sure we are running at 800Hz on IMU -param set IMU_GYRO_RATEMAX 800 - -# EKF2 Parameters -param set EKF2_IMU_POS_X 0.027 -param set EKF2_IMU_POS_Y 0.009 -param set EKF2_IMU_POS_Z -0.019 -param set EKF2_EV_DELAY 5 -param set EKF2_AID_MASK 280 -param set EKF2_ABL_LIM 0.8 -param set EKF2_TAU_POS 0.25 -param set EKF2_TAU_VEL 0.25 - -param set MC_AIRMODE 0 - -param set MC_YAW_P 2.0 -param set MC_YAWRATE_P 0.15 -param set MC_YAWRATE_I 0.1 -param set MC_YAWRATE_D 0.0 -param set MC_YAWRATE_K 1.0 - -param set MC_PITCH_P 5.5 -param set MC_PITCHRATE_P 0.08 -param set MC_PITCHRATE_I 0.2 -param set MC_PITCHRATE_D 0.0013 -param set MC_PITCHRATE_K 1.0 - -param set MC_ROLL_P 5.5 -param set MC_ROLLRATE_P 0.08 -param set MC_ROLLRATE_I 0.2 -param set MC_ROLLRATE_D 0.0013 -param set MC_ROLLRATE_K 1.0 - -param set MPC_VELD_LP 5.0 - -# tweak MPC_THR_MIN to prevent roll/pitch losing control -# authority under rapid downward acceleration -param set MPC_THR_MAX 0.75 -param set MPC_THR_MIN 0.08 -param set MPC_THR_HOVER 0.42 -param set MPC_MANTHR_MIN 0.05 - -# default position mode with a little expo, smooth mode is terrible -param set MPC_POS_MODE 0 -param set MPC_YAW_EXPO 0.20 -param set MPC_XY_MAN_EXPO 0.20 -param set MPC_Z_MAN_EXPO 0.20 - -# max velocities -param set MPC_VEL_MANUAL 5.0 -param set MPC_XY_VEL_MAX 5.0 -param set MPC_XY_CRUISE 5.0 -param set MPC_Z_VEL_MAX_DN 1.5 -param set MPC_Z_VEL_MAX_UP 4.0 -param set MPC_LAND_SPEED 1.0 - -# Horizontal position PID -param set MPC_XY_P 0.95 -param set MPC_XY_VEL_P_ACC 3.00 -param set MPC_XY_VEL_I_ACC 0.10 -param set MPC_XY_VEL_D_ACC 0.00 - -# Vertical position PID -# PX4 Defaults -param set MPC_Z_P 1.0 -param set MPC_Z_VEL_P_ACC 8.0 -param set MPC_Z_VEL_I_ACC 2.0 -param set MPC_Z_VEL_D_ACC 0.0 - -param set MPC_TKO_RAMP_T 1.50 -param set MPC_TKO_SPEED 1.50 - -# Set the ESC outputs to function as motors -param set VOXL_ESC_FUNC1 101 -param set VOXL_ESC_FUNC2 103 -param set VOXL_ESC_FUNC3 104 -param set VOXL_ESC_FUNC4 102 - -param set VOXL_ESC_SDIR1 0 -param set VOXL_ESC_SDIR2 0 -param set VOXL_ESC_SDIR3 0 -param set VOXL_ESC_SDIR4 0 - -param set VOXL_ESC_CONFIG 1 -param set VOXL_ESC_REV 0 -param set VOXL_ESC_MODE 0 -param set VOXL_ESC_BAUD 2000000 -param set VOXL_ESC_RPM_MAX 10500 -param set VOXL_ESC_RPM_MIN 1000 - -# Set the Voxl2 IO outputs to function as motors -param set VOXL2_IO_FUNC1 101 -param set VOXL2_IO_FUNC2 102 -param set VOXL2_IO_FUNC3 103 -param set VOXL2_IO_FUNC4 104 - -param set VOXL2_IO_BAUD 921600 -param set VOXL2_IO_MIN 1000 -param set VOXL2_IO_MAX 2000 - -# Some parameters for control allocation -param set CA_ROTOR_COUNT 4 -param set CA_R_REV 0 -param set CA_AIRFRAME 0 -param set CA_ROTOR_COUNT 4 -param set CA_ROTOR0_PX 0.15 -param set CA_ROTOR0_PY 0.15 -param set CA_ROTOR1_PX -0.15 -param set CA_ROTOR1_PY -0.15 -param set CA_ROTOR2_PX 0.15 -param set CA_ROTOR2_PY -0.15 -param set CA_ROTOR2_KM -0.05 -param set CA_ROTOR3_PX -0.15 -param set CA_ROTOR3_PY 0.15 -param set CA_ROTOR3_KM -0.05 - -# Some parameter settings to disable / ignore certain preflight checks - -# No GPS driver yet so disable it -param set SYS_HAS_GPS 0 - -# Allow arming wihtout a magnetometer (Need magnetometer driver) -param set SYS_HAS_MAG 0 -param set EKF2_MAG_TYPE 5 - -# Allow arming without battery check (Need voxlpm driver) -param set CBRK_SUPPLY_CHK 894281 - -# Allow arming without an SD card -param set COM_ARM_SDCARD 0 - -# Allow arming wihtout CPU load information -param set COM_CPU_MAX 0.0 - -# Disable auto disarm. This is number of seconds to wait for takeoff -# after arming. If no takeoff happens then it will disarm. A negative -# value disables this. -param set COM_DISARM_PRFLT -1 - -# This parameter doesn't exist anymore. Need to see what the new method is. -# param set MAV_BROADCAST 0 - -# Doesn't work without setting this to Quadcopter -param set MAV_TYPE 2 - -# Parameters that we don't use but QGC complains about if they aren't there -param set SYS_AUTOSTART 4001 - -# Default RC channel mappings -param set RC_MAP_ACRO_SW 0 -param set RC_MAP_ARM_SW 0 -param set RC_MAP_AUX1 0 -param set RC_MAP_AUX2 0 -param set RC_MAP_AUX3 0 -param set RC_MAP_AUX4 0 -param set RC_MAP_AUX5 0 -param set RC_MAP_AUX6 0 -param set RC_MAP_FAILSAFE 0 -param set RC_MAP_FLAPS 0 -param set RC_MAP_FLTMODE 6 -param set RC_MAP_GEAR_SW 0 -param set RC_MAP_KILL_SW 7 -param set RC_MAP_LOITER_SW 0 -param set RC_MAP_MAN_SW 0 -param set RC_MAP_MODE_SW 0 -param set RC_MAP_OFFB_SW 0 -param set RC_MAP_PARAM1 0 -param set RC_MAP_PARAM2 0 -param set RC_MAP_PARAM3 0 -param set RC_MAP_PITCH 2 -param set RC_MAP_POSCTL_SW 0 -param set RC_MAP_RATT_SW 0 -param set RC_MAP_RETURN_SW 0 -param set RC_MAP_ROLL 1 -param set RC_MAP_STAB_SW 0 -param set RC_MAP_THROTTLE 3 -param set RC_MAP_TRANS_SW 0 -param set RC_MAP_YAW 4 - -param save - -sleep 2 - -# Need px4-shutdown otherwise Linux system shutdown is called -px4-shutdown diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start index 5a9149f225a2..014d046c8be9 100755 --- a/boards/modalai/voxl2/target/voxl-px4-start +++ b/boards/modalai/voxl2/target/voxl-px4-start @@ -62,13 +62,6 @@ param select /data/px4/param/parameters # Load in all of the parameters that have been saved in the file param load -# Start logging and use timestamps for log files when possible. -# Add the "-e" option to start logging immediately. Default is -# to log only when armed. Caution must be used with the "-e" option -# because if power is removed without stopping the logger gracefully then -# the log file may be corrupted. -logger start -t - # IMU (accelerometer / gyroscope) if [ "$PLATFORM" == "M0104" ]; then /bin/echo "Starting IMU driver with rotation 12" @@ -95,7 +88,7 @@ if [ "$GPS" != "NONE" ]; then gps start -d /dev/ttyHS2 # On M0054 and M0104 the GPS driver runs on SLPI DSP else - qshell gps start + qshell gps start -d 6 fi fi @@ -106,8 +99,10 @@ qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56 # We do not change the value of SYS_AUTOCONFIG but if it does not # show up as used then it is not reported to QGC and we get a -# missing parameter error. +# missing parameter error. Also, we don't use SYS_AUTOSTART but QGC +# complains about it as well. param touch SYS_AUTOCONFIG +param touch SYS_AUTOSTART # ESC driver if [ "$ESC" == "VOXL_ESC" ]; then @@ -209,10 +204,12 @@ dataman start navigator start # This bridge allows raw data packets to be sent over UART to the ESC -voxl2_io_bridge start +# voxl2_io_bridge start + +# Start uxrce_dds_client for ros2 offboard messages from agent over localhost +uxrce_dds_client start -t udp -h 127.0.0.1 -p 8888 -# Start microdds_client for ros2 offboard messages from agent over localhost -microdds_client start -t udp -h 127.0.0.1 -p 8888 +voxl_save_cal_params start # On M0052 there is only one IMU. So, PX4 needs to # publish IMU samples externally for VIO to use. @@ -233,6 +230,14 @@ mavlink stream -u 14556 -s SCALED_PRESSURE -r 10 # start the slow normal mode for voxl-mavlink-server to forward to GCS mavlink start -x -u 14558 -o 14559 -r 100000 -n lo +# Start logging and use timestamps for log files when possible. +# Add the "-e" option to start logging immediately. Default is +# to log only when armed. Caution must be used with the "-e" option +# because if power is removed without stopping the logger gracefully then +# the log file may be corrupted. Rather than using "-e" option it's better +# to use the SDLOG_MODE to do that. +logger start -t -b 256 + mavlink boot_complete # Optional MSP OSD driver for DJI goggles diff --git a/boards/mro/ctrl-zero-classic/default.px4board b/boards/mro/ctrl-zero-classic/default.px4board index d07a7c1fc9d4..665fbc8626f4 100644 --- a/boards/mro/ctrl-zero-classic/default.px4board +++ b/boards/mro/ctrl-zero-classic/default.px4board @@ -12,6 +12,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin b/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin index 0a9fb48ed280..febba0103e72 100755 Binary files a/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin and b/boards/mro/ctrl-zero-classic/extras/mro_ctrl-zero-classic_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-classic/init/rc.board_defaults b/boards/mro/ctrl-zero-classic/init/rc.board_defaults index 110c240ee8a0..0495329e1a47 100644 --- a/boards/mro/ctrl-zero-classic/init/rc.board_defaults +++ b/boards/mro/ctrl-zero-classic/init/rc.board_defaults @@ -6,4 +6,5 @@ param set-default BAT1_V_DIV 10.1 param set-default BAT1_A_PER_V 17 +param set-default GPS_2_CONFIG 202 param set-default TEL_FRSKY_CONFIG 103 diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h index 52206cccd00f..ea2147442699 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-classic/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig index 7e56401723d0..1d75bb419dba 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -219,12 +219,12 @@ CONFIG_STM32H7_TIM2=y CONFIG_STM32H7_TIM3=y CONFIG_STM32H7_TIM4=y CONFIG_STM32H7_TIM8=y -CONFIG_STM32H7_USART1=y -CONFIG_STM32H7_USART2=y -CONFIG_STM32H7_USART3=y CONFIG_STM32H7_UART4=y CONFIG_STM32H7_UART7=y CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=y +CONFIG_STM32H7_USART3=y CONFIG_STM32H7_USART_BREAKS=y CONFIG_STM32H7_USART_INVERT=y CONFIG_STM32H7_USART_SINGLEWIRE=y diff --git a/boards/mro/ctrl-zero-f7-oem/default.px4board b/boards/mro/ctrl-zero-f7-oem/default.px4board index b0701855bf22..9fe340b87f1b 100644 --- a/boards/mro/ctrl-zero-f7-oem/default.px4board +++ b/boards/mro/ctrl-zero-f7-oem/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/ctrl-zero-f7/default.px4board b/boards/mro/ctrl-zero-f7/default.px4board index 074a4f2ca052..d09ca8a5b50f 100644 --- a/boards/mro/ctrl-zero-f7/default.px4board +++ b/boards/mro/ctrl-zero-f7/default.px4board @@ -9,6 +9,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/ctrl-zero-h7-oem/default.px4board b/boards/mro/ctrl-zero-h7-oem/default.px4board index 549ba99b7331..ec6c59888db5 100644 --- a/boards/mro/ctrl-zero-h7-oem/default.px4board +++ b/boards/mro/ctrl-zero-h7-oem/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin b/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin index 1a056aee87f0..f3e9467a4102 100755 Binary files a/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin and b/boards/mro/ctrl-zero-h7-oem/extras/mro_ctrl-zero-h7-oem_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h index d5ffa2c5848e..14a6a28d2b88 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/include/board.h @@ -197,6 +197,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/ctrl-zero-h7/default.px4board b/boards/mro/ctrl-zero-h7/default.px4board index c32ae10fc056..7bb96d3aca08 100644 --- a/boards/mro/ctrl-zero-h7/default.px4board +++ b/boards/mro/ctrl-zero-h7/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin b/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin index ac3050bee760..6815051bb37f 100755 Binary files a/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin and b/boards/mro/ctrl-zero-h7/extras/mro_ctrl-zero-h7_bootloader.bin differ diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h index cdbd3dd3e629..4a20575d70ae 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h +++ b/boards/mro/ctrl-zero-h7/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/pixracerpro/default.px4board b/boards/mro/pixracerpro/default.px4board index d4b1d29370aa..fb49c90866cb 100644 --- a/boards/mro/pixracerpro/default.px4board +++ b/boards/mro/pixracerpro/default.px4board @@ -9,9 +9,11 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_BOSCH_BMI085=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y diff --git a/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin b/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin index 5882beeea1de..657a25a58793 100755 Binary files a/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin and b/boards/mro/pixracerpro/extras/mro_pixracerpro_bootloader.bin differ diff --git a/boards/mro/pixracerpro/nuttx-config/include/board.h b/boards/mro/pixracerpro/nuttx-config/include/board.h index 5edda931dc10..96b2ed2022fb 100644 --- a/boards/mro/pixracerpro/nuttx-config/include/board.h +++ b/boards/mro/pixracerpro/nuttx-config/include/board.h @@ -196,6 +196,12 @@ #define STM32_FDCANCLK STM32_HSE_FREQUENCY +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FLASH wait states */ #define BOARD_FLASH_WAITSTATES 2 diff --git a/boards/mro/x21-777/default.px4board b/boards/mro/x21-777/default.px4board index 5a51a776dde0..361aca448471 100644 --- a/boards/mro/x21-777/default.px4board +++ b/boards/mro/x21-777/default.px4board @@ -9,6 +9,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ diff --git a/boards/mro/x21-777/nuttx-config/include/board.h b/boards/mro/x21-777/nuttx-config/include/board.h index 04cc6c1acd62..7b2863661fbd 100644 --- a/boards/mro/x21-777/nuttx-config/include/board.h +++ b/boards/mro/x21-777/nuttx-config/include/board.h @@ -282,4 +282,3 @@ #define GPIO_SPI2_MISO GPIO_SPI2_MISO_1 #define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_1 #define GPIO_SPI2_SCK GPIO_SPI2_SCK_3 - diff --git a/boards/mro/x21/default.px4board b/boards/mro/x21/default.px4board index 7c206260016f..8640bff623b0 100644 --- a/boards/mro/x21/default.px4board +++ b/boards/mro/x21/default.px4board @@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/mro/x21/extras/px4_io-v2_default.bin b/boards/mro/x21/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/mro/x21/extras/px4_io-v2_default.bin and b/boards/mro/x21/extras/px4_io-v2_default.bin differ diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index b8bc5cc7f424..749e62e5d54f 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MPL3115A2=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y diff --git a/boards/nxp/fmuk66-v3/default.px4board b/boards/nxp/fmuk66-v3/default.px4board index e53ae83c880f..268e89c70fd8 100644 --- a/boards/nxp/fmuk66-v3/default.px4board +++ b/boards/nxp/fmuk66-v3/default.px4board @@ -11,6 +11,7 @@ CONFIG_DRIVERS_BAROMETER_MPL3115A2=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DISTANCE_SENSOR_SRF05=y diff --git a/boards/nxp/fmuk66-v3/init/rc.board_defaults b/boards/nxp/fmuk66-v3/init/rc.board_defaults index df700b4dc350..eed00b78ecbe 100644 --- a/boards/nxp/fmuk66-v3/init/rc.board_defaults +++ b/boards/nxp/fmuk66-v3/init/rc.board_defaults @@ -8,4 +8,3 @@ param set-default BAT1_A_PER_V 15.391030303 rgbled_pwm start safety_button start - diff --git a/boards/nxp/mr-canhubk3/default.px4board b/boards/nxp/mr-canhubk3/default.px4board index 277c62de7889..63294e276f8a 100644 --- a/boards/nxp/mr-canhubk3/default.px4board +++ b/boards/nxp/mr-canhubk3/default.px4board @@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" CONFIG_BOARD_ARCHITECTURE="cortex-m7" CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_PARAM_FILE="/mnt/qspi/params" CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y @@ -16,6 +17,7 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y CONFIG_SYSTEMCMDS_MTD=y +CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SD_BENCH=y diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index d6b17b1094e0..48a7883fe259 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -59,7 +59,6 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y -CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REFLECT=y diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults index 7ef22a054cb6..27aef9019a59 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_defaults +++ b/boards/nxp/mr-canhubk3/init/rc.board_defaults @@ -28,6 +28,12 @@ then safety_button start fi +if [ -f "/mnt/qspi/mtd_net" ] +then +else + netman update -i eth0 +fi + if param greater -s UAVCAN_ENABLE 0 then ifup can0 diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index 77861ee5976b..3d9767bc9220 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -158,8 +158,8 @@ CONFIG_NETDEV_LATEINIT=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig index e9d287c2cd1e..88758c3d5852 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig @@ -158,8 +158,8 @@ CONFIG_NETDEV_LATEINIT=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/nxp/mr-canhubk3/sysview.px4board b/boards/nxp/mr-canhubk3/sysview.px4board index d190234fcbdb..2089b09931c5 100644 --- a/boards/nxp/mr-canhubk3/sysview.px4board +++ b/boards/nxp/mr-canhubk3/sysview.px4board @@ -45,7 +45,6 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y -CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REFLECT=y diff --git a/boards/nxp/mr-canhubk3/zenoh.px4board b/boards/nxp/mr-canhubk3/zenoh.px4board index 28de5ea5cc76..fe2297a5547f 100644 --- a/boards/nxp/mr-canhubk3/zenoh.px4board +++ b/boards/nxp/mr-canhubk3/zenoh.px4board @@ -50,7 +50,6 @@ CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y -CONFIG_SYSTEMCMDS_NETMAN=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REFLECT=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index f9530438758b..45430b9c51b9 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -3,6 +3,7 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m4" CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_PARAM_FILE="/dev/eeeprom0" CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_GPS=y diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 287d99165930..8d3c58a7f0f5 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -7,6 +7,7 @@ CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y diff --git a/boards/omnibus/f4sd/init/rc.board_defaults b/boards/omnibus/f4sd/init/rc.board_defaults index 9e6affd3454b..2678eae827cb 100644 --- a/boards/omnibus/f4sd/init/rc.board_defaults +++ b/boards/omnibus/f4sd/init/rc.board_defaults @@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281 # Disable safety switch by default param set-default CBRK_IO_SAFETY 22027 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - param set-default SYS_HAS_MAG 0 diff --git a/boards/omnibus/f4sd/init/rc.board_extras b/boards/omnibus/f4sd/init/rc.board_extras index 25e6c269a85f..e3aa0d196175 100644 --- a/boards/omnibus/f4sd/init/rc.board_extras +++ b/boards/omnibus/f4sd/init/rc.board_extras @@ -7,5 +7,3 @@ if ! param compare OSD_ATXXXX_CFG 0 then atxxxx start -s fi - - diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index 59d1c02a7331..b796d1bfbf49 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -9,6 +9,8 @@ CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS6" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y CONFIG_DRIVERS_IMU_ST_L3GD20=y diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v3/default.px4board b/boards/px4/fmu-v3/default.px4board index 1ced5b78737e..dd4b5f9621be 100644 --- a/boards/px4/fmu-v3/default.px4board +++ b/boards/px4/fmu-v3/default.px4board @@ -11,9 +11,11 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y @@ -53,7 +55,7 @@ CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y +CONFIG_MODULES_GYRO_FFT=n CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index 0e1bb8f54774..e7906db52c85 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -11,9 +11,11 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GNSS_SEPTENTRIO=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y diff --git a/boards/px4/fmu-v4/src/usb.c b/boards/px4/fmu-v4/src/usb.c index 5791d52cb0f8..b96898bc8635 100644 --- a/boards/px4/fmu-v4/src/usb.c +++ b/boards/px4/fmu-v4/src/usb.c @@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/boards/px4/fmu-v4pro/default.px4board b/boards/px4/fmu-v4pro/default.px4board index f722e649efae..ff8455c28588 100644 --- a/boards/px4/fmu-v4pro/default.px4board +++ b/boards/px4/fmu-v4pro/default.px4board @@ -12,6 +12,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v4pro/src/timer_config.cpp b/boards/px4/fmu-v4pro/src/timer_config.cpp index 5d16012f6b27..7bf538a032e8 100644 --- a/boards/px4/fmu-v4pro/src/timer_config.cpp +++ b/boards/px4/fmu-v4pro/src/timer_config.cpp @@ -49,4 +49,3 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers, timer_io_channels); - diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board index c023a5cb754c..831abbb824ab 100644 --- a/boards/px4/fmu-v5/cryptotest.px4board +++ b/boards/px4/fmu-v5/cryptotest.px4board @@ -1,5 +1,4 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_BOARD_CRYPTO=y CONFIG_DRIVERS_STUB_KEYSTORE=y CONFIG_DRIVERS_SW_CRYPTO=y diff --git a/boards/px4/fmu-v5/debug.px4board b/boards/px4/fmu-v5/debug.px4board index 0b8ab3fc3e99..557590ea180f 100644 --- a/boards/px4/fmu-v5/debug.px4board +++ b/boards/px4/fmu-v5/debug.px4board @@ -22,7 +22,6 @@ CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_GIMBAL=n CONFIG_MODULES_GYRO_CALIBRATION=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n @@ -37,7 +36,6 @@ CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8308=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y -CONFIG_DRIVERS_MAGNETOMETER_LIS2MDL=y CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index bfb8b6af4645..1132a48bbfb4 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -10,6 +10,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -61,7 +62,6 @@ 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 @@ -97,7 +97,6 @@ CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_REFLECT=y -CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y @@ -106,4 +105,3 @@ CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_USB_CONNECTED=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y -CONFIG_EXAMPLES_FAKE_GPS=y diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index 7350cb45322a..8435ec217bce 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -116,7 +116,6 @@ CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 -CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y @@ -139,7 +138,6 @@ 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 diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index e5371b2a7dfe..7ea307124c37 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -6,11 +6,11 @@ CONFIG_COMMON_OPTICAL_FLOW=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_CAMERA_CAPTURE=n CONFIG_DRIVERS_CAMERA_TRIGGER=n +CONFIG_DRIVERS_CDCACM_AUTOSTART=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_UAVCAN=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_AIRSPEED_SELECTOR=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n @@ -18,7 +18,6 @@ CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_ATT_CONTROL=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n @@ -35,7 +34,6 @@ CONFIG_SYSTEMCMDS_LED_CONTROL=n CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SD_BENCH=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_USB_CONNECTED=n CONFIG_BOARD_PROTECTED=y diff --git a/boards/px4/fmu-v5/rover.px4board b/boards/px4/fmu-v5/rover.px4board index 3284f0bb7a1b..07bfbe094121 100644 --- a/boards/px4/fmu-v5/rover.px4board +++ b/boards/px4/fmu-v5/rover.px4board @@ -7,7 +7,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_FW_POS_CONTROL=n CONFIG_MODULES_FW_RATE_CONTROL=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_MC_ATT_CONTROL=n CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n @@ -18,4 +17,6 @@ CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index 812a0f729570..03acb1eef06f 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -20,8 +20,8 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_POWER_MONITOR_INA226=n CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n +CONFIG_DRIVERS_TONE_ALARM=n CONFIG_DRIVERS_UAVCAN=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n @@ -29,7 +29,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_GIMBAL=n CONFIG_MODULES_GYRO_CALIBRATION=n CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index 2dcd6925f2f3..ea3fa6bf0b06 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -9,10 +9,8 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_UAVCAN=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_BOARD_TESTING=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index c9e9823c036f..7b89c5f0c9bf 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -12,12 +12,10 @@ CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n CONFIG_DRIVERS_PWM_INPUT=n CONFIG_DRIVERS_SMART_BATTERY_BATMON=n -CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_CAMERA_FEEDBACK=n CONFIG_MODULES_ESC_BATTERY=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_ROVER_POS_CONTROL=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n @@ -31,7 +29,6 @@ CONFIG_SYSTEMCMDS_GPIO=n CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SD_BENCH=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n CONFIG_BOARD_UAVCAN_PERIPHERALS="cuav_can-gps-v1_default" diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index e2b5833dfecb..a48b0956876b 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -14,6 +14,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -53,7 +54,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y @@ -81,7 +81,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y -CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y @@ -102,9 +101,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SD_BENCH=y -CONFIG_SYSTEMCMDS_SD_STRESS=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index 30ff624b2f63..e0381e98867a 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -3,23 +3,25 @@ # board specific defaults #------------------------------------------------------------------------------ -# Mavlink ethernet (CFG 1000) -param set-default MAV_2_CONFIG 1000 -param set-default MAV_2_BROADCAST 1 -param set-default MAV_2_MODE 0 -param set-default MAV_2_RADIO_CTL 0 -param set-default MAV_2_RATE 100000 -param set-default MAV_2_REMOTE_PRT 14550 -param set-default MAV_2_UDP_PRT 14550 - param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 if ver hwbasecmp 008 009 00a 010 011 then - # Skynode: use the "custom participant" config for uxrce_dds_client + # Skynode: use the "custom participant", IP=10.41.10.1 config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 + param set-default UXRCE_DDS_AG_IP 170461697 + param set-default UXRCE_DDS_CFG 1000 +else + # Mavlink ethernet (CFG 1000) + param set-default MAV_2_CONFIG 1000 + param set-default MAV_2_BROADCAST 1 + param set-default MAV_2_MODE 0 + param set-default MAV_2_RADIO_CTL 0 + param set-default MAV_2_RATE 100000 + param set-default MAV_2_REMOTE_PRT 14550 + param set-default MAV_2_UDP_PRT 14550 fi safety_button start diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 8c21923c6441..cfb7af2bd252 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -143,8 +143,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/px4/fmu-v5x/nuttx-config/test/defconfig b/boards/px4/fmu-v5x/nuttx-config/test/defconfig index 12333125f90b..3261d4700c33 100644 --- a/boards/px4/fmu-v5x/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/test/defconfig @@ -143,8 +143,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 CONFIG_NETUTILS_TELNETD=y diff --git a/boards/px4/fmu-v5x/rover.px4board b/boards/px4/fmu-v5x/rover.px4board index b8a1597b30ea..4a4458c2e6bc 100644 --- a/boards/px4/fmu-v5x/rover.px4board +++ b/boards/px4/fmu-v5x/rover.px4board @@ -14,3 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board index 278cc285a1ae..9440ac8b271e 100644 --- a/boards/px4/fmu-v5x/test.px4board +++ b/boards/px4/fmu-v5x/test.px4board @@ -6,9 +6,10 @@ CONFIG_DRIVERS_CAMERA_CAPTURE=n CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n +CONFIG_MODULES_DIFFERENTIAL_DRIVE=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n -CONFIG_MODULES_ROVER_POS_CONTROL=n +CONFIG_MODULES_PAYLOAD_DELIVERER=n CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index 148e6326fb26..adae635537e7 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -47,7 +48,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y diff --git a/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin index 21cc353f8ac8..b6b75bb868c9 100755 Binary files a/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin and b/boards/px4/fmu-v6c/extras/px4_fmu-v6c_bootloader.bin differ diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6c/nuttx-config/include/board.h b/boards/px4/fmu-v6c/nuttx-config/include/board.h index 1951031cb825..4b4b128cf2ba 100644 --- a/boards/px4/fmu-v6c/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6c/nuttx-config/include/board.h @@ -246,6 +246,12 @@ #define STM32_RCC_D2CCIP2R_USBSRC RCC_D2CCIP2R_USBSEL_PLL3 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* ADC 1 2 3 clock source */ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 diff --git a/boards/px4/fmu-v6c/rover.px4board b/boards/px4/fmu-v6c/rover.px4board index f754c54e36f7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6c/rover.px4board +++ b/boards/px4/fmu-v6c/rover.px4board @@ -13,4 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6u/default.px4board b/boards/px4/fmu-v6u/default.px4board index 50b4f6ba3d1d..e733dccc35fc 100644 --- a/boards/px4/fmu-v6u/default.px4board +++ b/boards/px4/fmu-v6u/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin b/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin index f5b0a7e5008e..60bbe67ae126 100755 Binary files a/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin and b/boards/px4/fmu-v6u/extras/px4_fmu-v6u_bootloader.bin differ diff --git a/boards/px4/fmu-v6u/rover.px4board b/boards/px4/fmu-v6u/rover.px4board index f754c54e36f7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6u/rover.px4board +++ b/boards/px4/fmu-v6u/rover.px4board @@ -13,4 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index 4e21f0fd0870..7c4bf0b6f6cb 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -14,9 +14,11 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y @@ -66,6 +68,7 @@ CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MAG_BIAS_ESTIMATOR=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y +CONFIG_MAVLINK_DIALECT="development" CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y diff --git a/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin index 9668fdb20b10..ecad9c2da1b4 100755 Binary files a/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin and b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin differ diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin index 957f0f13a12e..145089ae0d7c 100755 Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ diff --git a/boards/px4/fmu-v6x/init/rc.board_defaults b/boards/px4/fmu-v6x/init/rc.board_defaults index 95d873475b15..bdc99a68e7bd 100644 --- a/boards/px4/fmu-v6x/init/rc.board_defaults +++ b/boards/px4/fmu-v6x/init/rc.board_defaults @@ -3,15 +3,6 @@ # board specific defaults #------------------------------------------------------------------------------ -# Mavlink ethernet (CFG 1000) -param set-default MAV_2_CONFIG 1000 -param set-default MAV_2_BROADCAST 1 -param set-default MAV_2_MODE 0 -param set-default MAV_2_RADIO_CTL 0 -param set-default MAV_2_RATE 100000 -param set-default MAV_2_REMOTE_PRT 14550 -param set-default MAV_2_UDP_PRT 14550 - # By disabling all 3 INA modules, we use the # i2c_launcher instead. param set-default SENS_EN_INA238 0 @@ -20,8 +11,30 @@ param set-default SENS_EN_INA226 0 if ver hwbasecmp 009 010 011 then - # Skynode: use the "custom participant" config for uxrce_dds_client + # Skynode: use the "custom participant", IP=10.41.10.1 config for uxrce_dds_client param set-default UXRCE_DDS_PTCFG 2 + param set-default UXRCE_DDS_AG_IP 170461697 + param set-default UXRCE_DDS_CFG 1000 +else + # Mavlink ethernet (CFG 1000) + param set-default MAV_2_CONFIG 1000 + param set-default MAV_2_BROADCAST 1 + param set-default MAV_2_MODE 0 + param set-default MAV_2_RADIO_CTL 0 + param set-default MAV_2_RATE 100000 + param set-default MAV_2_REMOTE_PRT 14550 + param set-default MAV_2_UDP_PRT 14550 fi safety_button start + +# GPIO Expander driver on external I2C3 +if ver hwbasecmp 009 +then + # No USB + mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 +fi +if ver hwbasecmp 00a 008 +then + mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 +fi diff --git a/boards/px4/fmu-v6x/nuttx-config/include/board.h b/boards/px4/fmu-v6x/nuttx-config/include/board.h index d6b2925b795c..7907eafad194 100644 --- a/boards/px4/fmu-v6x/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6x/nuttx-config/include/board.h @@ -250,6 +250,12 @@ #define STM32_RCC_D3CCIPR_ADCSRC RCC_D3CCIPR_ADCSEL_PLL2 +/* UART clock selection */ +/* reset to default to overwrite any changes done by any bootloader */ + +#define STM32_RCC_D2CCIP2R_USART234578_SEL RCC_D2CCIP2R_USART234578SEL_RCC +#define STM32_RCC_D2CCIP2R_USART16_SEL RCC_D2CCIP2R_USART16SEL_RCC + /* FDCAN 1 2 clock source */ #define STM32_RCC_D2CCIP1R_FDCANSEL RCC_D2CCIP1R_FDCANSEL_HSE /* FDCAN 1 2 clock source */ diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index 87c8a46f04d1..f0a8d6ae8653 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -91,8 +91,10 @@ CONFIG_DEBUG_SYMBOLS=y CONFIG_DEBUG_TCBINFO=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_DEV_URANDOM=y CONFIG_ETH0_PHY_LAN8742A=y CONFIG_EXPERIMENTAL=y CONFIG_FAT_DMAMEMORY=y @@ -147,8 +149,8 @@ CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 diff --git a/boards/px4/fmu-v6x/rover.px4board b/boards/px4/fmu-v6x/rover.px4board index f754c54e36f7..7ded4008d8af 100644 --- a/boards/px4/fmu-v6x/rover.px4board +++ b/boards/px4/fmu-v6x/rover.px4board @@ -13,4 +13,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt index a120ebe33623..39ec808e1e9a 100644 --- a/boards/px4/fmu-v6x/src/CMakeLists.txt +++ b/boards/px4/fmu-v6x/src/CMakeLists.txt @@ -34,7 +34,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "bootloader") add_compile_definitions(BOOTLOADER) add_library(drivers_board bootloader_main.c - init.c + init.cpp usb.c timer_config.cpp ) @@ -52,7 +52,7 @@ else() add_library(drivers_board can.c i2c.cpp - init.c + init.cpp led.c mtd.cpp sdio.c @@ -71,5 +71,6 @@ else() nuttx_arch # sdio nuttx_drivers # sdio px4_layer + platform_gpio_mcp23009 ) endif() diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index 7cad5497ed2b..7c98f9490207 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -270,6 +270,11 @@ #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) #define GPIO_VDD_3V3_SD_CARD_EN /* PC13 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +/* MCP23009 GPIO expander */ +#define BOARD_GPIO_VDD_5V_COMP_VALID "/dev/gpio4" +#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpio5" + + /* Spare GPIO */ #define GPIO_PG6 /* PG6 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6) diff --git a/boards/px4/fmu-v6x/src/init.c b/boards/px4/fmu-v6x/src/init.cpp similarity index 98% rename from boards/px4/fmu-v6x/src/init.c rename to boards/px4/fmu-v6x/src/init.cpp index cd6714af1664..8901bd1766f3 100644 --- a/boards/px4/fmu-v6x/src/init.c +++ b/boards/px4/fmu-v6x/src/init.cpp @@ -74,6 +74,7 @@ #include #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -159,7 +160,7 @@ __EXPORT void board_on_reset(int status) * ************************************************************************************/ -__EXPORT void +extern "C" __EXPORT void stm32_boardinitialize(void) { board_on_reset(-1); /* Reset PWM first thing */ @@ -280,6 +281,13 @@ __EXPORT int board_app_initialize(uintptr_t arg) # endif /* CONFIG_MMCSD */ + ret = mcp23009_register_gpios(3, 0x25); + + if (ret != OK) { + led_on(LED_RED); + return ret; + } + #endif /* !defined(BOOTLOADER) */ return OK; diff --git a/boards/px4/fmu-v6xrt/allyes.px4board b/boards/px4/fmu-v6xrt/allyes.px4board new file mode 100644 index 000000000000..5f1bc9ab3d59 --- /dev/null +++ b/boards/px4/fmu-v6xrt/allyes.px4board @@ -0,0 +1 @@ +CONFIG_BOARD_LINKER_PREFIX="allyes" diff --git a/boards/px4/fmu-v6xrt/default.px4board b/boards/px4/fmu-v6xrt/default.px4board index f9961fd7bbfb..17434ed18d3c 100644 --- a/boards/px4/fmu-v6xrt/default.px4board +++ b/boards/px4/fmu-v6xrt/default.px4board @@ -13,6 +13,7 @@ CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -33,6 +34,7 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_INA228=y CONFIG_DRIVERS_POWER_MONITOR_INA238=y +CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_PX4IO=y CONFIG_DRIVERS_RC_INPUT=y @@ -84,6 +86,7 @@ CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y +CONFIG_SYSTEMCMDS_I2C_LAUNCHER=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_IO_BYPASS_CONTROL=y CONFIG_SYSTEMCMDS_LED_CONTROL=y diff --git a/boards/px4/fmu-v6xrt/extras/px4_fmu-v6xrt_bootloader.bin b/boards/px4/fmu-v6xrt/extras/px4_fmu-v6xrt_bootloader.bin index cb9cbb304e66..42e7c4b9d036 100755 Binary files a/boards/px4/fmu-v6xrt/extras/px4_fmu-v6xrt_bootloader.bin and b/boards/px4/fmu-v6xrt/extras/px4_fmu-v6xrt_bootloader.bin differ diff --git a/boards/px4/fmu-v6xrt/init/rc.board_defaults b/boards/px4/fmu-v6xrt/init/rc.board_defaults index 45282796efde..a5a3dc10162c 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_defaults +++ b/boards/px4/fmu-v6xrt/init/rc.board_defaults @@ -12,9 +12,11 @@ param set-default MAV_2_RATE 100000 param set-default MAV_2_REMOTE_PRT 14550 param set-default MAV_2_UDP_PRT 14550 +# By disabling all 3 INA modules, we use the +# i2c_launcher instead. param set-default SENS_EN_INA238 0 param set-default SENS_EN_INA228 0 -param set-default SENS_EN_INA226 1 +param set-default SENS_EN_INA226 0 safety_button start diff --git a/boards/px4/fmu-v6xrt/init/rc.board_sensors b/boards/px4/fmu-v6xrt/init/rc.board_sensors index 64779648a4b2..9271eab6549f 100644 --- a/boards/px4/fmu-v6xrt/init/rc.board_sensors +++ b/boards/px4/fmu-v6xrt/init/rc.board_sensors @@ -17,6 +17,7 @@ #------------------------------------------------------------------------------ set HAVE_PM2 yes +set INA_CONFIGURED no if mft query -q -k MFT -s MFT_PM2 -v 0 then @@ -39,6 +40,8 @@ then then ina226 -X -b 2 -t 2 -k start fi + + set INA_CONFIGURED yes fi if param compare SENS_EN_INA228 1 @@ -49,6 +52,8 @@ then then ina228 -X -b 2 -t 2 -k start fi + + set INA_CONFIGURED yes fi if param compare SENS_EN_INA238 1 @@ -59,6 +64,24 @@ then then ina238 -X -b 2 -t 2 -k start fi + + set INA_CONFIGURED yes +fi + +#Start Auterion Power Module selector for Skynode boards +if ver hwbasecmp 009 010 +then + pm_selector_auterion start +else + if [ $INA_CONFIGURED = no ] + then + # INA226, INA228, INA238 auto-start + i2c_launcher start -b 1 + if [ $HAVE_PM2 = yes ] + then + i2c_launcher start -b 2 + fi + fi fi # Internal SPI bus ICM42686p (hard-mounted) @@ -88,4 +111,5 @@ fi bmp388 -X -b 2 start +unset INA_CONFIGURED unset HAVE_PM2 diff --git a/boards/px4/fmu-v6xrt/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/bootloader/defconfig index 8ed0ed82d4c0..6495d6ba6226 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/bootloader/defconfig @@ -28,6 +28,7 @@ CONFIG_ARMV7M_ITCM=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU=y +CONFIG_ARM_MPU_RESET=y CONFIG_BOARDCTL=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_ASSERT_RESET_VALUE=0 diff --git a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h index 6b704139dc0c..3c0753df7dd4 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/include/board.h +++ b/boards/px4/fmu-v6xrt/nuttx-config/include/board.h @@ -52,6 +52,7 @@ #define IMXRT_IPG_PODF_DIVIDER 5 #define BOARD_GPT_FREQUENCY 24000000 #define BOARD_XTAL_FREQUENCY 24000000 +#define BOARD_FLEXIO_PREQ 108000000 /* SDIO *********************************************************************/ @@ -267,6 +268,10 @@ // This is the ENET_1G interface. +/* Dshot Disambiguation *******************************************************/ + +#define IOMUX_DSHOT_DEFAULT (IOMUX_DRIVE_HIGHSTRENGTH | IOMUX_SLEW_FAST) + // Compile time selection #if defined(CONFIG_ETH0_PHY_TJA1103) # define BOARD_PHY_ADDR (18) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig index 98431d0420b0..e58939226d33 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6xrt/nuttx-config/nsh/defconfig @@ -29,6 +29,7 @@ CONFIG_ARMV7M_ITCM=y CONFIG_ARMV7M_MEMCPY=y CONFIG_ARMV7M_USEBASEPRI=y CONFIG_ARM_MPU=y +CONFIG_ARM_MPU_RESET=y CONFIG_BOARDCTL_RESET=y CONFIG_BOARD_ASSERT_RESET_VALUE=0 CONFIG_BOARD_BOOTLOADER_FIXUP=y @@ -46,7 +47,10 @@ CONFIG_CDCACM_RXBUFSIZE=600 CONFIG_CDCACM_TXBUFSIZE=12000 CONFIG_CDCACM_VENDORID=0x3643 CONFIG_CDCACM_VENDORSTR="Dronecode Project, Inc." +CONFIG_DEBUG_ERROR=y +CONFIG_DEBUG_FEATURES=y CONFIG_DEBUG_HARDFAULT_ALERT=y +CONFIG_DEBUG_MEMFAULT=y CONFIG_DEBUG_SYMBOLS=y CONFIG_DEBUG_TCBINFO=y CONFIG_DEV_FIFO_SIZE=0 @@ -83,7 +87,6 @@ CONFIG_IMXRT_ENET=y CONFIG_IMXRT_FLEXCAN1=y CONFIG_IMXRT_FLEXCAN2=y CONFIG_IMXRT_FLEXCAN3=y -CONFIG_IMXRT_FLEXIO1=y CONFIG_IMXRT_FLEXSPI2=y CONFIG_IMXRT_GPIO13_IRQ=y CONFIG_IMXRT_GPIO1_0_15_IRQ=y @@ -185,7 +188,6 @@ CONFIG_LPUART8_TXDMA=y CONFIG_MEMSET_64BIT=y CONFIG_MEMSET_OPTSPEED=y CONFIG_MMCSD=y -CONFIG_MMCSD_MULTIBLOCK_LIMIT=1 CONFIG_MMCSD_SDIO=y CONFIG_MTD=y CONFIG_MTD_BYTE_WRITE=y @@ -200,8 +202,8 @@ CONFIG_NETDEV_LATEINIT=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DHCPC=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_MONITOR=y CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 @@ -258,7 +260,6 @@ CONFIG_SCHED_INSTRUMENTATION_SWITCH=y CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKSTACKSIZE=2032 -CONFIG_SCHED_WAITPID=y CONFIG_SDIO_BLOCKSETUP=y CONFIG_SEM_PREALLOCHOLDERS=32 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y @@ -277,6 +278,7 @@ CONFIG_SYSTEM_CLE=y CONFIG_SYSTEM_DHCPC_RENEW=y CONFIG_SYSTEM_NSH=y CONFIG_SYSTEM_PING=y +CONFIG_SYSTEM_SYSTEM=y CONFIG_TASK_NAME_SIZE=24 CONFIG_USBDEV=y CONFIG_USBDEV_BUSPOWERED=y diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/allyes-script.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/allyes-script.ld new file mode 100644 index 000000000000..2d4167f13819 --- /dev/null +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/allyes-script.ld @@ -0,0 +1,197 @@ +/**************************************************************************** + * boards/px4/fmu-v6xrt/nuttx-config/scripts/script.ld + * + * 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. + * + ****************************************************************************/ + +/* Specify the memory areas */ + + /* Reallocate + * Final Configuration is + * No DTCM + * 512k OCRAM M7 (FlexRAM) (2038:0000-203f:ffff) + * 128k OCRAMM7 FlexRAM ECC (2036:0000-2037:ffff) + * 64k OCRAM2 ECC parity (2035:0000-2035:ffff) + * 64k OCRAM1 ECC parity (2034:0000-2034:ffff) + * 512k FlexRAM OCRAM2 (202C:0000-2033:ffff) + * 512k FlexRAM OCRAM1 (2024:0000-202B:ffff) + * 256k System OCRAM M4 (2020:0000-2023:ffff) + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x30020000, LENGTH = 4M-128K /* We have 64M but we do not want to wait to program it all */ + sram (rwx) : ORIGIN = 0x20240000, LENGTH = 2M-256k-512k + itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 256K /* TODO FlexRAM partition */ + dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 256K +} + +OUTPUT_ARCH(arm) +EXTERN(_vectors) +EXTERN(g_flash_config) +EXTERN(g_image_vector_table) +EXTERN(g_boot_data) +EXTERN(board_get_manifest) +EXTERN(_bootdelay_signature) +EXTERN(imxrt_flexspi_initialize) + +ENTRY(__start) + +SECTIONS +{ + /* Image Vector Table and Boot Data for booting from external flash */ + + .boot_hdr : ALIGN(4) + { + FILL(0xff) + . = 0x400 ; + __boot_hdr_start__ = ABSOLUTE(.) ; + KEEP(*(.boot_hdr.conf)) + . = 0x1000 ; + KEEP(*(.boot_hdr.ivt)) + . = 0x1020 ; + KEEP(*(.boot_hdr.boot_data)) + . = 0x1030 ; + KEEP(*(.boot_hdr.dcd_data)) + __boot_hdr_end__ = ABSOLUTE(.) ; + . = 0x2000 ; + } >flash + + .vectors : + { + KEEP(*(.vectors)) + *(.text .text.__start) + } >flash + + .itcmfunc : + { + . = ALIGN(8); + _sitcmfuncs = ABSOLUTE(.); + FILL(0xFF) + . = 0x40 ; + INCLUDE "itcm_static_functions.ld" + . = ALIGN(8); + _eitcmfuncs = ABSOLUTE(.); + } > itcm AT > flash + + _fitcmfuncs = LOADADDR(.itcmfunc); + + /* The RAM vector table (if present) should lie at the beginning of SRAM */ + + .ram_vectors (COPY) : { + *(.ram_vectors) + } > dtcm + + .text : ALIGN(4) + { + _stext = ABSOLUTE(.); + *(.vectors) + . = ALIGN(32); + /* + This signature provides the bootloader with a way to delay booting + */ + _bootdelay_signature = ABSOLUTE(.); + FILL(0xffecc2925d7d05c5) + . += 8; + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.gnu.linkonce.t.*) + *(.glue_7) + *(.glue_7t) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + . = ALIGN(4096); + _etext = ABSOLUTE(.); + _srodata = ABSOLUTE(.); + *(.rodata .rodata.*) + . = ALIGN(4096); + _erodata = ABSOLUTE(.); + } > flash + + .init_section : + { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + .ARM.extab : + { + *(.ARM.extab*) + } > flash + + .ARM.exidx : + { + __exidx_start = ABSOLUTE(.); + *(.ARM.exidx*) + __exidx_end = ABSOLUTE(.); + } > flash + + _eronly = ABSOLUTE(.); + + .data : + { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + . = ALIGN(4); + _edata = ABSOLUTE(.); + } > sram AT > flash + + .ramfunc ALIGN(4): + { + _sramfuncs = ABSOLUTE(.); + *(.ramfunc .ramfunc.*) + _eramfuncs = ABSOLUTE(.); + } > sram AT > flash + + _framfuncs = LOADADDR(.ramfunc); + + .bss : + { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + . = ALIGN(4); + _ebss = ABSOLUTE(.); + } > sram + + /* 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) } + + _boot_loadaddr = ORIGIN(flash); + _boot_size = LENGTH(flash); + _ram_size = LENGTH(sram); + _sdtcm = ORIGIN(dtcm); + _edtcm = ORIGIN(dtcm) + LENGTH(dtcm); +} diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 5951ceed7742..9b169ae36005 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -1,65 +1,3 @@ -/* Static */ -*(.text.arm_ack_irq) -*(.text.arm_doirq) -*(.text.arm_svcall) -*(.text.arm_switchcontext) -*(.text.board_autoled_on) -*(.text.clock_timer) -*(.text.exception_common) -*(.text.hrt_absolute_time) -*(.text.hrt_tim_isr) -*(.text.imxrt_configwaitints) -*(.text.imxrt_dma_callback) -*(.text.imxrt_dmach_interrupt) -*(.text.imxrt_dmaterminate) -*(.text.imxrt_edma_interrupt) -*(.text.imxrt_endwait) -*(.text.imxrt_gpio3_16_31_interrupt) -*(.text.imxrt_interrupt) -*(.text.imxrt_lpi2c_isr) -*(.text.imxrt_recvdma) -*(.text.imxrt_tcd_free) -*(.text.imxrt_timerisr) -*(.text.imxrt_usbinterrupt) -*(.text.irq_dispatch) -*(.text.memcpy) -*(.text.nxsched_add_blocked) -*(.text.nxsched_add_prioritized) -*(.text.nxsched_add_readytorun) -*(.text.nxsched_get_files) -*(.text.nxsched_get_tcb) -*(.text.nxsched_merge_pending) -*(.text.nxsched_process_timer) -*(.text.nxsched_remove_blocked) -*(.text.nxsched_remove_readytorun) -*(.text.nxsched_resume_scheduler) -*(.text.nxsched_suspend_scheduler) -*(.text.nxsem_add_holder) -*(.text.nxsem_add_holder_tcb) -*(.text.nxsem_clockwait) -*(.text.nxsem_foreachholder) -*(.text.nxsem_freecount0holder) -*(.text.nxsem_freeholder) -*(.text.nxsem_post) -*(.text.nxsem_release_holder) -*(.text.nxsem_restore_baseprio) -*(.text.nxsem_tickwait) -*(.text.nxsem_timeout) -*(.text.nxsem_trywait) -*(.text.nxsem_wait) -*(.text.nxsem_wait_uninterruptible) -*(.text.nxsig_timedwait) -*(.text.sched_lock) -*(.text.sched_note_resume) -*(.text.sched_note_suspend) -*(.text.sched_unlock) -*(.text.sq_addafter) -*(.text.sq_addlast) -*(.text.sq_rem) -*(.text.sq_remafter) -*(.text.sq_remfirst) -*(.text.uart_connected) -*(.text.wd_timer) /* Auto-generated */ *(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj) *(.text._ZN13MavlinkStream6updateERKy) @@ -70,11 +8,9 @@ *(.text._ZN22MulticopterRateControl3RunEv.part.0) *(.text._ZN7Mavlink9task_mainEiPPc) *(.text._ZN7sensors22VehicleAngularVelocity3RunEv) -*(.text.memset) *(.text._ZN4uORB12Subscription9subscribeEv.part.0) *(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb) *(.text._ZN4uORB10DeviceNode5writeEP4filePKcj) -*(.text.strcmp) *(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv) *(.text._ZN4uORB12DeviceMaster19getDeviceNodeLockedEPK12orb_metadatah) *(.text._Z12get_orb_meta6ORB_ID) @@ -82,15 +18,12 @@ *(.text._ZN3px49WorkQueue3RunEv) *(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh) *(.text._ZN4EKF23RunEv) -*(.text.imxrt_lpspi_exchange) -*(.text.imxrt_dmach_xfrsetup) *(.text._ZN7sensors10VehicleIMU7PublishEv) *(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE) *(.text._ZN7sensors10VehicleIMU10UpdateGyroEv) *(.text._ZN9ICM42688P8FIFOReadERKyh) *(.text._ZN3Ekf20controlGravityFusionERKN9estimator9imuSampleE) *(.text._ZN16PX4Accelerometer10updateFIFOER19sensor_accel_fifo_s) -*(.text.up_block_task) *(.text._ZN7sensors22VehicleAngularVelocity19CalibrateAndPublishERKyRKN6matrix7Vector3IfEES7_) *(.text._ZN4uORB12Subscription10advertisedEv) *(.text._ZNK15AttitudeControl6updateERKN6matrix10QuaternionIfEE) @@ -99,7 +32,6 @@ *(.text._ZN4uORB12Subscription6updateEPv) *(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s) *(.text._ZN7sensors10VehicleIMU3RunEv) -*(.text.up_unblock_task) *(.text.__aeabi_l2f) *(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_) *(.text.pthread_mutex_timedlock) @@ -121,16 +53,12 @@ *(.text._ZN9ICM42688P7RunImplEv) *(.text._ZN4uORB12Subscription9subscribeEv) *(.text.param_get) -*(.text._do_memcpy) *(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb) *(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE) -*(.text.wd_start) -*(.text.hrt_call_enter) *(.text._ZN4EKF220PublishLocalPositionERKy) *(.text._mav_finalize_message_chan_send) *(.text._ZN3Ekf19fixCovarianceErrorsEb) *(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb) -*(.text.ioctl) *(.text._ZN6events12SendProtocol6updateERKy) *(.text._ZN6device3SPI8transferEPhS1_j) *(.text._ZN27MavlinkStreamDistanceSensor4sendEv) @@ -226,7 +154,6 @@ *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE) *(.text._ZN9Commander13dataLinkCheckEv) *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex) -*(.text._ZNK3Ekf26get_innovation_test_statusERtRfS1_S1_S1_S1_S1_S1_) *(.text._ZN12PX4Gyroscope9set_scaleEf) *(.text._ZN12FailsafeBase6updateERKyRKNS_5StateEbbRK16failsafe_flags_s) *(.text._ZN18MavlinkStreamDebug4sendEv) @@ -408,7 +335,6 @@ *(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) *(.text._ZN29MavlinkStreamObstacleDistance4sendEv) *(.text._ZN24MavlinkStreamOrbitStatus4sendEv) -*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf) *(.text._ZN9Navigator3runEv) *(.text._ZN24MavlinkParametersManager11send_paramsEv) *(.text._ZN17MavlinkLogHandler4sendEv) @@ -447,7 +373,6 @@ *(.text.imxrt_ioctl) *(.text._ZN3Ekf25checkMagBiasObservabilityEv) *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) -*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s) *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) *(.text._ZN19StickAccelerationXYC1EP12ModuleParams) *(.text.imxrt_epsubmit) @@ -531,7 +456,6 @@ *(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) *(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) *(.text._ZN25MavlinkStreamMagCalReport4sendEv) -*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf) *(.text.imxrt_config_gpio) *(.text.nxsig_timeout) *(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) @@ -666,7 +590,6 @@ *(.text.uart_xmitchars_done) *(.text._ZN4EKF225PublishYawEstimatorStatusERKy) *(.text.sin) -*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf) *(.text._ZN6Safety19safetyButtonHandlerEv) *(.text._ZN3Ekf19controlAuxVelFusionEv) *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_) @@ -740,10 +663,8 @@ *(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv) *(.text.iob_navail) *(.text._ZN12FailsafeBase25removeNonActivatedActionsEv) -*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf) *(.text._ZN15TakeoffHandling10updateRampEff) *(.text._Z7led_offi) -*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv) *(.text.led_off) *(.text.udp_wrbuffer_test) *(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s) @@ -755,7 +676,6 @@ *(.text._ZN18MavlinkStreamDebug8get_sizeEv) *(.text._ZN12GPSDriverUBX7receiveEj) *(.text._ZN13BatteryStatus21parameter_update_pollEb) -*(.text._ZN3Ekf26checkYawAngleObservabilityEv) *(.text._ZN3RTL18updateDatamanCacheEv) *(.text.__ieee754_sqrtf) *(.text._ZThn24_N18mag_bias_estimator16MagBiasEstimator3RunEv) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld new file mode 100644 index 000000000000..4968488245e5 --- /dev/null +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_static_functions.ld @@ -0,0 +1,73 @@ +/* Static */ +*(.text.arm_ack_irq) +*(.text.arm_doirq) +*(.text.arm_svcall) +*(.text.arm_switchcontext) +*(.text.board_autoled_on) +*(.text.clock_timer) +*(.text.exception_common) +*(.text.flexio_irq_handler) +*(.text.hrt_absolute_time) +*(.text.hrt_call_enter) +*(.text.hrt_tim_isr) +*(.text.imxrt_configwaitints) +*(.text.imxrt_dma_callback) +*(.text.imxrt_dmach_interrupt) +*(.text.imxrt_dmach_xfrsetup) +*(.text.imxrt_dmaterminate) +*(.text.imxrt_edma_interrupt) +*(.text.imxrt_endwait) +*(.text.imxrt_gpio3_16_31_interrupt) +*(.text.imxrt_interrupt) +*(.text.imxrt_lpi2c_isr) +*(.text.imxrt_lpspi_exchange) +*(.text.imxrt_recvdma) +*(.text.imxrt_tcd_free) +*(.text.imxrt_timerisr) +*(.text.imxrt_usbinterrupt) +*(.text.irq_dispatch) +*(.text.ioctl) +*(.text.memcpy) +*(.text.memset) +*(.text.nxsched_add_blocked) +*(.text.nxsched_add_prioritized) +*(.text.nxsched_add_readytorun) +*(.text.nxsched_get_files) +*(.text.nxsched_get_tcb) +*(.text.nxsched_merge_pending) +*(.text.nxsched_process_timer) +*(.text.nxsched_remove_blocked) +*(.text.nxsched_remove_readytorun) +*(.text.nxsched_resume_scheduler) +*(.text.nxsched_suspend_scheduler) +*(.text.nxsem_add_holder) +*(.text.nxsem_add_holder_tcb) +*(.text.nxsem_clockwait) +*(.text.nxsem_foreachholder) +*(.text.nxsem_freecount0holder) +*(.text.nxsem_freeholder) +*(.text.nxsem_post) +*(.text.nxsem_release_holder) +*(.text.nxsem_restore_baseprio) +*(.text.nxsem_tickwait) +*(.text.nxsem_timeout) +*(.text.nxsem_trywait) +*(.text.nxsem_wait) +*(.text.nxsem_wait_uninterruptible) +*(.text.nxsig_timedwait) +*(.text.sched_lock) +*(.text.sched_note_resume) +*(.text.sched_note_suspend) +*(.text.sched_unlock) +*(.text.strcmp) +*(.text.sq_addafter) +*(.text.sq_addlast) +*(.text.sq_rem) +*(.text.sq_remafter) +*(.text.sq_remfirst) +*(.text.uart_connected) +*(.text.up_block_task) +*(.text.up_unblock_task) +*(.text.wd_timer) +*(.text.wd_start) +*(.text._do_memcpy) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/script.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/script.ld index dc05748b6adf..70d861f30ab2 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/script.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/script.ld @@ -83,6 +83,7 @@ SECTIONS _sitcmfuncs = ABSOLUTE(.); FILL(0xFF) . = 0x40 ; + INCLUDE "itcm_static_functions.ld" INCLUDE "itcm_functions_includes.ld" . = ALIGN(8); _eitcmfuncs = ABSOLUTE(.); diff --git a/boards/px4/fmu-v6xrt/rover.px4board b/boards/px4/fmu-v6xrt/rover.px4board index 2e75c9bdb47b..95a01552267c 100644 --- a/boards/px4/fmu-v6xrt/rover.px4board +++ b/boards/px4/fmu-v6xrt/rover.px4board @@ -14,4 +14,6 @@ CONFIG_MODULES_MC_RATE_CONTROL=n CONFIG_MODULES_VTOL_ATT_CONTROL=n CONFIG_EKF2_AUX_GLOBAL_POSITION=y # CONFIG_EKF2_WIND is not set -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_DRIVERS_ROBOCLAW=y diff --git a/boards/px4/fmu-v6xrt/src/board_config.h b/boards/px4/fmu-v6xrt/src/board_config.h index a2edff8dabc8..059d0ac696d8 100644 --- a/boards/px4/fmu-v6xrt/src/board_config.h +++ b/boards/px4/fmu-v6xrt/src/board_config.h @@ -455,6 +455,7 @@ #define RC_SERIAL_SINGLEWIRE 1 // Suport Single wire wiring #define RC_SERIAL_SWAP_RXTX 1 // Set Swap (but not supported in HW) to use Single wire #define RC_SERIAL_SWAP_USING_SINGLEWIRE 1 // Set to use Single wire swap as HW does not support swap +#define BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT /* FLEXSPI4 */ diff --git a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c index d83265ce8f55..3223d585aa76 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c +++ b/boards/px4/fmu-v6xrt/src/imxrt_clockconfig.c @@ -114,11 +114,11 @@ const struct clock_configuration_s g_initial_clkconfig = { .div = 1, .mux = ACMP_CLK_ROOT_OSC_RC_48M_DIV2, }, - .flexio1_clk_root = + .flexio1_clk_root = /* 432 / 4 = 108Mhz */ { .enable = 1, - .div = 2, - .mux = FLEXIO1_CLK_ROOT_SYS_PLL3_DIV2, + .div = 4, + .mux = FLEXIO1_CLK_ROOT_SYS_PLL2_PFD3, }, .flexio2_clk_root = { @@ -492,9 +492,9 @@ const struct clock_configuration_s g_initial_clkconfig = { .mfd = 268435455, .ss_enable = 0, .pfd0 = 27, /* (528 * 18) / 27 = 352 MHz */ - .pfd1 = 16, /* (528 * 16) / 16 = 594 MHz */ - .pfd2 = 24, /* (528 * 24) / 27 = 396 MHz */ - .pfd3 = 32, /* (528 * 32) / 27 = 297 MHz */ + .pfd1 = 16, /* (528 * 18) / 16 = 594 MHz */ + .pfd2 = 24, /* (528 * 18) / 24 = 396 MHz */ + .pfd3 = 22, /* (528 * 18) / 22 = 216 MHz */ }, .sys_pll3 = { diff --git a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c b/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c index 6ae7589d60ef..dfdf13da6dbe 100644 --- a/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c +++ b/boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.c @@ -104,9 +104,10 @@ const struct flexspi_nor_config_s g_flash_fast_config = { .busyBitPolarity = 0u, .lookupTable = { - /* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data //200Mhz 18 dummy=10+8 + /* Read */// EEH+11H+32bit addr+20dummy cycles+ 4Bytes read data + /* Macronix manual says 20 dummy cycles @ 200Mhz, FlexSPI peripheral Operand value needs to be 2N in DDR mode hence 0x28 */ [0 + 0] = FLEXSPI_LUT_SEQ(CMD_DDR, FLEXSPI_8PAD, 0xEE, CMD_DDR, FLEXSPI_8PAD, 0x11), //0x871187ee, - [0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x04),//0xb3048b20, + [0 + 1] = FLEXSPI_LUT_SEQ(RADDR_DDR, FLEXSPI_8PAD, 0x20, DUMMY_DDR, FLEXSPI_8PAD, 0x28),//0xb3288b20, [0 + 2] = FLEXSPI_LUT_SEQ(READ_DDR, FLEXSPI_8PAD, 0x04, STOP_EXE, FLEXSPI_1PAD, 0x00), //0xa704, /* Read status */ diff --git a/boards/px4/fmu-v6xrt/src/mtd.cpp b/boards/px4/fmu-v6xrt/src/mtd.cpp index 38fa63a7eda3..68f79e4897e8 100644 --- a/boards/px4/fmu-v6xrt/src/mtd.cpp +++ b/boards/px4/fmu-v6xrt/src/mtd.cpp @@ -53,18 +53,12 @@ static const px4_mft_device_t i2c6 = { // 24LC64T on BASE 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &qspi_flash, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = 256 } }, }; diff --git a/boards/px4/fmu-v6xrt/src/timer_config.cpp b/boards/px4/fmu-v6xrt/src/timer_config.cpp index 1b1497c10137..04be42b4d8ed 100644 --- a/boards/px4/fmu-v6xrt/src/timer_config.cpp +++ b/boards/px4/fmu-v6xrt/src/timer_config.cpp @@ -91,14 +91,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { - initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO23_1, 23), - initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO25_1, 25), - initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO27_1, 27), - initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO06_1, 6), - initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1, GPIO_FLEXIO1_FLEXIO08_1, 8), - initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2, GPIO_FLEXIO1_FLEXIO10_1, 10), - initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3, GPIO_FLEXIO1_FLEXIO19_1, 19), - initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0, GPIO_FLEXIO1_FLEXIO29_1, 29), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule0), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule1), + initIOPWMDshot(PWM::FlexPWM1, PWM::Submodule2), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule0), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule1), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule2), + initIOPWMDshot(PWM::FlexPWM2, PWM::Submodule3), + initIOPWMDshot(PWM::FlexPWM3, PWM::Submodule0), initIOPWM(PWM::FlexPWM3, PWM::Submodule1), initIOPWM(PWM::FlexPWM3, PWM::Submodule3), initIOPWM(PWM::FlexPWM4, PWM::Submodule0), @@ -106,14 +106,14 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { }; constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { - /* FMU_CH1 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23), - /* FMU_CH2 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25), - /* FMU_CH3 */ initIOTimerChannel(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27), - /* FMU_CH4 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06), - /* FMU_CH5 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08), - /* FMU_CH6 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10), - /* FMU_CH7 */ initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19), - /* FMU_CH8 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29), + /* FMU_CH1 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_23, GPIO_FLEXIO1_FLEXIO23_1 | IOMUX_DSHOT_DEFAULT, 23), + /* FMU_CH2 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_25, GPIO_FLEXIO1_FLEXIO25_1 | IOMUX_DSHOT_DEFAULT, 25), + /* FMU_CH3 */ initIOTimerChannelDshot(io_timers, {PWM::PWM1_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_27, GPIO_FLEXIO1_FLEXIO27_1 | IOMUX_DSHOT_DEFAULT, 27), + /* FMU_CH4 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_06, GPIO_FLEXIO1_FLEXIO06_1 | IOMUX_DSHOT_DEFAULT, 6), + /* FMU_CH5 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_08, GPIO_FLEXIO1_FLEXIO08_1 | IOMUX_DSHOT_DEFAULT, 8), + /* FMU_CH6 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule2}, IOMUX::Pad::GPIO_EMC_B1_10, GPIO_FLEXIO1_FLEXIO10_1 | IOMUX_DSHOT_DEFAULT, 10), + /* FMU_CH7 */ initIOTimerChannelDshot(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_19, GPIO_FLEXIO1_FLEXIO19_1 | IOMUX_DSHOT_DEFAULT, 19), + /* FMU_CH8 */ initIOTimerChannelDshot(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_29, GPIO_FLEXIO1_FLEXIO29_1 | IOMUX_DSHOT_DEFAULT, 29), /* FMU_CH9 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule1}, IOMUX::Pad::GPIO_EMC_B1_31), /* FMU_CH10 */ initIOTimerChannel(io_timers, {PWM::PWM3_PWM_A, PWM::Submodule3}, IOMUX::Pad::GPIO_EMC_B1_21), /* FMU_CH11 */ initIOTimerChannel(io_timers, {PWM::PWM4_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_EMC_B1_00), diff --git a/boards/px4/sitl/allyes.px4board b/boards/px4/sitl/allyes.px4board new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index 7dd7ce481a3f..ac228113d5d8 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -1,6 +1,7 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_TESTING=y CONFIG_BOARD_ETHERNET=y +CONFIG_BOARD_ROOT_PATH="." CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_OSD_MSP_OSD=y @@ -12,7 +13,6 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_DIFFERENTIAL_DRIVE=y CONFIG_MODULES_EKF2=y CONFIG_EKF2_VERBOSE_STATUS=y CONFIG_EKF2_AUX_GLOBAL_POSITION=y @@ -45,6 +45,8 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_MODULES_PAYLOAD_DELIVERER=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_REPLAY=y +CONFIG_MODULES_ROVER_ACKERMANN=y +CONFIG_MODULES_ROVER_DIFFERENTIAL=y CONFIG_MODULES_ROVER_POS_CONTROL=y CONFIG_MODULES_SENSORS=y CONFIG_COMMON_SIMULATION=y diff --git a/boards/px4/sitl/zenoh.px4board b/boards/px4/sitl/zenoh.px4board index 58a29ad52fd8..20ff39d1b00e 100644 --- a/boards/px4/sitl/zenoh.px4board +++ b/boards/px4/sitl/zenoh.px4board @@ -1,3 +1,2 @@ CONFIG_MODULES_UXRCE_DDS_CLIENT=n CONFIG_MODULES_ZENOH=y -CONFIG_ZENOH_CONFIG_PATH="./zenoh" diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index a5727267a1fa..410f8d4788a9 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -6,6 +6,7 @@ CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y diff --git a/boards/raspberrypi/pico/init/rc.board_defaults b/boards/raspberrypi/pico/init/rc.board_defaults index f1a26f4a51b1..6dc2fb4121d4 100644 --- a/boards/raspberrypi/pico/init/rc.board_defaults +++ b/boards/raspberrypi/pico/init/rc.board_defaults @@ -11,4 +11,3 @@ param set-default CBRK_SUPPLY_CHK 894281 # Disable safety switch by default param set-default CBRK_IO_SAFETY 22027 - diff --git a/boards/raspberrypi/pico/src/usb.c b/boards/raspberrypi/pico/src/usb.c index bc6306ae6bc2..9278997cc8ba 100644 --- a/boards/raspberrypi/pico/src/usb.c +++ b/boards/raspberrypi/pico/src/usb.c @@ -82,4 +82,3 @@ __EXPORT void rp2040_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/boards/siyi/n7/default.px4board b/boards/siyi/n7/default.px4board index cb42f3530c8a..2a7a23258186 100644 --- a/boards/siyi/n7/default.px4board +++ b/boards/siyi/n7/default.px4board @@ -8,6 +8,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/siyi/n7/extras/siyi_n7_bootloader.bin b/boards/siyi/n7/extras/siyi_n7_bootloader.bin index 6f0ff949f506..3f8c6fa8c2b6 100755 Binary files a/boards/siyi/n7/extras/siyi_n7_bootloader.bin and b/boards/siyi/n7/extras/siyi_n7_bootloader.bin differ diff --git a/boards/siyi/n7/nuttx-config/include/board.h b/boards/siyi/n7/nuttx-config/include/board.h index 1df145ed26db..48edff42e4ef 100644 --- a/boards/siyi/n7/nuttx-config/include/board.h +++ b/boards/siyi/n7/nuttx-config/include/board.h @@ -376,6 +376,3 @@ #define GPIO_I2C4_SCL GPIO_I2C4_SCL_2 /* PF14 */ #define GPIO_I2C4_SDA GPIO_I2C4_SDA_2 /* PF15 */ - - - diff --git a/boards/siyi/n7/nuttx-config/include/board_dma_map.h b/boards/siyi/n7/nuttx-config/include/board_dma_map.h index 6577106c2363..fc7d7d27da79 100644 --- a/boards/siyi/n7/nuttx-config/include/board_dma_map.h +++ b/boards/siyi/n7/nuttx-config/include/board_dma_map.h @@ -42,4 +42,3 @@ #define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_0 /* DMA1:81 */ #define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_0 /* DMA1:82 */ - diff --git a/boards/sky-drones/smartap-airlink/default.px4board b/boards/sky-drones/smartap-airlink/default.px4board index c57168389c58..fdfdcdea6a1b 100644 --- a/boards/sky-drones/smartap-airlink/default.px4board +++ b/boards/sky-drones/smartap-airlink/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y @@ -49,7 +50,6 @@ CONFIG_MODULES_FW_POS_CONTROL=y CONFIG_MODULES_FW_RATE_CONTROL=y CONFIG_MODULES_GIMBAL=y CONFIG_MODULES_GYRO_CALIBRATION=y -CONFIG_MODULES_GYRO_FFT=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y CONFIG_MODULES_LOAD_MON=y @@ -86,7 +86,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_PERF=y CONFIG_SYSTEMCMDS_REBOOT=y -CONFIG_SYSTEMCMDS_REFLECT=y CONFIG_SYSTEMCMDS_SD_BENCH=y CONFIG_SYSTEMCMDS_SD_STRESS=y CONFIG_SYSTEMCMDS_SERIAL_TEST=y diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_defaults b/boards/sky-drones/smartap-airlink/init/rc.board_defaults index 212c72657f44..4a38b479f3c3 100644 --- a/boards/sky-drones/smartap-airlink/init/rc.board_defaults +++ b/boards/sky-drones/smartap-airlink/init/rc.board_defaults @@ -13,7 +13,6 @@ param set-default SENS_EN_THERMAL 1 # Enable heater param set-default SENS_TEMP_ID 2359314 # Heated IMU ID # Battery scaling -param set-default BAT_N_CELLS 4 param set-default BAT1_N_CELLS 4 param set-default BAT1_V_DIV 15.51 diff --git a/boards/sky-drones/smartap-airlink/init/rc.board_sensors b/boards/sky-drones/smartap-airlink/init/rc.board_sensors index 5ce477bf181d..95adb36b827d 100644 --- a/boards/sky-drones/smartap-airlink/init/rc.board_sensors +++ b/boards/sky-drones/smartap-airlink/init/rc.board_sensors @@ -31,4 +31,3 @@ lis3mdl -R 2 -X start # NCP5623 Led driver rgbled_ncp5623c -X -a 0x38 start - diff --git a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig index 25c5120d5740..b28719b13e93 100644 --- a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig +++ b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig @@ -141,8 +141,8 @@ CONFIG_NETDB_DNSCLIENT_ENTRIES=8 CONFIG_NETDB_DNSSERVER_NOADDR=y CONFIG_NETDEV_PHY_IOCTL=y CONFIG_NETINIT_DNS=y -CONFIG_NETINIT_DNSIPADDR=0XC0A800FE -CONFIG_NETINIT_DRIPADDR=0XC0A800FE +CONFIG_NETINIT_DNSIPADDR=0xA290AFE +CONFIG_NETINIT_DRIPADDR=0xA290AFE CONFIG_NETINIT_THREAD=y CONFIG_NETINIT_THREAD_PRIORITY=49 CONFIG_NETUTILS_TELNETD=y diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h index 5b957e0694cc..158fece58c8a 100644 --- a/boards/sky-drones/smartap-airlink/src/board_config.h +++ b/boards/sky-drones/smartap-airlink/src/board_config.h @@ -196,7 +196,7 @@ */ #define DIRECT_PWM_OUTPUT_CHANNELS 8 -/* Define Battery 1 g Divider and A per V. */ +/* Define Battery Voltage Divider and A per V */ #define BOARD_BATTERY_V_DIV (13.653333333f) #define BOARD_BATTERY_A_PER_V (36.367515152f) diff --git a/boards/spracing/h7extreme/default.px4board b/boards/spracing/h7extreme/default.px4board index c6c28729a73c..aa9790560907 100644 --- a/boards/spracing/h7extreme/default.px4board +++ b/boards/spracing/h7extreme/default.px4board @@ -4,6 +4,7 @@ CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y diff --git a/boards/spracing/h7extreme/init/rc.board_defaults b/boards/spracing/h7extreme/init/rc.board_defaults index d2b9948883d9..92a8d971ab7b 100644 --- a/boards/spracing/h7extreme/init/rc.board_defaults +++ b/boards/spracing/h7extreme/init/rc.board_defaults @@ -12,9 +12,4 @@ param set-default CBRK_SUPPLY_CHK 894281 # Select the Generic 250 Racer by default param set-default SYS_AUTOSTART 4050 -# use the Q attitude estimator, it works w/o mag or GPS. -param set-default SYS_MC_EST_GROUP 3 -param set-default ATT_W_ACC 0.4 -param set-default ATT_W_GYRO_BIAS 0 - param set-default SYS_HAS_MAG 0 diff --git a/boards/spracing/h7extreme/init/rc.board_extras b/boards/spracing/h7extreme/init/rc.board_extras index 25e6c269a85f..e3aa0d196175 100644 --- a/boards/spracing/h7extreme/init/rc.board_extras +++ b/boards/spracing/h7extreme/init/rc.board_extras @@ -7,5 +7,3 @@ if ! param compare OSD_ATXXXX_CFG 0 then atxxxx start -s fi - - diff --git a/boards/spracing/h7extreme/nuttx-config/Kconfig b/boards/spracing/h7extreme/nuttx-config/Kconfig index ab39ed07497e..8b3754476f0e 100644 --- a/boards/spracing/h7extreme/nuttx-config/Kconfig +++ b/boards/spracing/h7extreme/nuttx-config/Kconfig @@ -15,10 +15,9 @@ config BOARD_USE_PROBES ---help--- Select to use GPIO FMU-CH1-5, CAP1-6 to provide timing signals from selected drivers. - - + + config STM32_RAMFUNCS bool "Use stm32_board_clockconfig as ram function." default y select ARCH_HAVE_RAMFUNCS - diff --git a/boards/spracing/h7extreme/src/rcc.c b/boards/spracing/h7extreme/src/rcc.c index cd4299b880ea..73e9506040d8 100644 --- a/boards/spracing/h7extreme/src/rcc.c +++ b/boards/spracing/h7extreme/src/rcc.c @@ -364,7 +364,7 @@ __ramfunc__ void stm32_board_clockconfig(void) */ regval = getreg32(STM32_PWR_CR3); - regval |= STM32_PWR_CR3_LDOEN | STM32_PWR_CR3_LDOESCUEN; + regval |= STM32_PWR_CR3_LDOEN | STM32_PWR_CR3_SCUEN; putreg32(regval, STM32_PWR_CR3); /* Set the voltage output scale */ diff --git a/boards/spracing/h7extreme/src/timer_config.cpp b/boards/spracing/h7extreme/src/timer_config.cpp index d7c83bbd9aac..88bde6223e1f 100644 --- a/boards/spracing/h7extreme/src/timer_config.cpp +++ b/boards/spracing/h7extreme/src/timer_config.cpp @@ -60,4 +60,3 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = { constexpr io_timers_channel_mapping_t io_timers_channel_mapping = initIOTimerChannelMapping(io_timers, timer_io_channels); - diff --git a/boards/thepeach/k1/default.px4board b/boards/thepeach/k1/default.px4board index 39fefb9b68a8..d760279546c7 100644 --- a/boards/thepeach/k1/default.px4board +++ b/boards/thepeach/k1/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/thepeach/r1/default.px4board b/boards/thepeach/r1/default.px4board index 39fefb9b68a8..d760279546c7 100644 --- a/boards/thepeach/r1/default.px4board +++ b/boards/thepeach/r1/default.px4board @@ -11,6 +11,7 @@ CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y diff --git a/boards/uvify/core/default.px4board b/boards/uvify/core/default.px4board index bdc591a2810d..8c144aa6062e 100644 --- a/boards/uvify/core/default.px4board +++ b/boards/uvify/core/default.px4board @@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_MS5611=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y +CONFIG_DRIVERS_CDCACM_AUTOSTART=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y diff --git a/boards/uvify/core/src/usb.c b/boards/uvify/core/src/usb.c index 32f853b7472a..084a49ed7895 100644 --- a/boards/uvify/core/src/usb.c +++ b/boards/uvify/core/src/usb.c @@ -105,4 +105,3 @@ __EXPORT void stm32_usbsuspend(FAR struct usbdev_s *dev, bool resume) { uinfo("resume: %d\n", resume); } - diff --git a/cmake/doxygen.cmake b/cmake/doxygen.cmake index a410fe39db63..d9d64232a181 100644 --- a/cmake/doxygen.cmake +++ b/cmake/doxygen.cmake @@ -59,4 +59,3 @@ if (BUILD_DOXYGEN) message("Doxygen needs to be installed to generate documentation") endif() endif() - diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index eb418bf6cc8c..7eaaa00d676c 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -25,6 +25,7 @@ set(COMMON_KCONFIG_ENV_SETTINGS TOOLCHAIN=${CMAKE_TOOLCHAIN_FILE} ARCHITECTURE=${CMAKE_SYSTEM_PROCESSOR} ROMFSROOT=${config_romfs_root} + BASE_DEFCONFIG=${BOARD_CONFIG} ) set(config_user_list) @@ -52,6 +53,15 @@ if(EXISTS ${BOARD_DEFCONFIG}) ) endif() + if(${LABEL} MATCHES "allyes") + message(AUTHOR_WARNING "allyes build: allyes is for CI coverage and not for use in production") + execute_process( + COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} + ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/kconfig/allyesconfig.py + WORKING_DIRECTORY ${PX4_SOURCE_DIR} + ) + endif() + # Generate header file for C/C++ preprocessor execute_process( COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} @@ -438,7 +448,7 @@ if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCH COMMAND_EXPAND_LISTS ) -else() +elseif(NOT ${LABEL} MATCHES "allyes") # All other configs except allyes which isn't configurable add_custom_target(boardconfig ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${MENUCONFIG_PATH} Kconfig COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS} ${SAVEDEFCONFIG_PATH} diff --git a/cmake/metadata.cmake b/cmake/metadata.cmake index 59ae6bd82990..ef946c703159 100644 --- a/cmake/metadata.cmake +++ b/cmake/metadata.cmake @@ -74,13 +74,13 @@ add_custom_target(metadata_parameters --markdown ${PX4_BINARY_DIR}/docs/parameters.md COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --json ${PX4_BINARY_DIR}/docs/parameters.json --compress COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/src/lib/parameters/px_process_params.py - --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` + --src-path `find ${PX4_SOURCE_DIR}/src -maxdepth 4 -type d` ${generated_params_dir} --inject-xml ${PX4_SOURCE_DIR}/src/lib/parameters/parameters_injected.xml --xml ${PX4_BINARY_DIR}/docs/parameters.xml diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index 8d7f6e647828..0692f11ec1ec 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -308,7 +308,7 @@ def test_mission(self): self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) # TODO: fix by excluding initial heading init and reset preflight - self.assertTrue(res['yaw_error_std'] < 10.0, str(res)) + self.assertTrue(res['yaw_error_std'] < 15.0, str(res)) if __name__ == '__main__': diff --git a/integrationtests/python_src/px4_it/util/manual_input.py b/integrationtests/python_src/px4_it/util/manual_input.py index d1381d99d648..66d484381c88 100755 --- a/integrationtests/python_src/px4_it/util/manual_input.py +++ b/integrationtests/python_src/px4_it/util/manual_input.py @@ -166,4 +166,3 @@ def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=T self.pub_mcsp.publish(pos) rate.sleep() count = count + 1 - diff --git a/integrationtests/python_src/px4_it/util/px4_test_helper.py b/integrationtests/python_src/px4_it/util/px4_test_helper.py index 4dc8866341e8..b069b70c40f5 100644 --- a/integrationtests/python_src/px4_it/util/px4_test_helper.py +++ b/integrationtests/python_src/px4_it/util/px4_test_helper.py @@ -108,4 +108,3 @@ def bag_write(self, topic, data): rospy.logwarn("Trying to write to bag but it's already closed") finally: self.condition.release() - diff --git a/msg/AirspeedValidated.msg b/msg/AirspeedValidated.msg index 06731cc4106a..9ee0f518314e 100644 --- a/msg/AirspeedValidated.msg +++ b/msg/AirspeedValidated.msg @@ -10,3 +10,7 @@ float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspe bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid + +float32 airspeed_derivative_filtered # filtered indicated airspeed derivative [m/s/s] +float32 throttle_filtered # filtered fixed-wing throttle [-] +float32 pitch_filtered # filtered pitch [rad] diff --git a/msg/BatteryStatus.msg b/msg/BatteryStatus.msg index 66fcffa0e68f..b5155308f229 100644 --- a/msg/BatteryStatus.msg +++ b/msg/BatteryStatus.msg @@ -1,9 +1,7 @@ uint64 timestamp # time since system start (microseconds) bool connected # Whether or not a battery is connected, based on a voltage threshold float32 voltage_v # Battery voltage in volts, 0 if unknown -float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown float32 current_a # Battery current in amperes, -1 if unknown -float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown float32 discharged_mah # Discharged amount in mAh, -1 if unknown float32 remaining # From 1 to 0, -1 if unknown @@ -52,27 +50,23 @@ uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem -uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature +uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. -uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable. uint8 warning # Current battery warning -uint8 mode # Battery mode. Note, the normal operation mode - -uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational -uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level) -uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode -uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)! - uint8 MAX_INSTANCES = 4 -float32 average_power # The average power of the current discharge -float32 available_energy # The predicted charge or energy remaining in the battery float32 full_charge_capacity_wh # The compensated battery capacity float32 remaining_capacity_wh # The compensated battery capacity remaining -float32 design_capacity # The design capacity of the battery -uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes uint16 over_discharge_count # Number of battery overdischarge float32 nominal_voltage # Nominal voltage of the battery pack + +float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate +float32 ocv_estimate # [V] Open circuit voltage estimate +float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate +float32 volt_based_soc_estimate # [0, 1] Normalized volt based state of charge estimate +float32 voltage_prediction # [V] Predicted voltage +float32 prediction_error # [V] Prediction error +float32 estimation_covariance_norm # Norm of the covariance matrix diff --git a/msg/Buffer128.msg b/msg/Buffer128.msg index 342aa83dbe8d..4ff939a87d07 100644 --- a/msg/Buffer128.msg +++ b/msg/Buffer128.msg @@ -6,4 +6,3 @@ uint32 MAX_BUFLEN = 128 uint8[128] data # data # TOPICS voxl2_io_data - diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3b1e2647e3c5..3b31b7de554d 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -58,6 +58,7 @@ set(msg_files CameraCapture.msg CameraStatus.msg CameraTrigger.msg + CanInterfaceStatus.msg CellularStatus.msg CollisionConstraints.msg CollisionReport.msg @@ -70,7 +71,6 @@ set(msg_files DebugKeyValue.msg DebugValue.msg DebugVect.msg - DifferentialDriveSetpoint.msg DifferentialPressure.msg DistanceSensor.msg Ekf2Timestamps.msg @@ -97,6 +97,7 @@ set(msg_files FollowTarget.msg FollowTargetEstimator.msg FollowTargetStatus.msg + FuelTankStatus.msg GeneratorStatus.msg GeofenceResult.msg GeofenceStatus.msg @@ -145,6 +146,7 @@ set(msg_files MountOrientation.msg ModeCompleted.msg NavigatorMissionItem.msg + NavigatorStatus.msg NormalizedUnsignedSetpoint.msg NpfgStatus.msg ObstacleDistance.msg @@ -177,6 +179,10 @@ set(msg_files RcParameterMap.msg RegisterExtComponentReply.msg RegisterExtComponentRequest.msg + RoverAckermannGuidanceStatus.msg + RoverAckermannStatus.msg + RoverDifferentialGuidanceStatus.msg + RoverDifferentialStatus.msg Rpm.msg RtlStatus.msg RtlTimeEstimate.msg diff --git a/msg/CanInterfaceStatus.msg b/msg/CanInterfaceStatus.msg new file mode 100644 index 000000000000..4129c8d56386 --- /dev/null +++ b/msg/CanInterfaceStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint8 interface + +uint64 io_errors +uint64 frames_tx +uint64 frames_rx diff --git a/msg/DatamanRequest.msg b/msg/DatamanRequest.msg index f819771a4528..b65386cc1562 100644 --- a/msg/DatamanRequest.msg +++ b/msg/DatamanRequest.msg @@ -5,4 +5,4 @@ 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 +uint32 data_length diff --git a/msg/DebugArray.msg b/msg/DebugArray.msg index 4763a0f5a0ee..0c79d61dfed4 100644 --- a/msg/DebugArray.msg +++ b/msg/DebugArray.msg @@ -2,4 +2,4 @@ uint8 ARRAY_SIZE = 58 uint64 timestamp # time since system start (microseconds) uint16 id # unique ID of debug array, used to discriminate between arrays char[10] name # name of the debug array (max. 10 characters) -float32[58] data # data \ No newline at end of file +float32[58] data # data diff --git a/msg/DifferentialDriveSetpoint.msg b/msg/DifferentialDriveSetpoint.msg deleted file mode 100644 index f7e4c5840099..000000000000 --- a/msg/DifferentialDriveSetpoint.msg +++ /dev/null @@ -1,8 +0,0 @@ -uint64 timestamp # time since system start (microseconds) - -float32 speed # [m/s] collective roll-off speed in body x-axis -bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward -float32 yaw_rate # [rad/s] yaw rate -bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward - -# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 78273d6b0671..7bd8ea765827 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -11,8 +11,12 @@ float32 observation float32 observation_variance float32 innovation +float32 innovation_filtered + float32 innovation_variance -float32 test_ratio + +float32 test_ratio # normalized innovation squared +float32 test_ratio_filtered # signed filtered test ratio bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused @@ -20,5 +24,4 @@ bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt # TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip # TOPICS estimator_aid_src_fake_hgt -# TOPICS estimator_aid_src_mag_heading estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw -# TOPICS estimator_aid_src_terrain_range_finder +# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 98e645a3ec92..14e3ac3f846d 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -1,5 +1,5 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint8 estimator_instance @@ -11,12 +11,16 @@ float32[2] observation float32[2] observation_variance float32[2] innovation +float32[2] innovation_filtered + float32[2] innovation_variance -float32[2] test_ratio -bool innovation_rejected # true if the observation has been rejected -bool fused # true if the sample was successfully fused +float32[2] test_ratio # normalized innovation squared +float32[2] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position -# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow +# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow # TOPICS estimator_aid_src_drag diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index deb4c05bd0fb..b89add28e57e 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -1,5 +1,5 @@ -uint64 timestamp # time since system start (microseconds) -uint64 timestamp_sample # the timestamp of the raw data (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint8 estimator_instance @@ -11,10 +11,14 @@ float32[3] observation float32[3] observation_variance float32[3] innovation +float32[3] innovation_filtered + float32[3] innovation_variance -float32[3] test_ratio -bool innovation_rejected # true if the observation has been rejected -bool fused # true if the sample was successfully fused +float32[3] test_ratio # normalized innovation squared +float32[3] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag diff --git a/msg/EstimatorBias.msg b/msg/EstimatorBias.msg index 10dbca87bda7..bb65e47bc7d1 100644 --- a/msg/EstimatorBias.msg +++ b/msg/EstimatorBias.msg @@ -9,4 +9,4 @@ float32 innov # innovation of the last measurement fusion (m) float32 innov_var # innovation variance of the last measurement fusion (m^2) float32 innov_test_ratio # normalized innovation squared test ratio -# TOPICS estimator_baro_bias estimator_gnss_hgt_bias estimator_rng_hgt_bias +# TOPICS estimator_baro_bias estimator_gnss_hgt_bias diff --git a/msg/EstimatorEventFlags.msg b/msg/EstimatorEventFlags.msg index 1a47e676a5a4..7d5cf7ca343c 100644 --- a/msg/EstimatorEventFlags.msg +++ b/msg/EstimatorEventFlags.msg @@ -20,18 +20,3 @@ bool reset_hgt_to_baro # 13 - true when the vertical position s bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement - -# warning events -uint32 warning_event_changes # number of warning event changes -bool gps_quality_poor # 0 - true when the gps is failing quality checks -bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period -bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period -bool gps_data_stopped_using_alternate # 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation -bool height_sensor_timeout # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period -bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation -bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements -bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course -bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data -bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period -bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data -bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/msg/EstimatorGpsStatus.msg b/msg/EstimatorGpsStatus.msg index 2d2462ee5c30..b2a9c883944e 100644 --- a/msg/EstimatorGpsStatus.msg +++ b/msg/EstimatorGpsStatus.msg @@ -13,6 +13,7 @@ bool check_fail_max_horz_drift # 6 : maximum allowed horizontal position drift bool check_fail_max_vert_drift # 7 : maximum allowed vertical position drift fail - requires stationary vehicle bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail +bool check_fail_spoofed_gps # 10 : GPS signal is spoofed float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s) float32 position_drift_rate_vertical_m_s # Vertical position rate magnitude (m/s) diff --git a/msg/EstimatorInnovations.msg b/msg/EstimatorInnovations.msg index ecf637478d41..11cc6a58adb9 100644 --- a/msg/EstimatorInnovations.msg +++ b/msg/EstimatorInnovations.msg @@ -22,7 +22,6 @@ float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing targ # Optical flow float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) -float32[2] terr_flow # flow innvoation (rad/sec) and innovation variance computed by the terrain estimator ((rad/sec)**2) # Various float32 heading # heading innovation (rad) and innovation variance (rad**2) diff --git a/msg/EstimatorStates.msg b/msg/EstimatorStates.msg index 787a005f8797..885246d8a5b9 100644 --- a/msg/EstimatorStates.msg +++ b/msg/EstimatorStates.msg @@ -1,7 +1,7 @@ uint64 timestamp # time since system start (microseconds) uint64 timestamp_sample # the timestamp of the raw data (microseconds) -float32[24] states # Internal filter states +float32[25] states # Internal filter states uint8 n_states # Number of states effectively used -float32[23] covariances # Diagonal Elements of Covariance Matrix +float32[24] covariances # Diagonal Elements of Covariance Matrix diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index 5491734b1d8e..dd62bc4aca3f 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -15,6 +15,7 @@ uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6 # 6 : maximum allowed horizontal positi uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position drift fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail +uint8 GPS_CHECK_FAIL_SPOOFED = 10 # 10 : GPS signal is spoofed uint64 control_mode_flags # Bitmask to indicate EKF logic state uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete @@ -69,27 +70,14 @@ uint32 filter_fault_flags # Bitmask to indicate EKF internal faults float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) -uint16 innovation_check_flags # Bitmask to indicate pass/fail status of innovation consistency checks -# 0 - true if velocity observations have been rejected -# 1 - true if horizontal position observations have been rejected -# 2 - true if true if vertical position observations have been rejected -# 3 - true if the X magnetometer observation has been rejected -# 4 - true if the Y magnetometer observation has been rejected -# 5 - true if the Z magnetometer observation has been rejected -# 6 - true if the yaw observation has been rejected -# 7 - true if the airspeed observation has been rejected -# 8 - true if the synthetic sideslip observation has been rejected -# 9 - true if the height above ground observation has been rejected -# 10 - true if the X optical flow observation has been rejected -# 11 - true if the Y optical flow observation has been rejected -float32 mag_test_ratio # ratio of the largest magnetometer innovation component to the innovation test limit -float32 vel_test_ratio # ratio of the largest velocity innovation component to the innovation test limit -float32 pos_test_ratio # ratio of the largest horizontal position innovation component to the innovation test limit -float32 hgt_test_ratio # ratio of the vertical position innovation to the innovation test limit -float32 tas_test_ratio # ratio of the true airspeed innovation to the innovation test limit -float32 hagl_test_ratio # ratio of the height above ground innovation to the innovation test limit -float32 beta_test_ratio # ratio of the synthetic sideslip innovation to the innovation test limit +float32 hdg_test_ratio # low-pass filtered ratio of the largest heading innovation component to the innovation test limit +float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit +float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit +float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit +float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit +float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit +float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use. # 0 - True if the attitude estimate is good @@ -114,9 +102,10 @@ uint8 reset_count_quat # number of quaternion reset events (allow to wrap if c float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time bool pre_flt_fail_innov_heading +bool pre_flt_fail_innov_height +bool pre_flt_fail_innov_pos_horiz bool pre_flt_fail_innov_vel_horiz bool pre_flt_fail_innov_vel_vert -bool pre_flt_fail_innov_height bool pre_flt_fail_mag_field_disturbed uint32 accel_device_id diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index c6e0504f158f..23e6e15fd48f 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -26,12 +26,12 @@ bool cs_mag_fault # 18 - true when the magnetometer has been declare bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough -bool cs_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool cs_gnss_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest -bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used +bool cs_gnss_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements @@ -43,6 +43,8 @@ bool cs_mag # 35 - true if 3-axis magnetometer measurement f bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended +bool cs_rng_terrain # 39 - true if we are fusing range finder data for terrain +bool cs_opt_flow_terrain # 40 - true if we are fusing flow data for terrain # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index 44945afaead0..de514fb2db4b 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -49,11 +49,10 @@ bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure fai bool wind_limit_exceeded # Wind limit exceeded bool flight_time_limit_exceeded # Maximum flight time exceeded bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid +bool navigator_failure # Navigator failed to execute a mode # Failure detector bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) bool fd_esc_arming_failure # ESC failed to arm bool fd_imbalanced_prop # Imbalanced propeller detected bool fd_motor_failure # Motor failure - - diff --git a/msg/FuelTankStatus.msg b/msg/FuelTankStatus.msg new file mode 100644 index 000000000000..22d21e4a4a31 --- /dev/null +++ b/msg/FuelTankStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +float32 maximum_fuel_capacity # maximum fuel capacity. Must always be provided, either from the driver or a parameter +float32 consumed_fuel # consumed fuel, NaN if not measured. Should not be inferred from the max fuel capacity +float32 fuel_consumption_rate # fuel consumption rate, NaN if not measured + +uint8 percent_remaining # percentage of remaining fuel, UINT8_MAX if not provided +float32 remaining_fuel # remaining fuel, NaN if not measured. Should not be inferred from the max fuel capacity + +uint8 fuel_tank_id # identifier for the fuel tank. Must match ID of other messages for same fuel system. 0 by default when only a single tank exists + +uint32 fuel_type # type of fuel based on MAV_FUEL_TYPE enum. Set to MAV_FUEL_TYPE_UNKNOWN if unknown or it does not fit the provided types +uint8 MAV_FUEL_TYPE_UNKNOWN = 0 # fuel type not specified. Fuel levels are normalized (i.e., maximum is 1, and other levels are relative to 1). +uint8 MAV_FUEL_TYPE_LIQUID = 1 # represents generic liquid fuels, such as gasoline or diesel. Fuel levels are measured in millilitres (ml), and flow rates in millilitres per second (ml/s). +uint8 MAV_FUEL_TYPE_GAS = 2 # represents a gas fuel, such as hydrogen, methane, or propane. Fuel levels are in kilo-Pascal (kPa), and flow rates are in milliliters per second (ml/s). + +float32 temperature # fuel temperature in Kelvin, NaN if not measured diff --git a/msg/GpsDump.msg b/msg/GpsDump.msg index 3aa1313aa680..2477bcfa3a1e 100644 --- a/msg/GpsDump.msg +++ b/msg/GpsDump.msg @@ -1,12 +1,10 @@ # This message is used to dump the raw gps communication to the log. -# Set the parameter GPS_DUMP_COMM to 1 to use this. -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) -uint8 instance # Instance of GNSS receiver - -uint8 len # length of data, MSB bit set = message to the gps device, - # clear = message from the device -uint8[79] data # data to write to the log +uint8 instance # Instance of GNSS receiver +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log uint8 ORB_QUEUE_LENGTH = 8 diff --git a/msg/InputRc.msg b/msg/InputRc.msg index db4b3de233f5..782477407e33 100644 --- a/msg/InputRc.msg +++ b/msg/InputRc.msg @@ -37,4 +37,4 @@ uint8 input_source # Input source uint16[18] values # measured pulse widths for each of the supported channels int8 link_quality # link quality. Percentage 0-100%. -1 = invalid -float32 rssi_dbm # Actual rssi in units of dBm. NaN = invalid \ No newline at end of file +float32 rssi_dbm # Actual rssi in units of dBm. NaN = invalid diff --git a/msg/IridiumsbdStatus.msg b/msg/IridiumsbdStatus.msg index 39e3de780a00..436654e4ffab 100644 --- a/msg/IridiumsbdStatus.msg +++ b/msg/IridiumsbdStatus.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint64 last_heartbeat # timestamp of the last successful sbd session +uint64 last_at_ok_timestamp # timestamp of the last "OK" received after the "AT" command uint16 tx_buf_write_index # current size of the tx buffer uint16 rx_buf_read_index # the rx buffer is parsed up to that index uint16 rx_buf_end_index # current size of the rx buffer diff --git a/msg/LogMessage.msg b/msg/LogMessage.msg index 3ea1de23dd47..afb690b145c0 100644 --- a/msg/LogMessage.msg +++ b/msg/LogMessage.msg @@ -1,4 +1,4 @@ -# A logging message, output with PX4_{WARN,ERR,INFO} +# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO uint64 timestamp # time since system start (microseconds) diff --git a/msg/LoggerStatus.msg b/msg/LoggerStatus.msg index 9a07871a0f65..c67c88959252 100644 --- a/msg/LoggerStatus.msg +++ b/msg/LoggerStatus.msg @@ -9,6 +9,8 @@ uint8 BACKEND_MAVLINK = 2 uint8 BACKEND_ALL = 3 uint8 backend +bool is_logging + float32 total_written_kb # total written to log in kiloBytes float32 write_rate_kb_s # write rate in kiloBytes/s diff --git a/msg/ManualControlSetpoint.msg b/msg/ManualControlSetpoint.msg index 4e4e305fd0a8..95fa62228344 100644 --- a/msg/ManualControlSetpoint.msg +++ b/msg/ManualControlSetpoint.msg @@ -37,6 +37,8 @@ float32 aux6 bool sticks_moving +uint16 buttons # From uint16 buttons field of Mavlink manual_control message + # TOPICS manual_control_setpoint manual_control_input # DEPRECATED: float32 x # DEPRECATED: float32 y diff --git a/msg/MessageFormatResponse.msg b/msg/MessageFormatResponse.msg index 41ee96274985..75c1d4fe9f1e 100644 --- a/msg/MessageFormatResponse.msg +++ b/msg/MessageFormatResponse.msg @@ -8,4 +8,3 @@ char[50] topic_name # E.g. /fmu/in/vehicle_command bool success uint32 message_hash # hash over all message fields - diff --git a/msg/ModeCompleted.msg b/msg/ModeCompleted.msg index bacff4a94b45..0a20b0294e53 100644 --- a/msg/ModeCompleted.msg +++ b/msg/ModeCompleted.msg @@ -12,4 +12,3 @@ uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error) uint8 result # One of RESULT_* uint8 nav_state # Source mode (values in VehicleStatus) - diff --git a/msg/NavigatorMissionItem.msg b/msg/NavigatorMissionItem.msg index 64af762f75d8..a10aa4656d51 100644 --- a/msg/NavigatorMissionItem.msg +++ b/msg/NavigatorMissionItem.msg @@ -1,7 +1,5 @@ uint64 timestamp # time since system start (microseconds) -uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified - uint16 sequence_current # Sequence of the current mission item uint16 nav_cmd diff --git a/msg/NavigatorStatus.msg b/msg/NavigatorStatus.msg new file mode 100644 index 000000000000..da666533574f --- /dev/null +++ b/msg/NavigatorStatus.msg @@ -0,0 +1,9 @@ +# Current status of a Navigator mode +# The possible values of nav_state are defined in the VehicleStatus msg. +uint64 timestamp # time since system start (microseconds) + +uint8 nav_state # Source mode (values in VehicleStatus) +uint8 failure # Navigator failure enum + +uint8 FAILURE_NONE = 0 +uint8 FAILURE_HAGL = 1 # Target altitude exceeds maximum height above ground diff --git a/msg/ObstacleDistance.msg b/msg/ObstacleDistance.msg index e3c4963ab24e..e4a07d1cacc9 100644 --- a/msg/ObstacleDistance.msg +++ b/msg/ObstacleDistance.msg @@ -19,6 +19,6 @@ float32 increment # Angular width in degrees of each array element. uint16 min_distance # Minimum distance the sensor can measure in centimeters. uint16 max_distance # Maximum distance the sensor can measure in centimeters. -float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right. +float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. # TOPICS obstacle_distance obstacle_distance_fused diff --git a/msg/OrbitStatus.msg b/msg/OrbitStatus.msg index a04265db46c8..531fa4145306 100644 --- a/msg/OrbitStatus.msg +++ b/msg/OrbitStatus.msg @@ -4,6 +4,7 @@ uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 uint64 timestamp # time since system start (microseconds) float32 radius # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] diff --git a/msg/ParameterSetValueResponse.msg b/msg/ParameterSetValueResponse.msg index 09f8e3084a80..0bf94a190bf2 100644 --- a/msg/ParameterSetValueResponse.msg +++ b/msg/ParameterSetValueResponse.msg @@ -6,4 +6,4 @@ uint16 parameter_index uint8 ORB_QUEUE_LENGTH = 4 -# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response \ No newline at end of file +# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response diff --git a/msg/RoverAckermannGuidanceStatus.msg b/msg/RoverAckermannGuidanceStatus.msg new file mode 100644 index 000000000000..a06d1290bf50 --- /dev/null +++ b/msg/RoverAckermannGuidanceStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 desired_speed # [m/s] Rover desired ground speed +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error # [deg] Heading error of the pure pursuit controller +float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions + +# TOPICS rover_ackermann_guidance_status diff --git a/msg/RoverAckermannStatus.msg b/msg/RoverAckermannStatus.msg new file mode 100644 index 000000000000..bf0556b9453d --- /dev/null +++ b/msg/RoverAckermannStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +float32 throttle_setpoint # [-1, 1] Normalized throttle setpoint +float32 steering_setpoint # [-1, 1] Normalized steering setpoint +float32 actual_speed # [m/s] Rover ground speed + +# TOPICS rover_ackermann_status diff --git a/msg/RoverDifferentialGuidanceStatus.msg b/msg/RoverDifferentialGuidanceStatus.msg new file mode 100644 index 000000000000..836546e7ebb7 --- /dev/null +++ b/msg/RoverDifferentialGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 desired_speed # [m/s] Desired forward speed for the rover +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error_deg # [deg] Heading error of the pure pursuit controller +float32 pid_heading_integral # Integral of the PID for the desired yaw rate during missions +float32 pid_throttle_integral # Integral of the PID for the throttle during missions +uint8 state_machine # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED] + +# TOPICS rover_differential_guidance_status diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg new file mode 100644 index 000000000000..31907ffa6477 --- /dev/null +++ b/msg/RoverDifferentialStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Actual forward speed of the rover +float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate +float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 pid_yaw_rate_integral # Integral of the PID for the desired yaw rate controller + +# TOPICS rover_differential_status diff --git a/msg/RtlStatus.msg b/msg/RtlStatus.msg index 94c90d513e38..f25b22243a19 100644 --- a/msg/RtlStatus.msg +++ b/msg/RtlStatus.msg @@ -8,8 +8,8 @@ bool has_vtol_approach # flag if approaches are defined for current RTL_ uint8 rtl_type # Type of RTL chosen uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode -uint8 RTL_STATUS_TYPE_NONE=0 # RTL type is pending if evaluation can't pe performed currently e.g. when it is still loading the safe points -uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # RTL type is chosen to directly go to a safe point or home position -uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # RTL type is going straight to the beginning of the mission landing -uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # RTL type is following the mission from closest point to mission landing -uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # RTL type is following the mission in reverse to the start position +uint8 RTL_STATUS_TYPE_NONE=0 # pending if evaluation can't pe performed currently e.g. when it is still loading the safe points +uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # chosen to directly go to a safe point or home position +uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # going straight to the beginning of the mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # Following the mission from start index to mission landing. Start index is current WP if in Mission mode, and closest WP otherwise. +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # Following the mission in reverse from start index to the beginning of the mission. Start index is previous WP if in Mission mode, and closest WP otherwise. diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index db18899cb1c0..ce2bfad4fd8e 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -12,7 +12,14 @@ float64 altitude_ellipsoid_m # Altitude above Ellipsoid, meters float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) float32 c_variance_rad # GPS course accuracy estimate, (radians) -uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. +uint8 FIX_TYPE_NONE = 1 # Value 0 is also valid to represent no fix. +uint8 FIX_TYPE_2D = 2 +uint8 FIX_TYPE_3D = 3 +uint8 FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4 +uint8 FIX_TYPE_RTK_FLOAT = 5 +uint8 FIX_TYPE_RTK_FIXED = 6 +uint8 FIX_TYPE_EXTRAPOLATED = 8 +uint8 fix_type # Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. float32 eph # GPS horizontal position accuracy (metres) float32 epv # GPS vertical position accuracy (metres) diff --git a/msg/VehicleAttitude.msg b/msg/VehicleAttitude.msg index 46e1fc0bcb4e..99e6f25c2e42 100644 --- a/msg/VehicleAttitude.msg +++ b/msg/VehicleAttitude.msg @@ -1,4 +1,5 @@ # This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use +# The quaternion uses the Hamilton convention, and the order is q(w, x, y, z) uint64 timestamp # time since system start (microseconds) diff --git a/msg/VehicleAttitudeSetpoint.msg b/msg/VehicleAttitudeSetpoint.msg index e4f66c446a05..74a753023df5 100644 --- a/msg/VehicleAttitudeSetpoint.msg +++ b/msg/VehicleAttitudeSetpoint.msg @@ -1,9 +1,5 @@ uint64 timestamp # time since system start (microseconds) -float32 roll_body # body angle in NED frame (can be NaN for FW) -float32 pitch_body # body angle in NED frame (can be NaN for FW) -float32 yaw_body # body angle in NED frame (can be NaN for FW) - float32 yaw_sp_move_rate # rad/s (commanded by user) # For quaternion-based attitude control @@ -11,7 +7,7 @@ float32[4] q_d # Desired quaternion for quaternion control # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. -float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] +float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) diff --git a/msg/VehicleCommand.msg b/msg/VehicleCommand.msg index 801e5b279f61..8df0ca56b0ac 100644 --- a/msg/VehicleCommand.msg +++ b/msg/VehicleCommand.msg @@ -43,7 +43,7 @@ uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode an uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| -uint16 VEHICLE_CMD_DO_REPOSITION = 192 +uint16 VEHICLE_CMD_DO_REPOSITION = 192 # Reposition to specific WGS84 GPS position. |Ground speed [m/s] |Bitmask |Loiter radius [m] for planes |Yaw [deg] |Latitude |Longitude |Altitude | uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| @@ -102,6 +102,7 @@ uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibrati uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning +uint16 VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE = 43004 # PX4 vehicle commands (beyond 16 bit mavlink commands) uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) @@ -157,6 +158,14 @@ uint8 SPEED_TYPE_GROUNDSPEED = 1 uint8 SPEED_TYPE_CLIMB_SPEED = 2 uint8 SPEED_TYPE_DESCEND_SPEED = 3 +# used as param3 in CMD_DO_ORBIT +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 +uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 +uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 +uint8 ORBIT_YAW_BEHAVIOUR_UNCHANGED = 5 + # used as param1 in ARM_DISARM command int8 ARMING_ACTION_DISARM = 0 int8 ARMING_ACTION_ARM = 1 diff --git a/msg/VehicleGlobalPosition.msg b/msg/VehicleGlobalPosition.msg index c7d9ee78128a..e37155fe25a9 100644 --- a/msg/VehicleGlobalPosition.msg +++ b/msg/VehicleGlobalPosition.msg @@ -14,8 +14,10 @@ float32 alt # Altitude AMSL, (meters) float32 alt_ellipsoid # Altitude above ellipsoid, (meters) float32 delta_alt # Reset delta for altitude +float32 delta_terrain # Reset delta for terrain uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates uint8 alt_reset_counter # Counter for reset events on altitude +uint8 terrain_reset_counter # Counter for reset events on terrain float32 eph # Standard deviation of horizontal position error, (metres) float32 epv # Standard deviation of vertical position error, (metres) diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index c1a0dffe14f6..0e74ac0f4bfc 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -55,8 +55,13 @@ float64 ref_lon # Reference point longitude, (degrees) float32 ref_alt # Reference altitude AMSL, (metres) # Distance to surface -float32 dist_bottom # Distance from from bottom surface to ground, (metres) bool dist_bottom_valid # true if distance to bottom surface is valid +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +float32 dist_bottom_var # terrain estimate variance (m^2) + +float32 delta_dist_bottom # Amount of vertical shift of dist bottom estimate in latest reset [m] +uint8 dist_bottom_reset_counter # Index of latest dist bottom estimate reset + uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom uint8 DIST_BOTTOM_SENSOR_NONE = 0 uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field diff --git a/msg/VehicleRatesSetpoint.msg b/msg/VehicleRatesSetpoint.msg index 88adcf3bea98..35a06c35aa5f 100644 --- a/msg/VehicleRatesSetpoint.msg +++ b/msg/VehicleRatesSetpoint.msg @@ -1,6 +1,6 @@ uint64 timestamp # time since system start (microseconds) -# body angular rates in NED frame +# body angular rates in FRD frame float32 roll # [rad/s] roll rate setpoint float32 pitch # [rad/s] pitch rate setpoint float32 yaw # [rad/s] yaw rate setpoint diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a0bafe7d59de..cb0721fe6911 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(px4_platform STATIC shutdown.cpp spi.cpp pab_manifest.c + Serial.cpp ${SRCS} ) target_link_libraries(px4_platform prebuild_targets px4_work_queue) diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp new file mode 100644 index 000000000000..ca338eb190e6 --- /dev/null +++ b/platforms/common/Serial.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include + +namespace device +{ + +Serial::Serial() : + _impl(nullptr, 57600, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled) {} + + +Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _impl(port, baudrate, bytesize, parity, stopbits, flowcontrol) +{ + // If no baudrate was specified then set it to a reasonable default value + if (baudrate == 0) { + (void) _impl.setBaudrate(9600); + } +} + +Serial::~Serial() +{ +} + +bool Serial::open() +{ + return _impl.open(); +} + +bool Serial::isOpen() const +{ + return _impl.isOpen(); +} + +bool Serial::close() +{ + return _impl.close(); +} + +ssize_t Serial::read(uint8_t *buffer, size_t buffer_size) +{ + return _impl.read(buffer, buffer_size); +} + +ssize_t Serial::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms) +{ + return _impl.readAtLeast(buffer, buffer_size, character_count, timeout_ms); +} + +ssize_t Serial::write(const void *buffer, size_t buffer_size) +{ + return _impl.write(buffer, buffer_size); +} + +void Serial::flush() +{ + return _impl.flush(); +} + +uint32_t Serial::getBaudrate() const +{ + return _impl.getBaudrate(); +} + +bool Serial::setBaudrate(uint32_t baudrate) +{ + return _impl.setBaudrate(baudrate); +} + +ByteSize Serial::getBytesize() const +{ + return _impl.getBytesize(); +} + +bool Serial::setBytesize(ByteSize bytesize) +{ + return _impl.setBytesize(bytesize); +} + +Parity Serial::getParity() const +{ + return _impl.getParity(); +} + +bool Serial::setParity(Parity parity) +{ + return _impl.setParity(parity); +} + +StopBits Serial::getStopbits() const +{ + return _impl.getStopbits(); +} + +bool Serial::setStopbits(StopBits stopbits) +{ + return _impl.setStopbits(stopbits); +} + +FlowControl Serial::getFlowcontrol() const +{ + return _impl.getFlowcontrol(); +} + +bool Serial::setFlowcontrol(FlowControl flowcontrol) +{ + return _impl.setFlowcontrol(flowcontrol); +} + +bool Serial::getSingleWireMode() const +{ + return _impl.getSingleWireMode(); +} +bool Serial::setSingleWireMode() +{ + return _impl.setSingleWireMode(); +} + +bool Serial::getSwapRxTxMode() const +{ + return _impl.getSwapRxTxMode(); +} +bool Serial::setSwapRxTxMode() +{ + return _impl.setSwapRxTxMode(); +} + +bool Serial::getInvertedMode() const +{ + return _impl.getInvertedMode(); +} + +bool Serial::setInvertedMode(bool enable) +{ + return _impl.setInvertedMode(enable); +} + +const char *Serial::getPort() const +{ + return _impl.getPort(); +} + +bool Serial::validatePort(const char *port) +{ + return SerialImpl::validatePort(port); +} + +bool Serial::setPort(const char *port) +{ + return _impl.setPort(port); +} + +} // namespace device diff --git a/platforms/common/external_reset_lockout.cpp b/platforms/common/external_reset_lockout.cpp index 63f88d2349b5..fdc0adf67bd3 100644 --- a/platforms/common/external_reset_lockout.cpp +++ b/platforms/common/external_reset_lockout.cpp @@ -59,4 +59,3 @@ void px4_indicate_external_reset_lockout(LockoutComponent component, bool enable void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled) {} #endif /* BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE */ - diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp new file mode 100644 index 000000000000..fb45b5238502 --- /dev/null +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * 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 + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device __EXPORT +{ + +class Serial +{ +public: + Serial(); + Serial(const char *port, uint32_t baudrate = 57600, + ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None, + StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled); + virtual ~Serial(); + + // Open sets up the port and gets it configured based on desired configuration + // The port is always opened in NON BLOCKING mode. + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_ms = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + void flush(); + + // If port is already open then the following configuration functions + // will reconfigure the port. If the port is not yet open then they will + // simply store the configuration in preparation for the port to be opened. + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + + bool getInvertedMode() const; + bool setInvertedMode(bool enable); + + static bool validatePort(const char *port); + bool setPort(const char *port); + const char *getPort() const; + +private: + // Disable copy constructors + Serial(const Serial &); + Serial &operator=(const Serial &); + + // platform implementation + SerialImpl _impl; +}; + +} // namespace device diff --git a/platforms/common/include/px4_platform_common/SerialCommon.hpp b/platforms/common/include/px4_platform_common/SerialCommon.hpp new file mode 100644 index 000000000000..bbe9be0ad9fe --- /dev/null +++ b/platforms/common/include/px4_platform_common/SerialCommon.hpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * 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 + +namespace device +{ +namespace SerialConfig +{ + + +// ByteSize: number of data bits +enum class ByteSize { + FiveBits = 5, + SixBits = 6, + SevenBits = 7, + EightBits = 8, +}; + +// Parity: enable parity checking +enum class Parity { + None = 0, + Odd = 1, + Even = 2, +}; + +// StopBits: number of stop bits +enum class StopBits { + One = 1, + Two = 2 +}; + +// FlowControl: enable flow control +enum class FlowControl { + Disabled = 0, + Enabled = 1, +}; + +} // namespace SerialConfig +} // namespace device diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 970c47ec9e08..4dff12c1b24f 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -239,7 +239,7 @@ # else /* Use PX4IO FW search paths defaults based on version */ # if BOARD_USES_PX4IO_VERSION == 2 -# define PX4IO_FW_SEARCH_PATHS {"/etc/extras/px4_io-v2_default.bin","/fs/microsd/px4_io-v2_default.bin", "/fs/microsd/px4io2.bin", nullptr } +# define PX4IO_FW_SEARCH_PATHS {"/etc/extras/px4_io-v2_default.bin",CONFIG_BOARD_ROOT_PATH "/px4_io-v2_default.bin", CONFIG_BOARD_ROOT_PATH "/px4io2.bin", nullptr } # endif # endif #endif diff --git a/platforms/common/include/px4_platform_common/cli.h b/platforms/common/include/px4_platform_common/cli.h index 1e08cfabd0c7..5667ae685eb0 100644 --- a/platforms/common/include/px4_platform_common/cli.h +++ b/platforms/common/include/px4_platform_common/cli.h @@ -50,3 +50,15 @@ * @return 0 on success, -errno otherwise */ int px4_get_parameter_value(const char *option, int &value); + +/** + * Parse a CLI argument to a float. There are 2 valid formats: + * - 'p:' + * in this case the parameter is loaded from an integer parameter + * - + * a floating-point value, so just a string to float conversion is done + * @param option CLI argument + * @param value returned value + * @return 0 on success, -errno otherwise + */ +int px4_get_parameter_value(const char *option, float &value); diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 38d7b4201f4c..3b420f95f10f 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -64,7 +64,7 @@ static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite( ****************************************************************************/ #define PX4_ROOTFSDIR "" -#define PX4_STORAGEDIR PX4_ROOTFSDIR "/fs/microsd" +#define PX4_STORAGEDIR PX4_ROOTFSDIR CONFIG_BOARD_ROOT_PATH #define _PX4_IOC(x,y) _IOC(x,y) // mode for open with O_CREAT diff --git a/platforms/common/include/px4_platform_common/events.h b/platforms/common/include/px4_platform_common/events.h index b7356bed3de2..b3dad933ba54 100644 --- a/platforms/common/include/px4_platform_common/events.h +++ b/platforms/common/include/px4_platform_common/events.h @@ -45,7 +45,7 @@ #endif #include -#include +#include #include @@ -93,7 +93,7 @@ constexpr unsigned sizeofArguments(const T &t, const Args &... args) /** * publish/send an event */ -void send(EventType &event); +void send(event_s &event); /** * Generate event ID from an event name @@ -109,7 +109,7 @@ constexpr uint32_t ID(const char (&name)[N]) template inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args) { - EventType e{}; + event_s e{}; e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; e.id = id; static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments"); @@ -120,7 +120,7 @@ inline void send(uint32_t id, const LogLevels &log_levels, const char *message, inline void send(uint32_t id, const LogLevels &log_levels, const char *message) { - EventType e{}; + event_s e{}; e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external; e.id = id; CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message); diff --git a/platforms/common/include/px4_platform_common/external_reset_lockout.h b/platforms/common/include/px4_platform_common/external_reset_lockout.h index b4cdf8df6cbb..3be2901ab808 100644 --- a/platforms/common/include/px4_platform_common/external_reset_lockout.h +++ b/platforms/common/include/px4_platform_common/external_reset_lockout.h @@ -53,6 +53,3 @@ enum class LockoutComponent : uint8_t { * @param enabled true if compoment is in critical state */ void px4_indicate_external_reset_lockout(LockoutComponent component, bool enabled); - - - diff --git a/platforms/common/include/px4_platform_common/log.h b/platforms/common/include/px4_platform_common/log.h index 3450b7e90bf0..0b5d878ed7ae 100644 --- a/platforms/common/include/px4_platform_common/log.h +++ b/platforms/common/include/px4_platform_common/log.h @@ -446,4 +446,3 @@ __END_DECLS #define PX4_ANSI_COLOR_CYAN "\x1b[36m" #define PX4_ANSI_COLOR_GRAY "\x1B[37m" #define PX4_ANSI_COLOR_RESET "\x1b[0m" - 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 4df96fa5a996..68ac3f1ffea9 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 @@ -72,7 +72,7 @@ static constexpr wq_config_t INS1{"wq:INS1", 6000, -15}; static constexpr wq_config_t INS2{"wq:INS2", 6000, -16}; static constexpr wq_config_t INS3{"wq:INS3", 6000, -17}; -static constexpr wq_config_t hp_default{"wq:hp_default", 2392, -18}; +static constexpr wq_config_t hp_default{"wq:hp_default", 2800, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; @@ -89,7 +89,7 @@ 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 lp_default{"wq:lp_default", 1920, -50}; +static constexpr wq_config_t lp_default{"wq:lp_default", 2350, -50}; static constexpr wq_config_t test1{"wq:test1", 2000, 0}; static constexpr wq_config_t test2{"wq:test2", 2000, 0}; diff --git a/platforms/common/include/px4_platform_common/time.h b/platforms/common/include/px4_platform_common/time.h index 1b4d8a27560c..d61e23c75cfb 100644 --- a/platforms/common/include/px4_platform_common/time.h +++ b/platforms/common/include/px4_platform_common/time.h @@ -13,11 +13,21 @@ __END_DECLS #define px4_clock_gettime system_clock_gettime #endif -#if defined(ENABLE_LOCKSTEP_SCHEDULER) +#if defined(ENABLE_LOCKSTEP_SCHEDULER) || defined(__PX4_QURT) __BEGIN_DECLS __EXPORT int px4_clock_settime(clockid_t clk_id, const struct timespec *tp); +__END_DECLS + +#else + +#define px4_clock_settime system_clock_settime +#endif + +#if defined(ENABLE_LOCKSTEP_SCHEDULER) + +__BEGIN_DECLS __EXPORT int px4_usleep(useconds_t usec); __EXPORT unsigned int px4_sleep(unsigned int seconds); __EXPORT int px4_pthread_cond_timedwait(pthread_cond_t *cond, @@ -27,7 +37,6 @@ __END_DECLS #else -#define px4_clock_settime system_clock_settime #define px4_usleep system_usleep #define px4_sleep system_sleep #define px4_pthread_cond_timedwait system_pthread_cond_timedwait diff --git a/platforms/common/module.cpp b/platforms/common/module.cpp index 8b474c794100..aeba6c4624fd 100644 --- a/platforms/common/module.cpp +++ b/platforms/common/module.cpp @@ -238,4 +238,3 @@ void PRINT_MODULE_USAGE_ARG(const char *values, const char *description, bool is #endif } - diff --git a/platforms/common/pab_manifest.c b/platforms/common/pab_manifest.c index 979bb5ecbc9b..05cfa809969d 100644 --- a/platforms/common/pab_manifest.c +++ b/platforms/common/pab_manifest.c @@ -356,20 +356,22 @@ static const px4_hw_mft_item_t base_configuration_17[] = { }, }; +// BASE ID 0x100 Holybro Pixhawk Jetson Baseboard Alaised to ID 0 static px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev - {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO - {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO - {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base - {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY - {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base - {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini - {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 - {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 - {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 - {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 - {HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17 + {HW_BASE_ID(0), base_configuration_0, arraySize(base_configuration_0)}, // std Base with PX4IO + {HW_BASE_ID(1), base_configuration_1, arraySize(base_configuration_1)}, // std Base No PX4IO + {HW_BASE_ID(2), base_configuration_0, arraySize(base_configuration_0)}, // CUAV Base + {HW_BASE_ID(3), base_configuration_3, arraySize(base_configuration_3)}, // NXP T1 PHY + {HW_BASE_ID(4), base_configuration_0, arraySize(base_configuration_0)}, // HB CM4 base + {HW_BASE_ID(5), base_configuration_5, arraySize(base_configuration_5)}, // HB Mini + {HW_BASE_ID(8), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 8 Aliased to 0 + {HW_BASE_ID(9), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 9 + {HW_BASE_ID(10), base_configuration_9, arraySize(base_configuration_9)}, // Auterion Skynode ver 10 + {HW_BASE_ID(16), base_configuration_0, arraySize(base_configuration_0)}, // Auterion Skynode ver 16 + {HW_BASE_ID(17), base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17 + {HW_BASE_ID(0x100), base_configuration_0, arraySize(base_configuration_0)}, // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0 }; /************************************************************************************ diff --git a/platforms/common/px4_cli.cpp b/platforms/common/px4_cli.cpp index bf0e5f394967..d2ffa7230da5 100644 --- a/platforms/common/px4_cli.cpp +++ b/platforms/common/px4_cli.cpp @@ -59,6 +59,7 @@ int px4_get_parameter_value(const char *option, int &value) if (param_handle != PARAM_INVALID) { if (param_type(param_handle) != PARAM_TYPE_INT32) { + PX4_ERR("Type of param '%s' is different from INT32", param_name); return -EINVAL; } @@ -87,3 +88,48 @@ int px4_get_parameter_value(const char *option, int &value) return 0; } + +int px4_get_parameter_value(const char *option, float &value) +{ + value = 0; + + /* check if this is a param name */ + if (strncmp("p:", option, 2) == 0) { + + const char *param_name = option + 2; + + /* user wants to use a param name */ + param_t param_handle = param_find(param_name); + + if (param_handle != PARAM_INVALID) { + + if (param_type(param_handle) != PARAM_TYPE_FLOAT) { + PX4_ERR("Type of param '%s' is different from FLOAT", param_name); + return -EINVAL; + } + + float pwm_parm; + int ret = param_get(param_handle, &pwm_parm); + + if (ret != 0) { + return ret; + } + + value = pwm_parm; + + } else { + PX4_ERR("param name '%s' not found", param_name); + return -ENXIO; + } + + } else { + char *ep; + value = strtof(option, &ep); + + if (*ep != '\0') { + return -EINVAL; + } + } + + return 0; +} diff --git a/platforms/common/uORB/CMakeLists.txt b/platforms/common/uORB/CMakeLists.txt index 609d2aebe68f..2e3add770e1b 100644 --- a/platforms/common/uORB/CMakeLists.txt +++ b/platforms/common/uORB/CMakeLists.txt @@ -47,6 +47,7 @@ set(SRCS_COMMON Subscription.cpp Subscription.hpp SubscriptionCallback.hpp + SubscriptionInterval.cpp SubscriptionInterval.hpp SubscriptionMultiArray.hpp uORB.cpp diff --git a/platforms/common/uORB/ORBSet.hpp b/platforms/common/uORB/ORBSet.hpp index 28e283c7b9e7..967f6bd7e4b8 100644 --- a/platforms/common/uORB/ORBSet.hpp +++ b/platforms/common/uORB/ORBSet.hpp @@ -62,6 +62,11 @@ class ORBSet { Node **p; + // Don't allow duplicates to be inserted + if (find(node_name)) { + return; + } + if (_top == nullptr) { p = &_top; diff --git a/platforms/common/uORB/SubscriptionBlocking.hpp b/platforms/common/uORB/SubscriptionBlocking.hpp index 7ec29e329b6e..5c5580d855cb 100644 --- a/platforms/common/uORB/SubscriptionBlocking.hpp +++ b/platforms/common/uORB/SubscriptionBlocking.hpp @@ -56,31 +56,6 @@ class SubscriptionBlocking : public SubscriptionCallback SubscriptionBlocking(const orb_metadata *meta, uint32_t interval_us = 0, uint8_t instance = 0) : SubscriptionCallback(meta, interval_us, instance) { - // pthread_mutexattr_init - pthread_mutexattr_t attr; - int ret_attr_init = pthread_mutexattr_init(&attr); - - if (ret_attr_init != 0) { - PX4_ERR("pthread_mutexattr_init failed, status=%d", ret_attr_init); - } - -#if defined(PTHREAD_PRIO_NONE) - // pthread_mutexattr_settype - // PTHREAD_PRIO_NONE not available on cygwin - int ret_mutexattr_settype = pthread_mutexattr_settype(&attr, PTHREAD_PRIO_NONE); - - if (ret_mutexattr_settype != 0) { - PX4_ERR("pthread_mutexattr_settype failed, status=%d", ret_mutexattr_settype); - } - -#endif // PTHREAD_PRIO_NONE - - // pthread_mutex_init - int ret_mutex_init = pthread_mutex_init(&_mutex, &attr); - - if (ret_mutex_init != 0) { - PX4_ERR("pthread_mutex_init failed, status=%d", ret_mutex_init); - } } virtual ~SubscriptionBlocking() diff --git a/platforms/common/uORB/SubscriptionInterval.cpp b/platforms/common/uORB/SubscriptionInterval.cpp new file mode 100644 index 000000000000..8f8af333f2ea --- /dev/null +++ b/platforms/common/uORB/SubscriptionInterval.cpp @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SubscriptionInterval.hpp" + +namespace uORB +{ + +bool SubscriptionInterval::updated() +{ + if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) { + return _subscription.updated(); + } + + return false; +} + +bool SubscriptionInterval::update(void *dst) +{ + if (updated()) { + return copy(dst); + } + + return false; +} + +bool SubscriptionInterval::copy(void *dst) +{ + if (_subscription.copy(dst)) { + const hrt_abstime now = hrt_absolute_time(); + + // make sure we don't set a timestamp before the timer started counting (now - _interval_us would wrap because it's unsigned) + if (now > _interval_us) { + // shift last update time forward, but don't let it get further behind than the interval + _last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now); + + } else { + _last_update = now; + } + + return true; + } + + return false; +} + +} // namespace uORB diff --git a/platforms/common/uORB/SubscriptionInterval.hpp b/platforms/common/uORB/SubscriptionInterval.hpp index 31d1b0a7af12..fd6ccdfb61c5 100644 --- a/platforms/common/uORB/SubscriptionInterval.hpp +++ b/platforms/common/uORB/SubscriptionInterval.hpp @@ -93,45 +93,21 @@ class SubscriptionInterval /** * Check if there is a new update. * */ - bool updated() - { - if (advertised() && (hrt_elapsed_time(&_last_update) >= _interval_us)) { - return _subscription.updated(); - } - - return false; - } + bool updated(); /** * Copy the struct if updated. * @param dst The destination pointer where the struct will be copied. * @return true only if topic was updated and copied successfully. */ - bool update(void *dst) - { - if (updated()) { - return copy(dst); - } - - return false; - } + bool update(void *dst); /** * Copy the struct * @param dst The destination pointer where the struct will be copied. * @return true only if topic was copied successfully. */ - bool copy(void *dst) - { - if (_subscription.copy(dst)) { - const hrt_abstime now = hrt_absolute_time(); - // shift last update time forward, but don't let it get further behind than the interval - _last_update = math::constrain(_last_update + _interval_us, now - _interval_us, now); - return true; - } - - return false; - } + bool copy(void *dst); bool valid() const { return _subscription.valid(); } @@ -160,7 +136,7 @@ class SubscriptionInterval protected: Subscription _subscription; - uint64_t _last_update{0}; // last update in microseconds + uint64_t _last_update{0}; // last subscription update in microseconds uint32_t _interval_us{0}; // maximum update interval in microseconds }; diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index a8964b4d27cd..ee11923b59d2 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -254,4 +254,3 @@ typedef uint8_t main_state_t; typedef uint8_t hil_state_t; typedef uint8_t navigation_state_t; typedef uint8_t switch_pos_t; - diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index e83b3f3e04e3..fd4ef60e63e0 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -412,7 +412,10 @@ int16_t uORB::DeviceNode::process_add_subscription() uORBCommunicator::IChannel *ch = uORB::Manager::get_instance()->get_uorb_communicator(); 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); + // Only send the most recent data to initialize the remote end. + if (_data_valid) { + ch->send_message(_meta->o_name, _meta->o_size, _data + (_meta->o_size * ((_generation.load() - 1) % _meta->o_queue))); + } } return PX4_OK; diff --git a/platforms/common/work_queue/dq_addlast.c b/platforms/common/work_queue/dq_addlast.c index 35e42d46d3a7..99272da224e8 100644 --- a/platforms/common/work_queue/dq_addlast.c +++ b/platforms/common/work_queue/dq_addlast.c @@ -70,4 +70,3 @@ void dq_addlast(dq_entry_t *node, dq_queue_t *queue) queue->tail = node; } } - diff --git a/platforms/common/work_queue/dq_rem.c b/platforms/common/work_queue/dq_rem.c index 4249daec5050..7e91045f901d 100644 --- a/platforms/common/work_queue/dq_rem.c +++ b/platforms/common/work_queue/dq_rem.c @@ -78,4 +78,3 @@ void dq_rem(dq_entry_t *node, dq_queue_t *queue) node->flink = NULL; node->blink = NULL; } - diff --git a/platforms/common/work_queue/dq_remfirst.c b/platforms/common/work_queue/dq_remfirst.c index 10394d10634b..bb5538b7cc58 100644 --- a/platforms/common/work_queue/dq_remfirst.c +++ b/platforms/common/work_queue/dq_remfirst.c @@ -78,4 +78,3 @@ dq_entry_t *dq_remfirst(dq_queue_t *queue) return ret; } - diff --git a/platforms/common/work_queue/queue.c b/platforms/common/work_queue/queue.c index b81315381e2a..dc3e3e496763 100644 --- a/platforms/common/work_queue/queue.c +++ b/platforms/common/work_queue/queue.c @@ -91,5 +91,3 @@ void sq_addfirst(sq_entry_t *node, sq_queue_t *queue) queue->head = node; } - - diff --git a/platforms/common/work_queue/sq_addlast.c b/platforms/common/work_queue/sq_addlast.c index 1a0ff88a447c..d8915204bef2 100644 --- a/platforms/common/work_queue/sq_addlast.c +++ b/platforms/common/work_queue/sq_addlast.c @@ -69,4 +69,3 @@ void sq_addlast(sq_entry_t *node, sq_queue_t *queue) queue->tail = node; } } - diff --git a/platforms/common/work_queue/sq_remfirst.c b/platforms/common/work_queue/sq_remfirst.c index 42b0b6599dba..76b16b4a43ea 100644 --- a/platforms/common/work_queue/sq_remfirst.c +++ b/platforms/common/work_queue/sq_remfirst.c @@ -73,4 +73,3 @@ sq_entry_t *sq_remfirst(sq_queue_t *queue) return ret; } - diff --git a/platforms/nuttx/CMakeLists.txt b/platforms/nuttx/CMakeLists.txt index 1ab6cdfe00f4..cd8385827e85 100644 --- a/platforms/nuttx/CMakeLists.txt +++ b/platforms/nuttx/CMakeLists.txt @@ -266,7 +266,7 @@ else() -fno-exceptions -fno-rtti -Wl,--script=${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX}script.ld - -L${NUTTX_CONFIG_DIR_CYG}/scripts/${SCRIPT_PREFIX} + -L${NUTTX_CONFIG_DIR_CYG}/scripts -Wl,-Map=${PX4_CONFIG}.map -Wl,--warn-common -Wl,--gc-sections diff --git a/platforms/nuttx/Debug/jtrace_debug_ozone.jdebug b/platforms/nuttx/Debug/jtrace_debug_ozone.jdebug index 6e4b4efce568..dd540e34c213 100644 --- a/platforms/nuttx/Debug/jtrace_debug_ozone.jdebug +++ b/platforms/nuttx/Debug/jtrace_debug_ozone.jdebug @@ -3,21 +3,21 @@ * The Embedded Experts * * www.segger.com * ********************************************************************** - + File : jtrace_debug_ozone.jdebug Created : 26 Oct 2020 14:30 Ozone Version : V3.20e */ /********************************************************************* -* -* OnProjectLoad -* -* Function description -* Project load routine. Required. -* +* +* OnProjectLoad +* +* Function description +* Project load routine. Required. +* ********************************************************************** -*/ +*/ void OnProjectLoad (void) { // // Dialog-generated settings @@ -38,291 +38,290 @@ void OnProjectLoad (void) { } /********************************************************************* -* -* OnStartupComplete -* -* Function description -* Called when program execution has reached/passed -* the startup completion point. Optional. -* +* +* OnStartupComplete +* +* Function description +* Called when program execution has reached/passed +* the startup completion point. Optional. +* ********************************************************************** -*/ -//void OnStartupComplete (void) { -//} +*/ +//void OnStartupComplete (void) { +//} /********************************************************************* -* -* TargetReset -* -* Function description -* Replaces the default target device reset routine. Optional. -* -* Notes -* This example demonstrates the usage when -* debugging a RAM program on a Cortex-M target device -* +* +* TargetReset +* +* Function description +* Replaces the default target device reset routine. Optional. +* +* Notes +* This example demonstrates the usage when +* debugging a RAM program on a Cortex-M target device +* ********************************************************************** -*/ -//void TargetReset (void) { -// -// unsigned int SP; -// unsigned int PC; -// unsigned int VectorTableAddr; -// -// VectorTableAddr = Program.GetBaseAddr(); -// -// if (VectorTableAddr != 0xFFFFFFFF) { -// SP = Target.ReadU32(VectorTableAddr); -// Target.SetReg("SP", SP); -// } else { -// Util.Log("Project file error: failed to get program base"); -// } -// -// PC = Elf.GetEntryPointPC(); -// -// if (PC != 0xFFFFFFFF) { -// Target.SetReg("PC", PC); -// } else if (VectorTableAddr != 0xFFFFFFFF) { -// PC = Target.ReadU32(VectorTableAddr + 4); -// Target.SetReg("PC", PC); -//} +*/ +//void TargetReset (void) { +// +// unsigned int SP; +// unsigned int PC; +// unsigned int VectorTableAddr; +// +// VectorTableAddr = Program.GetBaseAddr(); +// +// if (VectorTableAddr != 0xFFFFFFFF) { +// SP = Target.ReadU32(VectorTableAddr); +// Target.SetReg("SP", SP); +// } else { +// Util.Log("Project file error: failed to get program base"); +// } +// +// PC = Elf.GetEntryPointPC(); +// +// if (PC != 0xFFFFFFFF) { +// Target.SetReg("PC", PC); +// } else if (VectorTableAddr != 0xFFFFFFFF) { +// PC = Target.ReadU32(VectorTableAddr + 4); +// Target.SetReg("PC", PC); +//} /********************************************************************* -* -* BeforeTargetReset -* -* Function description -* Event handler routine. Optional. -* +* +* BeforeTargetReset +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void BeforeTargetReset (void) { -//} +*/ +//void BeforeTargetReset (void) { +//} /********************************************************************* -* -* AfterTargetReset -* -* Function description -* Event handler routine. Optional. -* - Sets the PC register to program reset value. -* - Sets the SP register to program reset value on Cortex-M. -* +* +* AfterTargetReset +* +* Function description +* Event handler routine. Optional. +* - Sets the PC register to program reset value. +* - Sets the SP register to program reset value on Cortex-M. +* ********************************************************************** -*/ +*/ void AfterTargetReset (void) { - unsigned int SP; - unsigned int PC; - unsigned int VectorTableAddr; - - VectorTableAddr = Elf.GetBaseAddr(); - - if (VectorTableAddr == 0xFFFFFFFF) { - Util.Log("Project file error: failed to get program base"); - } else { - SP = Target.ReadU32(VectorTableAddr); - Target.SetReg("SP", SP); - - PC = Target.ReadU32(VectorTableAddr + 4); - Target.SetReg("PC", PC); + unsigned int SP; + unsigned int PC; + unsigned int VectorTableAddr; + + VectorTableAddr = Elf.GetBaseAddr(); + + if (VectorTableAddr == 0xFFFFFFFF) { + Util.Log("Project file error: failed to get program base"); + } else { + SP = Target.ReadU32(VectorTableAddr); + Target.SetReg("SP", SP); + + PC = Target.ReadU32(VectorTableAddr + 4); + Target.SetReg("PC", PC); } } /********************************************************************* -* -* DebugStart -* -* Function description -* Replaces the default debug session startup routine. Optional. -* +* +* DebugStart +* +* Function description +* Replaces the default debug session startup routine. Optional. +* ********************************************************************** -*/ -//void DebugStart (void) { -//} +*/ +//void DebugStart (void) { +//} /********************************************************************* -* -* TargetConnect -* -* Function description -* Replaces the default target IF connection routine. Optional. -* +* +* TargetConnect +* +* Function description +* Replaces the default target IF connection routine. Optional. +* ********************************************************************** -*/ -//void TargetConnect (void) { -//} +*/ +//void TargetConnect (void) { +//} /********************************************************************* -* -* BeforeTargetConnect -* -* Function description -* Event handler routine. Optional. -* +* +* BeforeTargetConnect +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -void BeforeTargetConnect (void) { +*/ +void BeforeTargetConnect (void) { Project.SetJLinkScript("FMUv6_Trace.pex"); -} +} /********************************************************************* -* -* AfterTargetConnect -* -* Function description -* Event handler routine. Optional. -* +* +* AfterTargetConnect +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void AfterTargetConnect (void) { -//} +*/ +//void AfterTargetConnect (void) { +//} /********************************************************************* -* -* TargetDownload -* -* Function description -* Replaces the default program download routine. Optional. -* +* +* TargetDownload +* +* Function description +* Replaces the default program download routine. Optional. +* ********************************************************************** -*/ -//void TargetDownload (void) { -//} +*/ +//void TargetDownload (void) { +//} /********************************************************************* -* -* BeforeTargetDownload -* -* Function description -* Event handler routine. Optional. -* +* +* BeforeTargetDownload +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void BeforeTargetDownload (void) { -//} +*/ +//void BeforeTargetDownload (void) { +//} /********************************************************************* -* -* AfterTargetDownload -* -* Function description -* Event handler routine. Optional. -* - Sets the PC register to program reset value. -* - Sets the SP register to program reset value on Cortex-M. -* +* +* AfterTargetDownload +* +* Function description +* Event handler routine. Optional. +* - Sets the PC register to program reset value. +* - Sets the SP register to program reset value on Cortex-M. +* ********************************************************************** -*/ +*/ void AfterTargetDownload (void) { - unsigned int SP; - unsigned int PC; - unsigned int VectorTableAddr; - - VectorTableAddr = Elf.GetBaseAddr(); - Util.Log("___"); - if (VectorTableAddr == 0xFFFFFFFF) { - Util.Log("Project file error: failed to get program base"); - } else { - SP = Target.ReadU32(VectorTableAddr); - Target.SetReg("SP", SP); - - PC = Target.ReadU32(VectorTableAddr + 4); - Target.SetReg("PC", PC); + unsigned int SP; + unsigned int PC; + unsigned int VectorTableAddr; + + VectorTableAddr = Elf.GetBaseAddr(); + Util.Log("___"); + if (VectorTableAddr == 0xFFFFFFFF) { + Util.Log("Project file error: failed to get program base"); + } else { + SP = Target.ReadU32(VectorTableAddr); + Target.SetReg("SP", SP); + + PC = Target.ReadU32(VectorTableAddr + 4); + Target.SetReg("PC", PC); } } /********************************************************************* -* -* BeforeTargetDisconnect -* -* Function description -* Event handler routine. Optional. -* +* +* BeforeTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void BeforeTargetDisconnect (void) { -//} +*/ +//void BeforeTargetDisconnect (void) { +//} /********************************************************************* -* -* AfterTargetDisconnect -* -* Function description -* Event handler routine. Optional. -* +* +* AfterTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void AfterTargetDisconnect (void) { -//} +*/ +//void AfterTargetDisconnect (void) { +//} /********************************************************************* -* -* AfterTargetHalt -* -* Function description -* Event handler routine. Optional. -* +* +* AfterTargetHalt +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void AfterTargetHalt (void) { -//} +*/ +//void AfterTargetHalt (void) { +//} /********************************************************************* -* -* BeforeTargetResume -* -* Function description -* Event handler routine. Optional. -* +* +* BeforeTargetResume +* +* Function description +* Event handler routine. Optional. +* ********************************************************************** -*/ -//void BeforeTargetResume (void) { -//} +*/ +//void BeforeTargetResume (void) { +//} /********************************************************************* -* -* OnSnapshotLoad -* -* Function description -* Called upon loading a snapshot. Optional. -* -* Additional information -* This function is used to restore the target state in cases -* where values cannot simply be written to the target. -* Typical use: GPIO clock needs to be enabled, before -* GPIO is configured. -* +* +* OnSnapshotLoad +* +* Function description +* Called upon loading a snapshot. Optional. +* +* Additional information +* This function is used to restore the target state in cases +* where values cannot simply be written to the target. +* Typical use: GPIO clock needs to be enabled, before +* GPIO is configured. +* ********************************************************************** -*/ -//void OnSnapshotLoad (void) { -//} +*/ +//void OnSnapshotLoad (void) { +//} /********************************************************************* -* -* OnSnapshotSave -* -* Function description -* Called upon saving a snapshot. Optional. -* -* Additional information -* This function is usually used to save values of the target -* state which can either not be trivially read, -* or need to be restored in a specific way or order. -* Typically use: Memory Mapped Registers, -* such as PLL and GPIO configuration. -* +* +* OnSnapshotSave +* +* Function description +* Called upon saving a snapshot. Optional. +* +* Additional information +* This function is usually used to save values of the target +* state which can either not be trivially read, +* or need to be restored in a specific way or order. +* Typically use: Memory Mapped Registers, +* such as PLL and GPIO configuration. +* ********************************************************************** -*/ -//void OnSnapshotSave (void) { -//} +*/ +//void OnSnapshotSave (void) { +//} /********************************************************************* -* -* OnError -* -* Function description -* Called when an error ocurred. Optional. -* +* +* OnError +* +* Function description +* Called when an error ocurred. Optional. +* ********************************************************************** -*/ -//void OnError (const char* sErrorMsg) { -//} - +*/ +//void OnError (const char* sErrorMsg) { +//} diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 693b9e782535..39bb38d82b12 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 693b9e782535f12e6a4ab657c7a0c3bd92b45fb1 +Subproject commit 39bb38d82b128a57d0ca40c25ba3ad6601ec0a83 diff --git a/platforms/nuttx/src/bootloader/common/CMakeLists.txt b/platforms/nuttx/src/bootloader/common/CMakeLists.txt index 7ba4af38266d..e61fb88b8979 100644 --- a/platforms/nuttx/src/bootloader/common/CMakeLists.txt +++ b/platforms/nuttx/src/bootloader/common/CMakeLists.txt @@ -37,6 +37,10 @@ add_library(bootloader crypto.c ) +target_compile_definitions(bootloader + PRIVATE BOOTLOADER_VERSION="PX4BLv${PX4_VERSION_MAJOR}.${PX4_VERSION_MINOR}.${PX4_VERSION_PATCH}g${PX4_GIT_HASH}" +) + target_link_libraries(bootloader PRIVATE arch_bootloader diff --git a/platforms/nuttx/src/bootloader/common/bl.c b/platforms/nuttx/src/bootloader/common/bl.c index 61f3881d6f9e..f67356be701a 100644 --- a/platforms/nuttx/src/bootloader/common/bl.c +++ b/platforms/nuttx/src/bootloader/common/bl.c @@ -80,7 +80,7 @@ // RESET finalise flash programming, reset chip and starts application // -#define BL_PROTOCOL_VERSION 5 // The revision of the bootloader protocol +#define BL_PROTOCOL_REVISION 5 // The revision of the bootloader protocol //* Next revision needs to update // protocol bytes @@ -106,14 +106,20 @@ #define PROTO_GET_CHIP 0x2c // read chip version (MCU IDCODE) #define PROTO_SET_DELAY 0x2d // set minimum boot delay #define PROTO_GET_CHIP_DES 0x2e // read chip version In ASCII +#define PROTO_GET_VERSION 0x2f // read version #define PROTO_BOOT 0x30 // boot the application #define PROTO_DEBUG 0x31 // emit debug information - format not defined #define PROTO_SET_BAUD 0x33 // set baud rate on uart -#define PROTO_RESERVED_0X36 0x36 // Reserved -#define PROTO_RESERVED_0X37 0x37 // Reserved +// Reserved for external flash programming +// #define PROTO_EXTF_ERASE 0x34 // Erase sectors from external flash +// #define PROTO_EXTF_PROG_MULTI 0x35 // write bytes at external flash program address and increment +// #define PROTO_EXTF_READ_MULTI 0x36 // read bytes at address and increment +// #define PROTO_EXTF_GET_CRC 0x37 // compute & return a CRC of data in external flash + #define PROTO_RESERVED_0X38 0x38 // Reserved #define PROTO_RESERVED_0X39 0x39 // Reserved +#define PROTO_CHIP_FULL_ERASE 0x40 // Full erase, without any flash wear optimization #define PROTO_PROG_MULTI_MAX 64 // maximum PROG_MULTI size #define PROTO_READ_MULTI_MAX 255 // size of the size field @@ -125,13 +131,6 @@ #define PROTO_DEVICE_FW_SIZE 4 // size of flashable area #define PROTO_DEVICE_VEC_AREA 5 // contents of reserved vectors 7-10 -#define STATE_PROTO_OK 0x10 // INSYNC/OK - 'ok' response -#define STATE_PROTO_FAILED 0x11 // INSYNC/FAILED - 'fail' response -#define STATE_PROTO_INVALID 0x13 // INSYNC/INVALID - 'invalid' response for bad commands -#define STATE_PROTO_BAD_SILICON_REV 0x14 // On the F4 series there is an issue with < Rev 3 silicon -#define STATE_PROTO_RESERVED_0X15 0x15 // Reserved - - // State #define STATE_PROTO_GET_SYNC 0x1 // Have Seen NOP for re-establishing sync #define STATE_PROTO_GET_DEVICE 0x2 // Have Seen get device ID bytes @@ -142,7 +141,8 @@ #define STATE_PROTO_GET_SN 0x40 // Have Seen read a word from UDID area ( Serial) at the given address #define STATE_PROTO_GET_CHIP 0x80 // Have Seen read chip version (MCU IDCODE) #define STATE_PROTO_GET_CHIP_DES 0x100 // Have Seen read chip version In ASCII -#define STATE_PROTO_BOOT 0x200 // Have Seen boot the application +#define STATE_PROTO_GET_VERSION 0x200 // Have Seen get version +#define STATE_PROTO_BOOT 0x400 // Have Seen boot the application #if defined(TARGET_HW_PX4_PIO_V1) #define STATE_ALLOWS_ERASE (STATE_PROTO_GET_SYNC) @@ -157,6 +157,18 @@ static uint8_t bl_type; static uint8_t last_input; +int get_version(int n, uint8_t *version_str) +{ + int len = strlen(BOOTLOADER_VERSION); + + if (len > n) { + len = n; + } + + strncpy((char *)version_str, BOOTLOADER_VERSION, n); + return len; +} + inline void cinit(void *config, uint8_t interface) { #if INTERFACE_USB @@ -257,7 +269,7 @@ inline void cout(uint8_t *buf, unsigned len) #endif -static const uint32_t bl_proto_rev = BL_PROTOCOL_VERSION; // value returned by PROTO_DEVICE_BL_REV +static const uint32_t bl_proto_rev = BL_PROTOCOL_REVISION; // value returned by PROTO_DEVICE_BL_REV static unsigned head, tail; static uint8_t rx_buf[256] USB_DATA_ALIGN; @@ -294,7 +306,7 @@ void jump_to_app() { const uint32_t *app_base = (const uint32_t *)APP_LOAD_ADDRESS; - const uint32_t *vec_base = (const uint32_t *)app_base + APP_VECTOR_OFFSET; + const uint32_t *vec_base = (const uint32_t *)((const uint32_t)app_base + APP_VECTOR_OFFSET); /* * We refuse to program the first word of the app until the upload is marked @@ -649,6 +661,8 @@ bootloader(unsigned timeout) led_on(LED_ACTIVITY); + bool full_erase = false; + // handle the command byte switch (c) { @@ -728,6 +742,10 @@ bootloader(unsigned timeout) // success reply: INSYNC/OK // erase failure: INSYNC/FAILURE // + case PROTO_CHIP_FULL_ERASE: + full_erase = true; + + // Fallthrough case PROTO_CHIP_ERASE: /* expect EOC */ @@ -755,17 +773,18 @@ bootloader(unsigned timeout) arch_flash_unlock(); for (int i = 0; flash_func_sector_size(i) != 0; i++) { - flash_func_erase_sector(i); + flash_func_erase_sector(i, full_erase); } // disable the LED while verifying the erase led_set(LED_OFF); // verify the erase - for (address = 0; address < board_info.fw_size; address += 4) + for (address = 0; address < board_info.fw_size; address += 4) { if (flash_func_read_word(address) != 0xffffffff) { goto cmd_fail; } + } address = 0; SET_BL_STATE(STATE_PROTO_CHIP_ERASE); @@ -965,7 +984,7 @@ bootloader(unsigned timeout) // read the chip description // // command: GET_CHIP_DES/EOC - // reply: /INSYNC/OK + // reply: /INSYNC/OK case PROTO_GET_CHIP_DES: { uint8_t buffer[MAX_DES_LENGTH]; unsigned len = MAX_DES_LENGTH; @@ -982,6 +1001,25 @@ bootloader(unsigned timeout) } break; + // read the bootloader version (not to be confused with protocol revision) + // + // command: GET_VERSION/EOC + // reply: /INSYNC/OK + case PROTO_GET_VERSION: { + uint8_t buffer[MAX_VERSION_LENGTH]; + + // expect EOC + if (!wait_for_eoc(2)) { + goto cmd_bad; + } + + int len = get_version(sizeof(buffer), buffer); + cout_word(len); + cout(buffer, len); + SET_BL_STATE(STATE_PROTO_GET_VERSION); + } + break; + #ifdef BOOT_DELAY_ADDRESS case PROTO_SET_DELAY: { diff --git a/platforms/nuttx/src/bootloader/common/bl.h b/platforms/nuttx/src/bootloader/common/bl.h index 4115200db3bb..080bc47e0f33 100644 --- a/platforms/nuttx/src/bootloader/common/bl.h +++ b/platforms/nuttx/src/bootloader/common/bl.h @@ -39,6 +39,8 @@ #pragma once +#include + /***************************************************************************** * Generic bootloader functions. */ @@ -94,6 +96,7 @@ extern int buf_get(void); #endif #define MAX_DES_LENGTH 20 +#define MAX_VERSION_LENGTH 32 #define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) extern void led_on(unsigned led); @@ -105,7 +108,7 @@ extern void board_deinit(void); extern uint32_t board_get_devices(void); extern void clock_deinit(void); extern uint32_t flash_func_sector_size(unsigned sector); -extern void flash_func_erase_sector(unsigned sector); +extern void flash_func_erase_sector(unsigned sector, bool force); extern void flash_func_write_word(uintptr_t address, uint32_t word); extern uint32_t flash_func_read_word(uintptr_t address); extern uint32_t flash_func_read_otp(uintptr_t address); @@ -121,6 +124,8 @@ extern uint32_t get_mcu_id(void); int get_mcu_desc(int max, uint8_t *revstr); extern int check_silicon(void); +int get_version(int max, uint8_t *version_str); + /***************************************************************************** * Interface in/output. */ diff --git a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c index 1197427a942c..dcdc50a91168 100644 --- a/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c +++ b/platforms/nuttx/src/bootloader/nxp/imxrt_common/main.c @@ -16,6 +16,7 @@ #include "imxrt_clockconfig.h" #include +#include #include #include @@ -395,6 +396,24 @@ flash_func_sector_size(unsigned sector) return 0; } +/* imxRT uses Flash lib, not up_progmem so let's stub it here */ +ssize_t up_progmem_ispageerased(unsigned sector) +{ + const uint32_t bytes_per_sector = flash_func_sector_size(sector); + uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); + const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address); + + int blank = 0; /* Assume it is Bank */ + + for (uint32_t i = 0; i < uint32_per_sector; i++) { + if (address[i] != 0xffffffff) { + blank = -1; /* It is not blank */ + break; + } + } + + return blank; +} /*! * @name Configuration Option @@ -407,31 +426,19 @@ flash_func_sector_size(unsigned sector) * */ locate_code(".ramfunc") void -flash_func_erase_sector(unsigned sector) +flash_func_erase_sector(unsigned sector, bool force) { - if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) { return; } - /* blank-check the sector */ - const uint32_t bytes_per_sector = flash_func_sector_size(sector); - uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); - const uint32_t uint32_per_sector = bytes_per_sector / sizeof(*address); - bool blank = true; - - for (uint32_t i = 0; i < uint32_per_sector; i++) { - if (address[i] != 0xffffffff) { - blank = false; - break; - } - } + if (force || up_progmem_ispageerased(sector) != 0) { + struct flexspi_nor_config_s *pConfig = &g_bootConfig; - struct flexspi_nor_config_s *pConfig = &g_bootConfig; + const uint32_t bytes_per_sector = flash_func_sector_size(sector); + uint32_t *address = (uint32_t *)(IMXRT_FLEXSPI1_CIPHER_BASE + (sector * bytes_per_sector)); - /* erase the sector if it failed the blank check */ - if (!blank) { uintptr_t offset = ((uintptr_t) address) - IMXRT_FLEXSPI1_CIPHER_BASE; irqstate_t flags; flags = enter_critical_section(); @@ -439,8 +446,6 @@ flash_func_erase_sector(unsigned sector) leave_critical_section(flags); UNUSED(status); } - - } void @@ -577,6 +582,21 @@ led_toggle(unsigned led) void arch_do_jump(const uint32_t *app_base) { + /* The MPU configuration after booting has ITCM set to MPU_RASR_AP_RORO + * We add this overlaping region to allow the Application to copy code into + * the ITCM when it is booted. With CONFIG_ARM_MPU_RESET defined. The mpu + * init will clear any added regions (after the copy) + */ + + mpu_configure_region(IMXRT_ITCM_BASE, 256 * 1024, + /* Instruction access Enabled */ + MPU_RASR_AP_RWRW | /* P:RW U:RW */ + MPU_RASR_TEX_NOR /* Normal */ + /* Not Cacheable */ + /* Not Bufferable */ + /* Not Shareable */ + /* No Subregion disable */ + ); /* extract the stack and entrypoint from the app vector table and go */ uint32_t stacktop = app_base[APP_VECTOR_OFFSET_WORDS]; diff --git a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c index 894834f078fa..cccfd41bea4c 100644 --- a/platforms/nuttx/src/bootloader/stm/stm32_common/main.c +++ b/platforms/nuttx/src/bootloader/stm/stm32_common/main.c @@ -71,6 +71,14 @@ const mcu_rev_t silicon_revs[] = { {MCU_REV_Z, 'Z'}, /* Revision Z */ }; +/* + * If APP_RESERVATION_SIZE is greater than 0 and + * FLASH_BASED_PARAMS is defined, throw a compile error + */ +#if defined(FLASH_BASED_PARAMS) && (APP_RESERVATION_SIZE <= 0) +# error "APP_RESERVATION_SIZE must be greater than 0 if FLASH_BASED_PARAMS is defined" +#endif + #define APP_SIZE_MAX (BOARD_FLASH_SIZE - (BOOTLOADER_RESERVATION_SIZE + APP_RESERVATION_SIZE)) @@ -458,18 +466,13 @@ flash_func_sector_size(unsigned sector) } void -flash_func_erase_sector(unsigned sector) +flash_func_erase_sector(unsigned sector, bool force) { if (sector > BOARD_FLASH_SECTORS || (int)sector < BOARD_FIRST_FLASH_SECTOR_TO_ERASE) { return; } - /* blank-check the sector */ - - bool blank = up_progmem_ispageerased(sector) == 0; - - /* erase the sector if it failed the blank check */ - if (!blank) { + if (force || (up_progmem_ispageerased(sector) != 0)) { up_progmem_eraseblock(sector); } } diff --git a/platforms/nuttx/src/px4/CMakeLists.txt b/platforms/nuttx/src/px4/CMakeLists.txt index 929788b587e2..a693422af66a 100644 --- a/platforms/nuttx/src/px4/CMakeLists.txt +++ b/platforms/nuttx/src/px4/CMakeLists.txt @@ -32,6 +32,4 @@ ############################################################################ add_subdirectory(common) - add_subdirectory(${PX4_CHIP_MANUFACTURER}) - diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index ab16e77a0e30..c74810b03a88 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -38,7 +38,6 @@ 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 diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp new file mode 100644 index 000000000000..4264976f4471 --- /dev/null +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -0,0 +1,511 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +#define MODULE_NAME "SerialImpl" + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (validatePort(port)) { + setPort(port); + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + switch (_baudrate) { + case 0: + // special case, if baudrate is 0 it hangs entire system + PX4_ERR("baudrate not specified"); + return false; + + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + + default: + speed = _baudrate; + PX4_WARN("Using non-standard baudrate: %lu", _baudrate); + break; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + // Do pin operations after port has been opened + if (_single_wire_mode) { + setSingleWireMode(); + } + + if (_swap_rx_tx_mode) { + setSwapRxTxMode(); + } + + setInvertedMode(_inverted_mode); + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + hrt_abstime timeout_us = timeout_ms * 1000; + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime elapsed_time_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_time_us > timeout_us) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), (timeout_us - elapsed_time_us) / 1000); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + + int err = 0; + int bytes_available = 0; + err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + + if (err != 0 || bytes_available < (int)character_count) { + px4_usleep(sleeptime); + } + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + } + + return written; +} + +void SerialImpl::flush() +{ + if (_open) { + tcflush(_serial_fd, TCIOFLUSH); + } +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::validatePort(const char *port) +{ + return (port && (access(port, R_OK | W_OK) == 0)); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +bool SerialImpl::getSingleWireMode() const +{ + return _single_wire_mode; +} + +bool SerialImpl::setSingleWireMode() +{ +#if defined(TIOCSSINGLEWIRE) + + if (_open) { + ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + } + + _single_wire_mode = true; + return true; +#else + return false; +#endif // TIOCSSINGLEWIRE +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return _swap_rx_tx_mode; +} + +bool SerialImpl::setSwapRxTxMode() +{ +#if defined(TIOCSSWAP) + + if (_open) { + ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED); + } + + _swap_rx_tx_mode = true; + return true; +#else + return false; +#endif // TIOCSSWAP +} + +bool SerialImpl::getInvertedMode() const +{ + return _inverted_mode; +} + +bool SerialImpl::setInvertedMode(bool enable) +{ +#if defined(TIOCSINVERT) + + if (_open) { + if (enable) { + ioctl(_serial_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX); + + } else { + ioctl(_serial_fd, TIOCSINVERT, 0); + } + } + + _inverted_mode = enable; + return true; +#else + return _inverted_mode == enable; +#endif // TIOCSINVERT +} + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt b/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt index 8dc5414ad4aa..da6dac979f20 100644 --- a/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt @@ -34,4 +34,3 @@ px4_add_library(platform_gpio_mcp23009 mcp23009.cpp ) target_link_libraries(platform_gpio_mcp23009 PRIVATE drivers__device) # device::I2C - diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp new file mode 100644 index 000000000000..543041bc978f --- /dev/null +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + void flush(); + + const char *getPort() const; + static bool validatePort(const char *port); + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + + bool getInvertedMode() const; + bool setInvertedMode(bool enable); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool configure(); + + bool _single_wire_mode{false}; + bool _swap_rx_tx_mode{false}; + bool _inverted_mode{false}; + +}; + +} // namespace device diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/adc.h b/platforms/nuttx/src/px4/common/include/px4_platform/adc.h index d7f6e85a5b94..228d609e8ee8 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/adc.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/adc.h @@ -37,4 +37,3 @@ #else # define SYSTEM_ADC_COUNT 2 #endif - diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h index 6daca09771df..670b702f922d 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h @@ -54,4 +54,3 @@ __BEGIN_DECLS #include __END_DECLS - diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index ceeb79bd7240..aa11b1cf703c 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -62,8 +62,6 @@ #include #endif -extern void cdcacm_init(void); - #if !defined(CONFIG_BUILD_FLAT) typedef CODE void (*initializer_t)(void); extern initializer_t _sinit; @@ -196,10 +194,6 @@ int px4_platform_init() px4_log_initialize(); -#if defined(CONFIG_SYSTEM_CDCACM) && defined(CONFIG_BUILD_FLAT) - cdcacm_init(); -#endif - return PX4_OK; } diff --git a/platforms/nuttx/src/px4/common/px4_layer.cmake b/platforms/nuttx/src/px4/common/px4_layer.cmake index 69fd1a007da9..de8fe5a0c683 100644 --- a/platforms/nuttx/src/px4/common/px4_layer.cmake +++ b/platforms/nuttx/src/px4/common/px4_layer.cmake @@ -2,7 +2,8 @@ add_library(px4_layer ${KERNEL_SRCS} - cdc_acm_check.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index 24d88ed3d89a..6723c35ecadb 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -75,11 +75,11 @@ static int ramtron_attach(mtd_instance_s &instance) return ENXIO; #else - /* start the RAMTRON driver, attempt 10 times */ + /* start the RAMTRON driver at 30MHz */ - int spi_speed_mhz = 10; + unsigned long spi_speed_hz = 30'000'000; - for (int i = 0; i < 10; i++) { + for (int i = 0; spi_speed_hz > 0; i++) { /* initialize the right spi */ struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); @@ -90,7 +90,7 @@ static int ramtron_attach(mtd_instance_s &instance) /* this resets the spi bus, set correct bus speed again */ SPI_LOCK(spi, true); - SPI_SETFREQUENCY(spi, spi_speed_mhz * 1000 * 1000); + SPI_SETFREQUENCY(spi, spi_speed_hz); SPI_SETBITS(spi, 8); SPI_SETMODE(spi, SPIDEV_MODE3); SPI_SELECT(spi, instance.devid, false); @@ -108,7 +108,7 @@ static int ramtron_attach(mtd_instance_s &instance) } // try reducing speed for next attempt - spi_speed_mhz--; + spi_speed_hz -= 1'000'000; px4_usleep(10000); } @@ -118,7 +118,7 @@ static int ramtron_attach(mtd_instance_s &instance) return -EIO; } - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)spi_speed_mhz * 1000 * 1000); + int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, spi_speed_hz); if (ret != OK) { // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 9c57b488b6d3..5fbbef164457 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -6,7 +6,6 @@ add_library(px4_layer board_fat_dma_alloc.c tasks.cpp console_buffer_usr.cpp - cdc_acm_check.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/print_load.cpp ${PX4_SOURCE_DIR}/platforms/posix/src/px4/common/cpuload.cpp px4_userspace_init.cpp @@ -15,6 +14,8 @@ add_library(px4_layer usr_board_ctrl.c usr_hrt.cpp usr_mcu_version.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_link_libraries(px4_layer @@ -44,6 +45,7 @@ target_link_libraries(px4_layer add_library(px4_kernel_layer ${KERNEL_SRCS} + SerialImpl.cpp ) target_link_libraries(px4_kernel_layer diff --git a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp index f5d9882ca23a..82cace9e4d24 100644 --- a/platforms/nuttx/src/px4/common/px4_userspace_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_userspace_init.cpp @@ -44,8 +44,6 @@ #include #include -extern void cdcacm_init(void); - extern "C" void px4_userspace_init(void) { hrt_init(); @@ -55,8 +53,4 @@ extern "C" void px4_userspace_init(void) px4::WorkQueueManagerStart(); uorb_start(); - -#if defined(CONFIG_SYSTEM_CDCACM) - cdcacm_init(); -#endif } diff --git a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp index 1fc3f57b2dd7..64d86ee92a16 100644 --- a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp +++ b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp @@ -114,4 +114,3 @@ int board_get_px4_guid_formated(char *format_buffer, int size) return offset; } - diff --git a/platforms/nuttx/src/px4/nxp/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/CMakeLists.txt index ec489bf559c3..72584f3178ec 100644 --- a/platforms/nuttx/src/px4/nxp/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/CMakeLists.txt @@ -31,6 +31,4 @@ # ############################################################################ - add_subdirectory(${PX4_CHIP}) - diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt index cc1563990c4a..1bc35a48eb70 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/CMakeLists.txt @@ -34,4 +34,4 @@ px4_add_library(arch_board_hw_info board_hw_rev_ver.c ) -target_link_libraries(arch_board_hw_info PRIVATE arch_adc crc) \ No newline at end of file +target_link_libraries(arch_board_hw_info PRIVATE arch_adc crc) diff --git a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c index ab3408c12ca8..22a27427f9dc 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/dshot/dshot.c @@ -33,145 +33,453 @@ ****************************************************************************/ #include #include +#include #include +#include +#include #include #include #include #include +#include "barriers.h" #include "arm_internal.h" +#define FLEXIO_BASE IMXRT_FLEXIO1_BASE #define DSHOT_TIMERS FLEXIO_SHIFTBUFNIS_COUNT #define DSHOT_THROTTLE_POSITION 5u #define DSHOT_TELEMETRY_POSITION 4u #define NIBBLES_SIZE 4u #define DSHOT_NUMBER_OF_NIBBLES 3u +#if defined(IOMUX_PULL_UP_47K) +#define IOMUX_PULL_UP IOMUX_PULL_UP_47K +#endif + +static const uint32_t gcr_decode[32] = { + 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, + 0x0, 0x9, 0xA, 0xB, 0x0, 0xD, 0xE, 0xF, + 0x0, 0x0, 0x2, 0x3, 0x0, 0x5, 0x6, 0x7, + 0x0, 0x0, 0x8, 0x1, 0x0, 0x4, 0xC, 0x0 +}; + +uint32_t erpms[DSHOT_TIMERS]; + +typedef enum { + DSHOT_START = 0, + DSHOT_12BIT_FIFO, + DSHOT_12BIT_TRANSFERRED, + DSHOT_TRANSMIT_COMPLETE, + BDSHOT_RECEIVE, + BDSHOT_RECEIVE_COMPLETE, +} dshot_state; + typedef struct dshot_handler_t { bool init; uint32_t data_seg1; uint32_t irq_data; + dshot_state state; + bool bdshot; + uint32_t raw_response; + uint16_t erpm; + uint32_t crc_error_cnt; + uint32_t frame_error_cnt; + uint32_t no_response_cnt; + uint32_t last_no_response_cnt; } dshot_handler_t; +#define BDSHOT_OFFLINE_COUNT 400 // If there are no responses for 400 setpoints ESC is offline + static dshot_handler_t dshot_inst[DSHOT_TIMERS] = {}; -struct flexio_dev_s *flexio_s; +static uint32_t dshot_tcmp; +static uint32_t bdshot_tcmp; +static uint32_t dshot_mask; +static uint32_t bdshot_recv_mask; +static uint32_t bdshot_parsed_recv_mask; -static int flexio_irq_handler(int irq, void *context, void *arg) +static inline uint32_t flexio_getreg32(uint32_t offset) { + return getreg32(FLEXIO_BASE + offset); +} - uint32_t flags = flexio_s->ops->get_shifter_status_flags(flexio_s); - uint32_t instance; +static inline void flexio_modifyreg32(unsigned int offset, + uint32_t clearbits, + uint32_t setbits) +{ + modifyreg32(FLEXIO_BASE + offset, clearbits, setbits); +} - for (instance = 0; flags && instance < DSHOT_TIMERS; instance++) { - if (flags & (1 << instance)) { - flexio_s->ops->disable_shifter_status_interrupts(flexio_s, (1 << instance)); - flexio_s->ops->disable_timer_status_interrupts(flexio_s, (1 << instance)); +static inline void flexio_putreg32(uint32_t value, uint32_t offset) +{ + putreg32(value, FLEXIO_BASE + offset); +} - if (dshot_inst[instance].irq_data != 0) { - uint32_t buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, instance); - putreg32(dshot_inst[instance].irq_data, IMXRT_FLEXIO1_BASE + buf_adr); - dshot_inst[instance].irq_data = 0; - } - } - } +static inline void enable_shifter_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, 0, mask); +} - return OK; +static inline void disable_shifter_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_SHIFTSIEN_OFFSET, mask, 0); } -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +static inline uint32_t get_shifter_status_flags(void) { - uint32_t timer_compare; + return flexio_getreg32(IMXRT_FLEXIO_SHIFTSTAT_OFFSET); +} - if (dshot_pwm_freq == 150000) { - timer_compare = 0x2F8A; +static inline void clear_shifter_status_flags(uint32_t mask) +{ + flexio_putreg32(mask, IMXRT_FLEXIO_SHIFTSTAT_OFFSET); +} - } else if (dshot_pwm_freq == 300000) { - timer_compare = 0x2F45; +static inline void enable_timer_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, 0, mask); +} - } else if (dshot_pwm_freq == 600000) { - timer_compare = 0x2F22; +static inline void disable_timer_status_interrupts(uint32_t mask) +{ + flexio_modifyreg32(IMXRT_FLEXIO_TIMIEN_OFFSET, mask, 0); +} - } else if (dshot_pwm_freq == 1200000) { - timer_compare = 0x2F11; +static inline uint32_t get_timer_status_flags(void) +{ + return flexio_getreg32(IMXRT_FLEXIO_TIMSTAT_OFFSET); +} - } else { - // Not supported Dshot frequency - return 0; +static inline void clear_timer_status_flags(uint32_t mask) +{ + flexio_putreg32(mask, IMXRT_FLEXIO_TIMSTAT_OFFSET); +} + +static void flexio_dshot_output(uint32_t channel, uint32_t pin, uint32_t timcmp, bool inverted) +{ + /* Disable Shifter */ + flexio_putreg32(0, IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* No start bit, stop bit low */ + flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) | + FLEXIO_SHIFTCFG_PWIDTH(0) | + FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_LOW) | + FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE), + IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4); + + /* Transmit mode, output to FXIO pin, inverted output for bdshot */ + flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) | + FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) | + FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT) | + FLEXIO_SHIFTCTL_PINSEL(pin) | + FLEXIO_SHIFTCTL_PINPOL(inverted) | + FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_TRANSMIT), + IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* Start transmitting on trigger, disable on compare */ + flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET) | + FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) | + FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_NEVER) | + FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) | + FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH) | + FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_DISABLED) | + FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_DISABLED), + IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4); + + flexio_putreg32(timcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); + + /* Baud mode, Trigger on shifter write */ + flexio_putreg32(FLEXIO_TIMCTL_TRGSEL((4 * channel) + 1) | + FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW) | + FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) | + FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_TIMCTL_PINSEL(0) | + FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT), + IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + +} + +static int flexio_irq_handler(int irq, void *context, void *arg) +{ + uint32_t flags = get_shifter_status_flags(); + uint32_t channel; + + for (channel = 0; flags && channel < DSHOT_TIMERS; channel++) { + if (flags & (1 << channel)) { + disable_shifter_status_interrupts(1 << channel); + + if (dshot_inst[channel].state == DSHOT_START) { + dshot_inst[channel].state = DSHOT_12BIT_FIFO; + flexio_putreg32(dshot_inst[channel].irq_data, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4); + + } else if (dshot_inst[channel].state == BDSHOT_RECEIVE) { + dshot_inst[channel].state = BDSHOT_RECEIVE_COMPLETE; + dshot_inst[channel].raw_response = flexio_getreg32(IMXRT_FLEXIO_SHIFTBUFBIS0_OFFSET + channel * 0x4); + + bdshot_recv_mask |= (1 << channel); + + if (bdshot_recv_mask == dshot_mask) { + // Received telemetry on all channels + // Schedule workqueue? + } + } + } } - /* Init FlexIO peripheral */ + flags = get_timer_status_flags(); + + for (channel = 0; flags; (channel = (channel + 1) % DSHOT_TIMERS)) { + flags = get_timer_status_flags(); + + if (flags & (1 << channel)) { + clear_timer_status_flags(1 << channel); + + if (dshot_inst[channel].state == DSHOT_12BIT_FIFO) { + dshot_inst[channel].state = DSHOT_12BIT_TRANSFERRED; + + } else if (!dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) { + dshot_inst[channel].state = DSHOT_TRANSMIT_COMPLETE; + + } else if (dshot_inst[channel].bdshot && dshot_inst[channel].state == DSHOT_12BIT_TRANSFERRED) { + disable_shifter_status_interrupts(1 << channel); + dshot_inst[channel].state = BDSHOT_RECEIVE; + + /* Transmit done, disable timer and reconfigure to receive*/ + flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + + /* Input data from pin, no start/stop bit*/ + flexio_putreg32(FLEXIO_SHIFTCFG_INSRC(FLEXIO_SHIFTER_INPUT_FROM_PIN) | + FLEXIO_SHIFTCFG_PWIDTH(0) | + FLEXIO_SHIFTCFG_SSTOP(FLEXIO_SHIFTER_STOP_BIT_DISABLE) | + FLEXIO_SHIFTCFG_SSTART(FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_SHIFT), + IMXRT_FLEXIO_SHIFTCFG0_OFFSET + channel * 0x4); + + /* Shifter receive mdoe, on FXIO pin input */ + flexio_putreg32(FLEXIO_SHIFTCTL_TIMSEL(channel) | + FLEXIO_SHIFTCTL_TIMPOL(FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE) | + FLEXIO_SHIFTCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_SHIFTCTL_PINSEL(timer_io_channels[channel].dshot.flexio_pin) | + FLEXIO_SHIFTCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_SHIFTCTL_SMOD(FLEXIO_SHIFTER_MODE_RECEIVE), + IMXRT_FLEXIO_SHIFTCTL0_OFFSET + channel * 0x4); + + /* Make sure there no shifter flags high from transmission */ + clear_shifter_status_flags(1 << channel); + + /* Enable on pin transition, resychronize through reset on rising edge */ + flexio_putreg32(FLEXIO_TIMCFG_TIMOUT(FLEXIO_TIMER_OUTPUT_ONE_AFFECTED_BY_RESET) | + FLEXIO_TIMCFG_TIMDEC(FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT) | + FLEXIO_TIMCFG_TIMRST(FLEXIO_TIMER_RESET_ON_TIMER_PIN_RISING_EDGE) | + FLEXIO_TIMCFG_TIMDIS(FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE) | + FLEXIO_TIMCFG_TIMENA(FLEXIO_TIMER_ENABLE_ON_TRIGGER_BOTH_EDGE) | + FLEXIO_TIMCFG_TSTOP(FLEXIO_TIMER_STOP_BIT_ENABLE_ON_TIMER_DISABLE) | + FLEXIO_TIMCFG_TSTART(FLEXIO_TIMER_START_BIT_ENABLED), + IMXRT_FLEXIO_TIMCFG0_OFFSET + channel * 0x4); + + /* Enable on pin transition, resychronize through reset on rising edge */ + flexio_putreg32(bdshot_tcmp, IMXRT_FLEXIO_TIMCMP0_OFFSET + channel * 0x4); + + /* Trigger on FXIO pin transition, Baud mode */ + flexio_putreg32(FLEXIO_TIMCTL_TRGSEL(2 * timer_io_channels[channel].dshot.flexio_pin) | + FLEXIO_TIMCTL_TRGPOL(FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_HIGH) | + FLEXIO_TIMCTL_TRGSRC(FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL) | + FLEXIO_TIMCTL_PINCFG(FLEXIO_PIN_CONFIG_OUTPUT_DISABLED) | + FLEXIO_TIMCTL_PINSEL(0) | + FLEXIO_TIMCTL_PINPOL(FLEXIO_PIN_ACTIVE_LOW) | + FLEXIO_TIMCTL_TIMOD(FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT), + IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + + /* Enable shifter interrupt for receiving data */ + enable_shifter_status_interrupts(1 << channel); + } + } + + } - flexio_s = imxrt_flexio_initialize(1); + return OK; +} + + +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) +{ + /* Calculate dshot timings based on dshot_pwm_freq */ + dshot_tcmp = 0x2F00 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 3) / 2) - 1) & 0xFF); + bdshot_tcmp = 0x2900 | (((BOARD_FLEXIO_PREQ / (dshot_pwm_freq * 5 / 4) / 2) - 3) & 0xFF); + + /* Clock FlexIO peripheral */ + imxrt_clockall_flexio1(); + + /* Reset FlexIO peripheral */ + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0, + FLEXIO_CTRL_SWRST_MASK); + flexio_putreg32(0, IMXRT_FLEXIO_CTRL_OFFSET); + + /* Initialize FlexIO peripheral */ + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, + (FLEXIO_CTRL_DOZEN_MASK | + FLEXIO_CTRL_DBGE_MASK | + FLEXIO_CTRL_FASTACC_MASK | + FLEXIO_CTRL_FLEXEN_MASK), + (FLEXIO_CTRL_DBGE(1) | + FLEXIO_CTRL_FASTACC(1) | + FLEXIO_CTRL_FLEXEN(0))); + + /* FlexIO IRQ handling */ up_enable_irq(IMXRT_IRQ_FLEXIO1); - irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, flexio_s); + irq_attach(IMXRT_IRQ_FLEXIO1, flexio_irq_handler, 0); + + dshot_mask = 0x0; for (unsigned channel = 0; (channel_mask != 0) && (channel < DSHOT_TIMERS); channel++) { if (channel_mask & (1 << channel)) { - uint8_t timer = timer_io_channels[channel].timer_index; - if (io_timers[timer].dshot.pinmux == 0) { // board does not configure dshot on this pin + if (timer_io_channels[channel].dshot.pinmux == 0) { // board does not configure dshot on this pin continue; } - imxrt_config_gpio(io_timers[timer].dshot.pinmux); - - struct flexio_shifter_config_s shft_cfg; - shft_cfg.timer_select = channel; - shft_cfg.timer_polarity = FLEXIO_SHIFTER_TIMER_POLARITY_ON_POSITIVE; - shft_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT; - shft_cfg.pin_select = io_timers[timer].dshot.flexio_pin; - shft_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_HIGH; - shft_cfg.shifter_mode = FLEXIO_SHIFTER_MODE_TRANSMIT; - shft_cfg.parallel_width = 0; - shft_cfg.input_source = FLEXIO_SHIFTER_INPUT_FROM_PIN; - shft_cfg.shifter_stop = FLEXIO_SHIFTER_STOP_BIT_LOW; - shft_cfg.shifter_start = FLEXIO_SHIFTER_START_BIT_DISABLED_LOAD_DATA_ON_ENABLE; - - flexio_s->ops->set_shifter_config(flexio_s, channel, &shft_cfg); - - struct flexio_timer_config_s tmr_cfg; - tmr_cfg.trigger_select = (4 * channel) + 1; - tmr_cfg.trigger_polarity = FLEXIO_TIMER_TRIGGER_POLARITY_ACTIVE_LOW; - tmr_cfg.trigger_source = FLEXIO_TIMER_TRIGGER_SOURCE_INTERNAL; - tmr_cfg.pin_config = FLEXIO_PIN_CONFIG_OUTPUT_DISABLED; - tmr_cfg.pin_select = 0; - tmr_cfg.pin_polarity = FLEXIO_PIN_ACTIVE_LOW; - tmr_cfg.timer_mode = FLEXIO_TIMER_MODE_DUAL8_BIT_BAUD_BIT; - tmr_cfg.timer_output = FLEXIO_TIMER_OUTPUT_ONE_NOT_AFFECTED_BY_RESET; - tmr_cfg.timer_decrement = FLEXIO_TIMER_DEC_SRC_ON_FLEX_IO_CLOCK_SHIFT_TIMER_OUTPUT; - tmr_cfg.timer_reset = FLEXIO_TIMER_RESET_NEVER; - tmr_cfg.timer_disable = FLEXIO_TIMER_DISABLE_ON_TIMER_COMPARE; - tmr_cfg.timer_enable = FLEXIO_TIMER_ENABLE_ON_TRIGGER_HIGH; - tmr_cfg.timer_stop = FLEXIO_TIMER_STOP_BIT_DISABLED; - tmr_cfg.timer_start = FLEXIO_TIMER_START_BIT_DISABLED; - tmr_cfg.timer_compare = timer_compare; - flexio_s->ops->set_timer_config(flexio_s, channel, &tmr_cfg); + imxrt_config_gpio(timer_io_channels[channel].dshot.pinmux | IOMUX_PULL_UP); + + dshot_inst[channel].bdshot = enable_bidirectional_dshot; + + flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); dshot_inst[channel].init = true; + + // Mask channel to be active on dshot + dshot_mask |= (1 << channel); } } - flexio_s->ops->enable(flexio_s, true); + flexio_modifyreg32(IMXRT_FLEXIO_CTRL_OFFSET, 0, + FLEXIO_CTRL_FLEXEN_MASK); return channel_mask; } +void up_bdshot_erpm(void) +{ + uint32_t value; + uint32_t data; + uint32_t csum_data; + uint8_t exponent; + uint16_t period; + uint16_t erpm; + + bdshot_parsed_recv_mask = 0; + + // Decode each individual channel + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + if (bdshot_recv_mask & (1 << channel)) { + value = ~dshot_inst[channel].raw_response & 0xFFFFF; + + /* if lowest significant isn't 1 we've got a framing error */ + if (value & 0x1) { + /* Decode RLL */ + value = (value ^ (value >> 1)); + + /* Decode GCR */ + data = gcr_decode[value & 0x1fU]; + data |= gcr_decode[(value >> 5U) & 0x1fU] << 4U; + data |= gcr_decode[(value >> 10U) & 0x1fU] << 8U; + data |= gcr_decode[(value >> 15U) & 0x1fU] << 12U; + + /* Calculate checksum */ + csum_data = data; + csum_data = csum_data ^ (csum_data >> 8U); + csum_data = csum_data ^ (csum_data >> NIBBLES_SIZE); + + if ((csum_data & 0xFU) != 0xFU) { + dshot_inst[channel].crc_error_cnt++; + + } else { + data = (data >> 4) & 0xFFF; + + if (data == 0xFFF) { + erpm = 0; + + } else { + exponent = ((data >> 9U) & 0x7U); /* 3 bit: exponent */ + period = (data & 0x1ffU); /* 9 bit: period base */ + period = period << exponent; /* Period in usec */ + erpm = ((1000000U * 60U / 100U + period / 2U) / period); + } + + dshot_inst[channel].erpm = erpm; + bdshot_parsed_recv_mask |= (1 << channel); + dshot_inst[channel].last_no_response_cnt = dshot_inst[channel].no_response_cnt; + } + + } else { + dshot_inst[channel].frame_error_cnt++; + } + } + } +} + + + +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + if (bdshot_parsed_recv_mask & (1 << channel)) { + *erpm = (int)dshot_inst[channel].erpm; + return 0; + } + + return -1; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + if (channel < DSHOT_TIMERS) { + return ((dshot_inst[channel].no_response_cnt - dshot_inst[channel].last_no_response_cnt) < BDSHOT_OFFLINE_COUNT); + } + + return -1; +} + +void up_bdshot_status(void) +{ + + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + + if (dshot_inst[channel].init) { + PX4_INFO("Channel %i %s Last erpm %i value", channel, up_bdshot_channel_status(channel) ? "online" : "offline", + dshot_inst[channel].erpm); + PX4_INFO("CRC errors Frame error No response"); + PX4_INFO("%10lu %11lu %11lu", dshot_inst[channel].crc_error_cnt, dshot_inst[channel].frame_error_cnt, + dshot_inst[channel].no_response_cnt); + } + } +} + void up_dshot_trigger(void) { - uint32_t buf_adr; + // Calc data now since we're not event driven + if (bdshot_recv_mask != 0x0) { + up_bdshot_erpm(); + } + + clear_timer_status_flags(0xFF); - for (uint8_t motor_number = 0; (motor_number < DSHOT_TIMERS); motor_number++) { - if (dshot_inst[motor_number].init && dshot_inst[motor_number].data_seg1 != 0) { - buf_adr = flexio_s->ops->get_shifter_buffer_address(flexio_s, FLEXIO_SHIFTER_BUFFER, motor_number); - putreg32(dshot_inst[motor_number].data_seg1, IMXRT_FLEXIO1_BASE + buf_adr); + for (uint8_t channel = 0; (channel < DSHOT_TIMERS); channel++) { + if (dshot_inst[channel].bdshot && (bdshot_recv_mask & (1 << channel)) == 0) { + dshot_inst[channel].no_response_cnt++; + } + + if (dshot_inst[channel].init && dshot_inst[channel].data_seg1 != 0) { + flexio_putreg32(dshot_inst[channel].data_seg1, IMXRT_FLEXIO_SHIFTBUF0_OFFSET + channel * 0x4); } } - flexio_s->ops->clear_timer_status_flags(flexio_s, 0xFF); - flexio_s->ops->enable_shifter_status_interrupts(flexio_s, 0xFF); + bdshot_recv_mask = 0x0; + + clear_timer_status_flags(0xFF); + enable_shifter_status_interrupts(0xFF); + enable_timer_status_interrupts(0xFF); } +/* Expand packet from 16 bits 48 to get T0H and T1H timing */ uint64_t dshot_expand_data(uint16_t packet) { unsigned int mask; @@ -197,16 +505,22 @@ uint64_t dshot_expand_data(uint16_t packet) * bit 12 - dshot telemetry enable/disable * bits 13-16 - XOR checksum **/ -void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemetry) +void dshot_motor_data_set(unsigned channel, uint16_t throttle, bool telemetry) { - if (motor_number < DSHOT_TIMERS && dshot_inst[motor_number].init) { + if (channel < DSHOT_TIMERS && dshot_inst[channel].init) { + uint16_t csum_data; uint16_t packet = 0; uint16_t checksum = 0; packet |= throttle << DSHOT_THROTTLE_POSITION; packet |= ((uint16_t)telemetry & 0x01) << DSHOT_TELEMETRY_POSITION; - uint16_t csum_data = packet; + if (dshot_inst[channel].bdshot) { + csum_data = ~packet; + + } else { + csum_data = packet; + } /* XOR checksum calculation */ csum_data >>= NIBBLES_SIZE; @@ -219,8 +533,20 @@ void dshot_motor_data_set(unsigned motor_number, uint16_t throttle, bool telemet packet |= (checksum & 0x0F); uint64_t dshot_expanded = dshot_expand_data(packet); - dshot_inst[motor_number].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF); - dshot_inst[motor_number].irq_data = (uint32_t)(dshot_expanded >> 24); + + dshot_inst[channel].data_seg1 = (uint32_t)(dshot_expanded & 0xFFFFFF); + dshot_inst[channel].irq_data = (uint32_t)(dshot_expanded >> 24); + dshot_inst[channel].state = DSHOT_START; + + if (dshot_inst[channel].bdshot) { + + flexio_putreg32(0x0, IMXRT_FLEXIO_TIMCTL0_OFFSET + channel * 0x4); + disable_shifter_status_interrupts(1 << channel); + + flexio_dshot_output(channel, timer_io_channels[channel].dshot.flexio_pin, dshot_tcmp, dshot_inst[channel].bdshot); + + clear_timer_status_flags(0xFF); + } } } diff --git a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h index 4cffe9c858a5..1fba2e04494f 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/io_timer.h @@ -87,7 +87,6 @@ typedef struct io_timers_t { uint32_t clock_register; /* SIM_SCGCn */ uint32_t clock_bit; /* SIM_SCGCn bit pos */ uint32_t vectorno; /* IRQ number */ - dshot_conf_t dshot; } io_timers_t; typedef struct io_timers_channel_mapping_element_t { @@ -112,6 +111,7 @@ typedef struct timer_io_channels_t { uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ uint8_t timer_channel; /* Unused */ + dshot_conf_t dshot; } timer_io_channels_t; #define SM0 0 diff --git a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c index 240c62d062dc..4fa7d7fdb6fb 100644 --- a/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c +++ b/platforms/nuttx/src/px4/nxp/imxrt/io_pins/io_timer.c @@ -255,6 +255,34 @@ static inline int channels_timer(unsigned channel) return timer_io_channels[channel].timer_index; } +static uint32_t get_timer_channels(unsigned timer) +{ + uint32_t channels = 0; + static uint32_t channels_cache[MAX_IO_TIMERS] = {0}; + + if (validate_timer_index(timer) < 0) { + return channels; + + } else { + if (channels_cache[timer] == 0) { + /* Gather the channel bits that belong to the timer */ + + uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; + uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; + + for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) { + channels |= 1 << chan_index; + } + + /* cache them */ + + channels_cache[timer] = channels; + } + } + + return channels_cache[timer]; +} + static uint32_t get_channel_mask(unsigned channel) { return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0; @@ -391,41 +419,66 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode) return rv; } -static int timer_set_rate(unsigned channel, unsigned rate) +static int timer_set_rate(unsigned timer, unsigned rate) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + + } + } + px4_leave_critical_section(flags); return 0; } -static inline void io_timer_set_oneshot_mode(unsigned channel) +static inline void io_timer_set_oneshot_mode(unsigned timer) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); - rvalue &= ~SMCTRL_PRSC_MASK; - rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~SMCTRL_PRSC_MASK; + rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + } + } + px4_leave_critical_section(flags); } -static inline void io_timer_set_PWM_mode(unsigned channel) +static inline void io_timer_set_PWM_mode(unsigned timer) { + int channels = get_timer_channels(timer); + irqstate_t flags = px4_enter_critical_section(); - uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); - rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); - rvalue |= SMCTRL_PRSC_DIV16; - rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT - ; - rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; - rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + + for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) { + if ((1 << channel) & channels) { + uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module); + rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD); + rvalue |= SMCTRL_PRSC_DIV16; + rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT + ; + rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module) = rvalue; + rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits; + } + } + px4_leave_critical_section(flags); } @@ -530,33 +583,35 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode) break; } - uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index; - uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count; - - for (uint32_t chan = first_channel_index; chan < last_channel_index; chan++) { - - /* Clear all Faults */ - rFSTS0(timer) = FSTS_FFLAG_MASK; - - rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; - rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; - /* Edge aligned at 0 */ - rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; - rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; - rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; - rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; - rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) - : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); - rDTSRCSEL(timer) = 0; - rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); - rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; - io_timer_set_PWM_mode(chan); - timer_set_rate(chan, 50); + int channels = get_timer_channels(timer); + + for (uint32_t chan = 0; chan < DIRECT_PWM_OUTPUT_CHANNELS; ++chan) { + if ((1 << chan) & channels) { + + /* Clear all Faults */ + rFSTS0(timer) = FSTS_FFLAG_MASK; + + rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP; + rCTRL(timer, timer_io_channels[chan].sub_module) = SMCTRL_PRSC_DIV16 | SMCTRL_FULL; + /* Edge aligned at 0 */ + rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0; + rFFILT0(timer) &= ~FFILT_FILT_PER_MASK; + rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000; + rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000; + rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module) + : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module); + rDTSRCSEL(timer) = 0; + rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module); + rMCTRL(timer) = timer_io_channels[chan].sub_module_bits; + } } + io_timer_set_PWM_mode(timer); + timer_set_rate(timer, 50); + px4_leave_critical_section(flags); } @@ -818,8 +873,7 @@ uint16_t io_channel_get_ccr(unsigned channel) return value; } -// The rt has 1:1 group to channel -uint32_t io_timer_get_group(unsigned group) +uint32_t io_timer_get_group(unsigned timer) { - return get_channel_mask(group); + return get_timer_channels(timer); } diff --git a/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt index 2b96e342f86a..5cacc8fcfc58 100644 --- a/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/k66/CMakeLists.txt @@ -40,6 +40,3 @@ add_subdirectory(../kinetis/hrt hrt) add_subdirectory(../kinetis/io_pins io_pins) add_subdirectory(../kinetis/tone_alarm tone_alarm) add_subdirectory(../kinetis/version version) - - - diff --git a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h index 743deaed331c..521bcf04d6c7 100644 --- a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/adc.h @@ -33,4 +33,3 @@ #pragma once #include "../../../kinetis/include/px4_arch/adc.h" - diff --git a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h index 11661f853a71..11fbae6cbc4a 100644 --- a/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/k66/include/px4_arch/io_timer.h @@ -32,6 +32,4 @@ ****************************************************************************/ #pragma once - #include "../../../kinetis/include/px4_arch/io_timer.h" - diff --git a/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt index ebd97f8a42fa..3edd775b6a63 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/kinetis/hrt/CMakeLists.txt @@ -35,4 +35,3 @@ px4_add_library(arch_hrt hrt.c ) target_compile_options(arch_hrt PRIVATE -Wno-cast-align) # TODO: fix and enable - diff --git a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h index 400305ebd21b..5ae5548411b5 100644 --- a/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/adc.h @@ -37,5 +37,3 @@ #define SYSTEM_ADC_BASE 0 // not used on kinetis #include - - diff --git a/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt index d94ea8d902f0..f487b031c7fe 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/rt106x/CMakeLists.txt @@ -36,7 +36,7 @@ add_subdirectory(../imxrt/adc adc) add_subdirectory(../imxrt/board_critmon board_critmon) add_subdirectory(../imxrt/board_hw_info board_hw_info) add_subdirectory(../imxrt/board_reset board_reset) -#add_subdirectory(../imxrt/dshot dshot) +add_subdirectory(../imxrt/dshot dshot) add_subdirectory(../imxrt/hrt hrt) add_subdirectory(../imxrt/led_pwm led_pwm) add_subdirectory(../imxrt/io_pins io_pins) diff --git a/platforms/common/include/libevents_definitions.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h similarity index 88% rename from platforms/common/include/libevents_definitions.h rename to platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h index 4706a20693ba..b6aaca410a9d 100644 --- a/platforms/common/include/libevents_definitions.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/dshot.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * 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 @@ -30,19 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ - -/* - * This header defines the events::EventType type. - */ - #pragma once -#include - -namespace events -{ -using EventType = event_s; -} // namespace events - - +#include "../../../imxrt/include/px4_arch/dshot.h" diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h index 613ac1b19777..83938d918bfe 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer.h @@ -41,6 +41,7 @@ #include #include +#include "dshot.h" #pragma once __BEGIN_DECLS @@ -110,6 +111,7 @@ typedef struct timer_io_channels_t { uint8_t sub_module; /* 0 based sub module offset */ uint8_t sub_module_bits; /* LDOK and CLDOK bits */ uint8_t timer_channel; /* Unused */ + dshot_conf_t dshot; } timer_io_channels_t; #define SM0 0 diff --git a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h index e371b6de83d6..5cbc33fdc3a9 100644 --- a/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt106x/include/px4_arch/io_timer_hw_description.h @@ -601,6 +601,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t return ret; } +static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad); + + ret.dshot.pinmux = dshot_pinmux; + ret.dshot.flexio_pin = flexio_pin; + return ret; +} + static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; @@ -609,3 +619,14 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm ret.submodle = sub; return ret; } + + + +static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) +{ + io_timers_t ret{}; + + ret.base = getFlexPWMBaseRegister(pwm); + ret.submodle = sub; + return ret; +} diff --git a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h index 77dbfe918417..0bcf5c302ab8 100644 --- a/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h +++ b/platforms/nuttx/src/px4/nxp/rt117x/include/px4_arch/io_timer_hw_description.h @@ -690,6 +690,16 @@ static inline constexpr timer_io_channels_t initIOTimerChannel(const io_timers_t return ret; } +static inline constexpr timer_io_channels_t initIOTimerChannelDshot(const io_timers_t io_timers_conf[MAX_IO_TIMERS], + PWM::FlexPWMConfig pwm_config, IOMUX::Pad pad, uint32_t dshot_pinmux, uint32_t flexio_pin) +{ + timer_io_channels_t ret = initIOTimerChannel(io_timers_conf, pwm_config, pad); + + ret.dshot.pinmux = dshot_pinmux; + ret.dshot.flexio_pin = flexio_pin; + return ret; +} + static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; @@ -699,14 +709,13 @@ static inline constexpr io_timers_t initIOPWM(PWM::FlexPWM pwm, PWM::FlexPWMSubm return ret; } -static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub, uint32_t pinmux, - uint32_t flexio_pin) + + +static inline constexpr io_timers_t initIOPWMDshot(PWM::FlexPWM pwm, PWM::FlexPWMSubmodule sub) { io_timers_t ret{}; ret.base = getFlexPWMBaseRegister(pwm); ret.submodle = sub; - ret.dshot.pinmux = pinmux; - ret.dshot.flexio_pin = flexio_pin; return ret; } diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h index 234971efbdcd..d31323b9eb0e 100644 --- a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/adc.h @@ -33,4 +33,3 @@ #pragma once #include "../../../rpi_common/include/px4_arch/adc.h" - diff --git a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h index 9655c49558b7..908cc4e9b0a5 100644 --- a/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h +++ b/platforms/nuttx/src/px4/rpi/rp2040/include/px4_arch/i2c_hw_description.h @@ -33,4 +33,3 @@ #pragma once #include "../../../rpi_common/include/px4_arch/i2c_hw_description.h" - diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h index 96aa40bc333b..ae3cfbedcf32 100644 --- a/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/adc.h @@ -37,4 +37,3 @@ #define SYSTEM_ADC_BASE RP2040_ADC_BASE #include - diff --git a/platforms/nuttx/src/px4/stm/CMakeLists.txt b/platforms/nuttx/src/px4/stm/CMakeLists.txt index ec489bf559c3..72584f3178ec 100644 --- a/platforms/nuttx/src/px4/stm/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/CMakeLists.txt @@ -31,6 +31,4 @@ # ############################################################################ - add_subdirectory(${PX4_CHIP}) - diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c index 6d484c0a826f..1a505e98217f 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c @@ -73,7 +73,7 @@ static char hw_base_info[HW_INFO_SIZE] = {0}; /**************************************************************************** * Protected Functions ****************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int dn_to_ordinal(uint16_t dn) { /* Table is scaled for 12, so if ADC is in 16 bit mode @@ -111,6 +111,7 @@ static int dn_to_ordinal(uint16_t dn) return -1; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ /************************************************************************************ * Name: read_id_dn @@ -143,7 +144,7 @@ static int dn_to_ordinal(uint16_t dn) * -EIO - FAiled to init or read the ADC * ************************************************************************************/ - +#if !defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc_channel) { int rv = -EIO; @@ -328,9 +329,15 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc stm32_configgpio(gpio_drive); return rv; } +#endif /* BOARD_HAS_ONLY_EEPROM_VERSIONING */ static int determine_hw_info(int *revision, int *version) { +#if defined(BOARD_HAS_ONLY_EEPROM_VERSIONING) + *revision = HW_ID_EEPROM; + *version = HW_ID_EEPROM; + return OK; +#else int dn; int rv = read_id_dn(&dn, GPIO_HW_REV_DRIVE, GPIO_HW_REV_SENSE, ADC_HW_REV_SENSE_CHANNEL); @@ -344,6 +351,7 @@ static int determine_hw_info(int *revision, int *version) } return rv; +#endif } /**************************************************************************** diff --git a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c index 5b501850668a..00491e976831 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c +++ b/platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c @@ -82,7 +82,7 @@ static uint8_t dshot_burst_buffer_array[DSHOT_TIMERS * DSHOT_BURST_BUFFER_SIZE(M px4_cache_aligned_data() = {}; static uint32_t *dshot_burst_buffer[DSHOT_TIMERS] = {}; -int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) +int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot) { unsigned buffer_offset = 0; @@ -152,6 +152,22 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq) return ret_val == OK ? channels_init_mask : ret_val; } +int up_bdshot_get_erpm(uint8_t channel, int *erpm) +{ + // Not implemented + return -1; +} + +int up_bdshot_channel_status(uint8_t channel) +{ + // Not implemented + return -1; +} + +void up_bdshot_status(void) +{ +} + void up_dshot_trigger(void) { for (uint8_t timer = 0; (timer < DSHOT_TIMERS); timer++) { diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h index 17ff58304537..1736386ce592 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/adc.h @@ -54,4 +54,3 @@ #endif #include - diff --git a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h index 964627c636df..bf33426d3ceb 100644 --- a/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/px4io_serial.h @@ -166,4 +166,3 @@ class ArchPX4IOSerial : public PX4IO_serial */ static uint8_t _io_buffer_storage[] px4_cache_aligned_data(); }; - diff --git a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h index 7a7f1791bd53..417c1bccb226 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/include/px4_arch/dshot.h @@ -34,4 +34,3 @@ #define px4_stm32_dmasetup stm32_dmasetup #include "../../../stm32_common/include/px4_arch/dshot.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h index 2960835d2ec2..6b565c384730 100644 --- a/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32f1/io_timer.h @@ -32,6 +32,4 @@ ****************************************************************************/ #pragma once - #include "../../../stm32_common/include/px4_arch/io_timer.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt index 22c6701f946e..98f0e1b7db8f 100644 --- a/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f3/CMakeLists.txt @@ -36,5 +36,3 @@ add_subdirectory(board_critmon) add_subdirectory(../stm32_common/board_reset board_reset) add_subdirectory(../stm32_common/version version) add_subdirectory(../stm32_common/hrt hrt) - - diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h index 9aa5b0c7aeba..6727319a5152 100644 --- a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/adc.h @@ -33,4 +33,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/adc.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h index 2960835d2ec2..6b565c384730 100644 --- a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/io_timer.h @@ -32,6 +32,4 @@ ****************************************************************************/ #pragma once - #include "../../../stm32_common/include/px4_arch/io_timer.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h index e43fabc2e39b..4ef37a9542f3 100644 --- a/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f3/include/px4_arch/micro_hal.h @@ -52,4 +52,3 @@ __BEGIN_DECLS #define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 16 __END_DECLS - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt index 9b49f31f84cb..cc751f5d5f92 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt +++ b/platforms/nuttx/src/px4/stm/stm32f4/CMakeLists.txt @@ -47,4 +47,3 @@ add_subdirectory(../stm32_common/version version) add_subdirectory(px4io_serial) add_subdirectory(watchdog) - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h index 9aa5b0c7aeba..6727319a5152 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/adc.h @@ -33,4 +33,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/adc.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h index 449ca66b96b4..c71f84857141 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/dshot.h @@ -35,4 +35,3 @@ #define px4_stm32_dmasetup stm32_dmasetup #include "../../../stm32_common/include/px4_arch/dshot.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/i2c_hw_description.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/i2c_hw_description.h index f125d3086e5e..dc883714e85f 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/i2c_hw_description.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/i2c_hw_description.h @@ -33,4 +33,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/i2c_hw_description.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h index d45a3add2b79..0844d7ed9475 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/micro_hal.h @@ -51,4 +51,3 @@ __BEGIN_DECLS #define PX4_NUMBER_I2C_BUSES STM32_NI2C #define PX4_ADC_INTERNAL_TEMP_SENSOR_CHANNEL 18 __END_DECLS - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h index 5898838ee4b4..714afe06eb1c 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32f4/include/px4_arch/px4io_serial.h @@ -34,4 +34,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/px4io_serial.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp index 0ab41dd384a7..9f247653648f 100644 --- a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp @@ -473,4 +473,3 @@ ArchPX4IOSerial::_abort_dma() stm32_dmastop(_tx_dma); stm32_dmastop(_rx_dma); } - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h index 9aa5b0c7aeba..6727319a5152 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/adc.h @@ -33,4 +33,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/adc.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h index 449ca66b96b4..c71f84857141 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/dshot.h @@ -35,4 +35,3 @@ #define px4_stm32_dmasetup stm32_dmasetup #include "../../../stm32_common/include/px4_arch/dshot.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h index 2960835d2ec2..6b565c384730 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/io_timer.h @@ -32,6 +32,4 @@ ****************************************************************************/ #pragma once - #include "../../../stm32_common/include/px4_arch/io_timer.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h index 82ce68f8a571..122853e81760 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/micro_hal.h @@ -61,4 +61,3 @@ int stm32_flash_unlock(void); int stm32_flash_writeprotect(size_t page, bool enabled); __END_DECLS - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h index 5898838ee4b4..714afe06eb1c 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h +++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/px4io_serial.h @@ -34,4 +34,3 @@ #pragma once #include "../../../stm32_common/include/px4_arch/px4io_serial.h" - diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp index 2899c8db3fba..68d04b8e5d9b 100644 --- a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp +++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp @@ -506,4 +506,3 @@ ArchPX4IOSerial::_abort_dma() rICR = rISR & rISR_ERR_FLAGS_MASK; /* clear the flags */ } - diff --git a/platforms/posix/Debug/launch_sitl.json.in b/platforms/posix/Debug/launch_sitl.json.in index 2fb0a4bc6b0e..6703f4290346 100644 --- a/platforms/posix/Debug/launch_sitl.json.in +++ b/platforms/posix/Debug/launch_sitl.json.in @@ -223,6 +223,7 @@ "options": [ "x500", "x500_depth", + "x500_lidar", "rc_cessna", "standard_vtol", ], diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp new file mode 100644 index 000000000000..543041bc978f --- /dev/null +++ b/platforms/posix/include/SerialImpl.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + void flush(); + + const char *getPort() const; + static bool validatePort(const char *port); + bool setPort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + + bool getInvertedMode() const; + bool setInvertedMode(bool enable); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool configure(); + + bool _single_wire_mode{false}; + bool _swap_rx_tx_mode{false}; + bool _inverted_mode{false}; + +}; + +} // namespace device diff --git a/platforms/posix/include/hrt_work.h b/platforms/posix/include/hrt_work.h index 7799d3815c1a..317387ed31b2 100644 --- a/platforms/posix/include/hrt_work.h +++ b/platforms/posix/include/hrt_work.h @@ -60,4 +60,3 @@ static inline void hrt_work_unlock(void) } __END_DECLS - diff --git a/platforms/posix/include/queue.h b/platforms/posix/include/queue.h index d6315ca1723c..b0a73322cf91 100644 --- a/platforms/posix/include/queue.h +++ b/platforms/posix/include/queue.h @@ -131,4 +131,3 @@ EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); #endif #endif /* __INCLUDE_QUEUE_H_ */ - diff --git a/platforms/posix/src/px4/CMakeLists.txt b/platforms/posix/src/px4/CMakeLists.txt index 929788b587e2..93a633982265 100644 --- a/platforms/posix/src/px4/CMakeLists.txt +++ b/platforms/posix/src/px4/CMakeLists.txt @@ -34,4 +34,3 @@ add_subdirectory(common) add_subdirectory(${PX4_CHIP_MANUFACTURER}) - diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 34b65cdf44aa..90d4a77992bb 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -46,6 +46,8 @@ add_library(px4_layer drv_hrt.cpp cpuload.cpp print_load.cpp + ${PX4_SOURCE_DIR}/platforms/common/Serial.cpp + SerialImpl.cpp ) target_compile_definitions(px4_layer PRIVATE MODULE_NAME="px4") target_compile_options(px4_layer PRIVATE -Wno-cast-align) # TODO: fix and enable diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp new file mode 100644 index 000000000000..2298c3829263 --- /dev/null +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -0,0 +1,498 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (validatePort(port)) { + setPort(port); + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::configure() +{ + /* process baud rate */ + int speed; + + switch (_baudrate) { + case 9600: speed = B9600; break; + + case 19200: speed = B19200; break; + + case 38400: speed = B38400; break; + + case 57600: speed = B57600; break; + + case 115200: speed = B115200; break; + + case 230400: speed = B230400; break; + +#ifndef B460800 +#define B460800 460800 +#endif + + case 460800: speed = B460800; break; + +#ifndef B500000 +#define B500000 500000 +#endif + + case 500000: speed = B500000; break; + +#ifndef B576000 +#define B576000 576000 +#endif + + case 576000: speed = B576000; break; + +#ifndef B921600 +#define B921600 921600 +#endif + + case 921600: speed = B921600; break; + +#ifndef B1000000 +#define B1000000 1000000 +#endif + + case 1000000: speed = B1000000; break; + +#ifndef B1500000 +#define B1500000 1500000 +#endif + + case 1500000: speed = B1500000; break; + + default: + speed = _baudrate; + PX4_WARN("Using non-standard baudrate: %u", _baudrate); + break; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + if ((termios_state = tcgetattr(_serial_fd, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcgetattr)", termios_state); + return false; + } + + /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ + + // + // Input flags - Turn off input processing + // + // convert break to null byte, no CR to NL translation, + // no NL to CR translation, don't mark parity errors or breaks + // no input parity check, don't strip high bit off, + // no XON/XOFF software flow control + // + uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | + INLCR | PARMRK | INPCK | ISTRIP | IXON); + + // + // Output flags - Turn off output processing + // + // no CR to NL translation, no NL to CR-NL translation, + // no NL to CR translation, no column 0 CR suppression, + // no Ctrl-D suppression, no fill characters, no case mapping, + // no local output processing + // + // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | + // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); + uart_config.c_oflag = 0; + + // + // No line processing + // + // echo off, echo newline off, canonical mode off, + // extended input processing off, signal chars off + // + uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); + + /* no parity, one stop bit, disable flow control */ + uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetispeed)", termios_state); + return false; + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("ERR: %d (cfsetospeed)", termios_state); + return false; + } + + if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { + PX4_ERR("ERR: %d (tcsetattr)", termios_state); + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + if (isOpen()) { + return true; + } + + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + + // Open the serial port + int serial_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s err: %d", _port, errno); + return false; + } + + _serial_fd = serial_fd; + + // Configure the serial port + if (! configure()) { + PX4_ERR("failed to configure %s err: %d", _port, errno); + return false; + } + + _open = true; + + if (_single_wire_mode) { + setSingleWireMode(); + } + + if (_swap_rx_tx_mode) { + setSwapRxTxMode(); + } + + setInvertedMode(_inverted_mode); + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + + if (_serial_fd >= 0) { + ::close(_serial_fd); + } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret = ::read(_serial_fd, buffer, buffer_size); + + if (ret < 0) { + PX4_DEBUG("%s read error %d", _port, ret); + + } + + return ret; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + hrt_abstime timeout_us = timeout_ms * 1000; + int total_bytes_read = 0; + + while ((total_bytes_read < (int) character_count) && (hrt_elapsed_time(&start_time_us) < timeout_us)) { + // Poll for incoming UART data. + pollfd fds[1]; + fds[0].fd = _serial_fd; + fds[0].events = POLLIN; + + hrt_abstime elapsed_time_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_time_us > timeout_us) { break; } + + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), (timeout_us - elapsed_time_us) / 1000); + + if (ret > 0) { + if (fds[0].revents & POLLIN) { + const unsigned sleeptime = character_count * 1000000 / (_baudrate / 10); + px4_usleep(sleeptime); + + ret = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (ret > 0) { + total_bytes_read += ret; + } + + } else { + PX4_ERR("Got a poll error"); + return -1; + } + } + } + + return total_bytes_read; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int written = ::write(_serial_fd, buffer, buffer_size); + ::fsync(_serial_fd); + + if (written < 0) { + PX4_ERR("%s write error %d", _port, written); + + } + + return written; +} + +void SerialImpl::flush() +{ + if (_open) { + tcflush(_serial_fd, TCIOFLUSH); + } +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::validatePort(const char *port) +{ + return (port && (access(port, R_OK | W_OK) == 0)); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + // check if already configured + if ((baudrate == _baudrate) && _open) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return configure(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +bool SerialImpl::getSingleWireMode() const +{ + return _single_wire_mode; +} + +bool SerialImpl::setSingleWireMode() +{ +#if defined(TIOCSSINGLEWIRE) + + if (_open) { + ioctl(_serial_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + } + + _single_wire_mode = true; + return true; +#else + return false; +#endif // TIOCSSINGLEWIRE +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return _swap_rx_tx_mode; +} + +bool SerialImpl::setSwapRxTxMode() +{ +#if defined(TIOCSSWAP) + + if (_open) { + ioctl(_serial_fd, TIOCSSWAP, SER_SWAP_ENABLED); + } + + _swap_rx_tx_mode = true; + return true; +#else + return false; +#endif // TIOCSSWAP +} + +bool SerialImpl::getInvertedMode() const +{ + return _inverted_mode; +} + +bool SerialImpl::setInvertedMode(bool enable) +{ +#if defined(TIOCSINVERT) + + if (_open) { + if (enable) { + ioctl(_serial_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX); + + } else { + ioctl(_serial_fd, TIOCSINVERT, 0); + } + } + + _inverted_mode = enable; + return true; +#else + return _inverted_mode == enable; +#endif // TIOCSINVERT +} + +} // namespace device diff --git a/platforms/posix/src/px4/common/drv_hrt.cpp b/platforms/posix/src/px4/common/drv_hrt.cpp index 9634511bf3fc..b510229bc0cf 100644 --- a/platforms/posix/src/px4/common/drv_hrt.cpp +++ b/platforms/posix/src/px4/common/drv_hrt.cpp @@ -51,6 +51,11 @@ #include #include "hrt_work.h" +// Voxl2 board specific API definitions to get time offset +#if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) +#include "fc_sensor.h" +#endif + #if defined(ENABLE_LOCKSTEP_SCHEDULER) #include static LockstepScheduler lockstep_scheduler {true}; @@ -107,6 +112,29 @@ hrt_abstime hrt_absolute_time() #else // defined(ENABLE_LOCKSTEP_SCHEDULER) struct timespec ts; px4_clock_gettime(CLOCK_MONOTONIC, &ts); + +# if defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) + hrt_abstime temp_abstime = ts_to_abstime(&ts); + int apps_time_offset = fc_sensor_get_time_offset(); + + if (apps_time_offset < 0) { + hrt_abstime temp_offset = -apps_time_offset; + + if (temp_offset >= temp_abstime) { + temp_abstime = 0; + + } else { + temp_abstime -= temp_offset; + } + + } else { + temp_abstime += (hrt_abstime) apps_time_offset; + } + + ts.tv_sec = temp_abstime / 1000000; + ts.tv_nsec = (temp_abstime % 1000000) * 1000; +# endif // defined(CONFIG_MUORB_APPS_SYNC_TIMESTAMP) + return ts_to_abstime(&ts); #endif // defined(ENABLE_LOCKSTEP_SCHEDULER) } @@ -449,6 +477,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp) #endif // defined(ENABLE_LOCKSTEP_SCHEDULER) return system_clock_gettime(clk_id, tp); + } #if defined(ENABLE_LOCKSTEP_SCHEDULER) diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h index 3dc032e5a99c..15041c2b40ea 100644 --- a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h +++ b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_components.h @@ -76,4 +76,3 @@ class LockstepComponents std::atomic_int _components_used_bitset{0}; std::atomic_int _components_progress_bitset{0}; }; - diff --git a/platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt b/platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt index e6cf9fcc7524..0e68741c181d 100644 --- a/platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt +++ b/platforms/posix/src/px4/common/px4_daemon/CMakeLists.txt @@ -39,4 +39,3 @@ px4_add_library(px4_daemon server_io.cpp sock_protocol.cpp ) - diff --git a/platforms/posix/src/px4/common/px4_daemon/client.h b/platforms/posix/src/px4/common/px4_daemon/client.h index 41f4cb838192..b7883af3ebe6 100644 --- a/platforms/posix/src/px4/common/px4_daemon/client.h +++ b/platforms/posix/src/px4/common/px4_daemon/client.h @@ -85,4 +85,3 @@ class Client }; } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_daemon/history.h b/platforms/posix/src/px4/common/px4_daemon/history.h index b29ca8c53107..207c265f605a 100644 --- a/platforms/posix/src/px4/common/px4_daemon/history.h +++ b/platforms/posix/src/px4/common/px4_daemon/history.h @@ -101,4 +101,3 @@ class History }; } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.h b/platforms/posix/src/px4/common/px4_daemon/pxh.h index 45bd4b863343..39009bed11c5 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.h +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.h @@ -103,4 +103,3 @@ class Pxh } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_daemon/server.cpp b/platforms/posix/src/px4/common/px4_daemon/server.cpp index c3b012bb8462..c26b364457f7 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/server.cpp @@ -153,9 +153,8 @@ Server::_server_main() // Reboot command causes System Interrupt to stop poll(). This is not an error if (errno != EINTR) { PX4_ERR("poll() failed: %s", strerror(errno)); + break; } - - break; } _lock(); diff --git a/platforms/posix/src/px4/common/px4_daemon/server.h b/platforms/posix/src/px4/common/px4_daemon/server.h index 2932a977ff29..36f347df6764 100644 --- a/platforms/posix/src/px4/common/px4_daemon/server.h +++ b/platforms/posix/src/px4/common/px4_daemon/server.h @@ -124,4 +124,3 @@ class Server } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp index 65b9e18000c4..48d154ca8534 100644 --- a/platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.cpp @@ -48,4 +48,3 @@ std::string get_socket_path(int instance_id) } } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_daemon/sock_protocol.h b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.h index f158a29b66d0..725dfe281e1e 100644 --- a/platforms/posix/src/px4/common/px4_daemon/sock_protocol.h +++ b/platforms/posix/src/px4/common/px4_daemon/sock_protocol.h @@ -45,4 +45,3 @@ namespace px4_daemon std::string get_socket_path(int instance_id); } // namespace px4_daemon - diff --git a/platforms/posix/src/px4/common/px4_posix_impl.cpp b/platforms/posix/src/px4/common/px4_posix_impl.cpp index 385c1a244551..31c1bb125cb4 100644 --- a/platforms/posix/src/px4/common/px4_posix_impl.cpp +++ b/platforms/posix/src/px4/common/px4_posix_impl.cpp @@ -97,4 +97,3 @@ void init(int argc, char *argv[], const char *app_name) } } - diff --git a/platforms/posix/src/px4/generic/CMakeLists.txt b/platforms/posix/src/px4/generic/CMakeLists.txt index ec489bf559c3..72584f3178ec 100644 --- a/platforms/posix/src/px4/generic/CMakeLists.txt +++ b/platforms/posix/src/px4/generic/CMakeLists.txt @@ -31,6 +31,4 @@ # ############################################################################ - add_subdirectory(${PX4_CHIP}) - diff --git a/platforms/posix/src/px4/generic/generic/CMakeLists.txt b/platforms/posix/src/px4/generic/generic/CMakeLists.txt index 4db79a429f8e..f99e817798ef 100644 --- a/platforms/posix/src/px4/generic/generic/CMakeLists.txt +++ b/platforms/posix/src/px4/generic/generic/CMakeLists.txt @@ -31,6 +31,4 @@ # ############################################################################ - add_subdirectory(tone_alarm) - diff --git a/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h b/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h index d09b73c687dd..2d85741cac90 100644 --- a/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h +++ b/platforms/posix/src/px4/generic/generic/include/px4_arch/micro_hal.h @@ -33,4 +33,3 @@ #pragma once #include - diff --git a/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp index 61f9fc69ca24..639c16560381 100644 --- a/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp +++ b/platforms/posix/src/px4/generic/generic/tone_alarm/ToneAlarmInterface.cpp @@ -58,4 +58,3 @@ void stop_note() } } /* namespace ToneAlarmInterface */ - diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index d85f3fcb7bff..0bb5f5d712b7 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -44,11 +44,11 @@ include_directories( add_library(px4 SHARED ${PX4_SOURCE_DIR}/platforms/qurt/unresolved_symbols.c + ${PX4_SOURCE_DIR}/platforms/qurt/new_delete.cpp ${PX4_BINARY_DIR}/apps.cpp ) target_link_libraries(px4 modules__muorb__slpi ${module_libraries} - px4_layer ) diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake index e0c2dbaba17f..dcc28e176a2e 100644 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ b/platforms/qurt/cmake/px4_impl_os.cmake @@ -124,13 +124,7 @@ endfunction() # function(px4_os_add_flags) - set(DSPAL_ROOT platforms/qurt/dspal) include_directories( - ${DSPAL_ROOT}/include - ${DSPAL_ROOT}/sys - ${DSPAL_ROOT}/sys/sys - - platforms/posix/include platforms/qurt/include ) diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp new file mode 100644 index 000000000000..39c3d63553d5 --- /dev/null +++ b/platforms/qurt/include/SerialImpl.hpp @@ -0,0 +1,120 @@ +/**************************************************************************** + * + * 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 + +using device::SerialConfig::ByteSize; +using device::SerialConfig::Parity; +using device::SerialConfig::StopBits; +using device::SerialConfig::FlowControl; + +namespace device +{ + +class SerialImpl +{ +public: + + SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol); + virtual ~SerialImpl(); + + bool open(); + bool isOpen() const; + + bool close(); + + ssize_t read(uint8_t *buffer, size_t buffer_size); + ssize_t readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count = 1, uint32_t timeout_us = 0); + + ssize_t write(const void *buffer, size_t buffer_size); + + void flush(); + + const char *getPort() const; + bool setPort(const char *port); + static bool validatePort(const char *port); + + uint32_t getBaudrate() const; + bool setBaudrate(uint32_t baudrate); + + ByteSize getBytesize() const; + bool setBytesize(ByteSize bytesize); + + Parity getParity() const; + bool setParity(Parity parity); + + StopBits getStopbits() const; + bool setStopbits(StopBits stopbits); + + FlowControl getFlowcontrol() const; + bool setFlowcontrol(FlowControl flowcontrol); + + bool getSingleWireMode() const; + bool setSingleWireMode(); + + bool getSwapRxTxMode() const; + bool setSwapRxTxMode(); + + bool getInvertedMode() const; + bool setInvertedMode(bool enable); + +private: + + int _serial_fd{-1}; + + bool _open{false}; + + char _port[32] {}; + + uint32_t _baudrate{0}; + + ByteSize _bytesize{ByteSize::EightBits}; + Parity _parity{Parity::None}; + StopBits _stopbits{StopBits::One}; + FlowControl _flowcontrol{FlowControl::Disabled}; + + bool validateBaudrate(uint32_t baudrate); + + // Mutex used to lock the read functions + //pthread_mutex_t read_mutex; + + // Mutex used to lock the write functions + //pthread_mutex_t write_mutex; +}; + +} // namespace device diff --git a/platforms/qurt/include/crc32.h b/platforms/qurt/include/crc32.h new file mode 100644 index 000000000000..34e080b1c2e3 --- /dev/null +++ b/platforms/qurt/include/crc32.h @@ -0,0 +1,83 @@ +/**************************************************************************** + * include/crc.h + * + * Copyright (C) 2010 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ****************************************************************************/ + +#ifndef __INCLUDE_CRC32_H +#define __INCLUDE_CRC32_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/**************************************************************************** + * Name: crc32part + * + * Description: + * Continue CRC calculation on a part of the buffer. + * + ****************************************************************************/ + +EXTERN uint32_t crc32part(const uint8_t *src, size_t len, + uint32_t crc32val); + +/**************************************************************************** + * Name: crc32 + * + * Description: + * Return a 32-bit CRC of the contents of the 'src' buffer, length 'len' + * + ****************************************************************************/ + +EXTERN uint32_t crc32(const uint8_t *src, size_t len); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_CRC32_H */ diff --git a/platforms/qurt/include/px4_arch/micro_hal.h b/platforms/qurt/include/px4_arch/micro_hal.h index c23a40dbcfd6..d88e2ee29ebd 100644 --- a/platforms/qurt/include/px4_arch/micro_hal.h +++ b/platforms/qurt/include/px4_arch/micro_hal.h @@ -32,4 +32,3 @@ ****************************************************************************/ // Placeholder - diff --git a/platforms/qurt/include/queue.h b/platforms/qurt/include/queue.h new file mode 100644 index 000000000000..b0a73322cf91 --- /dev/null +++ b/platforms/qurt/include/queue.h @@ -0,0 +1,133 @@ +/************************************************************************ + * include/queue.h + * + * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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. + * + ************************************************************************/ + +#ifndef __INCLUDE_QUEUE_H +#define __INCLUDE_QUEUE_H + +/************************************************************************ + * Included Files + ************************************************************************/ + +#include + +#ifdef __cplusplus +#include // NULL +#else +#include // NULL +#endif + +/************************************************************************ + * Pre-processor Definitions + ************************************************************************/ + +#define sq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) +#define dq_init(q) do { (q)->head = NULL; (q)->tail = NULL; } while (0) + +#define sq_next(p) ((p)->flink) +#define dq_next(p) ((p)->flink) +#define dq_prev(p) ((p)->blink) + +#define sq_empty(q) ((q)->head == NULL) +#define dq_empty(q) ((q)->head == NULL) + +#define sq_peek(q) ((q)->head) +#define dq_peek(q) ((q)->head) + +// Required for Linux +#define FAR + +/************************************************************************ + * Global Type Declarations + ************************************************************************/ + +struct sq_entry_s { + FAR struct sq_entry_s *flink; +}; +typedef struct sq_entry_s sq_entry_t; + +struct dq_entry_s { + FAR struct dq_entry_s *flink; + FAR struct dq_entry_s *blink; +}; +typedef struct dq_entry_s dq_entry_t; + +struct sq_queue_s { + FAR sq_entry_t *head; + FAR sq_entry_t *tail; +}; +typedef struct sq_queue_s sq_queue_t; + +struct dq_queue_s { + FAR dq_entry_t *head; + FAR dq_entry_t *tail; +}; +typedef struct dq_queue_s dq_queue_t; + +/************************************************************************ + * Global Function Prototypes + ************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +EXTERN void sq_addfirst(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addfirst(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addlast(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_addlast(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN void sq_addafter(FAR sq_entry_t *prev, FAR sq_entry_t *node, + sq_queue_t *queue); +EXTERN void dq_addafter(FAR dq_entry_t *prev, FAR dq_entry_t *node, + dq_queue_t *queue); +EXTERN void dq_addbefore(FAR dq_entry_t *next, FAR dq_entry_t *node, + dq_queue_t *queue); + +EXTERN FAR sq_entry_t *sq_remafter(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void sq_rem(FAR sq_entry_t *node, sq_queue_t *queue); +EXTERN void dq_rem(FAR dq_entry_t *node, dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remlast(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remlast(dq_queue_t *queue); +EXTERN FAR sq_entry_t *sq_remfirst(sq_queue_t *queue); +EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); + +#undef EXTERN +#ifdef __cplusplus +} +#endif + +#endif /* __INCLUDE_QUEUE_H_ */ diff --git a/platforms/qurt/include/system_config.h b/platforms/qurt/include/system_config.h new file mode 100644 index 000000000000..7d3162cbe0ce --- /dev/null +++ b/platforms/qurt/include/system_config.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2017 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 system_config.h + * + * Definitions used by all SITL and Linux targets + */ + +#pragma once + +#define PX4_CPU_UUID_BYTE_LENGTH 16 +#define PX4_CPU_UUID_WORD32_LENGTH 4 + +#define PX4_CPU_MFGUID_BYTE_LENGTH PX4_CPU_UUID_BYTE_LENGTH +#define PX4_CPU_UUID_WORD32_UNIQUE_H 2 /* Most significant digits change the least */ +#define PX4_CPU_UUID_WORD32_UNIQUE_M 1 /* Middle significant digits */ +#define PX4_CPU_UUID_WORD32_UNIQUE_L 0 /* Least significant digits change the most */ +#define PX4_CPU_UUID_WORD32_FORMAT_SIZE (PX4_CPU_UUID_WORD32_LENGTH-1+(2*PX4_CPU_UUID_BYTE_LENGTH)+1) +#define PX4_CPU_MFGUID_FORMAT_SIZE ((2*PX4_CPU_MFGUID_BYTE_LENGTH)+1) + +#define BOARD_OVERRIDE_CPU_VERSION (-1) +#define board_mcu_version(rev, revstr, errata) BOARD_OVERRIDE_CPU_VERSION + +#define BOARD_HAS_NO_UUID + +#define CONFIG_NFILE_STREAMS 1 +#define CONFIG_SCHED_WORKQUEUE 1 +#define CONFIG_SCHED_HPWORK 1 +#define CONFIG_SCHED_LPWORK 1 + +/** time in ms between checks for work in work queues **/ +#define CONFIG_SCHED_WORKPERIOD 50000 + +#define CONFIG_SCHED_INSTRUMENTATION 1 diff --git a/platforms/qurt/include/termios.h b/platforms/qurt/include/termios.h index d36d80ee9388..2e59ca1815bb 100644 --- a/platforms/qurt/include/termios.h +++ b/platforms/qurt/include/termios.h @@ -1,37 +1,318 @@ /**************************************************************************** + * include/termios.h * - * Copyright (C) 2022 ModalAI, Inc. 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. + * 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 __INCLUDE_TERMIOS_H +#define __INCLUDE_TERMIOS_H + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include + +/**************************************************************************** + * Pre-processor Definitions ****************************************************************************/ -// This file prevents a missing header file error but since Qurt doesn't support -// termios the actual code will still need to be changed for Qurt +/* Terminal input modes (c_iflag in the termios structure) */ + +#define IGNBRK (1 << 0) /* Bit 0: Ignore break condition */ +#define BRKINT (1 << 1) /* Bit 1: Signal interrupt on break */ +#define IGNPAR (1 << 2) /* Bit 2: Ignore characters with parity errors */ +#define PARMRK (1 << 3) /* Bit 3: Mark parity errors */ +#define INPCK (1 << 4) /* Bit 4: Enable input parity check */ +#define ISTRIP (1 << 5) /* Bit 5: Strip character */ +#define INLCR (1 << 6) /* Bit 6: Map NL to CR on input */ +#define IGNCR (1 << 7) /* Bit 7: Ignore CR */ +#define ICRNL (1 << 8) /* Bit 8: Map CR to NL on input */ +#define IUCLC (1 << 9) /* Bit 9: Map upper-case to lower-case on input (LEGACY) */ +#define IXON (1 << 10) /* Bit 10: Enable start/stop output control */ +#define IXANY (1 << 11) /* Bit 11: Enable any character to restart output */ +#define IXOFF (1 << 12) /* Bit 12: Enable start/stop input control */ + +/* Terminal output modes (c_oflag in the termios structure) */ + +#define OPOST (1 << 0) /* Bit 0: Post-process output */ +#define OLCUC (1 << 1) /* Bit 1: Map lower-case to upper-case on output (LEGACY) */ +#define ONLCR (1 << 2) /* Bit 2: Map NL to CR-NL on output */ +#define OCRNL (1 << 3) /* Bit 3: Map CR to NL on output */ +#define ONOCR (1 << 4) /* Bit 4: No CR output at column 0 */ +#define ONLRET (1 << 5) /* Bit 5: NL performs CR function */ +#define OFILL (1 << 6) /* Bit 6: Use fill characters for delay */ +#define NLDLY (1 << 8) /* Bit 8: Select newline delays: */ +# define NL0 (0 << 8) /* Newline character type 0 */ +# define NL1 (1 << 8) /* Newline character type 1 */ +#define CRDLY (3 << 9) /* Bits 9-10: Select carriage-return delays: */ +# define CR0 (0 << 9) /* Carriage-return delay type 0 */ +# define CR1 (1 << 9) /* Carriage-return delay type 1 */ +# define CR2 (2 << 9) /* Carriage-return delay type 2 */ +# define CR3 (3 << 9) /* Carriage-return delay type 3 */ +#define TABDLY (3 << 11) /* Bits 11-12: Select horizontal-tab delays: */ +# define TAB0 (0 << 11) /* Horizontal-tab delay type 0 */ +# define TAB1 (1 << 11) /* Horizontal-tab delay type 1 */ +# define TAB2 (2 << 11) /* Horizontal-tab delay type 2 */ +# define TAB3 (3 << 11) /* Expand tabs to spaces */ +#define BSDLY (1 << 13) /* Bit 13: Select backspace delays: */ +# define BS0 (0 << 13) /* Backspace-delay type 0 */ +# define BS1 (1 << 13) /* Backspace-delay type 1 */ +#define VTDLY (1 << 14) /* Bit 14: Select vertical-tab delays: */ +# define VT0 (0 << 14) /* Vertical-tab delay type 0 */ +# define VT1 (1 << 14) /* Vertical-tab delay type 1 */ +#define FFDLY (1 << 15) /* Bit 15: Select form-feed delays: */ +# define FF0 (0 << 15) /* Form-feed delay type 0 */ +# define FF1 (1 << 15) /* Form-feed delay type 1 */ + +/* Control Modes (c_cflag in the termios structure) */ + +#define CSIZE (3 << 4) /* Bits 4-5: Character size: */ +# define CS5 (0 << 4) /* 5 bits */ +# define CS6 (1 << 4) /* 6 bits */ +# define CS7 (2 << 4) /* 7 bits */ +# define CS8 (3 << 4) /* 8 bits */ +#define CSTOPB (1 << 6) /* Bit 6: Send two stop bits, else one */ +#define CREAD (1 << 7) /* Bit 7: Enable receiver */ +#define PARENB (1 << 8) /* Bit 8: Parity enable */ +#define PARODD (1 << 9) /* Bit 9: Odd parity, else even */ +#define HUPCL (1 << 10) /* Bit 10: Hang up on last close */ +#define CLOCAL (1 << 11) /* Bit 11: Ignore modem status lines */ +#define CCTS_OFLOW (1 << 29) /* Bit 29: CTS flow control of output */ +#define CRTS_IFLOW (1u << 31) /* Bit 31: RTS flow control of input */ +#define CRTSCTS (CCTS_OFLOW | CRTS_IFLOW) + +/* Local Modes (c_lflag in the termios structure) */ + +#define ISIG (1 << 0) /* Bit 0: Enable signals */ +#define ICANON (1 << 1) /* Bit 1: Canonical input (erase and kill processing) */ +#define XCASE (1 << 2) /* Bit 2: Canonical upper/lower presentation (LEGACY) */ +#define ECHO (1 << 3) /* Bit 3: Enable echo */ +#define ECHOE (1 << 4) /* Bit 4: Echo erase character as error correcting backspace */ +#define ECHOK (1 << 5) /* Bit 5: Echo KILL */ +#define ECHONL (1 << 6) /* Bit 6: Echo NL */ +#define NOFLSH (1 << 7) /* Bit 7: Disable flush after interrupt or quit */ +#define TOSTOP (1 << 8) /* Bit 8: Send SIGTTOU for background output */ +#define IEXTEN (1 << 15) /* Bit 15: Enable extended input character processing */ + +/* The following are subscript names for the termios c_cc array. + * + * Common characters: VINTR, VQUIT, VSTART, VSTOP, VSUSP + * + * VINTR: Interrupt character (Default ETX, Control-C) + * VQUIT: Quit character (Default FS, Control-\) + * VSTART: Start character (Default DC1, Control-Q) + * VSTOP: Stop character (Default DC3, Control-S) + * VSUSP: Suspend character (Default SUB, Control-Z) + * + * Canonical mode: Adds VEOF, VEOL, VERASE, VKILL + * + * VEOL: End-of-file character (Default SUB, Control-Z) + * VEOF: End-of-line character (Default NUL) + * VERASE: Erase character (Default DEL or BS, Control-H) + * VKILL: Kill character (Default NAK or BS, Control-U) + * + * Non-canonical mode: Adds VMIN, VTIME + * + * VMIN: Minimum number of characters for non-canonical read + * VTIME: Timeout in deciseconds for non-canonical read + */ + +#define VINTR 0 /* Bit 0: INTR character */ +#define VQUIT 1 /* Bit 1: QUIT character */ +#define VERASE 2 /* Bit 2: ERASE character (canonical mode) */ +#define VKILL 3 /* Bit 3: KILL character (canonical mode) */ +#define VEOF 4 /* Bit 4: EOF character (canonical mode) */ +#define VTIME 5 /* Bit 5: TIME value (non-canonical mode) */ +#define VMIN 6 /* Bit 6: MIN value (non-canonical mode) */ +#define VSTART 8 /* Bit 8: START character */ +#define VSTOP 9 /* Bit 9: STOP character */ +#define VSUSP 10 /* Bit 10: SUSP character */ +#define VEOL 11 /* Bit 11: EOL character (canonical mode) */ +#define NCCS 12 /* Bit 12: Size of the array c_cc for control characters */ + +/* Baud Rate Selection. These are instances of type speed_t. Values of + * 38400 and below are specified by POSIX; values above 38400 are sometimes + * referred to as extended values and most values appear in most termios.h + * implementations. + */ + +#define B0 0000000 /* Hang up */ +#define B50 0000001 /* 50 baud */ +#define B75 0000002 /* 75 baud */ +#define B110 0000003 /* 110 baud */ +#define B134 0000004 /* 134.5 baud */ +#define B150 0000005 /* 150 baud */ +#define B200 0000006 /* 200 baud */ +#define B300 0000007 /* 300 baud */ +#define B600 0000010 /* 600 baud */ +#define B1200 0000011 /* 1,200 baud */ +#define B1800 0000012 /* 1,800 baud */ +#define B2400 0000013 /* 2,400 baud */ +#define B4800 0000014 /* 4,800 baud */ +#define B9600 0000015 /* 9,600 baud */ +#define B19200 0000016 /* 19,200 baud */ +#define B38400 0000017 /* 38,400 baud */ + +#define B57600 0010001 /* 57,600 baud */ +#define B115200 0010002 /* 115,200 baud */ +#define B230400 0010003 /* 230,400 baud */ +#define B460800 0010004 /* 460,800 baud */ +#define B500000 0010005 /* 500,000 baud */ +#define B576000 0010006 /* 576,000 baud */ +#define B921600 0010007 /* 921,600 baud */ +#define B1000000 0010010 /* 1,000,000 baud */ +#define B1152000 0010011 /* 1,152,000 baud */ +#define B1500000 0010012 /* 1,500,000 baud */ +#define B2000000 0010013 /* 2,000,000 baud */ +#define B2500000 0010014 /* 2,500,000 baud */ +#define B3000000 0010015 /* 3,000,000 baud */ +#define B3500000 0010016 /* 3,500,000 baud */ +#define B4000000 0010017 /* 4,000,000 baud */ + +/* Attribute Selection (used with tcsetattr()) */ + +#define TCSANOW 0 /* Change attributes immediately */ +#define TCSADRAIN 1 /* Change attributes when output has drained */ +#define TCSAFLUSH 2 /* Change attributes when output has drained; + * also flush pending input */ + +/* Line Control (used with tcflush()) */ + +#define TCIFLUSH 0 /* Flush pending input */ +#define TCOFLUSH 1 /* Flush untransmitted output */ +#define TCIOFLUSH 2 /* Flush both pending input and untransmitted + * output */ + +/* Constants for use with tcflow() */ + +#define TCOOFF 0 /* Suspend output */ +#define TCOON 1 /* Restart output */ +#define TCIOFF 2 /* Transmit a STOP character, intended to + * suspend input data */ +#define TCION 3 /* Transmit a START character, intended to + * restart input data */ + +/**************************************************************************** + * Public Type Definitions + ****************************************************************************/ + +/* Baud rate selection */ + +typedef unsigned long speed_t; /* Used for terminal baud rates */ + +/* Types used within the termios structure */ + +typedef unsigned int tcflag_t; /* Used for terminal modes */ +typedef unsigned char cc_t; /* Used for terminal special characters */ + +/* The termios structure */ + +struct termios { + /* Exposed fields defined by POSIX */ + + tcflag_t c_iflag; /* Input modes */ + tcflag_t c_oflag; /* Output modes */ + tcflag_t c_cflag; /* Control modes */ + tcflag_t c_lflag; /* Local modes */ + cc_t c_cc[NCCS]; /* Control chars */ + + /* Implementation specific fields. For portability reasons, these fields + * should not be accessed directly, but rather through only through the + * cf[set|get][o|i]speed() POSIX interfaces. + */ + + speed_t c_speed; /* Input/output speed (non-POSIX) */ +}; + +/**************************************************************************** + * Public Function Prototypes + ****************************************************************************/ + +#ifdef __cplusplus +#define EXTERN extern "C" +extern "C" +{ +#else +#define EXTERN extern +#endif + +/* The cfgetspeed() function is a non-POSIX function will extract the baud + * from the termios structure to which the termiosp argument points. NuttX + * does not control input/output baud independently. Both must be the same. + * The POSIX standard interfaces, cfigetispeed() and cfigetospeed() are + * supported by simply defining them to be cfgetspeed(). + * The return value is baud value(9600). + */ + +speed_t cfgetspeed(const struct termios *termiosp); +#define cfgetispeed(termiosp) cfgetspeed(termiosp) +#define cfgetospeed(termiosp) cfgetspeed(termiosp) + +/* The cfsetspeed() function is a non-POSIX function that sets the baud + * stored in the structure pointed to by termiosp to speed. NuttX does + * not control input/output baud independently. Both must be the same. + * The POSIX standard interfaces, cfigetispeed() and cfigetospeed() are + * supported by simply defining them to be cfsetspeed(). + * Speed could be baud value(9600) or could be baud mask(B9600). + */ + +int cfsetspeed(struct termios *termiosp, speed_t speed); +#define cfsetispeed(termiosp,speed) cfsetspeed(termiosp,speed) +#define cfsetospeed(termiosp,speed) cfsetspeed(termiosp,speed) + +/* The cfmakeraw() function is a non-POSIX function that sets the terminal + * to something like the "raw" mode. + */ + +void cfmakeraw(struct termios *termiosp); + +/* Wait for transmission of output */ + +int tcdrain(int fd); + +/* Suspend or restart the transmission or reception of data */ + +int tcflow(int fd, int action); + +/* Flush non-transmitted output data, non-read input data or both */ + +int tcflush(int fd, int cmd); + +/* Get the parameters associated with the terminal */ + +int tcgetattr(int fd, struct termios *termiosp); + +/* Get process group ID for session leader for controlling terminal */ + +pid_t tcgetsid(int fd); + +/* Send a "break" for a specific duration */ + +int tcsendbreak(int fd, int duration); + +/* Set the parameters associated with the terminal */ + +int tcsetattr(int fd, int options, const struct termios *termiosp); + +#undef EXTERN +#ifdef __cplusplus +} +#endif -typedef unsigned int speed_t; +#endif /* __INCLUDE_TERMIOS_H */ diff --git a/platforms/qurt/new_delete.cpp b/platforms/qurt/new_delete.cpp new file mode 100644 index 000000000000..1e8b004950cf --- /dev/null +++ b/platforms/qurt/new_delete.cpp @@ -0,0 +1,92 @@ +#include +#include + +/* + These are the heap access function exported by the SLPI DSP image. +*/ +extern "C" { + void *fc_heap_alloc(size_t size); + void fc_heap_free(void *ptr); +} + +/* + Globally override new and delete so that it can use the correct + heap manager for the Qurt platform. + + Note that new comes in multiple different variants. When new is used + without std::nothrow the compiler is free to assume it will not fail + as it assumes exceptions are enabled. This makes code like this + unsafe when using -fno-exceptions: + + a = new b; + if (a == nullptr) { + handle_error() + } + + The compiler may remove the error handling. With g++ you can use + -fcheck-new to avoid this, but on clang++ the compiler accepts + -fcheck-new as a valid flag, but doesn't implement it, and may remove + the error checking. That makes using clang++ unsafe with + -fno-exceptions if you ever call new without std::nothrow. +..It has been verified that hexagon-clang++ will remove the nullptr checks + after new if any optimization is selected for the compiler. +*/ + +/* + variant for new(std::nothrow), which is all that should be used in + PX4 + */ +void *operator new (size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +void *operator new[](size_t size, std::nothrow_t const ¬hrow) noexcept +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + These variants are for new without std::nothrow. We don't want to ever + use these from PX4 code + */ +void *operator new (size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + + +void *operator new[](size_t size) +{ + if (size < 1) { + size = 1; + } + + return (fc_heap_alloc(size)); +} + +/* + Override delete to free up memory to correct heap +*/ + +void operator delete (void *p) noexcept +{ + if (p) { fc_heap_free(p); } +} + +void operator delete[](void *ptr) noexcept +{ + if (ptr) { fc_heap_free(ptr); } +} diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index e685b8d42acf..8a25322755a9 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/platforms/qurt/src/px4/CMakeLists.txt @@ -38,6 +38,7 @@ set(QURT_LAYER_SRCS px4_qurt_impl.cpp main.cpp qurt_log.cpp + SerialImpl.cpp ) add_library(px4_layer diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp new file mode 100644 index 000000000000..73f8ce8cc4ba --- /dev/null +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -0,0 +1,389 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include // strncpy +#include +#include +#include + +namespace device +{ + +SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, + FlowControl flowcontrol) : + _baudrate(baudrate), + _bytesize(bytesize), + _parity(parity), + _stopbits(stopbits), + _flowcontrol(flowcontrol) +{ + if (validatePort(port)) { + setPort(port); + + } else { + _port[0] = 0; + } +} + +SerialImpl::~SerialImpl() +{ + if (isOpen()) { + close(); + } +} + +bool SerialImpl::validateBaudrate(uint32_t baudrate) +{ + if ((baudrate != 9600) && + (baudrate != 38400) && + (baudrate != 57600) && + (baudrate != 115200) && + (baudrate != 230400) && + (baudrate != 250000) && + (baudrate != 420000) && + (baudrate != 460800) && + (baudrate != 921600) && + (baudrate != 1000000) && + (baudrate != 1843200) && + (baudrate != 2000000)) { + return false; + } + + return true; +} + +bool SerialImpl::open() +{ + // There's no harm in calling open multiple times on the same port. + // In fact, that's the only way to change the baudrate + + _open = false; + _serial_fd = -1; + + if (! validateBaudrate(_baudrate)) { + PX4_ERR("Invalid baudrate: %u", _baudrate); + return false; + } + + if (_bytesize != ByteSize::EightBits) { + PX4_ERR("Qurt platform only supports ByteSize::EightBits"); + return false; + } + + if (_parity != Parity::None) { + PX4_ERR("Qurt platform only supports Parity::None"); + return false; + } + + if (_stopbits != StopBits::One) { + PX4_ERR("Qurt platform only supports StopBits::One"); + return false; + } + + if (_flowcontrol != FlowControl::Disabled) { + PX4_ERR("Qurt platform only supports FlowControl::Disabled"); + return false; + } + + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + + // qurt_uart_open will check validity of port and baudrate + int serial_fd = qurt_uart_open(_port, _baudrate); + + if (serial_fd < 0) { + PX4_ERR("failed to open %s, fd returned: %d", _port, serial_fd); + return false; + + } else { + PX4_INFO("Successfully opened UART %s with baudrate %u", _port, _baudrate); + } + + _serial_fd = serial_fd; + _open = true; + + return _open; +} + +bool SerialImpl::isOpen() const +{ + return _open; +} + +bool SerialImpl::close() +{ + // No close defined for qurt uart yet + // if (_serial_fd >= 0) { + // qurt_uart_close(_serial_fd); + // } + + _serial_fd = -1; + _open = false; + + return true; +} + +ssize_t SerialImpl::read(uint8_t *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot read from serial device until it has been opened"); + return -1; + } + + int ret_read = qurt_uart_read(_serial_fd, (char *) buffer, buffer_size, 100); + + if (ret_read < 0) { + PX4_DEBUG("%s read error %d", _port, ret_read); + + } + + return ret_read; +} + +ssize_t SerialImpl::readAtLeast(uint8_t *buffer, size_t buffer_size, size_t character_count, uint32_t timeout_ms) +{ + if (!_open) { + PX4_ERR("Cannot readAtLeast from serial device until it has been opened"); + return -1; + } + + if (buffer_size < character_count) { + PX4_ERR("%s: Buffer not big enough to hold desired amount of read data", __FUNCTION__); + return -1; + } + + const hrt_abstime start_time_us = hrt_absolute_time(); + hrt_abstime timeout_us = timeout_ms * 1000; + int total_bytes_read = 0; + + while (total_bytes_read < (int) character_count) { + + if (timeout_us > 0) { + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + + if (elapsed_us >= timeout_us) { + // If there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + // PX4_WARN("%s timeout %d bytes read (%llu us elapsed)", __FUNCTION__, total_bytes_read, elapsed_us); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return total_bytes_read; + } + } + + int current_bytes_read = read(&buffer[total_bytes_read], buffer_size - total_bytes_read); + + if (current_bytes_read < 0) { + // Again, if there was a partial read but not enough to satisfy the minimum then they will be lost + // but this really should never happen when everything is working normally. + PX4_ERR("%s failed to read uart", __FUNCTION__); + // Or, instead of returning an error, should we return the number of bytes read (assuming it is greater than zero)? + return -1; + } + + // Current bytes read could be zero + total_bytes_read += current_bytes_read; + + // If we have at least reached our desired minimum number of characters + // then we can return now + if (total_bytes_read >= (int) character_count) { + return total_bytes_read; + } + + // Wait a set amount of time before trying again or the remaining time + // until the timeout if we are getting close + const uint64_t elapsed_us = hrt_elapsed_time(&start_time_us); + int64_t time_until_timeout = timeout_us - elapsed_us; + uint64_t time_to_sleep = 5000; + + if ((time_until_timeout >= 0) && + (time_until_timeout < (int64_t) time_to_sleep)) { + time_to_sleep = time_until_timeout; + } + + px4_usleep(time_to_sleep); + } + + return -1; +} + +ssize_t SerialImpl::write(const void *buffer, size_t buffer_size) +{ + if (!_open) { + PX4_ERR("Cannot write to serial device until it has been opened"); + return -1; + } + + int ret_write = qurt_uart_write(_serial_fd, (const char *) buffer, buffer_size); + + if (ret_write < 0) { + PX4_ERR("%s write error %d", _port, ret_write); + + } + + return ret_write; +} + +void SerialImpl::flush() +{ + // TODO: Flush not implemented yet on Qurt +} + +const char *SerialImpl::getPort() const +{ + return _port; +} + +bool SerialImpl::validatePort(const char *port) +{ + return (qurt_uart_get_port(port) >= 0); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + +uint32_t SerialImpl::getBaudrate() const +{ + return _baudrate; +} + +bool SerialImpl::setBaudrate(uint32_t baudrate) +{ + if (! validateBaudrate(baudrate)) { + PX4_ERR("Invalid baudrate: %u", baudrate); + return false; + } + + // check if already configured + if (baudrate == _baudrate) { + return true; + } + + _baudrate = baudrate; + + // process baud rate change now if port is already open + if (_open) { + return open(); + } + + return true; +} + +ByteSize SerialImpl::getBytesize() const +{ + return _bytesize; +} + +bool SerialImpl::setBytesize(ByteSize bytesize) +{ + return bytesize == ByteSize::EightBits; +} + +Parity SerialImpl::getParity() const +{ + return _parity; +} + +bool SerialImpl::setParity(Parity parity) +{ + return parity == Parity::None; +} + +StopBits SerialImpl::getStopbits() const +{ + return _stopbits; +} + +bool SerialImpl::setStopbits(StopBits stopbits) +{ + return stopbits == StopBits::One; +} + +FlowControl SerialImpl::getFlowcontrol() const +{ + return _flowcontrol; +} + +bool SerialImpl::setFlowcontrol(FlowControl flowcontrol) +{ + return flowcontrol == FlowControl::Disabled; +} + +bool SerialImpl::getSingleWireMode() const +{ + return false; +} + +bool SerialImpl::setSingleWireMode() +{ + // Qurt platform does not support single wire mode + return false; +} + +bool SerialImpl::getSwapRxTxMode() const +{ + return false; +} + +bool SerialImpl::setSwapRxTxMode() +{ + // Qurt platform does not support swap rx tx mode + return false; +} + +bool SerialImpl::setInvertedMode(bool enable) +{ + // Qurt platform does not support inverted mode + return false == enable; +} +bool SerialImpl::getInvertedMode() const +{ + return false; +} + +} // namespace device diff --git a/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h b/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h index b4c397098a40..c54221e6817d 100644 --- a/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h +++ b/platforms/qurt/src/px4/common/include/px4_platform/cpuload.h @@ -34,4 +34,3 @@ #pragma once extern "C" float px4muorb_get_cpu_load(void); - diff --git a/platforms/qurt/src/px4/drv_hrt.cpp b/platforms/qurt/src/px4/drv_hrt.cpp index 8bb9546a01db..2ef152083249 100644 --- a/platforms/qurt/src/px4/drv_hrt.cpp +++ b/platforms/qurt/src/px4/drv_hrt.cpp @@ -81,7 +81,7 @@ static void hrt_unlock() px4_sem_post(&_hrt_lock); } -int px4_clock_settime(clockid_t clk_id, struct timespec *tp) +int px4_clock_settime(clockid_t clk_id, const struct timespec *tp) { return 0; } diff --git a/platforms/qurt/src/px4/qurt_log.cpp b/platforms/qurt/src/px4/qurt_log.cpp index 239f9e52b87f..bc6bf2b44ac1 100644 --- a/platforms/qurt/src/px4/qurt_log.cpp +++ b/platforms/qurt/src/px4/qurt_log.cpp @@ -45,4 +45,4 @@ extern "C" void qurt_log_to_apps(int level, const char *message) else { ch->send_message("slpi_debug", strlen(message) + 1, (uint8_t *) message); } } -} \ No newline at end of file +} diff --git a/platforms/qurt/unresolved_symbols.c b/platforms/qurt/unresolved_symbols.c index 0d5ef43d5882..820ea1f4c40b 100644 --- a/platforms/qurt/unresolved_symbols.c +++ b/platforms/qurt/unresolved_symbols.c @@ -2,6 +2,7 @@ #include #include #include +#include __attribute__((visibility("default"))) void free(void *ptr) { @@ -31,3 +32,21 @@ __attribute__((visibility("default"))) int nanosleep(const struct timespec *req, PX4_ERR("Undefined nanosleep called"); return -1; } + +__attribute__((visibility("default"))) int tcgetattr(int fd, struct termios *termiosp) +{ + PX4_ERR("Undefined tcgetattr called"); + return -1; +} + +__attribute__((visibility("default"))) int cfsetspeed(struct termios *termiosp, speed_t speed) +{ + PX4_ERR("Undefined cfsetspeed called"); + return -1; +} + +__attribute__((visibility("default"))) int tcsetattr(int fd, int options, const struct termios *termiosp) +{ + PX4_ERR("Undefined tcsetattr called"); + return -1; +} diff --git a/platforms/ros2/include/hrt_work.h b/platforms/ros2/include/hrt_work.h index 2394ec09570d..7d4e17acfeb7 100644 --- a/platforms/ros2/include/hrt_work.h +++ b/platforms/ros2/include/hrt_work.h @@ -61,4 +61,3 @@ static inline void hrt_work_unlock() } __END_DECLS - diff --git a/platforms/ros2/include/queue.h b/platforms/ros2/include/queue.h index d6315ca1723c..b0a73322cf91 100644 --- a/platforms/ros2/include/queue.h +++ b/platforms/ros2/include/queue.h @@ -131,4 +131,3 @@ EXTERN FAR dq_entry_t *dq_remfirst(dq_queue_t *queue); #endif #endif /* __INCLUDE_QUEUE_H_ */ - diff --git a/platforms/ros2/src/px4/CMakeLists.txt b/platforms/ros2/src/px4/CMakeLists.txt index bf200988a5ed..a8bb11576e96 100644 --- a/platforms/ros2/src/px4/CMakeLists.txt +++ b/platforms/ros2/src/px4/CMakeLists.txt @@ -34,4 +34,3 @@ add_subdirectory(common) add_subdirectory(${PX4_CHIP_MANUFACTURER}) - diff --git a/platforms/ros2/src/px4/generic/generic/include/px4_arch/micro_hal.h b/platforms/ros2/src/px4/generic/generic/include/px4_arch/micro_hal.h index d09b73c687dd..2d85741cac90 100644 --- a/platforms/ros2/src/px4/generic/generic/include/px4_arch/micro_hal.h +++ b/platforms/ros2/src/px4/generic/generic/include/px4_arch/micro_hal.h @@ -33,4 +33,3 @@ #pragma once #include - diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 6554f52d4525..bdd987e80193 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -42,4 +42,3 @@ control_allocator start logger start -t -b 200 mavlink boot_complete - diff --git a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp index 0c5bc85ca8a5..91df70639d30 100644 --- a/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp +++ b/src/drivers/actuators/vertiq_io/vertiq_telemetry_manager.cpp @@ -113,7 +113,8 @@ uint16_t VertiqTelemetryManager::UpdateTelemetry() // also update our internal report for logging _esc_status.esc[_current_module_id_target_index].esc_address = _module_ids_in_use[_number_of_module_ids_for_telem]; _esc_status.esc[_current_module_id_target_index].timestamp = time_now; - _esc_status.esc[_current_module_id_target_index].esc_rpm = telem_response.speed; + _esc_status.esc[_current_module_id_target_index].esc_rpm = telem_response.speed * 60.0f * M_1_PI_F * + 0.5f; //We get back rad/s, convert to rpm _esc_status.esc[_current_module_id_target_index].esc_voltage = telem_response.voltage * 0.01; _esc_status.esc[_current_module_id_target_index].esc_current = telem_response.current * 0.01; _esc_status.esc[_current_module_id_target_index].esc_power = diff --git a/src/drivers/actuators/voxl_esc/CMakeLists.txt b/src/drivers/actuators/voxl_esc/CMakeLists.txt index d588dc29a69d..dbfa0c42c384 100644 --- a/src/drivers/actuators/voxl_esc/CMakeLists.txt +++ b/src/drivers/actuators/voxl_esc/CMakeLists.txt @@ -38,8 +38,6 @@ px4_add_module( crc16.c crc16.h - voxl_esc_serial.cpp - voxl_esc_serial.hpp voxl_esc.cpp voxl_esc.hpp qc_esc_packet_types.h diff --git a/src/drivers/actuators/voxl_esc/module.yaml b/src/drivers/actuators/voxl_esc/module.yaml index 1e227ae0b71d..c3bb1f4e6ed3 100644 --- a/src/drivers/actuators/voxl_esc/module.yaml +++ b/src/drivers/actuators/voxl_esc/module.yaml @@ -38,4 +38,4 @@ parameters: reboot_required: true num_instances: 1 instance_start: 1 - default: 0 \ No newline at end of file + default: 0 diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.cpp b/src/drivers/actuators/voxl_esc/voxl_esc.cpp index 88d070a7cf6c..f5568d7eebc7 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.cpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.cpp @@ -36,7 +36,6 @@ #include #include "voxl_esc.hpp" -#include "voxl_esc_serial.hpp" // future use: #define MODALAI_PUBLISH_ESC_STATUS 0 @@ -83,10 +82,7 @@ VoxlEsc::~VoxlEsc() { _outputs_on = false; - if (_uart_port) { - _uart_port->uart_close(); - _uart_port = nullptr; - } + _uart_port.close(); perf_free(_cycle_perf); perf_free(_output_update_perf); @@ -94,26 +90,181 @@ VoxlEsc::~VoxlEsc() int VoxlEsc::init() { + PX4_INFO("VOXL_ESC: Starting VOXL ESC driver"); /* Getting initial parameter values */ int ret = update_params(); if (ret != OK) { + PX4_ERR("VOXL_ESC: Failed to update params during init"); return ret; } - _uart_port = new VoxlEscSerial(); + print_params(); + + //WARING: uart port initialization and device detection does not happen here + //because init() is called from a different thread from Run(), so fd opened in init() cannot be used in Run() + //this is an issue (feature?) specific to nuttx where each thread group gets separate set of fds + //https://cwiki.apache.org/confluence/display/NUTTX/Detaching+File+Descriptors + //detaching file descriptors is not implemented in the current version of nuttx that px4 uses + // + //There is no problem when running on VOXL2, but in order to have the same logical flow on both systems, + //we will initialize uart and query the device in Run() + + ScheduleNow(); + + return 0; +} + +int VoxlEsc::device_init() +{ + if (_device_initialized) { + return 0; + } + + // Open serial port + if (!_uart_port.isOpen()) { + PX4_INFO("VOXL_ESC: Opening UART ESC device %s, baud rate %" PRIi32, _device, _parameters.baud_rate); +#ifndef __PX4_QURT + + //warn user that unless DMA is enabled for UART RX, data can be lost due to high frequency of per char cpu interrupts + //at least at 2mbit, there are definitely losses, did not test other baud rates to find the cut off + if (_parameters.baud_rate > 250000) { + PX4_WARN("VOXL_ESC: Baud rate is too high for non-DMA based UART, this can lead to loss of RX data"); + } + +#endif + + // Configure UART port + if (! _uart_port.setPort(_device)) { + PX4_ERR("Error configuring serial device on port %s", _device); + return -1; + } + + if (! _uart_port.setBaudrate(_parameters.baud_rate)) { + PX4_ERR("Error setting baudrate to %d on %s", (int) _parameters.baud_rate, _device); + return -1; + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart_port.open()) { + PX4_ERR("Error opening serial device %s", _device); + return -1; + } + } + + // Reset output channel values memset(&_esc_chans, 0x00, sizeof(_esc_chans)); + //reset the ESC version info before requesting for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { - _version_info[esc_id].sw_version = UINT16_MAX; - _version_info[esc_id].hw_version = UINT16_MAX; - _version_info[esc_id].id = esc_id; + memset(&(_version_info[esc_id]), 0, sizeof(_version_info[esc_id])); + //_version_info[esc_id].sw_version = 0; //invalid + //_version_info[esc_id].hw_version = 0; //invalid + _version_info[esc_id].id = esc_id; } - //get_instance()->ScheduleOnInterval(10000); //100hz + // Detect ESCs + PX4_INFO("VOXL_ESC: Detecting ESCs..."); + qc_esc_packet_init(&_fb_packet); + + //request extended version info from each ESC and wait for reply + for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { + Command cmd; + cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); - ScheduleNow(); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL_ESC: Could not write version request packet to UART port"); + return -1; + } + + hrt_abstime t_request = hrt_absolute_time(); + hrt_abstime t_timeout = 50000; //50ms timeout for version info response + bool got_response = false; + + while ((!got_response) && (hrt_elapsed_time(&t_request) < t_timeout)) { + px4_usleep(100); //sleep a bit while waiting for ESC to respond + + int nread = _uart_port.read(_read_buf, sizeof(_read_buf)); + + for (int i = 0; i < nread; i++) { + int16_t parse_ret = qc_esc_packet_process_char(_read_buf[i], &_fb_packet); + + if (parse_ret > 0) { + hrt_abstime response_time = hrt_elapsed_time(&t_request); + //PX4_INFO("got packet of length %i",ret); + _rx_packet_count++; + uint8_t packet_type = qc_esc_packet_get_type(&_fb_packet); + uint8_t packet_size = qc_esc_packet_get_size(&_fb_packet); + + if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) { + QC_ESC_EXTENDED_VERSION_INFO ver; + memcpy(&ver, _fb_packet.buffer, packet_size); + + PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); + PX4_INFO("VOXL_ESC: \tBoard Type : %i: %s", ver.hw_version, board_id_to_name(ver.hw_version)); + + uint8_t *u = &ver.unique_id[0]; + PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); + + PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL_ESC: \tReply time : %" PRIu32 "us", (uint32_t)response_time); + PX4_INFO("VOXL_ESC:"); + + if (ver.id == esc_id) { + memcpy(&_version_info[esc_id], &ver, sizeof(ver)); + got_response = true; + } + } + } + } + } + + if (!got_response) { + PX4_ERR("VOXL_ESC: ESC %d version info response timeout", esc_id); + } + } + + //check the SW version of the ESCs + bool esc_detection_fault = false; + + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { + if (_version_info[esc_id].sw_version == 0) { + PX4_ERR("VOXL_ESC: ESC ID %d was not detected", esc_id); + esc_detection_fault = true; + } + } + + //check the firmware hashes to make sure they are the same. Firmware hash has 8 chars plus optional "*" + for (int esc_id = 1; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { + if (strncmp(_version_info[0].firmware_git_version, _version_info[esc_id].firmware_git_version, 9) != 0) { + PX4_ERR("VOXL_ESC: ESC %d Firmware hash does not match ESC 0 firmware hash: (%.12s) != (%.12s)", + esc_id, _version_info[esc_id].firmware_git_version, _version_info[0].firmware_git_version); + esc_detection_fault = true; + } + } + + //if firmware version is equal or greater than VOXL_ESC_EXT_RPM, ESC packet with extended rpm range is supported. use it + _extended_rpm = true; + + for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; esc_id++) { + if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) { + _extended_rpm = false; + } + } + + if (esc_detection_fault) { + PX4_ERR("VOXL_ESC: Critical error during ESC initialization"); + return -1; + } + + PX4_INFO("VOXL_ESC: Use extened rpm packet : %d", _extended_rpm); + + PX4_INFO("VOXL_ESC: All ESCs successfully detected"); + + _device_initialized = true; return 0; } @@ -155,45 +306,48 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) param_get(param_find("VOXL_ESC_VLOG"), ¶ms->verbose_logging); param_get(param_find("VOXL_ESC_PUB_BST"), ¶ms->publish_battery_status); + param_get(param_find("VOXL_ESC_T_WARN"), ¶ms->esc_warn_temp_threshold); + param_get(param_find("VOXL_ESC_T_OVER"), ¶ms->esc_over_temp_threshold); + if (params->rpm_min >= params->rpm_max) { - PX4_ERR("Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_RPM_MIN. Please verify parameters."); params->rpm_min = 0; ret = PX4_ERROR; } if (params->turtle_motor_percent < 0 || params->turtle_motor_percent > 100) { - PX4_ERR("Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_PERC. Please verify parameters."); params->turtle_motor_percent = 0; ret = PX4_ERROR; } if (params->turtle_motor_deadband < 0 || params->turtle_motor_deadband > 100) { - PX4_ERR("Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_DEAD. Please verify parameters."); params->turtle_motor_deadband = 0; ret = PX4_ERROR; } if (params->turtle_motor_expo < 0 || params->turtle_motor_expo > 100) { - PX4_ERR("Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_EXPO. Please verify parameters."); params->turtle_motor_expo = 0; ret = PX4_ERROR; } if (params->turtle_stick_minf < 0.0f || params->turtle_stick_minf > 100.0f) { - PX4_ERR("Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_MINF. Please verify parameters."); params->turtle_stick_minf = 0.0f; ret = PX4_ERROR; } if (params->turtle_cosphi < 0.0f || params->turtle_cosphi > 100.0f) { - PX4_ERR("Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_T_COSP. Please verify parameters."); params->turtle_cosphi = 0.0f; ret = PX4_ERROR; } for (int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++) { if (params->function_map[i] < (int)OutputFunction::Motor1 || params->function_map[i] > (int)OutputFunction::Motor4) { - PX4_ERR("Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_FUNCX. Only supports motors 1-4. Please verify parameters."); params->function_map[i] = 0; ret = PX4_ERROR; @@ -211,7 +365,7 @@ int VoxlEsc::load_params(voxl_esc_params_t *params, ch_assign_t *map) if (params->motor_map[i] == VOXL_ESC_OUTPUT_DISABLED || params->motor_map[i] < -(VOXL_ESC_OUTPUT_CHANNELS) || params->motor_map[i] > VOXL_ESC_OUTPUT_CHANNELS) { - PX4_ERR("Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); + PX4_ERR("VOXL_ESC: Invalid parameter VOXL_ESC_MOTORX. Please verify parameters."); params->motor_map[i] = 0; ret = PX4_ERROR; } @@ -264,35 +418,11 @@ int VoxlEsc::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -int VoxlEsc::flush_uart_rx() -{ - while (_uart_port->uart_read(_read_buf, sizeof(_read_buf)) > 0) {} - - return 0; -} - -bool VoxlEsc::check_versions_updated() -{ - for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { - if (_version_info[esc_id].sw_version == UINT16_MAX) { return false; } - } - - // PX4_INFO("Got all ESC Version info!"); - _extended_rpm = true; - _need_version_info = false; - - for (int esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { - if (_version_info[esc_id].sw_version < VOXL_ESC_EXT_RPM) { _extended_rpm = false; } - } - - return true; -} - int VoxlEsc::read_response(Command *out_cmd) { px4_usleep(_current_cmd.resp_delay_us); - int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); + int res = _uart_port.read(_read_buf, sizeof(_read_buf)); if (res > 0) { //PX4_INFO("read %i bytes",res); @@ -341,7 +471,8 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint32_t voltage = fb.voltage; int32_t current = fb.current * 8; int32_t temperature = fb.temperature / 100; - PX4_INFO("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, motor_idx + 1, + PX4_INFO("VOXL_ESC: [%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, + motor_idx + 1, (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); } @@ -382,6 +513,19 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) _esc_status.timestamp = _esc_status.esc[id].timestamp; _esc_status.counter++; + + if ((_parameters.esc_over_temp_threshold > 0) + && (_esc_status.esc[id].esc_temperature > _parameters.esc_over_temp_threshold)) { + _esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_OVER_ESC_TEMPERATURE); + } + + //TODO: do we also issue a warning if over-temperature threshold is exceeded? + if ((_parameters.esc_warn_temp_threshold > 0) + && (_esc_status.esc[id].esc_temperature > _parameters.esc_warn_temp_threshold)) { + _esc_status.esc[id].failures |= 1 << (esc_report_s::FAILURE_WARN_ESC_TEMPERATURE); + } + + //print ESC status just for debugging /* PX4_INFO("[%lld] ID=%d, ADDR %d, STATE=%d, RPM=%5d, PWR=%3d%%, V=%.2fdV, I=%.2fA, T=%+3dC, CNT %d, FAIL %d", @@ -397,30 +541,24 @@ int VoxlEsc::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) QC_ESC_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - if (_need_version_info) { - memcpy(&_version_info[ver.id], &ver, sizeof(QC_ESC_VERSION_INFO)); - check_versions_updated(); - break; - } - - PX4_INFO("ESC ID: %i", ver.id); - PX4_INFO("HW Version: %i", ver.hw_version); - PX4_INFO("SW Version: %i", ver.sw_version); - PX4_INFO("Unique ID: %i", (int)ver.unique_id); + PX4_INFO("VOXL_ESC: ESC ID: %i", ver.id); + PX4_INFO("VOXL_ESC: HW Version: %i", ver.hw_version); + PX4_INFO("VOXL_ESC: SW Version: %i", ver.sw_version); + PX4_INFO("VOXL_ESC: Unique ID: %i", (int)ver.unique_id); } else if (packet_type == ESC_PACKET_TYPE_VERSION_EXT_RESPONSE && packet_size == sizeof(QC_ESC_EXTENDED_VERSION_INFO)) { QC_ESC_EXTENDED_VERSION_INFO ver; memcpy(&ver, _fb_packet.buffer, packet_size); - PX4_INFO("\tESC ID : %i", ver.id); - PX4_INFO("\tBoard : %i", ver.hw_version); - PX4_INFO("\tSW Version : %i", ver.sw_version); + PX4_INFO("VOXL_ESC: \tESC ID : %i", ver.id); + PX4_INFO("VOXL_ESC: \tBoard : %i", ver.hw_version); + PX4_INFO("VOXL_ESC: \tSW Version : %i", ver.sw_version); uint8_t *u = &ver.unique_id[0]; - PX4_INFO("\tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", + PX4_INFO("VOXL_ESC: \tUnique ID : 0x%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X", u[11], u[10], u[9], u[8], u[7], u[6], u[5], u[4], u[3], u[2], u[1], u[0]); - PX4_INFO("\tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); - PX4_INFO("\tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); + PX4_INFO("VOXL_ESC: \tFirmware : version %4d, hash %.12s", ver.sw_version, ver.firmware_git_version); + PX4_INFO("VOXL_ESC: \tBootloader : version %4d, hash %.12s", ver.bootloader_version, ver.bootloader_git_version); } else if (packet_type == ESC_PACKET_TYPE_FB_POWER_STATUS && packet_size == sizeof(QC_ESC_FB_POWER_STATUS)) { QC_ESC_FB_POWER_STATUS packet; @@ -525,7 +663,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) const char *verb = argv[argc - 1]; - /* start the FMU if not running */ + /* start the driver if not running */ if (!strcmp(verb, "start")) { if (!is_running()) { return VoxlEsc::task_spawn(argc, argv); @@ -533,7 +671,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } if (!is_running()) { - PX4_INFO("Not running"); + PX4_INFO("VOXL_ESC:Not running"); return -1; } @@ -592,7 +730,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (!strcmp(verb, "reset")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Reset ESC: %i", esc_id); + PX4_INFO("VOXL_ESC: Reset ESC: %i", esc_id); cmd.len = qc_esc_create_reset_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -604,7 +742,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Request version for ESC: %i", esc_id); + PX4_INFO("VOXL_ESC: Request version for ESC: %i", esc_id); cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 2000; @@ -617,7 +755,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "version-ext")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Request extended version for ESC: %i", esc_id); + PX4_INFO("VOXL_ESC: Request extended version for ESC: %i", esc_id); cmd.len = qc_esc_create_extended_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = true; cmd.resp_delay_us = 5000; @@ -630,7 +768,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "tone")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Request tone for ESC mask: %i", esc_id); + PX4_INFO("VOXL_ESC: Request tone for ESC mask: %i", esc_id); cmd.len = qc_esc_create_sound_packet(period, duration, power, esc_id, cmd.buf, sizeof(cmd.buf)); cmd.response = false; return get_instance()->send_cmd_thread_safe(&cmd); @@ -644,7 +782,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) if (led_mask <= 0x0FFF) { get_instance()->_led_rsc.test = true; get_instance()->_led_rsc.breath_en = false; - PX4_INFO("Request LED control for ESCs with mask: %i", led_mask); + PX4_INFO("VOXL_ESC: Request LED control for ESCs with mask: %i", led_mask); get_instance()->_esc_chans[0].led = (led_mask & 0x0007); get_instance()->_esc_chans[1].led = (led_mask & 0x0038) >> 3; @@ -659,7 +797,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "rpm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); + PX4_INFO("VOXL_ESC: Request RPM for ESC ID: %i - RPM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -693,8 +831,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART ESC RPM command %i", rate); + PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); + PX4_INFO("VOXL_ESC: Sending UART ESC RPM command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -705,7 +843,7 @@ int VoxlEsc::custom_command(int argc, char *argv[]) } else if (!strcmp(verb, "pwm")) { if (esc_id < VOXL_ESC_OUTPUT_CHANNELS) { - PX4_INFO("Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); + PX4_INFO("VOXL_ESC: Request PWM for ESC ID: %i - PWM: %i", esc_id, rate); int16_t rate_req[VOXL_ESC_OUTPUT_CHANNELS] = {0, 0, 0, 0}; uint8_t id_fb = 0; @@ -738,8 +876,8 @@ int VoxlEsc::custom_command(int argc, char *argv[]) cmd.repeat_delay_us = repeat_delay_us; cmd.print_feedback = true; - PX4_INFO("feedback id debug: %i", id_fb); - PX4_INFO("Sending UART ESC power command %i", rate); + PX4_INFO("VOXL_ESC: Feedback id debug: %i", id_fb); + PX4_INFO("VOXL_ESC: Sending UART ESC power command %i", rate); return get_instance()->send_cmd_thread_safe(&cmd); @@ -1103,8 +1241,8 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], sizeof(cmd.buf), _extended_rpm); - if (_uart_port->uart_write(cmd.buf, cmd.len) != cmd.len) { - PX4_ERR("Failed to send packet"); + if (_uart_port.write(cmd.buf, cmd.len) != cmd.len) { + PX4_ERR("VOXL_ESC: Failed to send packet"); return false; } @@ -1118,7 +1256,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], * uart_read is non-blocking and we will just parse whatever bytes came in up until this point */ - int res = _uart_port->uart_read(_read_buf, sizeof(_read_buf)); + int res = _uart_port.read(_read_buf, sizeof(_read_buf)); if (res > 0) { parse_response(_read_buf, res, false); @@ -1155,8 +1293,8 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], // PX4_INFO(" 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x 0x%.2x", // io_data.data[0], io_data.data[1], io_data.data[2], io_data.data[3], // io_data.data[4], io_data.data[5], io_data.data[6], io_data.data[7]); - if (_uart_port->uart_write(io_data.data, io_data.len) != io_data.len) { - PX4_ERR("Failed to send modal io data to esc"); + if (_uart_port.write(io_data.data, io_data.len) != io_data.len) { + PX4_ERR("VOXL_ESC: Failed to send modal io data to esc"); return false; } } @@ -1170,6 +1308,7 @@ bool VoxlEsc::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], void VoxlEsc::Run() { if (should_exit()) { + PX4_ERR("VOXL_ESC: Stopping the module"); ScheduleClear(); _mixing_output.unregister(); @@ -1179,32 +1318,28 @@ void VoxlEsc::Run() perf_begin(_cycle_perf); - /* Open serial port in this thread */ - if (!_uart_port->is_open()) { - if (_uart_port->uart_open(_device, _parameters.baud_rate) == PX4_OK) { - PX4_INFO("Opened UART ESC device"); + //check to see if we need to open uart port and query the device + //see comment in init() regarding why we do not initialize the device there - } else { - PX4_ERR("Failed openening device"); - return; - } - } + int retries_left = VOXL_ESC_NUM_INIT_RETRIES; - /* Get ESC FW version info */ - if (_need_version_info) { - for (uint8_t esc_id = 0; esc_id < VOXL_ESC_OUTPUT_CHANNELS; ++esc_id) { - Command cmd; - cmd.len = qc_esc_create_version_request_packet(esc_id, cmd.buf, sizeof(cmd.buf)); - - if (_uart_port->uart_write(cmd.buf, cmd.len) == cmd.len) { - if (read_response(&_current_cmd) != 0) { PX4_ERR("Failed to parse version request response packet!"); } + while ((!_device_initialized) && (retries_left > 0)) { + retries_left--; + int dev_init_ret = device_init(); - } else { - PX4_ERR("Failed to send version request packet!"); - } + if (dev_init_ret != 0) { + PX4_WARN("VOXL_ESC: Failed to initialize device, retries left %d", retries_left); } } + if (!_device_initialized) { + PX4_ERR("VOXL_ESC: Failed to initialize device, exiting the module"); + ScheduleClear(); + _mixing_output.unregister(); + exit_and_cleanup(); + return; + } + _mixing_output.update(); //calls MixingOutput::limitAndUpdateOutputs which calls updateOutputs in this module /* update output status if armed */ @@ -1278,11 +1413,11 @@ void VoxlEsc::Run() if (!_outputs_on) { if (_current_cmd.valid()) { //PX4_INFO("sending %d commands with delay %dus",_current_cmd.repeats,_current_cmd.repeat_delay_us); - flush_uart_rx(); + _uart_port.flush(); do { //PX4_INFO("CMDs left %d",_current_cmd.repeats); - if (_uart_port->uart_write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { + if (_uart_port.write(_current_cmd.buf, _current_cmd.len) == _current_cmd.len) { if (_current_cmd.repeats == 0) { _current_cmd.clear(); } @@ -1296,19 +1431,19 @@ void VoxlEsc::Run() } else { if (_current_cmd.retries == 0) { _current_cmd.clear(); - PX4_ERR("Failed to send command, errno: %i", errno); + PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); } else { _current_cmd.retries--; - PX4_ERR("Failed to send command, errno: %i", errno); + PX4_ERR("VOXL_ESC: Failed to send command, errno: %i", errno); } } px4_usleep(_current_cmd.repeat_delay_us); } while (_current_cmd.repeats-- > 0); - PX4_INFO("RX packet count: %d", (int)_rx_packet_count); - PX4_INFO("CRC error count: %d", (int)_rx_crc_error_count); + PX4_INFO("VOXL_ESC: RX packet count: %d", (int)_rx_packet_count); + PX4_INFO("VOXL_ESC: CRC error count: %d", (int)_rx_crc_error_count); } else { Command *new_cmd = _pending_cmd.load(); @@ -1385,16 +1520,10 @@ It is typically started with: return 0; } -int VoxlEsc::print_status() +void VoxlEsc::print_params() { - PX4_INFO("Max update rate: %i Hz", _current_update_rate); - PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); - PX4_INFO("UART port: %s", _device); - PX4_INFO("UART open: %s", _uart_port->is_open() ? "yes" : "no"); - - PX4_INFO(""); - PX4_INFO("Params: VOXL_ESC_CONFIG: %" PRId32, _parameters.config); + PX4_INFO("Params: VOXL_ESC_MODE: %" PRId32, _parameters.mode); PX4_INFO("Params: VOXL_ESC_BAUD: %" PRId32, _parameters.baud_rate); PX4_INFO("Params: VOXL_ESC_FUNC1: %" PRId32, _parameters.function_map[0]); @@ -1410,6 +1539,28 @@ int VoxlEsc::print_status() PX4_INFO("Params: VOXL_ESC_RPM_MIN: %" PRId32, _parameters.rpm_min); PX4_INFO("Params: VOXL_ESC_RPM_MAX: %" PRId32, _parameters.rpm_max); + PX4_INFO("Params: VOXL_ESC_T_PERC: %" PRId32, _parameters.turtle_motor_percent); + PX4_INFO("Params: VOXL_ESC_T_DEAD: %" PRId32, _parameters.turtle_motor_deadband); + PX4_INFO("Params: VOXL_ESC_T_EXPO: %" PRId32, _parameters.turtle_motor_expo); + PX4_INFO("Params: VOXL_ESC_T_MINF: %f", (double)_parameters.turtle_stick_minf); + PX4_INFO("Params: VOXL_ESC_T_COSP: %f", (double)_parameters.turtle_cosphi); + + PX4_INFO("Params: VOXL_ESC_VLOG: %" PRId32, _parameters.verbose_logging); + PX4_INFO("Params: VOXL_ESC_PUB_BST: %" PRId32, _parameters.publish_battery_status); + + PX4_INFO("Params: VOXL_ESC_T_WARN: %" PRId32, _parameters.esc_warn_temp_threshold); + PX4_INFO("Params: VOXL_ESC_T_OVER: %" PRId32, _parameters.esc_over_temp_threshold); +} + +int VoxlEsc::print_status() +{ + PX4_INFO("Max update rate: %i Hz", _current_update_rate); + PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); + PX4_INFO("UART port: %s", _device); + PX4_INFO("UART open: %s", _uart_port.isOpen() ? "yes" : "no"); + + PX4_INFO(""); + print_params(); PX4_INFO(""); for( int i = 0; i < VOXL_ESC_OUTPUT_CHANNELS; i++){ @@ -1432,6 +1583,25 @@ int VoxlEsc::print_status() return 0; } +const char * VoxlEsc::board_id_to_name(int board_id) +{ + switch(board_id){ + case 31: return "ModalAi 4-in-1 ESC V2 RevB (M0049)"; + case 32: return "Blheli32 4-in-1 ESC Type A (Tmotor F55A PRO F051)"; + case 33: return "Blheli32 4-in-1 ESC Type B (Tmotor F55A PRO G071)"; + case 34: return "ModalAi 4-in-1 ESC (M0117-1)"; + case 35: return "ModalAi I/O Expander (M0065)"; + case 36: return "ModalAi 4-in-1 ESC (M0117-3)"; + case 37: return "ModalAi 4-in-1 ESC (M0134-1)"; + case 38: return "ModalAi 4-in-1 ESC (M0134-3)"; + case 39: return "ModalAi 4-in-1 ESC (M0129-1)"; + case 40: return "ModalAi 4-in-1 ESC (M0129-3)"; + case 41: return "ModalAi 4-in-1 ESC (M0134-6)"; + case 42: return "ModalAi 4-in-1 ESC (M0138-1)"; + default: return "Unknown Board"; + } +} + extern "C" __EXPORT int voxl_esc_main(int argc, char *argv[]); int voxl_esc_main(int argc, char *argv[]) diff --git a/src/drivers/actuators/voxl_esc/voxl_esc.hpp b/src/drivers/actuators/voxl_esc/voxl_esc.hpp index 8cc84435fae4..d894cb93266f 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc.hpp +++ b/src/drivers/actuators/voxl_esc/voxl_esc.hpp @@ -51,11 +51,13 @@ #include #include -#include "voxl_esc_serial.hpp" +#include #include "qc_esc_packet.h" #include "qc_esc_packet_types.h" +using namespace device; + class VoxlEsc : public ModuleBase, public OutputModuleInterface { public: @@ -76,16 +78,18 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface /** @see ModuleBase::print_status() */ int print_status() override; + void print_params(); /** @see OutputModuleInterface */ bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; virtual int init(); + int device_init(); // function where uart port is opened and ESC queried struct Command { uint16_t id = 0; - uint8_t len = 0; + uint8_t len = 0; uint16_t repeats = 0; uint16_t repeat_delay_us = 2000; uint8_t retries = 0; @@ -94,7 +98,7 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface bool print_feedback = false; static const uint8_t BUF_SIZE = 128; - uint8_t buf[BUF_SIZE]; + uint8_t buf[BUF_SIZE]; bool valid() const { return len > 0; } void clear() { len = 0; } @@ -125,16 +129,19 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX1 = 1; static constexpr uint32_t VOXL_ESC_MODE_TURTLE_AUX2 = 2; - static constexpr uint16_t VOXL_ESC_EXT_RPM = 39; + static constexpr uint16_t VOXL_ESC_EXT_RPM = + 39; // minimum firmware version for extended RPM command support static constexpr uint16_t VOXL_ESC_RPM_MAX = INT16_MAX - 1; // 32K, Limit max standard range RPM to prevent overflow (rpm packet packing function accepts int32_t) static constexpr uint16_t VOXL_ESC_RPM_MAX_EXT = UINT16_MAX - 5; // 65K, Limit max extended range RPM to prevent overflow (rpm packet packing function accepts int32_t) + static constexpr uint16_t VOXL_ESC_NUM_INIT_RETRIES = 3; + //static constexpr uint16_t max_pwm(uint16_t pwm) { return math::min(pwm, VOXL_ESC_PWM_MAX); } //static constexpr uint16_t max_rpm(uint16_t rpm) { return math::min(rpm, VOXL_ESC_RPM_MAX); } - VoxlEscSerial *_uart_port; + Serial _uart_port{}; typedef struct { int32_t config{VOXL_ESC_UART_CONFIG}; @@ -151,7 +158,9 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface int32_t motor_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 2, 3, 4}; int32_t direction_map[VOXL_ESC_OUTPUT_CHANNELS] {1, 1, 1, 1}; int32_t verbose_logging{0}; - int32_t publish_battery_status{0}; + int32_t publish_battery_status{0}; + int32_t esc_warn_temp_threshold{0}; + int32_t esc_over_temp_threshold{0}; } voxl_esc_params_t; struct EscChan { @@ -161,7 +170,7 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface uint8_t power_applied; uint8_t led; uint8_t cmd_counter; - float voltage; //Volts + float voltage; //Volts float current; //Amps float temperature; //deg C hrt_abstime feedback_time; @@ -182,7 +191,7 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface } led_rsc_t; ch_assign_t _output_map[VOXL_ESC_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; - MixingOutput _mixing_output; + MixingOutput _mixing_output; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; @@ -193,8 +202,8 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; uORB::Subscription _led_update_sub{ORB_ID(led_control)}; uORB::Subscription _voxl2_io_data_sub{ORB_ID(voxl2_io_data)}; @@ -203,12 +212,12 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface bool _extended_rpm{false}; bool _need_version_info{true}; - QC_ESC_VERSION_INFO _version_info[4]; - bool check_versions_updated(); + QC_ESC_EXTENDED_VERSION_INFO _version_info[VOXL_ESC_OUTPUT_CHANNELS]; voxl_esc_params_t _parameters; int update_params(); int load_params(voxl_esc_params_t *params, ch_assign_t *map); + const char *board_id_to_name(int board_id); bool _turtle_mode_en{false}; int32_t _rpm_turtle_min{0}; @@ -216,7 +225,7 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface manual_control_setpoint_s _manual_control_setpoint{}; uint16_t _cmd_id{0}; - Command _current_cmd; + Command _current_cmd; px4::atomic _pending_cmd{nullptr}; EscChan _esc_chans[VOXL_ESC_OUTPUT_CHANNELS]; @@ -224,24 +233,25 @@ class VoxlEsc : public ModuleBase, public OutputModuleInterface esc_status_s _esc_status; EscPacket _fb_packet; - led_rsc_t _led_rsc; + led_rsc_t _led_rsc; int _fb_idx; uint32_t _rx_crc_error_count{0}; uint32_t _rx_packet_count{0}; - static const uint8_t READ_BUF_SIZE = 128; + static const uint8_t READ_BUF_SIZE = 128; uint8_t _read_buf[READ_BUF_SIZE]; - Battery _battery; + Battery _battery; static constexpr unsigned _battery_report_interval{100_ms}; hrt_abstime _last_battery_report_time; - void update_leds(vehicle_control_mode_s mode, led_control_s control); + bool _device_initialized{false}; + + void update_leds(vehicle_control_mode_s mode, led_control_s control); - int read_response(Command *out_cmd); - int parse_response(uint8_t *buf, uint8_t len, bool print_feedback); - int flush_uart_rx(); - int check_for_esc_timeout(); + int read_response(Command *out_cmd); + int parse_response(uint8_t *buf, uint8_t len, bool print_feedback); + int check_for_esc_timeout(); void mix_turtle_mode(uint16_t outputs[]); void handle_actuator_test(); }; diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_params.c b/src/drivers/actuators/voxl_esc/voxl_esc_params.c index 8c1d03c6e7c1..2752d9d7448a 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_params.c +++ b/src/drivers/actuators/voxl_esc/voxl_esc_params.c @@ -231,4 +231,34 @@ PARAM_DEFINE_INT32(VOXL_ESC_VLOG, 0); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1); \ No newline at end of file +PARAM_DEFINE_INT32(VOXL_ESC_PUB_BST, 1); + + +/** + * UART ESC Temperature Warning Threshold (Degrees C) + * + * Only applicable to ESCs that report temperature + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @min 0 + * @max 200 + */ +PARAM_DEFINE_INT32(VOXL_ESC_T_WARN, 0); + + +/** + * UART ESC Over-Temperature Threshold (Degrees C) + * + * Only applicable to ESCs that report temperature + * + * @reboot_required true + * + * @group VOXL ESC + * @value 0 - Disabled + * @min 0 + * @max 200 + */ +PARAM_DEFINE_INT32(VOXL_ESC_T_OVER, 0); diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp b/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp deleted file mode 100644 index 1064dc4365c3..000000000000 --- a/src/drivers/actuators/voxl_esc/voxl_esc_serial.cpp +++ /dev/null @@ -1,191 +0,0 @@ -/**************************************************************************** - * - * 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 "string.h" -#include "voxl_esc_serial.hpp" - -VoxlEscSerial::VoxlEscSerial() -{ -} - -VoxlEscSerial::~VoxlEscSerial() -{ - if (_uart_fd >= 0) { - uart_close(); - } -} - -int VoxlEscSerial::uart_open(const char *dev, speed_t speed) -{ - if (_uart_fd >= 0) { - PX4_ERR("Port in use: %s (%i)", dev, errno); - return -1; - } - - /* Open UART */ -#ifdef __PX4_QURT - _uart_fd = qurt_uart_open(dev, speed); -#else - _uart_fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK); -#endif - - if (_uart_fd < 0) { - PX4_ERR("Error opening port: %s (%i)", dev, errno); - return -1; - } - -#ifndef __PX4_QURT - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(_uart_fd, &_orig_cfg)) < 0) { - PX4_ERR("Error configuring port: tcgetattr %s: %d", dev, termios_state); - uart_close(); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(_uart_fd, &_cfg); - - /* Disable output post-processing */ - _cfg.c_oflag &= ~OPOST; - - _cfg.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */ - _cfg.c_cflag &= ~CSIZE; - _cfg.c_cflag |= CS8; /* 8-bit characters */ - _cfg.c_cflag &= ~PARENB; /* no parity bit */ - _cfg.c_cflag &= ~CSTOPB; /* only need 1 stop bit */ - _cfg.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */ - - /* setup for non-canonical mode */ - _cfg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON); - _cfg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN); - - if (cfsetispeed(&_cfg, speed) < 0 || cfsetospeed(&_cfg, speed) < 0) { - PX4_ERR("Error configuring port: %s: %d (cfsetispeed, cfsetospeed)", dev, termios_state); - uart_close(); - return -1; - } - - if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &_cfg)) < 0) { - PX4_ERR("Error configuring port: %s (tcsetattr)", dev); - uart_close(); - return -1; - } - -#endif - - _speed = speed; - - return 0; -} - -int VoxlEscSerial::uart_set_baud(speed_t speed) -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - return -1; - } - - if (cfsetispeed(&_cfg, speed) < 0) { - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_cfg) < 0) { - return -1; - } - - _speed = speed; - - return 0; -#endif - - return -1; -} - -int VoxlEscSerial::uart_close() -{ -#ifndef __PX4_QURT - - if (_uart_fd < 0) { - PX4_ERR("invalid state for closing"); - return -1; - } - - if (tcsetattr(_uart_fd, TCSANOW, &_orig_cfg)) { - PX4_ERR("failed restoring uart to original state"); - } - - if (close(_uart_fd)) { - PX4_ERR("error closing uart"); - } - -#endif - - _uart_fd = -1; - - return 0; -} - -int VoxlEscSerial::uart_write(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for writing or buffer"); - return -1; - } - -#ifdef __PX4_QURT - return qurt_uart_write(_uart_fd, (const char *) buf, len); -#else - return write(_uart_fd, buf, len); -#endif -} - -int VoxlEscSerial::uart_read(FAR void *buf, size_t len) -{ - if (_uart_fd < 0 || buf == NULL) { - PX4_ERR("invalid state for reading or buffer"); - return -1; - } - -#ifdef __PX4_QURT -#define ASYNC_UART_READ_WAIT_US 2000 - // The UART read on SLPI is via an asynchronous service so specify a timeout - // for the return. The driver will poll periodically until the read comes in - // so this may block for a while. However, it will timeout if no read comes in. - return qurt_uart_read(_uart_fd, (char *) buf, len, ASYNC_UART_READ_WAIT_US); -#else - return read(_uart_fd, buf, len); -#endif -} diff --git a/src/drivers/adc/ads1115/ads1115_params.c b/src/drivers/adc/ads1115/ads1115_params.c index 560637db00a7..b1af67f30760 100644 --- a/src/drivers/adc/ads1115/ads1115_params.c +++ b/src/drivers/adc/ads1115/ads1115_params.c @@ -41,4 +41,3 @@ * @group Sensors */ PARAM_DEFINE_INT32(ADC_ADS1115_EN, 0); - diff --git a/src/drivers/barometer/CMakeLists.txt b/src/drivers/barometer/CMakeLists.txt index ba90108c38c2..5bb31ec3452d 100644 --- a/src/drivers/barometer/CMakeLists.txt +++ b/src/drivers/barometer/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2017 PX4 Development Team. All rights reserved. +# Copyright (c) 2017-2024 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,6 +33,7 @@ add_subdirectory(bmp280) add_subdirectory(bmp388) +add_subdirectory(bmp581) add_subdirectory(dps310) add_subdirectory(lps22hb) #add_subdirectory(lps25h) # not ready for general inclusion diff --git a/src/drivers/barometer/Kconfig b/src/drivers/barometer/Kconfig index e13c24c27251..da9c2dd7143a 100644 --- a/src/drivers/barometer/Kconfig +++ b/src/drivers/barometer/Kconfig @@ -4,6 +4,7 @@ menu "barometer" default n select DRIVERS_BAROMETER_BMP280 select DRIVERS_BAROMETER_BMP388 + select DRIVERS_BAROMETER_BMP581 select DRIVERS_BAROMETER_DPS310 select DRIVERS_BAROMETER_LPS22HB select DRIVERS_BAROMETER_LPS33HW diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp index 50c40e0686f8..04d29dbccfbb 100644 --- a/src/drivers/barometer/bmp388/bmp388.cpp +++ b/src/drivers/barometer/bmp388/bmp388.cpp @@ -68,7 +68,10 @@ BMP388::init() return -EIO; } - _chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR); + if (_interface->get_reg(BMP3_CHIP_ID_ADDR, &_chip_id) != OK) { + PX4_WARN("failed to get chip id"); + return -EIO; + } if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) { PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID); @@ -80,7 +83,10 @@ BMP388::init() this->_item_name = "bmp390"; } - _chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR); + if (_interface->get_reg(BMP3_REV_ID_ADDR, &_chip_rev_id) != OK) { + PX4_WARN("failed to get chip rev id"); + return -EIO; + } _cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR); @@ -204,14 +210,22 @@ BMP388::soft_reset() uint8_t status; int ret; - status = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR); + ret = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR, &status); + + if (ret != OK) { + return false; + } if (status & BMP3_CMD_RDY) { ret = _interface->set_reg(BPM3_CMD_SOFT_RESET, BMP3_CMD_ADDR); if (ret == OK) { usleep(BMP3_POST_RESET_WAIT_TIME); - status = _interface->get_reg(BMP3_ERR_REG_ADDR); + ret = _interface->get_reg(BMP3_ERR_REG_ADDR, &status); + + if (ret != OK) { + return false; + } if ((status & BMP3_CMD_ERR) == 0) { result = true; @@ -269,7 +283,9 @@ BMP388::validate_trimming_param() crc = (crc ^ 0xFF); - stored_crc = _interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR); + if (_interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR, &stored_crc) != OK) { + return false; + } return stored_crc == crc; } @@ -404,7 +420,12 @@ BMP388::set_op_mode(uint8_t op_mode) uint8_t op_mode_reg_val; int ret = OK; - op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR); + ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val); + + if (ret != OK) { + return false; + } + last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE); /* Device needs to be put in sleep mode to transition */ @@ -420,7 +441,12 @@ BMP388::set_op_mode(uint8_t op_mode) } if (ret == OK) { - op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR); + ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val); + + if (ret != OK) { + return false; + } + op_mode_reg_val = BMP3_SET_BITS(op_mode_reg_val, BMP3_OP_MODE, op_mode); ret = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR); diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h index 2db3e2238255..a72b95aef4f1 100644 --- a/src/drivers/barometer/bmp388/bmp388.h +++ b/src/drivers/barometer/bmp388/bmp388.h @@ -292,7 +292,7 @@ class IBMP388 virtual int init() = 0; // read reg value - virtual uint8_t get_reg(uint8_t addr) = 0; + virtual int get_reg(uint8_t addr, uint8_t *value) = 0; // bulk read reg value virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0; diff --git a/src/drivers/barometer/bmp388/bmp388_i2c.cpp b/src/drivers/barometer/bmp388/bmp388_i2c.cpp index 0b735d35df4a..73fe0427052a 100644 --- a/src/drivers/barometer/bmp388/bmp388_i2c.cpp +++ b/src/drivers/barometer/bmp388/bmp388_i2c.cpp @@ -49,7 +49,7 @@ class BMP388_I2C: public device::I2C, public IBMP388 int init(); - uint8_t get_reg(uint8_t addr); + int get_reg(uint8_t addr, uint8_t *value); int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); int set_reg(uint8_t value, uint8_t addr); calibration_s *get_calibration(uint8_t addr); @@ -78,12 +78,10 @@ int BMP388_I2C::init() return I2C::init(); } -uint8_t BMP388_I2C::get_reg(uint8_t addr) +int BMP388_I2C::get_reg(uint8_t addr, uint8_t *value) { - uint8_t cmd[2] = { (uint8_t)(addr), 0}; - transfer(&cmd[0], 1, &cmd[1], 1); - - return cmd[1]; + const uint8_t cmd = (uint8_t)(addr); + return transfer(&cmd, sizeof(cmd), value, 1); } int BMP388_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) diff --git a/src/drivers/barometer/bmp388/bmp388_spi.cpp b/src/drivers/barometer/bmp388/bmp388_spi.cpp index 18479a4aaf78..2970d2bc00b1 100644 --- a/src/drivers/barometer/bmp388/bmp388_spi.cpp +++ b/src/drivers/barometer/bmp388/bmp388_spi.cpp @@ -66,7 +66,7 @@ class BMP388_SPI: public device::SPI, public IBMP388 int init(); - uint8_t get_reg(uint8_t addr); + int get_reg(uint8_t addr, uint8_t *value); int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); int set_reg(uint8_t value, uint8_t addr); calibration_s *get_calibration(uint8_t addr); @@ -95,12 +95,16 @@ int BMP388_SPI::init() return SPI::init(); }; -uint8_t BMP388_SPI::get_reg(uint8_t addr) +int BMP388_SPI::get_reg(uint8_t addr, uint8_t *value) { uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit - transfer(&cmd[0], &cmd[0], 2); + int ret = transfer(&cmd[0], &cmd[0], 2); - return cmd[1]; + if (ret == OK) { + *value = cmd[1]; + } + + return ret; } int BMP388_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) diff --git a/src/drivers/barometer/bmp581/CMakeLists.txt b/src/drivers/barometer/bmp581/CMakeLists.txt new file mode 100644 index 000000000000..c5de8e20b3f6 --- /dev/null +++ b/src/drivers/barometer/bmp581/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2024 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_module( + MODULE drivers__barometer__bmp581 + MAIN bmp581 + SRCS + bmp581_spi.cpp + bmp581_i2c.cpp + bmp581.cpp + bmp581_main.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/barometer/bmp581/Kconfig b/src/drivers/barometer/bmp581/Kconfig new file mode 100644 index 000000000000..49f99b08b2b6 --- /dev/null +++ b/src/drivers/barometer/bmp581/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_BAROMETER_BMP581 + bool "bmp581" + default n + ---help--- + Enable support for bmp581 diff --git a/src/drivers/barometer/bmp581/bmp581.cpp b/src/drivers/barometer/bmp581/bmp581.cpp new file mode 100644 index 000000000000..a4a085397c4a --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581.cpp @@ -0,0 +1,727 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 FITNESSf + * 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 "bmp581.h" +#include + +BMP581::BMP581(const I2CSPIDriverConfig &config, IBMP581 *interface) : + I2CSPIDriver(config), + _interface(interface), + _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), + _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), + _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")) +{ +} + +BMP581::~BMP581() +{ + /* free perf counters */ + perf_free(_sample_perf); + perf_free(_measure_perf); + perf_free(_comms_errors); + + delete _interface; +} + +int BMP581::init() +{ + int ret; + + ret = soft_reset(); + + if (ret != PX4_OK) { + PX4_DEBUG("failed to reset baro during init"); + return -EIO; + } + + _chip_id = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR); + + if ((_chip_id != BMP581_CHIP_ID_PRIM) && (_chip_id != BMP581_CHIP_ID_SEC)) { + PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP581_CHIP_ID_PRIM, BMP581_CHIP_ID_SEC); + return -EIO; + } + + _chip_rev_id = _interface->get_reg(BMP5_REG_REV_ID_ADDR); + + ret = set_config(); + + if (ret != PX4_OK) { + PX4_WARN("failed to set_config"); + return -EIO; + } + + start(); + + return PX4_OK; +} + +void BMP581::print_status() +{ + I2CSPIDriverBase::print_status(); + printf("chip id: 0x%02x rev id: 0x%02x\n", _chip_id, _chip_rev_id); + perf_print_counter(_sample_perf); + perf_print_counter(_measure_perf); + perf_print_counter(_comms_errors); + printf("measurement interval: %u us \n", _measure_interval); +} + +void BMP581::start() +{ + _collect_phase = false; + + ScheduleOnInterval(_measure_interval, _measure_interval * 3); +} + +void BMP581::RunImpl() +{ + + if (_collect_phase) { + collect(); + } + + measure(); +} + +int BMP581::measure() +{ + int ret; + + _collect_phase = true; + + perf_begin(_measure_perf); + + /* start measurement */ + ret = set_power_mode(BMP5_POWERMODE_FORCED); + + if (ret != PX4_OK) { + PX4_DEBUG("failed to set power mode"); + perf_count(_comms_errors); + perf_cancel(_measure_perf); + return -EIO; + } + + perf_end(_measure_perf); + + return PX4_OK; +} + +int BMP581::collect() +{ + _collect_phase = false; + + bmp5_sensor_data data{}; + + uint8_t int_status; + int ret; + + perf_begin(_sample_perf); + + /* this should be fairly close to the end of the conversion, so the best approximation of the time */ + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + int_status = get_interrupt_status(); + + if (int_status & BMP5_INT_ASSERTED_DRDY) { + ret = get_sensor_data(&data); + + if (ret != PX4_OK) { + perf_count(_comms_errors); + perf_cancel(_sample_perf); + return -EIO; + } + } + + //publish + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp_sample = timestamp_sample; + sensor_baro.device_id = _interface->get_device_id(); + sensor_baro.pressure = data.pressure; + sensor_baro.temperature = data.temperature; + sensor_baro.error_count = perf_event_count(_comms_errors); + sensor_baro.timestamp = hrt_absolute_time(); + _sensor_baro_pub.publish(sensor_baro); + + perf_end(_sample_perf); + + return PX4_OK; +} + +/*! + * @brief This API performs the soft reset of the sensor + * + * https://github.com/boschsensortec/BMP5-Sensor-API/blob/master/bmp5.c + */ +int BMP581::soft_reset() +{ + uint8_t status; + int ret; + + if (intf == BMP5_SPI_INTF) { + /* Performing a single read via SPI of registers, + * e.g. registers CHIP_ID, before the actual + * SPI communication with the device. + */ + status = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR); + } + + ret = _interface->set_reg(BMP5_SOFT_RESET_CMD, BMP5_REG_CMD_ADDR); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + usleep(BMP5_DELAY_US_SOFT_RESET); + + if (intf == BMP5_SPI_INTF) { + /* Performing a single read via SPI of registers, + * e.g. registers CHIP_ID, before the actual + * SPI communication with the device. + */ + status = _interface->get_reg(BMP5_REG_CHIP_ID_ADDR); + } + + status = _interface->get_reg(BMP5_REG_STATUS_ADDR); + + /* Check if nvm_rdy status = 1 and nvm_err status = 0 to proceed */ + if (!((status & BMP5_INT_NVM_RDY) && (!(status & BMP5_INT_NVM_ERR)))) { + return PX4_ERROR; + } + + status = _interface->get_reg(BMP5_REG_INT_STATUS_ADDR); + + if (status & BMP5_INT_ASSERTED_POR_SOFTRESET_COMPLETE) { + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int BMP581::set_config() +{ + int ret; + + ret = set_power_mode(BMP5_POWERMODE_STANDBY); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + ret = set_osr_odr_press_config(); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + ret = set_iir_config(); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + ret = configure_interrupt(); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + ret = int_source_select(); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +/*! + * @brief This API is used to get interrupt status. + */ +uint8_t BMP581::get_interrupt_status() +{ + uint8_t status = 0xFF; + + status = _interface->get_reg(BMP5_REG_INT_STATUS_ADDR); + + return status; +} + +uint8_t BMP581::check_deepstandby_mode() +{ + int rslt; + uint8_t fifo_frame_sel; + uint8_t reg_data[2]; + uint8_t powermode = 0xFF; + + fifo_frame_sel = _interface->get_reg(BMP5_REG_FIFO_SEL_ADDR); + fifo_frame_sel = BMP5_GET_BITS_POS_0(fifo_frame_sel, BMP5_FIFO_FRAME_SEL); + + rslt = _interface->get_reg_buf(BMP5_REG_OSR_CONFIG_ADDR, reg_data, 2); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + uint8_t odr_reg = 0xFF; + odr_reg = BMP5_GET_BITSLICE(reg_data[1], BMP5_ODR); + + rslt = _interface->get_reg_buf(BMP5_REG_DSP_CONFIG_ADDR, reg_data, 2); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + uint8_t set_iir_t_reg = 0xFF; + uint8_t set_iir_p_reg = 0xFF; + + set_iir_t_reg = BMP5_GET_BITS_POS_0(reg_data[1], BMP5_SET_IIR_TEMP); + set_iir_p_reg = BMP5_GET_BITSLICE(reg_data[1], BMP5_SET_IIR_PRESS); + + /* As per datasheet odr should be less than 5Hz. But register value for 5Hz is less than 4Hz and so, + * thus in this below condition odr is checked whether greater than 5Hz macro. + */ + if ((odr_reg > BMP5_ODR_05_HZ) && (fifo_frame_sel == BMP5_DISABLE) && + (set_iir_t_reg == BMP5_IIR_FILTER_BYPASS) && (set_iir_p_reg == BMP5_IIR_FILTER_BYPASS)) { + powermode = BMP5_POWERMODE_DEEP_STANDBY; + } + + return powermode; +} + +/*! + * @brief This API is used to get powermode of the sensor. + */ +uint8_t BMP581::get_power_mode() +{ + uint8_t deep_dis; + uint8_t reg_data; + uint8_t powermode; + + reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR); + powermode = BMP5_GET_BITS_POS_0(reg_data, BMP5_POWERMODE); + + if (powermode == BMP5_POWERMODE_STANDBY) { + deep_dis = BMP5_GET_BITSLICE(reg_data, BMP5_DEEP_DISABLE); + + if (deep_dis == BMP5_DEEP_ENABLED) { + powermode = check_deepstandby_mode(); + } + } + + return powermode; +} + +/*! + * @brief This API is used to set powermode of the sensor. + */ +int BMP581::set_power_mode(uint8_t power_mode) +{ + uint8_t lst_pwrmode; + uint8_t reg_data; + int rslt; + + lst_pwrmode = get_power_mode(); + + if (lst_pwrmode != BMP5_POWERMODE_STANDBY) { + reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_DEEP_DISABLE, BMP5_DEEP_DISABLED); + reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_POWERMODE, BMP5_POWERMODE_STANDBY); + rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + ScheduleDelayed(BMP5_DELAY_US_STANDBY); + } + + switch (power_mode) { + case BMP5_POWERMODE_DEEP_STANDBY: + rslt = set_deep_standby_mode(); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + break; + + case BMP5_POWERMODE_STANDBY: + break; + + case BMP5_POWERMODE_NORMAL: + case BMP5_POWERMODE_FORCED: + case BMP5_POWERMODE_CONTINOUS: + reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_DEEP_DISABLE, BMP5_DEEP_DISABLED); + reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_POWERMODE, power_mode); + rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + break; + } + + return PX4_OK; +} + +/*! + * @brief This internal API is used to set sensor in deep standby mode. + */ +int BMP581::set_deep_standby_mode() +{ + uint8_t reg_data; + int rslt; + + reg_data = _interface->get_reg(BMP5_REG_ODR_CONFIG_ADDR); + reg_data = BMP5_SET_BIT_VAL_0(reg_data, BMP5_DEEP_DISABLE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_ODR, BMP5_ODR_01_HZ); + rslt = _interface->set_reg(reg_data, BMP5_REG_ODR_CONFIG_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + reg_data = _interface->get_reg(BMP5_REG_DSP_IIR_ADDR); + reg_data = reg_data & BMP5_IIR_BYPASS; + rslt = _interface->set_reg(reg_data, BMP5_REG_DSP_IIR_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + reg_data = _interface->get_reg(BMP5_REG_FIFO_SEL_ADDR); + reg_data = BMP5_SET_BIT_VAL_0(reg_data, BMP5_FIFO_FRAME_SEL); + rslt = _interface->set_reg(reg_data, BMP5_REG_FIFO_SEL_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +/*! + * @brief This internal API is used to set sensor in standby powermode when powermode is deepstandby mode. + */ +int BMP581::set_standby_mode() +{ + uint8_t powermode; + int ret; + + powermode = get_power_mode(); + + if (powermode == BMP5_POWERMODE_DEEP_STANDBY) { + ret = set_power_mode(BMP5_POWERMODE_STANDBY); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + } + + return PX4_OK; +} + +uint32_t BMP581::get_measurement_time() +{ + /* + From BST-BMP3581-DS004-04.pdf, page 19, table 9 + + Pressure Temperature Measurement + Oversample Oversample Time + 1x 1x 2.7 ms + 2x 1x 3.3 ms + 4x 1x 4.6 ms + 8x 1x 7.2 ms + 16x 1x 12.5 ms + 32x 2x 23.3 ms + 64x 4x 44.2 ms + 128x 8x 88.0 ms + */ + + uint32_t meas_time_us = 0; // unsupported value by default + + if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_1X) { + switch (OVERSAMPLING_PRESSURE) { + case BMP5_OVERSAMPLING_1X: + meas_time_us = 2700; + break; + + case BMP5_OVERSAMPLING_2X: + meas_time_us = 3300; + break; + + case BMP5_OVERSAMPLING_4X: + meas_time_us = 4600; + break; + + case BMP5_OVERSAMPLING_8X: + meas_time_us = 7200; + break; + + case BMP5_OVERSAMPLING_16X: + meas_time_us = 12500; + break; + } + + } else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_2X) { + switch (OVERSAMPLING_PRESSURE) { + case BMP5_OVERSAMPLING_32X: + meas_time_us = 23300; + break; + + } + + } else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_4X) { + switch (OVERSAMPLING_PRESSURE) { + case BMP5_OVERSAMPLING_64X: + meas_time_us = 44200; + break; + + } + + } else if (OVERSAMPLING_TEMPERATURE == BMP5_OVERSAMPLING_8X) { + switch (OVERSAMPLING_PRESSURE) { + case BMP5_OVERSAMPLING_128X: + meas_time_us = 88000; + break; + + } + } + + return meas_time_us; +} + +/*! + * @brief This API sets the configuration for oversampling temperature, oversampling of + * pressure and ODR configuration along with pressure enable. + * + * @note If ODR is set to a value higher than 5Hz then powermode is set as standby mode, as ODR value greater than 5HZ + * without disabling deep-standby mode makes powermode invalid. + */ +int BMP581::set_osr_odr_press_config() +{ + uint8_t reg_data[2] = {0}; + int rslt; + + _measure_interval = get_measurement_time(); + + if (_measure_interval == 0) { + PX4_WARN("unsupported oversampling selected"); + return false; + } + + if (OUTPUT_DATA_RATE < BMP5_ODR_05_HZ) { + rslt = set_standby_mode(); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + } + + rslt = _interface->get_reg_buf(BMP5_REG_OSR_CONFIG_ADDR, reg_data, 2); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + reg_data[0] = BMP5_SET_BITS_POS_0(reg_data[0], BMP5_TEMP_OS, OVERSAMPLING_TEMPERATURE); + reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_PRESS_OS, OVERSAMPLING_PRESSURE); + reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_PRESS_EN, PRESSURE_ENABLE); + reg_data[1] = BMP5_SET_BITSLICE(reg_data[1], BMP5_ODR, OUTPUT_DATA_RATE); + + rslt = _interface->set_reg(reg_data[0], BMP5_REG_OSR_CONFIG_ADDR); + rslt = _interface->set_reg(reg_data[1], BMP5_REG_ODR_CONFIG_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +/*! + * @brief This API sets the configuration for IIR of temperature and pressure. + * + * @note If IIR value for both temperature and pressure is set a value other than bypass then powermode is set + * as standby mode, as IIR with value other than bypass without disabling deep-standby mode makes powermode invalid. + */ +int BMP581::set_iir_config() +{ + uint8_t curr_powermdoe; + uint8_t reg_data[2]; + int rslt; + + if ((IIR_FILTER_COEFF_TEMPERATURE != BMP5_IIR_FILTER_BYPASS) || (IIR_FILTER_COEFF_PRESSURE != BMP5_IIR_FILTER_BYPASS)) { + rslt = set_standby_mode(); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + } + + curr_powermdoe = get_power_mode(); + + if (curr_powermdoe != BMP5_POWERMODE_STANDBY) { + rslt = set_power_mode(BMP5_POWERMODE_STANDBY); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + } + + rslt = _interface->get_reg_buf(BMP5_REG_DSP_CONFIG_ADDR, reg_data, 2); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_SHDW_SET_IIR_TEMP, BMP5_ENABLE); + reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_SHDW_SET_IIR_PRESS, BMP5_ENABLE); + reg_data[0] = BMP5_SET_BITSLICE(reg_data[0], BMP5_IIR_FLUSH_FORCED_EN, BMP5_ENABLE); + + reg_data[1] = IIR_FILTER_COEFF_TEMPERATURE; + reg_data[1] = BMP5_SET_BITSLICE(reg_data[1], BMP5_SET_IIR_PRESS, IIR_FILTER_COEFF_PRESSURE); + + rslt = _interface->set_reg(reg_data[0], BMP5_REG_DSP_CONFIG_ADDR); + rslt = _interface->set_reg(reg_data[1], BMP5_REG_DSP_IIR_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + /* Since IIR works only in standby mode we are not re-writing to deepstandby mode + * as deep standby mode resets the IIR settings to default + */ + if ((curr_powermdoe != BMP5_POWERMODE_STANDBY) && (curr_powermdoe != BMP5_POWERMODE_DEEP_STANDBY)) { + if (!set_power_mode(curr_powermdoe)) { + return PX4_ERROR; + } + + } + + return PX4_OK; +} + +/*! +* @brief This API is used to configure the interrupt settings. +*/ +int BMP581::configure_interrupt() +{ + uint8_t reg_data = 0; + uint8_t int_source = 0; + int rslt; + + reg_data = _interface->get_reg(BMP5_REG_INT_CONFIG_ADDR); + + /* Any change between latched/pulsed mode has to be applied while interrupt is disabled */ + /* Step 1 : Turn off all INT sources (INT_SOURCE -> 0x00) */ + rslt = _interface->set_reg(int_source, BMP5_REG_INT_SOURCE_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + /* Step 2 : Read the INT_STATUS register to clear the status */ + _interface->get_reg(BMP5_REG_INT_STATUS_ADDR); + + /* Step 3 : Set the desired mode in INT_CONFIG.int_mode */ + reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_INT_MODE, INTERRUPT_MODE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_POL, INTERRUPT_POLARITY); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_OD, INTERRUPT_DRIVE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_EN, BMP5_ENABLE); + + rslt = _interface->set_reg(reg_data, BMP5_REG_INT_CONFIG_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +int BMP581::int_source_select() +{ + uint8_t reg_data; + int rslt; + + reg_data = _interface->get_reg(BMP5_REG_INT_SOURCE_ADDR); + + reg_data = BMP5_SET_BITS_POS_0(reg_data, BMP5_INT_DRDY_EN, BMP5_ENABLE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_FIFO_FULL_EN, BMP5_DISABLE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_FIFO_THRES_EN, BMP5_DISABLE); + reg_data = BMP5_SET_BITSLICE(reg_data, BMP5_INT_OOR_PRESS_EN, BMP5_DISABLE); + + rslt = _interface->set_reg(reg_data, BMP5_REG_INT_SOURCE_ADDR); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + return PX4_OK; +} + +/*! + * @brief This API reads the temperature(deg C) or both pressure(Pa) and temperature(deg C) data from the + * sensor and store it in the bmp5_sensor_data structure instance passed by the user. + */ +int BMP581::get_sensor_data(bmp5_sensor_data *sensor_data) +{ + uint8_t reg_data[6] = {0}; + int32_t raw_data_t; + uint32_t raw_data_p; + int8_t rslt; + + rslt = _interface->get_reg_buf(BMP5_REG_TEMP_DATA_XLSB_ADDR, reg_data, 6); + + if (rslt != PX4_OK) { + return PX4_ERROR; + } + + raw_data_t = (int32_t)((int32_t)((uint32_t)(((uint32_t)reg_data[2] << 16) | ((uint16_t)reg_data[1] << 8) | reg_data[0]) + << 8) >> 8); + /* Division by 2^16(whose equivalent value is 65536) is performed to get temperature data in deg C */ + sensor_data->temperature = (float)(raw_data_t / 65536.0); + + if (PRESSURE_ENABLE == BMP5_ENABLE) { + raw_data_p = (uint32_t)((uint32_t)(reg_data[5] << 16) | (uint16_t)(reg_data[4] << 8) | reg_data[3]); + /* Division by 2^6(whose equivalent value is 64) is performed to get pressure data in Pa */ + sensor_data->pressure = (float)(raw_data_p / 64.0); + + } else { + sensor_data->pressure = 0.0; + } + + return PX4_OK; +} diff --git a/src/drivers/barometer/bmp581/bmp581.h b/src/drivers/barometer/bmp581/bmp581.h new file mode 100644 index 000000000000..56d87fa3f1df --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581.h @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 bmp581.h + * + * Shared defines for the bmp581 driver. + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "board_config.h" + +// From https://github.com/boschsensortec/BMP5-Sensor-API/blob/master/bmp5_defs.h + +/* BIT SLICE GET AND SET FUNCTIONS */ +#define BMP5_GET_BITSLICE(regvar, bitname) \ + ((regvar & bitname##_MSK) >> bitname##_POS) + +#define BMP5_SET_BITSLICE(regvar, bitname, val) \ + ((regvar & ~bitname##_MSK) | \ + ((val << bitname##_POS) & bitname##_MSK)) + +#define BMP5_SET_BIT_VAL_0(reg_data, bitname) (reg_data & ~(bitname##_MSK)) + +#define BMP5_SET_BITS_POS_0(reg_data, bitname, data) \ + ((reg_data & ~(bitname##_MSK)) | \ + (data & bitname##_MSK)) + +#define BMP5_GET_BITS_POS_0(reg_data, bitname) (reg_data & (bitname##_MSK)) + +/* Chip id of BMP5 */ +#define BMP581_CHIP_ID_PRIM (0x50) +#define BMP581_CHIP_ID_SEC (0x51) + +#define BMP5_ENABLE (0x01) +#define BMP5_DISABLE (0x00) + +/* Register addresses */ +#define BMP5_REG_CHIP_ID_ADDR (0x01) +#define BMP5_REG_REV_ID_ADDR (0x02) +#define BMP5_REG_INT_CONFIG_ADDR (0x14) +#define BMP5_REG_INT_SOURCE_ADDR (0x15) +#define BMP5_REG_FIFO_SEL_ADDR (0x18) +#define BMP5_REG_TEMP_DATA_XLSB_ADDR (0x1D) +#define BMP5_REG_INT_STATUS_ADDR (0x27) +#define BMP5_REG_STATUS_ADDR (0x28) +#define BMP5_REG_NVM_ADDR (0x2B) +#define BMP5_REG_DSP_CONFIG_ADDR (0x30) +#define BMP5_REG_DSP_IIR_ADDR (0x31) +#define BMP5_REG_OSR_CONFIG_ADDR (0x36) +#define BMP5_REG_ODR_CONFIG_ADDR (0x37) +#define BMP5_REG_CMD_ADDR (0x7E) + +/* Delay definition */ +#define BMP5_DELAY_US_SOFT_RESET (2000) +#define BMP5_DELAY_US_STANDBY (2500) + +/* Soft reset command */ +#define BMP5_SOFT_RESET_CMD (0xB6) + +/* Deepstandby enable/disable */ +#define BMP5_DEEP_ENABLED (0) +#define BMP5_DEEP_DISABLED (1) + +/* ODR settings */ +#define BMP5_ODR_50_HZ (0x0F) +#define BMP5_ODR_05_HZ (0x18) +#define BMP5_ODR_01_HZ (0x1C) + + +/* Oversampling for temperature and pressure */ +#define BMP5_OVERSAMPLING_1X (0x00) +#define BMP5_OVERSAMPLING_2X (0x01) +#define BMP5_OVERSAMPLING_4X (0x02) +#define BMP5_OVERSAMPLING_8X (0x03) +#define BMP5_OVERSAMPLING_16X (0x04) +#define BMP5_OVERSAMPLING_32X (0x05) +#define BMP5_OVERSAMPLING_64X (0x06) +#define BMP5_OVERSAMPLING_128X (0x07) + +/* IIR filter for temperature and pressure */ +#define BMP5_IIR_FILTER_BYPASS (0x00) +#define BMP5_IIR_FILTER_COEFF_1 (0x01) + +/* Macro is used to bypass both iir_t and iir_p together */ +#define BMP5_IIR_BYPASS (0xC0) + +/* Interrupt configurations */ +#define BMP5_INT_MODE_PULSED (0) +#define BMP5_INT_POL_ACTIVE_HIGH (1) +#define BMP5_INT_OD_PUSHPULL (0) + +/* NVM and Interrupt status asserted macros */ +#define BMP5_INT_ASSERTED_POR_SOFTRESET_COMPLETE (0x10) +#define BMP5_INT_NVM_RDY (0x02) +#define BMP5_INT_NVM_ERR (0x04) + +/* Interrupt configurations */ +#define BMP5_INT_MODE_MSK (0x01) + +#define BMP5_INT_POL_MSK (0x02) +#define BMP5_INT_POL_POS (1) + +#define BMP5_INT_OD_MSK (0x04) +#define BMP5_INT_OD_POS (2) + +#define BMP5_INT_EN_MSK (0x08) +#define BMP5_INT_EN_POS (3) + +#define BMP5_INT_DRDY_EN_MSK (0x01) + +#define BMP5_INT_FIFO_FULL_EN_MSK (0x02) +#define BMP5_INT_FIFO_FULL_EN_POS (1) + +#define BMP5_INT_FIFO_THRES_EN_MSK (0x04) +#define BMP5_INT_FIFO_THRES_EN_POS (2) + +#define BMP5_INT_OOR_PRESS_EN_MSK (0x08) +#define BMP5_INT_OOR_PRESS_EN_POS (3) + +/* ODR configuration */ +#define BMP5_ODR_MSK (0x7C) +#define BMP5_ODR_POS (2) + +/* OSR configurations */ +#define BMP5_TEMP_OS_MSK (0x07) + +#define BMP5_PRESS_OS_MSK (0x38) +#define BMP5_PRESS_OS_POS (3) + +/* Pressure enable */ +#define BMP5_PRESS_EN_MSK (0x40) +#define BMP5_PRESS_EN_POS (6) + +/* IIR configurations */ +#define BMP5_SET_IIR_TEMP_MSK (0x07) + +#define BMP5_SET_IIR_PRESS_MSK (0x38) +#define BMP5_SET_IIR_PRESS_POS (3) + +#define BMP5_SHDW_SET_IIR_TEMP_MSK (0x08) +#define BMP5_SHDW_SET_IIR_TEMP_POS (3) + +#define BMP5_SHDW_SET_IIR_PRESS_MSK (0x20) +#define BMP5_SHDW_SET_IIR_PRESS_POS (5) + +#define BMP5_IIR_FLUSH_FORCED_EN_MSK (0x04) +#define BMP5_IIR_FLUSH_FORCED_EN_POS (2) + +/* Powermode */ +#define BMP5_POWERMODE_MSK (0x03) + +#define BMP5_DEEP_DISABLE_MSK (0x80) +#define BMP5_DEEP_DISABLE_POS (7) + +/* Fifo configurations */ +#define BMP5_FIFO_FRAME_SEL_MSK (0x03) + +/* NVM and Interrupt status asserted macros */ +#define BMP5_INT_ASSERTED_DRDY (0x01) + +/*! + * @brief Enumerator for powermode selection + */ +enum bmp5_powermode { + BMP5_POWERMODE_STANDBY, + BMP5_POWERMODE_NORMAL, + BMP5_POWERMODE_FORCED, + BMP5_POWERMODE_CONTINOUS, + BMP5_POWERMODE_DEEP_STANDBY +}; + +/*! + * @brief Enumerator for interface selection + */ +enum bmp5_intf { + BMP5_SPI_INTF, + BMP5_I2C_INTF, +}; + +/*! + * @brief BMP5 sensor data structure which comprises of temperature and pressure in floating point with data type as + * float for pressure and temperature. + */ +struct bmp5_sensor_data { + float pressure; + float temperature; +}; + +/* + * BMP581 internal constants and data structures. + */ +class IBMP581 +{ +public: + virtual ~IBMP581() = default; + + virtual int init() = 0; + + // read reg value + virtual uint8_t get_reg(uint8_t addr) = 0; + + // bulk read reg value + virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0; + + // write reg value + virtual int set_reg(uint8_t value, uint8_t addr) = 0; + + virtual uint32_t get_device_id() const = 0; + + virtual uint8_t get_device_address() const = 0; +}; + +class BMP581 : public I2CSPIDriver +{ +public: + BMP581(const I2CSPIDriverConfig &config, IBMP581 *interface); + virtual ~BMP581(); + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + virtual int init(); + + void print_status(); + + void RunImpl(); + + +private: + static constexpr uint8_t OVERSAMPLING_TEMPERATURE{BMP5_OVERSAMPLING_2X}; + static constexpr uint8_t OVERSAMPLING_PRESSURE{BMP5_OVERSAMPLING_32X}; + static constexpr uint8_t OUTPUT_DATA_RATE{BMP5_ODR_50_HZ}; + static constexpr uint8_t PRESSURE_ENABLE{BMP5_ENABLE}; + static constexpr uint8_t IIR_FILTER_COEFF_TEMPERATURE{BMP5_IIR_FILTER_COEFF_1}; + static constexpr uint8_t IIR_FILTER_COEFF_PRESSURE{BMP5_IIR_FILTER_COEFF_1}; + + static constexpr uint8_t INTERRUPT_MODE{BMP5_INT_MODE_PULSED}; + static constexpr uint8_t INTERRUPT_POLARITY{BMP5_INT_POL_ACTIVE_HIGH}; + static constexpr uint8_t INTERRUPT_DRIVE{BMP5_INT_OD_PUSHPULL}; + + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + IBMP581 *_interface{nullptr}; + + unsigned _measure_interval{0}; // interval in microseconds needed to measure + + perf_counter_t _sample_perf; + perf_counter_t _measure_perf; + perf_counter_t _comms_errors; + + bool _collect_phase{false}; + + uint8_t _chip_id{0}; + uint8_t _chip_rev_id{0}; + + void start(); + int measure(); + int collect(); // get results and publish + + uint32_t get_measurement_time(); + + int soft_reset(); + int set_config(); + uint8_t get_interrupt_status(); + int configure_interrupt(); + int int_source_select(); + uint8_t get_power_mode(); + int set_power_mode(uint8_t power_mode); + uint8_t check_deepstandby_mode(); + int set_standby_mode(); + int set_deep_standby_mode(); + int set_osr_odr_press_config(); + int set_iir_config(); + int get_sensor_data(bmp5_sensor_data *sensor_data); +}; + + +/* interface factories */ +extern IBMP581 *bmp581_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode); +extern IBMP581 *bmp581_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency); +extern enum bmp5_intf intf; diff --git a/src/modules/ekf2/ekf2_params_selector.c b/src/drivers/barometer/bmp581/bmp581_i2c.cpp similarity index 53% rename from src/modules/ekf2/ekf2_params_selector.c rename to src/drivers/barometer/bmp581/bmp581_i2c.cpp index 43a630fe8fb3..92a4d7e79d77 100644 --- a/src/modules/ekf2/ekf2_params_selector.c +++ b/src/drivers/barometer/bmp581/bmp581_i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -32,50 +32,63 @@ ****************************************************************************/ /** - * Selector error reduce threshold + * @file bmp581_i2c.cpp * - * EKF2 instances have to be better than the selected by at least this amount before their relative score can be reduced. - * - * @group EKF2 + * I2C interface for BMP581 */ -PARAM_DEFINE_FLOAT(EKF2_SEL_ERR_RED, 0.2f); -/** - * Selector angular rate threshold - * - * EKF2 selector angular rate error threshold for comparing gyros. Angular rate vector differences larger than this will result in accumulated angular error. - * - * @group EKF2 - * @unit deg/s - */ -PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_RAT, 7.0f); +#include -/** - * Selector angular threshold. - * - * EKF2 selector maximum accumulated angular error threshold for comparing gyros. Accumulated angular error larger than this will result in the sensor being declared faulty. - * - * @group EKF2 - * @unit deg - */ -PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ANG, 15.0f); +#include "bmp581.h" -/** - * Selector acceleration threshold - * - * EKF2 selector acceleration error threshold for comparing accelerometers. Acceleration vector differences larger than this will result in accumulated velocity error. - * - * @group EKF2 - * @unit m/s^2 - */ -PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_ACC, 1.0f); +class BMP581_I2C: public device::I2C, public IBMP581 +{ +public: + BMP581_I2C(uint8_t bus, uint32_t device, int bus_frequency); + virtual ~BMP581_I2C() = default; -/** - * Selector angular threshold. - * - * EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. Accumulated velocity error larger than this will result in the sensor being declared faulty. - * - * @group EKF2 - * @unit m/s - */ -PARAM_DEFINE_FLOAT(EKF2_SEL_IMU_VEL, 2.0f); + int init(); + + uint8_t get_reg(uint8_t addr); + int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); + int set_reg(uint8_t value, uint8_t addr); + + uint32_t get_device_id() const override { return device::I2C::get_device_id(); } + + uint8_t get_device_address() const override { return device::I2C::get_device_address(); } +}; + +IBMP581 *bmp581_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency) +{ + return new BMP581_I2C(busnum, device, bus_frequency); +} + +BMP581_I2C::BMP581_I2C(uint8_t bus, uint32_t device, int bus_frequency) : + I2C(DRV_BARO_DEVTYPE_BMP581, MODULE_NAME, bus, device, bus_frequency) +{ +} + +int BMP581_I2C::init() +{ + return I2C::init(); +} + +uint8_t BMP581_I2C::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), 0}; + transfer(&cmd[0], 1, &cmd[1], 1); + + return cmd[1]; +} + +int BMP581_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) +{ + const uint8_t cmd = (uint8_t)(addr); + return transfer(&cmd, sizeof(cmd), buf, len); +} + +int BMP581_I2C::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr), value}; + return transfer(cmd, sizeof(cmd), nullptr, 0); +} diff --git a/src/drivers/barometer/bmp581/bmp581_main.cpp b/src/drivers/barometer/bmp581/bmp581_main.cpp new file mode 100644 index 000000000000..d92ba376ac89 --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581_main.cpp @@ -0,0 +1,127 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "bmp581.h" + +enum bmp5_intf intf; + +void BMP581::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmp581", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("baro"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x46); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *BMP581::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + IBMP581 *interface = nullptr; + + if (config.bus_type == BOARD_I2C_BUS) { + interface = bmp581_i2c_interface(config.bus, config.i2c_address, config.bus_frequency); + intf = BMP5_I2C_INTF; + + } else if (config.bus_type == BOARD_SPI_BUS) { + interface = bmp581_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode); + intf = BMP5_SPI_INTF; + } + + if (interface == nullptr) { + PX4_ERR("failed creating interface for bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%" PRIx32 ")", config.bus, config.spi_devid); + return nullptr; + } + + BMP581 *dev = new BMP581(config, interface); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (dev->init() != OK) { + delete dev; + return nullptr; + } + + return dev; +} + + +extern "C" int bmp581_main(int argc, char *argv[]) +{ + using ThisDriver = BMP581; + BusCLIArguments cli{true, true}; + cli.i2c_address = 0x46; + cli.default_i2c_frequency = 100 * 1000; + cli.default_spi_frequency = 10 * 1000 * 1000; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_BMP581); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; + + + + +} diff --git a/src/drivers/barometer/bmp581/bmp581_spi.cpp b/src/drivers/barometer/bmp581/bmp581_spi.cpp new file mode 100644 index 000000000000..cda214e623bd --- /dev/null +++ b/src/drivers/barometer/bmp581/bmp581_spi.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 bmp581_spi.cpp + * + * SPI interface for BMP 581 (NOTE: untested!) + */ + +#include + +#include "bmp581.h" + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) //for set +#define DIR_WRITE ~(1<<7) //for clear + +class BMP581_SPI: public device::SPI, public IBMP581 +{ +public: + BMP581_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); + virtual ~BMP581_SPI() = default; + + int init(); + + uint8_t get_reg(uint8_t addr); + int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len); + int set_reg(uint8_t value, uint8_t addr); + + uint32_t get_device_id() const override { return device::SPI::get_device_id(); } + + uint8_t get_device_address() const override { return device::SPI::get_device_address(); } +}; + +IBMP581 *bmp581_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode) +{ + return new BMP581_SPI(busnum, device, bus_frequency, spi_mode); +} + +BMP581_SPI::BMP581_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : + SPI(DRV_BARO_DEVTYPE_BMP581, MODULE_NAME, bus, device, spi_mode, bus_frequency) +{ +} + +int BMP581_SPI::init() +{ + return SPI::init(); +} + +uint8_t BMP581_SPI::get_reg(uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit + transfer(&cmd[0], &cmd[0], 2); + return cmd[1]; +} + +int BMP581_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) +{ + uint8_t cmd[len + 1] = {(uint8_t)(addr | DIR_READ)}; + int ret; + + ret = transfer(&cmd[0], &cmd[0], (len + 1)); + memcpy(buf, &cmd[1], len); + + return ret; +} + +int BMP581_SPI::set_reg(uint8_t value, uint8_t addr) +{ + uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; //clear MSB bit + return transfer(&cmd[0], nullptr, 2); +} diff --git a/src/drivers/barometer/invensense/icp10100/Kconfig b/src/drivers/barometer/invensense/icp10100/Kconfig deleted file mode 100644 index 2f883bccd420..000000000000 --- a/src/drivers/barometer/invensense/icp10100/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP10100 - bool "icp10100" - default n - ---help--- - Enable support for icp10100 diff --git a/src/drivers/barometer/invensense/icp10111/Kconfig b/src/drivers/barometer/invensense/icp10111/Kconfig deleted file mode 100644 index 1681c07d3831..000000000000 --- a/src/drivers/barometer/invensense/icp10111/Kconfig +++ /dev/null @@ -1,6 +0,0 @@ -menuconfig DRIVERS_BAROMETER_INVENSENSE_ICP10111 - bool "icp10100" - default n - ---help--- - Enable support for icp10111 - diff --git a/src/drivers/barometer/ms5837/MS5837.cpp b/src/drivers/barometer/ms5837/MS5837.cpp index 7041d886d937..00382070dedf 100644 --- a/src/drivers/barometer/ms5837/MS5837.cpp +++ b/src/drivers/barometer/ms5837/MS5837.cpp @@ -340,7 +340,7 @@ int MS5837::_collect() sensor_baro.timestamp_sample = timestamp_sample; sensor_baro.device_id = get_device_id(); sensor_baro.pressure = P; - sensor_baro.temperature = T; + sensor_baro.temperature = _last_temperature; sensor_baro.error_count = perf_event_count(_comms_errors); sensor_baro.timestamp = hrt_absolute_time(); _sensor_baro_pub.publish(sensor_baro); diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 913bbbe7b82a..194b88fa94c0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -116,13 +116,11 @@ void BATT_SMBUS::RunImpl() // Convert millivolts to volts. new_report.voltage_v = ((float)result) / 1000.0f; - new_report.voltage_filtered_v = new_report.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f) * _c_mult; - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp index 4f49e5692baf..33d89f3c0166 100644 --- a/src/drivers/camera_trigger/camera_trigger.cpp +++ b/src/drivers/camera_trigger/camera_trigger.cpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include @@ -924,7 +925,45 @@ CameraTrigger::status() static int usage() { - PX4_INFO("usage: camera_trigger {start|stop|status|test|test_power}\n"); + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Camera trigger driver. + +This module triggers cameras that are connected to the flight-controller outputs, +or simple MAVLink cameras that implement the MAVLink trigger protocol. + +The driver responds to the following MAVLink trigger commands being found in missions or recieved over MAVLink: + +- `MAV_CMD_DO_TRIGGER_CONTROL` +- `MAV_CMD_DO_DIGICAM_CONTROL` +- `MAV_CMD_DO_SET_CAM_TRIGG_DIST` +- `MAV_CMD_OBLIQUE_SURVEY` + +The commands cause the driver to trigger camera image capture based on time or distance. +Each time an image capture is triggered, the `CAMERA_TRIGGER` MAVLink message is emitted. + +A "simple MAVLink camera" is one that supports the above command set. +When configured for this kind of camera, all the driver does is emit the `CAMERA_TRIGGER` MAVLink message as expected. +The incoming commands must be forwarded to the MAVLink camera, and are automatically emitted to MAVLink channels +when found in missions. + +The driver is configured using [Camera Trigger parameters](../advanced_config/parameter_reference.md#camera-trigger). +In particular: + +- `TRIG_INTERFACE` - How the camera is connected to flight controller (PWM, GPIO, Seagull, MAVLink) +- `TRIG_MODE` - Distance or time based triggering, with values set by `TRIG_DISTANCE` and `TRIG_INTERVAL`. + +[Setup/usage information](../camera/index.md). +)DESCR_STR"); + PRINT_MODULE_USAGE_NAME("camera_trigger", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("camera"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop","Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status","Print driver status information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test","Trigger one image (not logged or forwarded to GCS)"); + PRINT_MODULE_USAGE_COMMAND_DESCR("test_power","Toggle power"); return 1; } diff --git a/src/drivers/camera_trigger/camera_trigger_params.c b/src/drivers/camera_trigger/camera_trigger_params.c index 181c212e6a2d..3841e20e5642 100644 --- a/src/drivers/camera_trigger/camera_trigger_params.c +++ b/src/drivers/camera_trigger/camera_trigger_params.c @@ -163,4 +163,3 @@ PARAM_DEFINE_INT32(TRIG_PWM_SHOOT, 1900); * @reboot_required true */ PARAM_DEFINE_INT32(TRIG_PWM_NEUTRAL, 1500); - diff --git a/src/drivers/camera_trigger/interfaces/src/camera_interface.h b/src/drivers/camera_trigger/interfaces/src/camera_interface.h index f9079ea2bad2..d3c9a699ff4d 100644 --- a/src/drivers/camera_trigger/interfaces/src/camera_interface.h +++ b/src/drivers/camera_trigger/interfaces/src/camera_interface.h @@ -39,8 +39,7 @@ #include #include - -#define arraySize(a) (sizeof((a))/sizeof(((a)[0]))) +#include class CameraInterface { diff --git a/src/drivers/cdcacm_autostart/CMakeLists.txt b/src/drivers/cdcacm_autostart/CMakeLists.txt new file mode 100644 index 000000000000..8e78ba1cd24c --- /dev/null +++ b/src/drivers/cdcacm_autostart/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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_module( + MODULE drivers__cdcacm_autostart + MAIN cdcacm_autostart + COMPILE_FLAGS + # -DDEBUG_BUILD + SRCS + cdcacm_autostart.cpp + ) diff --git a/src/drivers/cdcacm_autostart/Kconfig b/src/drivers/cdcacm_autostart/Kconfig new file mode 100644 index 000000000000..575d7a31fd18 --- /dev/null +++ b/src/drivers/cdcacm_autostart/Kconfig @@ -0,0 +1,6 @@ +menuconfig DRIVERS_CDCACM_AUTOSTART + bool "cdcacm_autostart" + default n + depends on MODULES_MAVLINK + ---help--- + Enable support for cdcacm_autostart diff --git a/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp b/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp new file mode 100644 index 000000000000..3c963e798820 --- /dev/null +++ b/src/drivers/cdcacm_autostart/cdcacm_autostart.cpp @@ -0,0 +1,664 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#if defined(CONFIG_SYSTEM_CDCACM) + +#include "cdcacm_autostart.h" + +__BEGIN_DECLS +#include +#include + +extern int sercon_main(int c, char **argv); +extern int serdis_main(int c, char **argv); +__END_DECLS + +#include + +#define USB_DEVICE_PATH "/dev/ttyACM0" + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) +# undef SERIAL_PASSTHRU_UBLOX_DEV +# if defined(CONFIG_SERIAL_PASSTHRU_GPS1) && defined(CONFIG_BOARD_SERIAL_GPS1) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS1 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS2)&& defined(CONFIG_BOARD_SERIAL_GPS2) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS2 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS3)&& defined(CONFIG_BOARD_SERIAL_GPS3) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS3 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS4)&& defined(CONFIG_BOARD_SERIAL_GPS4) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS4 +# elif defined(CONFIG_SERIAL_PASSTHRU_GPS5) && defined(CONFIG_BOARD_SERIAL_GPS5) +# define SERIAL_PASSTHRU_UBLOX_DEV CONFIG_BOARD_SERIAL_GPS5 +# endif +# if !defined(SERIAL_PASSTHRU_UBLOX_DEV) +# error "CONFIG_SERIAL_PASSTHRU_GPSn and CONFIG_BOARD_SERIAL_GPSn must be defined" +# endif +#endif + +CdcAcmAutostart::CdcAcmAutostart() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) +{} + +CdcAcmAutostart::~CdcAcmAutostart() +{ + PX4_INFO("Stopping CDC/ACM autostart"); + + if (_ttyacm_fd >= 0) { + px4_close(_ttyacm_fd); + } + + ScheduleClear(); +} + +int CdcAcmAutostart::Start() +{ + PX4_INFO("Starting CDC/ACM autostart"); + UpdateParams(true); + + ScheduleNow(); + + return PX4_OK; +} + +void CdcAcmAutostart::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + UpdateParams(); + + run_state_machine(); +} + +void CdcAcmAutostart::run_state_machine() +{ + _reschedule_time = 500_ms; + _vbus_present = (board_read_VBUS_state() == PX4_OK); + + // If the hardware supports RESET lockout that has nArmed ANDed with VBUS + // vbus_sense may drop during a param save which uses + // BOARD_INDICATE_EXTERNAL_LOCKOUT_STATE to prevent external resets + // while writing the params. If we are not armed and nARMRED is low + // we are in such a lock out so ignore changes on VBUS_SENSE during this + // time. +#if defined(BOARD_GET_EXTERNAL_LOCKOUT_STATE) + + if (BOARD_GET_EXTERNAL_LOCKOUT_STATE() == 0) { + _vbus_present = _vbus_present_prev; + ScheduleDelayed(500_ms); + return; + } + +#endif + + // Do not reconfigure USB while flying + actuator_armed_s report; + _actuator_armed_sub.copy(&report); + + if (report.armed) { + _vbus_present_prev = _vbus_present; + + } else { + + switch (_state) { + case UsbAutoStartState::disconnected: + state_disconnected(); + break; + + case UsbAutoStartState::connecting: + state_connecting(); + break; + + case UsbAutoStartState::connected: + state_connected(); + break; + + case UsbAutoStartState::disconnecting: + state_disconnecting(); + break; + } + } + + _vbus_present_prev = _vbus_present; + ScheduleDelayed(_reschedule_time); +} + +void CdcAcmAutostart::state_connected() +{ + if (!_vbus_present && !_vbus_present_prev && (_active_protocol == UsbProtocol::mavlink)) { + PX4_DEBUG("lost vbus!"); + 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); + sched_unlock(); + _state = UsbAutoStartState::disconnecting; + } +} + +void CdcAcmAutostart::state_disconnected() +{ + if (_vbus_present && _vbus_present_prev) { + PX4_DEBUG("starting sercon"); + + if (sercon_main(0, nullptr) == EXIT_SUCCESS) { + _state = UsbAutoStartState::connecting; + PX4_DEBUG("state connecting"); + _reschedule_time = 1_s; + } + + } else if (_vbus_present && !_vbus_present_prev) { + // USB just connected, try again soon + _reschedule_time = 100_ms; + } +} + +void CdcAcmAutostart::state_connecting() +{ + int bytes_available = 0; +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + struct termios uart_config; + speed_t baudrate; +#endif + + if (!_vbus_present) { + PX4_DEBUG("No VBUS"); + goto fail; + } + + if (_ttyacm_fd < 0) { + PX4_DEBUG("opening port"); + _ttyacm_fd = px4_open(USB_DEVICE_PATH, O_RDONLY | O_NONBLOCK); + } + + if (_ttyacm_fd < 0) { + PX4_DEBUG("can't open port"); + // fail silently and keep trying to open the port + return; + } + + if (_sys_usb_auto.get() == 2) { + PX4_INFO("Starting mavlink on %s (SYS_USB_AUTO=2)", USB_DEVICE_PATH); + + if (start_mavlink()) { + _state = UsbAutoStartState::connected; + _active_protocol = UsbProtocol::mavlink; + + } else { + _state = UsbAutoStartState::disconnecting; + _reschedule_time = 100_ms; + } + + return; + + } else if (_sys_usb_auto.get() == 0) { + // Do nothing + _state = UsbAutoStartState::connected; + _active_protocol = UsbProtocol::none; + return; + } + + // Otherwise autodetect + + if ((px4_ioctl(_ttyacm_fd, FIONREAD, &bytes_available) != PX4_OK) || + (bytes_available < 3)) { + PX4_DEBUG("bytes_available: %d", bytes_available); + // Return back to connecting state to check again soon + return; + } + + // Non-blocking read + _bytes_read = px4_read(_ttyacm_fd, _buffer, sizeof(_buffer)); + +#if defined(DEBUG_BUILD) + + if (_bytes_read > 0) { + fprintf(stderr, "%d bytes\n", _bytes_read); + + for (int i = 0; i < _bytes_read; i++) { + fprintf(stderr, "|%X", _buffer[i]); + } + + fprintf(stderr, "\n"); + } + +#endif // DEBUG_BUILD + + if (_bytes_read <= 0) { + PX4_DEBUG("no _bytes_read"); + // Return back to connecting state to check again soon + return; + } + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + // Get the baudrate for serial passthru before closing the port. + tcgetattr(_ttyacm_fd, &uart_config); + baudrate = cfgetspeed(&uart_config); +#endif + PX4_DEBUG("_bytes_read %d", _bytes_read); + px4_close(_ttyacm_fd); + _ttyacm_fd = -1; + + // Parse for mavlink reboot command + if (scan_buffer_for_mavlink_reboot()) { + // Reboot incoming. Return without rescheduling. + return; + } + + // Parse for mavlink heartbeats (v1 and v2). + if (scan_buffer_for_mavlink_heartbeat()) { + if (start_mavlink()) { + _state = UsbAutoStartState::connected; + _active_protocol = UsbProtocol::mavlink; + + } else { + _state = UsbAutoStartState::disconnecting; + _reschedule_time = 100_ms; + } + + return; + } + + // Parse for carriage returns indicating someone is trying to use the nsh. + if (scan_buffer_for_carriage_returns()) { + if (start_nsh()) { + _state = UsbAutoStartState::connected; + _active_protocol = UsbProtocol::nsh; + + } else { + _state = UsbAutoStartState::disconnecting; + _reschedule_time = 100_ms; + } + + return; + } + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + + // Parse for ublox start of packet byte sequence. + if (scan_buffer_for_ublox_bytes()) { + if (start_ublox_serial_passthru(baudrate)) { + _state = UsbAutoStartState::connected; + _active_protocol = UsbProtocol::ublox; + + } else { + _state = UsbAutoStartState::disconnecting; + _reschedule_time = 100_ms; + } + + return; + } + +#endif + + return; + +fail: + PX4_DEBUG("fail..."); + + // VBUS not present, open failed + if (_ttyacm_fd >= 0) { + px4_close(_ttyacm_fd); + _ttyacm_fd = -1; + } + + _state = UsbAutoStartState::disconnecting; +} + +void CdcAcmAutostart::state_disconnecting() +{ + PX4_DEBUG("state_disconnecting"); + + if (_ttyacm_fd > 0) { + px4_close(_ttyacm_fd); + } + + // Disconnect serial + serdis_main(0, NULL); + _state = UsbAutoStartState::disconnected; + _active_protocol = UsbProtocol::none; +} + +bool CdcAcmAutostart::scan_buffer_for_mavlink_reboot() +{ + bool rebooting = false; + + // Mavlink reboot/shutdown command + // COMMAND_LONG (#76) with command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246) + static constexpr int MAVLINK_COMMAND_LONG_MIN_LENGTH = 41; + + if (_bytes_read < MAVLINK_COMMAND_LONG_MIN_LENGTH) { + return rebooting; + } + + // scan buffer for mavlink COMMAND_LONG + for (int i = 0; i < _bytes_read - MAVLINK_COMMAND_LONG_MIN_LENGTH; i++) { + if ((_buffer[i] == 0xFE) // Mavlink v1 start byte + && (_buffer[i + 5] == 76) // 76=0x4C COMMAND_LONG + && (_buffer[i + 34] == 246) // 246=0xF6 MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN + ) { + // mavlink v1 COMMAND_LONG + // buffer[0]: start byte (0xFE for mavlink v1) + // buffer[3]: SYSID + // buffer[4]: COMPID + // buffer[5]: message id (COMMAND_LONG 76=0x4C) + // buffer[6-10]: COMMAND_LONG param 1 (little endian float) + // buffer[34]: COMMAND_LONG command MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN (246/0xF6) + float param1_raw = 0; + memcpy(¶m1_raw, &_buffer[i + 6], 4); + int param1 = roundf(param1_raw); + + PX4_INFO("%s: Mavlink MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN param 1: %d (SYSID:%d COMPID:%d)", + USB_DEVICE_PATH, param1, _buffer[i + 3], _buffer[i + 4]); + + if (param1 == 1) { + // 1: Reboot autopilot + rebooting = true; + px4_reboot_request(REBOOT_REQUEST, 0); + + } else if (param1 == 2) { + // 2: Shutdown autopilot +#if defined(BOARD_HAS_POWER_CONTROL) + rebooting = true; + px4_shutdown_request(0); +#endif // BOARD_HAS_POWER_CONTROL + + } else if (param1 == 3) { + // 3: Reboot autopilot and keep it in the bootloader until upgraded. + rebooting = true; + px4_reboot_request(REBOOT_TO_BOOTLOADER, 0); + } + } + } + + return rebooting; +} + +bool CdcAcmAutostart::scan_buffer_for_mavlink_heartbeat() +{ + static constexpr int MAVLINK_HEARTBEAT_MIN_LENGTH = 9; + bool start_mavlink = false; + + if (_bytes_read < MAVLINK_HEARTBEAT_MIN_LENGTH) { + return start_mavlink; + } + + // scan buffer for mavlink HEARTBEAT (v1 & v2) + for (int i = 0; i < _bytes_read - MAVLINK_HEARTBEAT_MIN_LENGTH; i++) { + if ((_buffer[i] == 0xFE) && (_buffer[i + 1] == 9) && (_buffer[i + 5] == 0)) { + // mavlink v1 HEARTBEAT + // buffer[0]: start byte (0xFE for mavlink v1) + // buffer[1]: length (9 for HEARTBEAT) + // buffer[3]: SYSID + // buffer[4]: COMPID + // buffer[5]: mavlink message id (0 for HEARTBEAT) + PX4_INFO("%s: launching mavlink (HEARTBEAT v1 from SYSID:%d COMPID:%d)", + USB_DEVICE_PATH, _buffer[i + 3], _buffer[i + 4]); + start_mavlink = true; + + } else if ((_buffer[i] == 0xFD) && (_buffer[i + 1] == 9) + && (_buffer[i + 7] == 0) && (_buffer[i + 8] == 0) && (_buffer[i + 9] == 0)) { + // mavlink v2 HEARTBEAT + // buffer[0]: start byte (0xFD for mavlink v2) + // buffer[1]: length (9 for HEARTBEAT) + // buffer[5]: SYSID + // buffer[6]: COMPID + // buffer[7:9]: mavlink message id (0 for HEARTBEAT) + PX4_INFO("%s: launching mavlink (HEARTBEAT v2 from SYSID:%d COMPID:%d)", + USB_DEVICE_PATH, _buffer[i + 5], _buffer[i + 6]); + start_mavlink = true; + } + } + + return start_mavlink; +} + +bool CdcAcmAutostart::scan_buffer_for_carriage_returns() +{ + bool start_nsh = false; + + if (_bytes_read < 3) { + return start_nsh; + } + + // nshterm (3 carriage returns) + // scan buffer looking for 3 consecutive carriage returns (0xD) + for (int i = 1; i < _bytes_read - 1; i++) { + if (_buffer[i - 1] == 0xD && _buffer[i] == 0xD && _buffer[i + 1] == 0xD) { + PX4_INFO("%s: launching nshterm", USB_DEVICE_PATH); + start_nsh = true; + break; + } + } + + return start_nsh; +} + +bool CdcAcmAutostart::scan_buffer_for_ublox_bytes() +{ + bool success = false; + + if (_bytes_read < 4) { + return success; + } + + // scan buffer looking for 0xb5 0x62 which indicates the start of a packet + for (int i = 0; i < _bytes_read; i++) { + bool ub = _buffer[i] == 0xb5 && _buffer[i + 1] == 0x62; + + if (ub && ((_buffer[i + 2 ] == 0x6 && (_buffer[i + 3 ] == 0xb8 || _buffer[i + 3 ] == 0x13)) || + (_buffer[i + 2 ] == 0xa && _buffer[i + 3 ] == 0x4))) { + PX4_INFO("%s: launching ublox serial passthru", USB_DEVICE_PATH); + success = true; + break; + } + } + + return success; +} + +bool CdcAcmAutostart::start_mavlink() +{ + bool success = false; + char mavlink_mode_string[3]; + snprintf(mavlink_mode_string, sizeof(mavlink_mode_string), "%ld", _usb_mav_mode.get()); + static const char *argv[] {"mavlink", "start", "-d", USB_DEVICE_PATH, "-m", mavlink_mode_string, nullptr}; + + if (execute_process((char **)argv) > 0) { + success = true; + } + + return success; +} + +bool CdcAcmAutostart::start_nsh() +{ + bool success = false; + static const char *argv[] {"nshterm", USB_DEVICE_PATH, nullptr}; + + if (execute_process((char **)argv) > 0) { + success = true; + } + + return success; +} + +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) +bool CdcAcmAutostart::start_ublox_serial_passthru(speed_t baudrate) +{ + bool success = false; + char baudstring[16]; + snprintf(baudstring, sizeof(baudstring), "%ld", baudrate); + + // Stop the GPS driver first + static const char *gps_argv[] {"gps", "stop", nullptr}; + static const char *passthru_argv[] {"serial_passthru", "start", "-t", "-b", baudstring, "-e", USB_DEVICE_PATH, "-d", SERIAL_PASSTHRU_UBLOX_DEV, nullptr}; + + if (execute_process((char **)gps_argv) > 0) { + if (execute_process((char **)passthru_argv) > 0) { + success = true; + } + } + + return success; +} +#endif + +int CdcAcmAutostart::execute_process(char **argv) +{ + int pid = -1; + sched_lock(); + + pid = exec_builtin(argv[0], argv, nullptr, 0); + + sched_unlock(); + return pid; +} + +int CdcAcmAutostart::task_spawn(int argc, char *argv[]) +{ + CdcAcmAutostart *instance = new CdcAcmAutostart(); + + if (!instance) { + PX4_ERR("alloc failed"); + return -1; + } + + int ret = instance->Start(); + + if (ret != PX4_OK) { + delete instance; + return ret; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + return ret; +} + +void CdcAcmAutostart::UpdateParams(const bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + ModuleParams::updateParams(); + } +} + +int CdcAcmAutostart::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int CdcAcmAutostart::print_status() +{ + const char *state = ""; + const char *protocol = ""; + + switch (_state) { + case UsbAutoStartState::disconnected: + state = "disconnected"; + break; + + case UsbAutoStartState::connecting: + state = "connecting"; + break; + + case UsbAutoStartState::connected: + state = "connected"; + break; + + case UsbAutoStartState::disconnecting: + state = "disconnecting"; + break; + } + + switch (_active_protocol) { + case UsbProtocol::none: + protocol = "none"; + break; + + case UsbProtocol::mavlink: + protocol = "mavlink"; + break; + + case UsbProtocol::nsh: + protocol = "nsh"; + break; + + case UsbProtocol::ublox: + protocol = "ublox"; + break; + } + + PX4_INFO("Running"); + PX4_INFO("State: %s", state); + PX4_INFO("Protocol: %s", protocol); + return PX4_OK; +} + +int CdcAcmAutostart::print_usage(const char *reason) +{ + if (reason) { + printf("%s\n\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module listens on USB and auto-configures the protocol depending on the bytes received. +The supported protocols are: MAVLink, nsh, and ublox serial passthrough. If the parameter SYS_USB_AUTO=2 +the module will only try to start mavlink as long as the USB VBUS is detected. Otherwise it will spin +and continue to check for VBUS and start mavlink once it is detected. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("cdcacm_autostart", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +#endif + +extern "C" __EXPORT int cdcacm_autostart_main(int argc, char *argv[]) +{ +#if defined(CONFIG_SYSTEM_CDCACM) + return CdcAcmAutostart::main(argc, argv); +#endif + return 1; +} diff --git a/src/drivers/cdcacm_autostart/cdcacm_autostart.h b/src/drivers/cdcacm_autostart/cdcacm_autostart.h new file mode 100644 index 000000000000..a0e12715151b --- /dev/null +++ b/src/drivers/cdcacm_autostart/cdcacm_autostart.h @@ -0,0 +1,124 @@ +/**************************************************************************** + * + * 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 + +using namespace time_literals; + +class CdcAcmAutostart : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + CdcAcmAutostart(); + ~CdcAcmAutostart() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase */ + int print_status() override; + + int Start(); + +private: + + enum class UsbAutoStartState { + disconnected, + connecting, + connected, + disconnecting, + }; + + enum class UsbProtocol { + none, + mavlink, + nsh, + ublox, + }; + + void Run() override; + + void UpdateParams(const bool force = false); + + void run_state_machine(); + + void state_disconnected(); + void state_connecting(); + void state_connected(); + void state_disconnecting(); + + bool scan_buffer_for_mavlink_reboot(); + bool scan_buffer_for_mavlink_heartbeat(); + bool scan_buffer_for_carriage_returns(); + bool scan_buffer_for_ublox_bytes(); + + bool start_mavlink(); + bool start_nsh(); +#if defined(CONFIG_SERIAL_PASSTHRU_UBLOX) + bool start_ublox_serial_passthru(speed_t baudrate); +#endif + int execute_process(char **argv); + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 500_ms}; + + UsbAutoStartState _state{UsbAutoStartState::disconnected}; + UsbProtocol _active_protocol{UsbProtocol::none}; + bool _vbus_present = false; + bool _vbus_present_prev = false; + int _ttyacm_fd = -1; + + char _buffer[80] = {}; + int _bytes_read = 0; + + uint32_t _reschedule_time = 0; + + DEFINE_PARAMETERS( + (ParamInt) _sys_usb_auto, + (ParamInt) _usb_mav_mode + ) +}; diff --git a/src/drivers/cdcacm_autostart/cdcacm_autostart_params.c b/src/drivers/cdcacm_autostart/cdcacm_autostart_params.c new file mode 100644 index 000000000000..20d436ba0601 --- /dev/null +++ b/src/drivers/cdcacm_autostart/cdcacm_autostart_params.c @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +/** + * Enable USB autostart + * + * @value 0 Disabled + * @value 1 Auto-detect + * @value 2 MAVLink + * + * @reboot_required true + * + * @group CDCACM + */ +PARAM_DEFINE_INT32(SYS_USB_AUTO, 2); + +/** + * Specify USB MAVLink mode + * + * @value 0 normal + * @value 1 custom + * @value 2 onboard + * @value 3 osd + * @value 4 magic + * @value 5 config + * @value 6 iridium + * @value 7 minimal + * @value 8 extvision + * @value 9 extvisionmin + * @value 10 gimbal + * @value 11 onboard_low_bandwidth + * @value 12 uavionix + * + * @reboot_required true + * + * @group CDCACM + */ +PARAM_DEFINE_INT32(USB_MAV_MODE, 2); diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index 8d3f75fba632..8dbe2aff90ad 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -37,12 +37,11 @@ * Client-side implementation of UDRAL specification ESC service * * Publishes the following Cyphal messages: - * reg.drone.service.actuator.common.sp.Value8.0.1 - * reg.drone.service.common.Readiness.0.1 + * reg.udral.service.actuator.common.sp.Value31.0.1 + * reg.udral.service.common.Readiness.0.1 * * Subscribes to the following Cyphal messages: - * reg.drone.service.actuator.common.Feedback.0.1 - * reg.drone.service.actuator.common.Status.0.1 + * zubax.telega.CompactFeedback.0.1 * * @author Pavel Kirienko * @author Jacob Crabill @@ -51,11 +50,13 @@ #pragma once #include -#include +#include #include #include #include #include +#include "../Subscribers/DynamicPortSubscriber.hpp" +#include "../Publishers/Publisher.hpp" #include // UDRAL Specification Messages @@ -63,16 +64,15 @@ using std::isfinite; #include #include -/// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status -class UavcanEscController : public UavcanPublisher + +class ReadinessPublisher : public UavcanPublisher { public: - static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : - UavcanPublisher(handle, pmgr, "udral.", "esc") { }; + ReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral.", "readiness") { }; - ~UavcanEscController() {}; + ~ReadinessPublisher() {}; void update() override { @@ -95,58 +95,18 @@ class UavcanEscController : public UavcanPublisher if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) { publish_readiness(); } - }; + } - void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) - { - if (_port_id > 0) { - reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; - size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - - for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { - if (i < num_outputs) { - msg_sp.value[i] = static_cast(outputs[i]); - - } else { - // "unset" values published as NaN - msg_sp.value[i] = NAN; - } - } + static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000; + hrt_abstime _previous_pub_time = 0; - uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - const CanardTransferMetadata transfer_metadata = { - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = _port_id, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _transfer_id, - }; - - int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, - &payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - &transfer_metadata, - payload_size, - &esc_sp_payload_buffer); - } - } - }; + uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; + actuator_armed_s _armed {}; - /** - * Sets the number of rotors - */ - void set_rotor_count(uint8_t count) { _rotor_count = count; } + uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; + uint64_t _actuator_test_timestamp{0}; -private: - /** - * ESC status message reception will be reported via this callback. - */ - void esc_status_sub_cb(const CanardRxTransfer &msg); + CanardTransferID _arming_transfer_id; void publish_readiness() { @@ -155,8 +115,7 @@ class UavcanEscController : public UavcanPublisher size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - // Only publish if we have a valid publication ID set - if (_port_id == 0) { + if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { return; } @@ -174,12 +133,12 @@ class UavcanEscController : public UavcanPublisher uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); + CanardPortID arming_pid = static_cast(static_cast(_port_id)); const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, - .port_id = arming_pid, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .port_id = arming_pid, + .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _arming_transfer_id, }; @@ -187,25 +146,143 @@ class UavcanEscController : public UavcanPublisher &payload_size); if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + ++_arming_transfer_id; result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, &transfer_metadata, payload_size, &arming_payload_buffer); } }; +}; - uint8_t _rotor_count {0}; +class UavcanEscController : public UavcanPublisher +{ +public: + static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000; - hrt_abstime _previous_pub_time = 0; + UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral.", "esc") { } - uORB::Subscription _armed_sub{ORB_ID(actuator_armed)}; - actuator_armed_s _armed {}; + ~UavcanEscController() {} - uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)}; - uint64_t _actuator_test_timestamp{0}; + void update() override + { + } - CanardTransferID _arming_transfer_id; + void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) + { + if (_port_id == 0 || _port_id == CANARD_PORT_ID_UNSET) { + return; + } + + uint8_t max_num_outputs = MAX_ACTUATORS > num_outputs ? num_outputs : MAX_ACTUATORS; + + for (int8_t i = max_num_outputs - 1; i >= _max_number_of_nonzero_outputs; i--) { + if (outputs[i] != 0) { + _max_number_of_nonzero_outputs = i + 1; + break; + } + } + + uint16_t payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_value_ARRAY_CAPACITY_]; + + for (uint8_t i = 0; i < _max_number_of_nonzero_outputs; i++) { + payload_buffer[i] = nunavutFloat16Pack(outputs[i] / 8192.0); + } + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = _port_id, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _transfer_id, + }; + + ++_transfer_id; + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + _max_number_of_nonzero_outputs * 2, + &payload_buffer); + } + +private: + uint8_t _max_number_of_nonzero_outputs{1}; +}; + +class UavcanEscFeedbackSubscriber : public UavcanDynamicPortSubscriber +{ +public: + UavcanEscFeedbackSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "zubax.", "feedback", instance) {} + + void subscribe() override + { + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); + _esc_status.esc_armed_flags |= 1 << _instance; + _esc_status.esc_count++; + }; + + void unsubscribe() override + { + _canard_handle.RxUnsubscribe(CanardTransferKindMessage, _subj_sub._canard_sub.port_id); + _esc_status.esc_armed_flags &= ~(1 << _instance); + _esc_status.esc_count--; + }; + + void callback(const CanardRxTransfer &receive) override + { + if (_instance >= esc_status_s::CONNECTED_ESC_MAX) { + return; + } + + auto &ref = _esc_status.esc[_instance]; + const ZubaxCompactFeedback *feedback = ((const ZubaxCompactFeedback *)(receive.payload)); + + ref.timestamp = hrt_absolute_time(); + ref.esc_address = receive.metadata.remote_node_id; + ref.esc_voltage = 0.2 * feedback->dc_voltage; + ref.esc_current = 0.2 * feedback->dc_current; + ref.esc_temperature = NAN; + ref.esc_rpm = feedback->velocity * RAD_PER_SEC_TO_RPM; + ref.esc_errorcount = 0; + + _esc_status.counter++; + _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_CAN; + _esc_status.esc_armed_flags = (1 << _esc_status.esc_count) - 1; + _esc_status.timestamp = hrt_absolute_time(); + _esc_status_pub.publish(_esc_status); + + _esc_status.esc_online_flags = 0; + const hrt_abstime now = hrt_absolute_time(); + + for (int index = 0; index < esc_status_s::CONNECTED_ESC_MAX; index++) { + if (_esc_status.esc[index].timestamp > 0 && now - _esc_status.esc[index].timestamp < 1200_ms) { + _esc_status.esc_online_flags |= (1 << index); + } + } + }; + +private: + static constexpr float RAD_PER_SEC_TO_RPM = 9.5492968; + static constexpr size_t zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES = 7; + + // https://telega.zubax.com/interfaces/cyphal.html#compact +#pragma pack(push, 1) + struct ZubaxCompactFeedback { + uint32_t dc_voltage : 11; + int32_t dc_current : 12; + int32_t phase_current_amplitude : 12; + int32_t velocity : 13; + int8_t demand_factor_pct : 8; + }; +#pragma pack(pop) + static_assert(sizeof(ZubaxCompactFeedback) == zubax_telega_CompactFeedback_0_1_SERIALIZATION_BUFFER_SIZE_BYTES); + + static esc_status_s _esc_status; + + uORB::Publication _esc_status_pub{ORB_ID(esc_status)}; }; diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index 73de1992ff51..5a288274cf5d 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -62,6 +62,8 @@ using namespace time_literals; CyphalNode *CyphalNode::_instance; +esc_status_s UavcanEscFeedbackSubscriber::_esc_status; + CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), @@ -125,7 +127,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate) _instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD); } else { - _instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC); + _instance = new CyphalNode(node_id, 512, CANARD_MTU_CAN_CLASSIC); } if (_instance == nullptr) { @@ -188,6 +190,8 @@ void CyphalNode::Run() // send uavcan::node::Heartbeat_1_0 @ 1 Hz sendHeartbeat(); + sendPortList(); + // Check all publishers _pub_manager.update(); @@ -359,10 +363,10 @@ void CyphalNode::sendHeartbeat() if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { uavcan_node_Heartbeat_1_0 heartbeat{}; - heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime + const hrt_abstime now = hrt_absolute_time(); + heartbeat.uptime = now / 1000000; heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; - const hrt_abstime now = hrt_absolute_time(); size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; const CanardTransferMetadata transfer_metadata = { @@ -392,6 +396,45 @@ void CyphalNode::sendHeartbeat() } } +void CyphalNode::sendPortList() +{ + static hrt_abstime _uavcan_node_port_List_last{0}; + + if (hrt_elapsed_time(&_uavcan_node_port_List_last) < 3_s) { + return; + } + + static uavcan_node_port_List_0_1 msg{}; + static uint8_t uavcan_node_port_List_0_1_buffer[uavcan_node_port_List_0_1_EXTENT_BYTES_]; + static CanardTransferID _uavcan_node_port_List_transfer_id{0}; + size_t payload_size = uavcan_node_port_List_0_1_EXTENT_BYTES_; + const hrt_abstime now = hrt_absolute_time(); + + const CanardTransferMetadata transfer_metadata = { + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _uavcan_node_port_List_transfer_id++ + }; + + // memset(uavcan_node_port_List_0_1_buffer, 0x00, uavcan_node_port_List_0_1_EXTENT_BYTES_); + uavcan_node_port_List_0_1_initialize_(&msg); + + _pub_manager.fillSubjectIdList(msg.publishers); + _sub_manager.fillSubjectIdList(msg.subscribers); + + uavcan_node_port_List_0_1_serialize_(&msg, uavcan_node_port_List_0_1_buffer, &payload_size); + + _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &uavcan_node_port_List_0_1_buffer + ); + + _uavcan_node_port_List_last = now; +} + bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index 004a509f4039..0132830fb6c9 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -137,6 +137,9 @@ class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem // Sends a heartbeat at 1s intervals void sendHeartbeat(); + // Sends a port.List at 3s intervals + void sendPortList(); + px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver bool _initialized{false}; ///< number of actuators currently available diff --git a/src/drivers/cyphal/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp index 15a57f3744ed..0dba575cde1d 100644 --- a/src/drivers/cyphal/ParamManager.cpp +++ b/src/drivers/cyphal/ParamManager.cpp @@ -56,6 +56,15 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_ } } + for (auto ¶m : _type_registers) { + if (strcmp(param_name, param.name) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } @@ -73,18 +82,35 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua } } + for (auto ¶m : _type_registers) { + if (strncmp((char *)name.name.elements, param.name, name.name.count) == 0) { + uavcan_register_Value_1_0_select_string_(&value); + value._string.value.count = strlen(param.value); + memcpy(&value._string, param.value, value._string.value.count); + return true; + } + } + return false; } bool UavcanParamManager::GetParamName(uint32_t id, uavcan_register_Name_1_0 &name) { - if (id >= sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { - return false; - } + size_t number_of_integer_registers = sizeof(_uavcan_params) / sizeof(UavcanParamBinder); + size_t number_of_type_registers = sizeof(_type_registers) / sizeof(CyphalTypeRegister); + + if (id < sizeof(_uavcan_params) / sizeof(UavcanParamBinder)) { + strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + name.name.count = strlen(_uavcan_params[id].uavcan_name); - strncpy((char *)name.name.elements, _uavcan_params[id].uavcan_name, uavcan_register_Name_1_0_name_ARRAY_CAPACITY_); + } else if (id < number_of_integer_registers + number_of_type_registers) { + id -= number_of_integer_registers; + strncpy((char *)name.name.elements, _type_registers[id].name, strlen(_type_registers[id].name)); + name.name.count = strlen(_type_registers[id].name); - name.name.count = strlen(_uavcan_params[id].uavcan_name); + } else { + return false; + } return true; } diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index 67c5dbc20bf6..e1330c4e5f31 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/src/drivers/cyphal/ParamManager.hpp @@ -103,6 +103,10 @@ typedef struct { bool is_persistent {true}; } UavcanParamBinder; +typedef struct { + const char *name; + const char *value; +} CyphalTypeRegister; class UavcanParamManager { @@ -116,8 +120,9 @@ class UavcanParamManager private: - const UavcanParamBinder _uavcan_params[13] { + const UavcanParamBinder _uavcan_params[22] { {"uavcan.pub.udral.esc.0.id", "UCAN1_ESC_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.pub.udral.readiness.0.id", "UCAN1_READ_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.servo.0.id", "UCAN1_SERVO_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.gps.0.id", "UCAN1_GPS_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.udral.actuator_outputs.0.id", "UCAN1_ACTR_PUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, @@ -130,7 +135,28 @@ class UavcanParamManager {"uavcan.sub.udral.legacy_bms.0.id", "UCAN1_LG_BMS_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, {"uavcan.pub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_P", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.0.id", "UCAN1_FB0_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.1.id", "UCAN1_FB1_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.2.id", "UCAN1_FB2_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.3.id", "UCAN1_FB3_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.4.id", "UCAN1_FB4_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.5.id", "UCAN1_FB5_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.6.id", "UCAN1_FB6_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, + {"uavcan.sub.zubax.feedback.7.id", "UCAN1_FB7_SUB", px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param}, //{"uavcan.sub.bms.0.id", "UCAN1_BMS0_SUB"}, //FIXME instancing //{"uavcan.sub.bms.1.id", "UCAN1_BMS1_SUB"}, }; + + CyphalTypeRegister _type_registers[10] { + {"uavcan.pub.udral.esc.0.type", "reg.udral.service.actuator.common.sp.Vector31"}, + {"uavcan.pub.udral.readiness.0.type", "reg.udral.service.common.Readiness.0.1"}, + {"uavcan.sub.zubax.feedback.0.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.1.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.2.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.3.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.4.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.5.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.6.type", "zubax.telega.CompactFeedback.0.1"}, + {"uavcan.sub.zubax.feedback.7.type", "zubax.telega.CompactFeedback.0.1"}, + }; }; diff --git a/src/drivers/cyphal/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp index 74ef3699c4d7..84133e037240 100644 --- a/src/drivers/cyphal/PublicationManager.cpp +++ b/src/drivers/cyphal/PublicationManager.cpp @@ -125,6 +125,15 @@ UavcanPublisher *PublicationManager::getPublisher(const char *subject_name) return NULL; } +void PublicationManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list) +{ + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&publishers_list); + + for (auto &dynpub : _dynpublishers) { + publishers_list.sparse_list.elements[publishers_list.sparse_list.count].value = dynpub->id(); + publishers_list.sparse_list.count++; + } +} void PublicationManager::update() { diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index 2c3da87d9d58..8defb8d8f479 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -67,7 +67,7 @@ /* Preprocessor calculation of publisher count */ #define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \ - CONFIG_CYPHAL_ESC_CONTROLLER + \ + 2 * CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_READINESS_PUBLISHER + \ CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER @@ -79,6 +79,7 @@ #include #include +#include #include "Actuators/EscClient.hpp" #include "Publishers/udral/Readiness.hpp" @@ -103,6 +104,7 @@ class PublicationManager UavcanPublisher *getPublisher(const char *subject_name); + void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &publishers_list); private: void updateDynamicPublications(); @@ -131,6 +133,14 @@ class PublicationManager "udral.esc", 0 }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * + { + return new ReadinessPublisher(handle, pmgr); + }, + "udral.readiness", + 0 + }, #endif #if CONFIG_CYPHAL_READINESS_PUBLISHER { diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 4a4d316ee09c..718a70844fda 100644 --- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -76,8 +76,6 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber battery_status_s bat_status {0}; bat_status.timestamp = hrt_absolute_time(); - bat_status.voltage_filtered_v = bat_info.voltage; - bat_status.current_filtered_a = bat_info.current; bat_status.current_average_a = bat_info.average_power_10sec; bat_status.remaining = bat_info.state_of_charge_pct / 100.0f; bat_status.scale = -1; diff --git a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp index a80edf292d75..63498e8c676f 100644 --- a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp +++ b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp @@ -85,7 +85,7 @@ class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber _uorb_pub.publish(*data); } else { - PX4_ERR("uORB over UAVCAN %s payload size mismatch got %d expected %d", + PX4_ERR("uORB over UAVCAN %s payload size mismatch got %zu expected %zu", _subj_sub._subject_name, receive.payload_size, get_payload_size(data)); } }; diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp index e489357fc2f5..ef4ae73f2bd9 100644 --- a/src/drivers/cyphal/SubscriptionManager.cpp +++ b/src/drivers/cyphal/SubscriptionManager.cpp @@ -158,3 +158,24 @@ void SubscriptionManager::updateParams() // Check for any newly-enabled subscriptions updateDynamicSubscriptions(); } + +void SubscriptionManager::fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list) +{ + uavcan_node_port_SubjectIDList_0_1_select_sparse_list_(&subscribers_list); + + UavcanDynamicPortSubscriber *dynsub = _dynsubscribers; + + auto &sparse_list = subscribers_list.sparse_list; + + while (dynsub != nullptr) { + int32_t instance_idx = 0; + + while (dynsub->isValidPortId(dynsub->id(instance_idx))) { + sparse_list.elements[sparse_list.count].value = dynsub->id(instance_idx); + sparse_list.count++; + instance_idx++; + } + + dynsub = dynsub->next(); + } +} diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index bb56e1e05776..c9218d2f886c 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -45,6 +45,10 @@ #define CONFIG_CYPHAL_ESC_SUBSCRIBER 0 #endif +#ifndef CONFIG_CYPHAL_ESC_CONTROLLER +#define CONFIG_CYPHAL_ESC_CONTROLLER 0 +#endif + #ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 #define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0 #endif @@ -65,12 +69,15 @@ #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ + 8 * CONFIG_CYPHAL_ESC_CONTROLLER + \ CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ CONFIG_CYPHAL_BMS_SUBSCRIBER + \ CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER #include #include +#include +#include "Actuators/EscClient.hpp" #include "Subscribers/DynamicPortSubscriber.hpp" #include "CanardInterface.hpp" @@ -100,6 +107,7 @@ class SubscriptionManager void subscribe(); void printInfo(); void updateParams(); + void fillSubjectIdList(uavcan_node_port_SubjectIDList_0_1 &subscribers_list); private: void updateDynamicSubscriptions(); @@ -130,6 +138,72 @@ class SubscriptionManager 0 }, #endif +#if CONFIG_CYPHAL_ESC_CONTROLLER + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 0); + }, + "zubax.feedback", + 0 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 1); + }, + "zubax.feedback", + 1 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 2); + }, + "zubax.feedback", + 2 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 3); + }, + "zubax.feedback", + 3 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 4); + }, + "zubax.feedback", + 4 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 5); + }, + "zubax.feedback", + 5 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 6); + }, + "zubax.feedback", + 6 + }, + { + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + { + return new UavcanEscFeedbackSubscriber(handle, pmgr, 7); + }, + "zubax.feedback", + 7 + }, +#endif #if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 { [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index becd33fc426a..4731f1c14e9c 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -162,6 +162,87 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1); */ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); +/** + * Cyphal ESC readiness port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_READ_PUB, -1); + +/** + * Cyphal ESC 0 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB0_SUB, -1); + +/** + * Cyphal ESC 1 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB1_SUB, -1); + +/** + * Cyphal ESC 2 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB2_SUB, -1); + +/** + * Cyphal ESC 3 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB3_SUB, -1); + +/** + * Cyphal ESC 4 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB4_SUB, -1); + +/** + * Cyphal ESC 5 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB5_SUB, -1); + +/** + * Cyphal ESC 6 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB6_SUB, -1); + +/** + * Cyphal ESC 7 zubax feedback port ID. + * + * @min -1 + * @max 6143 + * @group Cyphal + */ +PARAM_DEFINE_INT32(UCAN1_FB7_SUB, -1); + /** * Cyphal GPS publication port ID. * diff --git a/src/drivers/differential_pressure/asp5033/ASP5033.hpp b/src/drivers/differential_pressure/asp5033/ASP5033.hpp index 37a9fdeb8f7e..d8c385ef3a16 100644 --- a/src/drivers/differential_pressure/asp5033/ASP5033.hpp +++ b/src/drivers/differential_pressure/asp5033/ASP5033.hpp @@ -134,4 +134,3 @@ class ASP5033 : public device::I2C, public I2CSPIDriver perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")}; perf_counter_t _fault_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": fault detected")}; }; - diff --git a/src/drivers/distance_sensor/Kconfig b/src/drivers/distance_sensor/Kconfig index 3c93bcb07b5b..f45abe877c50 100644 --- a/src/drivers/distance_sensor/Kconfig +++ b/src/drivers/distance_sensor/Kconfig @@ -8,8 +8,6 @@ menu "Distance sensors" select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_I2C select DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL select DRIVERS_DISTANCE_SENSOR_LL40LS - select DRIVERS_DISTANCE_SENSOR_MB12XX - select DRIVERS_DISTANCE_SENSOR_PGA460 select DRIVERS_DISTANCE_SENSOR_SRF02 select DRIVERS_DISTANCE_SENSOR_TERARANGER select DRIVERS_DISTANCE_SENSOR_TF02PRO diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index 44195305158f..91e3366d5ec8 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -118,142 +118,145 @@ void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd) int AFBRS50::init() { - if (_hnd != nullptr) { - // retry - Argus_Deinit(_hnd); - Argus_DestroyHandle(_hnd); - _hnd = nullptr; - } - - _hnd = Argus_CreateHandle(); - - if (_hnd == nullptr) { - PX4_ERR("Handle not initialized"); - return PX4_ERROR; - } - - // Initialize the S2PI hardware required by the API. - S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE); - - int32_t mode_param = _p_sens_afbr_mode.get(); - - if (mode_param < 0 || mode_param > 3) { - PX4_ERR("Invalid mode parameter: %li", mode_param); - return PX4_ERROR; - } - - argus_mode_t mode = ARGUS_MODE_LONG_RANGE; - - switch (mode_param) { - case 0: - mode = ARGUS_MODE_SHORT_RANGE; - break; - - case 1: - mode = ARGUS_MODE_LONG_RANGE; - break; - - case 2: - mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE; - break; - - case 3: - mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE; - break; - - default: - break; - } - - status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode); + // Retry initialization 3 times + for (int32_t i = 0; i < 3; i++) { + if (_hnd != nullptr) { + // retry + Argus_Deinit(_hnd); + Argus_DestroyHandle(_hnd); + _hnd = nullptr; + } - if (status == STATUS_OK) { - uint32_t id = Argus_GetChipID(_hnd); - uint32_t value = Argus_GetAPIVersion(); - uint8_t a = (value >> 24) & 0xFFU; - uint8_t b = (value >> 16) & 0xFFU; - uint8_t c = value & 0xFFFFU; - PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c); + _hnd = Argus_CreateHandle(); - argus_module_version_t mv = Argus_GetModuleVersion(_hnd); + if (_hnd == nullptr) { + PX4_ERR("Handle not initialized"); + return PX4_ERROR; + } - switch (mv) { - case AFBR_S50MV85G_V1: + // Initialize the S2PI hardware required by the API. + S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE); - // FALLTHROUGH - case AFBR_S50MV85G_V2: + int32_t mode_param = _p_sens_afbr_mode.get(); - // FALLTHROUGH - case AFBR_S50MV85G_V3: - _min_distance = 0.08f; - _max_distance = 10.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(6.f)); - PX4_INFO_RAW("AFBR-S50MV85G\n"); - break; + if (mode_param < 0 || mode_param > 3) { + PX4_ERR("Invalid mode parameter: %li", mode_param); + return PX4_ERROR; + } - case AFBR_S50LV85D_V1: - _min_distance = 0.08f; - _max_distance = 30.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(6.f)); - PX4_INFO_RAW("AFBR-S50LV85D\n"); - break; + argus_mode_t mode = ARGUS_MODE_SHORT_RANGE; - case AFBR_S50LX85D_V1: - _min_distance = 0.08f; - _max_distance = 50.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(6.f)); - PX4_INFO_RAW("AFBR-S50LX85D\n"); + switch (mode_param) { + case 0: + mode = ARGUS_MODE_SHORT_RANGE; break; - case AFBR_S50MV68B_V1: - _min_distance = 0.08f; - _max_distance = 10.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(1.f)); - PX4_INFO_RAW("AFBR-S50MV68B (v1)\n"); + case 1: + mode = ARGUS_MODE_LONG_RANGE; break; - case AFBR_S50MV85I_V1: - _min_distance = 0.08f; - _max_distance = 5.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(6.f)); - PX4_INFO_RAW("AFBR-S50MV85I (v1)\n"); + case 2: + mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE; break; - case AFBR_S50SV85K_V1: - _min_distance = 0.08f; - _max_distance = 10.f; - _px4_rangefinder.set_min_distance(_min_distance); - _px4_rangefinder.set_max_distance(_max_distance); - _px4_rangefinder.set_fov(math::radians(4.f)); - PX4_INFO_RAW("AFBR-S50SV85K (v1)\n"); + case 3: + mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE; break; default: break; } - if (_testing) { - _state = STATE::TEST; + status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode); - } else { - _state = STATE::CONFIGURE; - } + if (status == STATUS_OK) { + uint32_t id = Argus_GetChipID(_hnd); + uint32_t value = Argus_GetAPIVersion(); + uint8_t a = (value >> 24) & 0xFFU; + uint8_t b = (value >> 16) & 0xFFU; + uint8_t c = value & 0xFFFFU; + PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c); + + argus_module_version_t mv = Argus_GetModuleVersion(_hnd); + + switch (mv) { + case AFBR_S50MV85G_V1: + + // FALLTHROUGH + case AFBR_S50MV85G_V2: + + // FALLTHROUGH + case AFBR_S50MV85G_V3: + _min_distance = 0.0f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50MV85G\n"); + break; + + case AFBR_S50LV85D_V1: + _min_distance = 0.0f; + _max_distance = 30.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50LV85D\n"); + break; + + case AFBR_S50LX85D_V1: + _min_distance = 0.0f; + _max_distance = 50.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50LX85D\n"); + break; + + case AFBR_S50MV68B_V1: + _min_distance = 0.0f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(1.f)); + PX4_INFO_RAW("AFBR-S50MV68B (v1)\n"); + break; + + case AFBR_S50MV85I_V1: + _min_distance = 0.0f; + _max_distance = 5.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(6.f)); + PX4_INFO_RAW("AFBR-S50MV85I (v1)\n"); + break; + + case AFBR_S50SV85K_V1: + _min_distance = 0.0f; + _max_distance = 10.f; + _px4_rangefinder.set_min_distance(_min_distance); + _px4_rangefinder.set_max_distance(_max_distance); + _px4_rangefinder.set_fov(math::radians(4.f)); + PX4_INFO_RAW("AFBR-S50SV85K (v1)\n"); + break; + + default: + break; + } - ScheduleDelayed(_measure_interval); - return PX4_OK; + if (_testing) { + _state = STATE::TEST; - } else { - PX4_ERR("Argus_InitMode failed: %ld", status); + } else { + _state = STATE::CONFIGURE; + } + + ScheduleDelayed(_measure_interval); + return PX4_OK; + + } else { + PX4_ERR("Argus_InitMode failed: %ld", status); + } } return PX4_ERROR; @@ -284,7 +287,7 @@ void AFBRS50::Run() case STATE::CONFIGURE: { _current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); - status_t status = set_rate(_current_rate); + status_t status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF); if (status != STATUS_OK) { PX4_ERR("CONFIGURE status not okay: %i", (int)status); @@ -292,14 +295,6 @@ void AFBRS50::Run() ScheduleNow(); } - status = Argus_SetConfigurationDFMMode(_hnd, DFM_MODE_8X); - - if (status != STATUS_OK) { - PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); - _state = STATE::STOP; - ScheduleNow(); - } - status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false); if (status != STATUS_OK) { @@ -352,7 +347,7 @@ void AFBRS50::Evaluate_rate() && (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) { _current_rate = (uint32_t)_p_sens_afbr_l_rate.get(); - status = set_rate(_current_rate); + status = set_rate_and_dfm(_current_rate, DFM_MODE_8X); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); @@ -366,7 +361,7 @@ void AFBRS50::Evaluate_rate() && (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) { _current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); - status = set_rate(_current_rate); + status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); @@ -400,13 +395,20 @@ void AFBRS50::print_info() get_info(); } -status_t AFBRS50::set_rate(uint32_t rate_hz) +status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode) { while (Argus_GetStatus(_hnd) != STATUS_IDLE) { px4_usleep(1_ms); } - status_t status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz)); + status_t status = Argus_SetConfigurationDFMMode(_hnd, dfm_mode); + + if (status != STATUS_OK) { + PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); + return status; + } + + status = Argus_SetConfigurationFrameTime(_hnd, (1000000 / rate_hz)); if (status != STATUS_OK) { PX4_ERR("Argus_SetConfigurationFrameTime status not okay: %i", (int)status); diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index 2ad767b2fa3a..7b24c5b4baeb 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -85,7 +85,7 @@ class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd); void get_info(); - status_t set_rate(uint32_t rate_hz); + status_t set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode); argus_hnd_t *_hnd{nullptr}; diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c index 015249f859f5..367c5417df60 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c @@ -362,9 +362,9 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8 } /* Check the spi slave.*/ - if (spi_slave != S2PI_S2) { - return ERROR_S2PI_INVALID_SLAVE; - } + // if (spi_slave != S2PI_S2) { + // return ERROR_S2PI_INVALID_SLAVE; + // } /* Check the driver status, lock if idle. */ IRQ_LOCK(); diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c index 28ef2a17c1aa..26f3683e96d8 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c @@ -46,7 +46,7 @@ * @value 2 High Speed Short Range Mode * @value 3 High Speed Long Range Mode */ -PARAM_DEFINE_INT32(SENS_AFBR_MODE, 1); +PARAM_DEFINE_INT32(SENS_AFBR_MODE, 0); /** * AFBR Rangefinder Short Range Rate @@ -85,7 +85,7 @@ PARAM_DEFINE_INT32(SENS_AFBR_L_RATE, 25); * @group Sensors * */ -PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 5); +PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 4); /** diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/CMakeLists.txt b/src/drivers/distance_sensor/lightware_laser_i2c/CMakeLists.txt index 657ffa105efb..a19f16d5b904 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/CMakeLists.txt +++ b/src/drivers/distance_sensor/lightware_laser_i2c/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( DEPENDS drivers_rangefinder ) - diff --git a/src/drivers/distance_sensor/lightware_laser_serial/module.yaml b/src/drivers/distance_sensor/lightware_laser_serial/module.yaml index 269fe85ffd8c..6ad9eb656786 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/module.yaml +++ b/src/drivers/distance_sensor/lightware_laser_serial/module.yaml @@ -4,4 +4,3 @@ serial_config: port_config_param: name: SENS_SF0X_CFG group: Sensors - diff --git a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp index 1a8c9534283a..e28bcdfc2b62 100755 --- a/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp +++ b/src/drivers/distance_sensor/lightware_sf45_serial/lightware_sf45_serial.cpp @@ -75,7 +75,7 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) : // populate obstacle map members _obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; _obstacle_map_msg.increment = 5; - _obstacle_map_msg.angle_offset = -2.5; + _obstacle_map_msg.angle_offset = 2.5; _obstacle_map_msg.min_distance = UINT16_MAX; _obstacle_map_msg.max_distance = 5000; @@ -157,7 +157,6 @@ int SF45LaserSerial::collect() int64_t read_elapsed = hrt_elapsed_time(&_last_read); int ret; /* the buffer for read chars is buflen minus null termination */ - param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint); uint8_t readbuf[SF45_MAX_PAYLOAD]; float distance_m = -1.0f; @@ -669,34 +668,35 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m) raw_yaw = raw_yaw * -1; } + // SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}" + scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; + switch (_yaw_cfg) { case 0: break; case 1: - if (raw_yaw > 180) { - raw_yaw = raw_yaw - 180; + if (scaled_yaw > 180) { + scaled_yaw = scaled_yaw - 180; } else { - raw_yaw = raw_yaw + 180; // rotation facing aft + scaled_yaw = scaled_yaw + 180; // rotation facing aft } break; case 2: - raw_yaw = raw_yaw + 90; // rotation facing right + scaled_yaw = scaled_yaw + 90; // rotation facing right break; case 3: - raw_yaw = raw_yaw - 90; // rotation facing left + scaled_yaw = scaled_yaw - 90; // rotation facing left break; default: break; } - scaled_yaw = raw_yaw * SF45_SCALE_FACTOR; - // Convert to meters for rangefinder update *distance_m = raw_distance * SF45_SCALE_FACTOR; obstacle_dist_cm = (uint16_t)raw_distance; diff --git a/src/drivers/distance_sensor/tfmini/module.yaml b/src/drivers/distance_sensor/tfmini/module.yaml index 770cbe979c37..59cb6b234311 100644 --- a/src/drivers/distance_sensor/tfmini/module.yaml +++ b/src/drivers/distance_sensor/tfmini/module.yaml @@ -4,4 +4,3 @@ serial_config: port_config_param: name: SENS_TFMINI_CFG group: Sensors - diff --git a/src/drivers/drv_dshot.h b/src/drivers/drv_dshot.h index 2e8dab2d6db9..81c92b4d50ee 100644 --- a/src/drivers/drv_dshot.h +++ b/src/drivers/drv_dshot.h @@ -91,7 +91,7 @@ typedef enum { * @param dshot_pwm_freq Frequency of DSHOT signal. Usually DSHOT150, DSHOT300, DSHOT600 or DSHOT1200 * @return <0 on error, the initialized channels mask. */ -__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq); +__EXPORT extern int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bidirectional_dshot); /** * Set Dshot motor data, used by up_dshot_motor_data_set() and up_dshot_motor_command() (internal method) @@ -137,4 +137,28 @@ __EXPORT extern void up_dshot_trigger(void); */ __EXPORT extern int up_dshot_arm(bool armed); +/** + * Print bidrectional dshot status + */ +__EXPORT extern void up_bdshot_status(void); + + +/** + * Get bidrectional dshot erpm for a channel + * @param channel Dshot channel + * @param erpm pointer to write the erpm value + * @return <0 on error, OK on succes + */ +__EXPORT extern int up_bdshot_get_erpm(uint8_t channel, int *erpm); + + +/** + * Get bidrectional dshot status for a channel + * @param channel Dshot channel + * @param erpm pointer to write the erpm value + * @return <0 on error / not supported, 0 on offline, 1 on online + */ +__EXPORT extern int up_bdshot_channel_status(uint8_t channel); + + __END_DECLS diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 3e295abfc949..bb62da0189c3 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -162,7 +162,16 @@ static inline void abstime_to_ts(struct timespec *ts, hrt_abstime abstime) */ static inline hrt_abstime hrt_elapsed_time(const hrt_abstime *then) { - return hrt_absolute_time() - *then; + hrt_abstime now = hrt_absolute_time(); + + // Cannot allow a negative elapsed time as this would appear + // to be a huge positive elapsed time when represented as an + // unsigned value! + if (*then > now) { + return 0; + } + + return now - *then; } /** diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 549b7313c19f..53eecce4be71 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2024 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 @@ -63,6 +63,8 @@ #define DRV_MAG_DEVTYPE_IST8308 0x0B #define DRV_MAG_DEVTYPE_LIS2MDL 0x0C +#define DRV_MAG_DEVTYPE_MMC5983MA 0x0D +#define DRV_MAG_DEVTYPE_IIS2MDC 0x0E #define DRV_IMU_DEVTYPE_LSM303D 0x11 @@ -131,6 +133,8 @@ #define DRV_IMU_DEVTYPE_ADIS16477 0x59 #define DRV_IMU_DEVTYPE_ADIS16507 0x5A +#define DRV_IMU_DEVTYPE_SCH16T 0x5B + #define DRV_BARO_DEVTYPE_MPC2520 0x5F #define DRV_BARO_DEVTYPE_LPS22HB 0x60 @@ -147,6 +151,7 @@ #define DRV_ACC_DEVTYPE_BMI085 0x6C #define DRV_GYR_DEVTYPE_BMI085 0x6D #define DRV_BARO_DEVTYPE_BMP390 0x6E +#define DRV_BARO_DEVTYPE_BMP581 0x6F #define DRV_DIST_DEVTYPE_LL40LS 0x70 #define DRV_DIST_DEVTYPE_MAPPYDOT 0x71 @@ -237,6 +242,8 @@ #define DRV_INS_DEVTYPE_VN300 0xE3 #define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 +#define DRV_MAG_DEVTYPE_BMM350 0xE5 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5ae0150efba9..940019d9bf04 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -144,7 +144,9 @@ void DShot::enable_dshot_outputs(const bool enabled) } } - int ret = up_dshot_init(_output_mask, dshot_frequency); + _bidirectional_dshot_enabled = _param_bidirectional_enable.get(); + + int ret = up_dshot_init(_output_mask, dshot_frequency, _bidirectional_dshot_enabled); if (ret < 0) { PX4_ERR("up_dshot_init failed (%i)", ret); @@ -157,6 +159,7 @@ void DShot::enable_dshot_outputs(const bool enabled) for (unsigned i = 0; i < _num_outputs; ++i) { if (((1 << i) & _output_mask) == 0) { _mixing_output.disableFunction(i); + } } @@ -167,6 +170,10 @@ void DShot::enable_dshot_outputs(const bool enabled) } _outputs_initialized = true; + + if (_bidirectional_dshot_enabled) { + init_telemetry(NULL); + } } if (_outputs_initialized) { @@ -206,17 +213,20 @@ void DShot::init_telemetry(const char *device) _telemetry->esc_status_pub.advertise(); - int ret = _telemetry->handler.init(device); + if (device != NULL) { + int ret = _telemetry->handler.init(device); - if (ret != 0) { - PX4_ERR("telemetry init failed (%i)", ret); + if (ret != 0) { + PX4_ERR("telemetry init failed (%i)", ret); + } } update_telemetry_num_motors(); } -void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) +int DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data) { + int ret = 0; // fill in new motor data esc_status_s &esc_status = _telemetry->esc_status_pub.get(); @@ -239,18 +249,83 @@ void DShot::handle_new_telemetry_data(const int telemetry_index, const DShotTele esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; esc_status.esc_count = _telemetry->handler.numMotors(); ++esc_status.counter; - // FIXME: mark all ESC's as online, otherwise commander complains even for a single dropout - esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; - esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; - - _telemetry->esc_status_pub.update(); - // reset esc data (in case a motor times out, so we won't send stale data) - memset(&esc_status.esc, 0, sizeof(_telemetry->esc_status_pub.get().esc)); - esc_status.esc_online_flags = 0; + ret = 1; // Indicate we wrapped, so we publish data } _telemetry->last_telemetry_index = telemetry_index; + + return ret; +} + +void DShot::publish_esc_status(void) +{ + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + int telemetry_index = 0; + + // clear data of the esc that are offline + for (int index = 0; (index < _telemetry->last_telemetry_index); index++) { + if ((esc_status.esc_online_flags & (1 << index)) == 0) { + memset(&esc_status.esc[index], 0, sizeof(struct esc_report_s)); + } + } + + // FIXME: mark all UART Telemetry ESC's as online, otherwise commander complains even for a single dropout + esc_status.esc_count = _telemetry->handler.numMotors(); + esc_status.esc_online_flags = (1 << esc_status.esc_count) - 1; + esc_status.esc_armed_flags = (1 << esc_status.esc_count) - 1; + + if (_bidirectional_dshot_enabled) { + for (unsigned i = 0; i < _num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + if (up_bdshot_channel_status(i)) { + esc_status.esc_online_flags |= 1 << i; + + } else { + esc_status.esc_online_flags &= ~(1 << i); + } + + ++telemetry_index; + } + } + } + + // ESC telem wrap around or bdshot update + _telemetry->esc_status_pub.update(); + + // reset esc online flags + esc_status.esc_online_flags = 0; +} + +int DShot::handle_new_bdshot_erpm(void) +{ + int num_erpms = 0; + int telemetry_index = 0; + int erpm; + esc_status_s &esc_status = _telemetry->esc_status_pub.get(); + + esc_status.timestamp = hrt_absolute_time(); + esc_status.counter = _esc_status_counter++; + esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; + esc_status.esc_armed_flags = _outputs_on; + + for (unsigned i = 0; i < _num_outputs; i++) { + if (_mixing_output.isFunctionSet(i)) { + if (up_bdshot_get_erpm(i, &erpm) == 0) { + num_erpms++; + esc_status.esc_online_flags |= 1 << telemetry_index; + esc_status.esc[telemetry_index].timestamp = hrt_absolute_time(); + esc_status.esc[telemetry_index].esc_rpm = (erpm * 100) / (_param_mot_pole_count.get() / 2); + esc_status.esc[telemetry_index].actuator_function = _telemetry->actuator_functions[telemetry_index]; + } + + ++telemetry_index; + } + + + } + + return num_erpms; } int DShot::send_command_thread_safe(const dshot_command_t command, const int num_repetitions, const int motor_index) @@ -463,6 +538,7 @@ void DShot::Run() if (_telemetry) { int telem_update = _telemetry->handler.update(); + int need_to_publish = 0; // Are we waiting for ESC info? if (_waiting_for_esc_info) { @@ -472,10 +548,21 @@ void DShot::Run() } } else if (telem_update >= 0) { - handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); + need_to_publish = handle_new_telemetry_data(telem_update, _telemetry->handler.latestESCData()); + } + + if (_bidirectional_dshot_enabled) { + // Add bdshot data to esc status + need_to_publish += handle_new_bdshot_erpm(); + } + + if (need_to_publish > 0) { + // ESC telem wrap around or bdshot update + publish_esc_status(); } } + if (_parameter_update_sub.updated()) { update_params(); } @@ -713,6 +800,11 @@ int DShot::print_status() _telemetry->handler.printStatus(); } + /* Print dshot status */ + if (_bidirectional_dshot_enabled) { + up_bdshot_status(); + } + return 0; } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 4f314cebb75f..e60b33b86279 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -131,7 +131,11 @@ class DShot final : public ModuleBase, public OutputModuleInterface void init_telemetry(const char *device); - void handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + int handle_new_telemetry_data(const int telemetry_index, const DShotTelemetry::EscData &data); + + void publish_esc_status(void); + + int handle_new_bdshot_erpm(void); int request_esc_info(); @@ -158,6 +162,7 @@ class DShot final : public ModuleBase, public OutputModuleInterface bool _outputs_initialized{false}; bool _outputs_on{false}; bool _waiting_for_esc_info{false}; + bool _bidirectional_dshot_enabled{false}; static constexpr unsigned _num_outputs{DIRECT_PWM_OUTPUT_CHANNELS}; uint32_t _output_mask{0}; @@ -169,12 +174,14 @@ class DShot final : public ModuleBase, public OutputModuleInterface uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uint16_t _esc_status_counter{0}; DEFINE_PARAMETERS( (ParamFloat) _param_dshot_min, (ParamBool) _param_dshot_3d_enable, (ParamInt) _param_dshot_3d_dead_h, (ParamInt) _param_dshot_3d_dead_l, - (ParamInt) _param_mot_pole_count + (ParamInt) _param_mot_pole_count, + (ParamBool) _param_bidirectional_enable ) }; diff --git a/src/drivers/dshot/DShotTelemetry.cpp b/src/drivers/dshot/DShotTelemetry.cpp index 14a21d5654e0..0132a9b04941 100644 --- a/src/drivers/dshot/DShotTelemetry.cpp +++ b/src/drivers/dshot/DShotTelemetry.cpp @@ -183,6 +183,9 @@ bool DShotTelemetry::decodeByte(uint8_t byte, bool &successful_decoding) _latest_data.erpm); ++_num_successful_responses; successful_decoding = true; + + } else { + ++_num_checksum_errors; } return true; @@ -195,6 +198,7 @@ void DShotTelemetry::printStatus() const { PX4_INFO("Number of successful ESC frames: %i", _num_successful_responses); PX4_INFO("Number of timeouts: %i", _num_timeouts); + PX4_INFO("Number of CRC errors: %i", _num_checksum_errors); } uint8_t DShotTelemetry::updateCrc8(uint8_t crc, uint8_t crc_seed) @@ -472,4 +476,3 @@ int DShotTelemetry::setBaudrate(unsigned baud) return 0; } - diff --git a/src/drivers/dshot/DShotTelemetry.h b/src/drivers/dshot/DShotTelemetry.h index a5c549c47676..1164d1e2008c 100644 --- a/src/drivers/dshot/DShotTelemetry.h +++ b/src/drivers/dshot/DShotTelemetry.h @@ -140,4 +140,5 @@ class DShotTelemetry // statistics int _num_timeouts{0}; int _num_successful_responses{0}; + int _num_checksum_errors{0}; }; diff --git a/src/drivers/dshot/module.yaml b/src/drivers/dshot/module.yaml index edd38edd23f1..e2dcf8bc97e8 100644 --- a/src/drivers/dshot/module.yaml +++ b/src/drivers/dshot/module.yaml @@ -33,6 +33,16 @@ parameters: When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent. type: boolean default: 0 + DSHOT_BIDIR_EN: + description: + short: Enable bidirectional DShot + long: | + This parameter enables bidirectional DShot which provides RPM feedback. + Note that this requires ESCs that support bidirectional DSHot, e.g. BlHeli32. + This is not the same as DShot telemetry which requires an additional serial connection. + type: boolean + default: 0 + reboot_required: true DSHOT_3D_DEAD_H: description: short: DSHOT 3D deadband high diff --git a/src/drivers/gnss/Kconfig b/src/drivers/gnss/Kconfig new file mode 100644 index 000000000000..6353836988b2 --- /dev/null +++ b/src/drivers/gnss/Kconfig @@ -0,0 +1 @@ +rsource "*/Kconfig" diff --git a/src/drivers/gnss/septentrio/CMakeLists.txt b/src/drivers/gnss/septentrio/CMakeLists.txt new file mode 100644 index 000000000000..62451fc9b1f3 --- /dev/null +++ b/src/drivers/gnss/septentrio/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2024 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_module( + MODULE driver__septentrio + MAIN septentrio + COMPILE_FLAGS + # -DDEBUG_BUILD # Enable during development of the module + -DSEP_LOG_ERROR # Enable module-level error logs + # -DSEP_LOG_WARN # Enable module-level warning logs + # -DSEP_LOG_INFO # Enable module-level info logs + # -DSEP_LOG_TRACE_PARSING # Tracing of parsing steps + SRCS + septentrio.cpp + util.cpp + rtcm.cpp + sbf/decoder.cpp + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/gnss/septentrio/Kconfig b/src/drivers/gnss/septentrio/Kconfig new file mode 100644 index 000000000000..5d3d52388587 --- /dev/null +++ b/src/drivers/gnss/septentrio/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_GNSS_SEPTENTRIO + bool "Septentrio GNSS receivers" + default n + ---help--- + Enable support for Septentrio receivers diff --git a/src/drivers/gnss/septentrio/README.md b/src/drivers/gnss/septentrio/README.md new file mode 100644 index 000000000000..7f5b83938c57 --- /dev/null +++ b/src/drivers/gnss/septentrio/README.md @@ -0,0 +1,6 @@ +# Septentrio GNSS Driver + +## SBF Library + +The `sbf/` directory contains all the logic required to work with SBF, including message +definitions, block IDs and a simple parser for the messages used by PX4. diff --git a/src/drivers/gnss/septentrio/module.h b/src/drivers/gnss/septentrio/module.h new file mode 100644 index 000000000000..c935c1263e44 --- /dev/null +++ b/src/drivers/gnss/septentrio/module.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 module.h + * + * Module functionality for the Septentrio GNSS driver. + * + * @author Thomas Frans +*/ + +#pragma once + +#include + +#ifdef DEBUG_BUILD +#ifndef SEP_LOG_ERROR +#define SEP_LOG_ERROR +#endif +#ifndef SEP_LOG_WARN +#define SEP_LOG_WARN +#endif +#ifndef SEP_LOG_INFO +#define SEP_LOG_INFO +#endif +#endif + +#ifdef SEP_LOG_ERROR +#define SEP_ERR(...) {PX4_ERR(__VA_ARGS__);} +#else +#define SEP_ERR(...) {} +#endif + +#ifdef SEP_LOG_WARN +#define SEP_WARN(...) {PX4_WARN(__VA_ARGS__);} +#else +#define SEP_WARN(...) {} +#endif + +#ifdef SEP_LOG_INFO +#define SEP_INFO(...) {PX4_INFO(__VA_ARGS__);} +#else +#define SEP_INFO(...) {} +#endif + +#ifdef SEP_LOG_TRACE_PARSING +#define SEP_TRACE_PARSING(...) {PX4_DEBUG(__VA_ARGS__);} +#else +#define SEP_TRACE_PARSING(...) {} +#endif diff --git a/src/drivers/gnss/septentrio/module.yaml b/src/drivers/gnss/septentrio/module.yaml new file mode 100644 index 000000000000..1b151f3618ba --- /dev/null +++ b/src/drivers/gnss/septentrio/module.yaml @@ -0,0 +1,206 @@ +module_name: Septentrio + +serial_config: + - command: set DUAL_GPS_ARGS "-e ${SERIAL_DEV} -g p:${BAUD_PARAM}" + port_config_param: + name: SEP_PORT2_CFG + group: Septentrio + label: Secondary GPS port + - command: septentrio start -d ${SERIAL_DEV} -b p:${BAUD_PARAM} ${DUAL_GPS_ARGS} + port_config_param: + name: SEP_PORT1_CFG + group: Septentrio + default: GPS1 + label: GPS Port + +parameters: + - group: Septentrio + definitions: + SEP_STREAM_MAIN: + description: + short: Main stream used during automatic configuration + long: | + The stream the autopilot sets up on the receiver to output the main data. + + Set this to another value if the default stream is already used for another purpose. + type: int32 + min: 1 + max: 10 + default: 1 + reboot_required: true + SEP_STREAM_LOG: + description: + short: Logging stream used during automatic configuration + long: | + The stream the autopilot sets up on the receiver to output the logging data. + + Set this to another value if the default stream is already used for another purpose. + type: int32 + min: 1 + max: 10 + default: 2 + reboot_required: true + SEP_OUTP_HZ: + description: + short: Output frequency of main SBF blocks + long: | + The output frequency of the main SBF blocks needed for PVT information. + type: enum + min: 0 + max: 3 + default: 1 + values: + 0: 5 Hz + 1: 10 Hz + 2: 20 Hz + 3: 25 Hz + reboot_required: true + SEP_YAW_OFFS: + description: + short: Heading/Yaw offset for dual antenna GPS + long: | + Heading offset angle for dual antenna GPS setups that support heading estimation. + + Set this to 0 if the antennas are parallel to the forward-facing direction + of the vehicle and the rover antenna is in front. + + The offset angle increases clockwise. + + Set this to 90 if the rover antenna is placed on the + right side of the vehicle and the moving base antenna is on the left side. + type: float + decimal: 3 + default: 0 + min: -360 + max: 360 + unit: deg + reboot_required: true + SEP_SAT_INFO: + description: + short: Enable sat info + long: | + Enable publication of satellite info (ORB_ID(satellite_info)) if possible. + type: boolean + default: 0 + reboot_required: true + SEP_PITCH_OFFS: + description: + short: Pitch offset for dual antenna GPS + long: | + Vertical offsets can be compensated for by adjusting the Pitch offset. + + Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. + This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. + Since pitch is defined as the right-handed rotation about the vehicle Y axis, + a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch. + type: float + decimal: 3 + default: 0 + min: -90 + max: 90 + unit: deg + reboot_required: true + SEP_DUMP_COMM: + description: + short: Log GPS communication data + long: | + Log raw communication between the driver and connected receivers. + For example, "To receiver" will log all commands and corrections sent by the driver to the receiver. + type: enum + default: 0 + min: 0 + max: 3 + values: + 0: Disabled + 1: From receiver + 2: To receiver + 3: Both + SEP_AUTO_CONFIG: + description: + short: Toggle automatic receiver configuration + long: | + By default, the receiver is automatically configured. Sometimes it may be used for multiple purposes. + If the offered parameters aren't sufficient, this parameter can be disabled to have full control of the receiver configuration. + A good way to use this is to enable automatic configuration, let the receiver be configured, and then disable it to make manual adjustments. + type: boolean + default: true + reboot_required: true + SEP_CONST_USAGE: + description: + short: Usage of different constellations + long: | + Choice of which constellations the receiver should use for PVT computation. + + When this is 0, the constellation usage isn't changed. + type: bitmask + bit: + 0: GPS + 1: GLONASS + 2: Galileo + 3: SBAS + 4: BeiDou + default: 0 + min: 0 + max: 63 + reboot_required: true + SEP_LOG_HZ: + description: + short: Logging frequency for the receiver + long: | + Select the frequency at which the connected receiver should log data to its internal storage. + type: enum + default: 0 + min: 0 + max: 10 + values: + 0: Disabled + 1: 0.1 Hz + 2: 0.2 Hz + 3: 0.5 Hz + 4: 1 Hz + 5: 2 Hz + 6: 5 Hz + 7: 10 Hz + 8: 20 Hz + 9: 25 Hz + 10: 50 Hz + reboot_required: true + SEP_LOG_LEVEL: + description: + short: Logging level for the receiver + long: | + Select the level of detail that needs to be logged by the receiver. + type: enum + default: 2 + min: 0 + max: 3 + values: + 0: Lite + 1: Basic + 2: Default + 3: Full + reboot_required: true + SEP_LOG_FORCE: + description: + short: Whether to overwrite or add to existing logging + long: | + When the receiver is already set up to log data, this decides whether extra logged data should be added or overwrite existing data. + type: boolean + default: false + reboot_required: true + SEP_HARDW_SETUP: + description: + short: Setup and expected use of the hardware + long: | + Setup and expected use of the hardware. + + - Default: Use two receivers as completely separate instances. + - Moving base: Use two receivers in a rover & moving base setup for heading. + type: enum + default: 0 + min: 0 + max: 1 + values: + 0: Default + 1: Moving base + reboot_required: true diff --git a/src/drivers/gnss/septentrio/rtcm.cpp b/src/drivers/gnss/septentrio/rtcm.cpp new file mode 100644 index 000000000000..007b2a168e33 --- /dev/null +++ b/src/drivers/gnss/septentrio/rtcm.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 rtcm.cpp + * + * @author Thomas Frans +*/ + +#include "rtcm.h" + +#include +#include + +#include "module.h" + +namespace septentrio +{ + +namespace rtcm +{ + +Decoder::Decoder() +{ + reset(); +} + +Decoder::~Decoder() +{ + delete[] _message; +} + +Decoder::State Decoder::add_byte(uint8_t byte) +{ + switch (_state) { + case State::SearchingPreamble: + if (byte == PREAMBLE) { + _message[_current_index] = byte; + _current_index++; + _state = State::GettingHeader; + } + + break; + + case State::GettingHeader: + _message[_current_index] = byte; + _current_index++; + + if (header_received()) { + _message_length = parse_message_length(); + + if (_message_length > MAX_BODY_SIZE) { + reset(); + return _state; + + } else if (_message_length + HEADER_SIZE + CRC_SIZE > INITIAL_BUFFER_LENGTH) { + uint16_t new_buffer_size = _message_length + HEADER_SIZE + CRC_SIZE; + uint8_t *new_buffer = new uint8_t[new_buffer_size]; + + if (!new_buffer) { + reset(); + return _state; + } + + memcpy(new_buffer, _message, HEADER_SIZE); + delete[](_message); + + _message = new_buffer; + } + + _state = State::Busy; + } + + break; + + case State::Busy: + _message[_current_index] = byte; + _current_index++; + + if (_message_length + HEADER_SIZE + CRC_SIZE == _current_index) { + _state = State::Done; + } + + break; + + case State::Done: + SEP_WARN("RTCM: Discarding excess byte"); + break; + } + + return _state; +} + +void Decoder::reset() +{ + if (_message) { + delete[] _message; + } + + _message = new uint8_t[INITIAL_BUFFER_LENGTH]; + _current_index = 0; + _message_length = 0; + _state = State::SearchingPreamble; +} + +uint16_t Decoder::parse_message_length() const +{ + if (!header_received()) { + return PX4_ERROR; + } + + return ((static_cast(_message[1]) & 3) << 8) | _message[2]; +} + +bool Decoder::header_received() const +{ + return _current_index >= HEADER_SIZE; +} + +uint16_t Decoder::received_bytes() const +{ + return _current_index; +} + +uint16_t Decoder::message_id() const +{ + return (_message[3] << 4) | (_message[4] >> 4); +} + +} // namespace rtcm + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/rtcm.h b/src/drivers/gnss/septentrio/rtcm.h new file mode 100644 index 000000000000..672ac08da9e1 --- /dev/null +++ b/src/drivers/gnss/septentrio/rtcm.h @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 rtcm.h + * + * @author Thomas Frans + */ + +#pragma once + +#include +#include + +namespace septentrio +{ + +namespace rtcm +{ + +constexpr uint8_t PREAMBLE = 0xD3; +constexpr uint8_t HEADER_SIZE = 3; ///< Total number of bytes in a message header. +constexpr uint8_t CRC_SIZE = 3; ///< Total number of bytes in the CRC. +constexpr uint8_t LENGTH_FIELD_BITS = 10; ///< Total number of bits used for the length. +constexpr uint16_t MAX_BODY_SIZE = 1 << LENGTH_FIELD_BITS; ///< Maximum allowed size of the message body. + +class Decoder +{ +public: + enum class State { + /// Searching for the first byte of an RTCM message. + SearchingPreamble, + + /// Getting the complete header of an RTCM message. + GettingHeader, + + /// Getting a complete RTCM message. + Busy, + + /// Complete RTCM message is available. + Done, + }; + + Decoder(); + ~Decoder(); + + /** + * Add a byte to the current message. + * + * @param byte The new byte. + * + * @return true if message complete (use @message to get it) + */ + State add_byte(uint8_t b); + + /** + * @brief Reset the parser to a clean state. + */ + void reset(); + + uint8_t *message() const { return _message; } + + /** + * @brief Number of received bytes of the current message. + */ + uint16_t received_bytes() const; + + /** + * @brief The id of the current message. + * + * This should only be called if the message has been received completely. + * + * @return The id of the current complete message. + */ + uint16_t message_id() const; + +private: + static constexpr uint16_t INITIAL_BUFFER_LENGTH = 300; + + /** + * @brief Parse the message lentgh of the current message. + * + * @return The expected length of the current message without header and CRC. + */ + uint16_t parse_message_length() const; + + /** + * @brief Check whether the full header has been received. + * + * @return `true` if the full header is available, `false` otherwise. + */ + bool header_received() const; + + uint8_t *_message{nullptr}; + uint16_t _message_length; ///< The total length of the message excluding header and CRC (3 bytes each). + uint16_t _current_index; ///< The current index of the byte we expect to read into the buffer. + State _state{State::SearchingPreamble}; ///< Current state of the parser. +}; + +} // namespace rtcm + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf/decoder.cpp b/src/drivers/gnss/septentrio/sbf/decoder.cpp new file mode 100644 index 000000000000..318a5c4db968 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/decoder.cpp @@ -0,0 +1,252 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 decoder.cpp + * + * Decoding logic for the Septentrio Binary Format (SBF). + * + * @author Thomas Frans + */ + +#include "decoder.h" + +#include +#include +#include +#include + +#include "../module.h" +#include "../util.h" +#include "drivers/gnss/septentrio/sbf/messages.h" + +namespace septentrio +{ + +namespace sbf +{ + +Decoder::State Decoder::add_byte(uint8_t byte) +{ + uint8_t *message = reinterpret_cast(&_message); + + switch (_state) { + case State::SearchingSync1: + if (byte == (uint8_t)k_sync1) { + // Sync is always same, so we don't store it. + _current_index++; + _state = State::SearchingSync2; + } + + break; + + case State::SearchingSync2: + if (byte == (uint8_t)k_sync2) { + // Sync is always same, so we don't store it. + _current_index++; + _state = State::Busy; + + } else { + reset(); + } + + break; + + case State::Busy: + message[_current_index] = byte; + _current_index++; + + if (done()) { + _state = State::Done; + } + + break; + + case State::Done: + SEP_WARN("SBF: Discarding excess byte"); + break; + } + + return _state; +} + +BlockID Decoder::id() const +{ + return _state == State::Done ? _message.header.id_number : BlockID::Invalid; +} + +int Decoder::parse(Header *header) const +{ + if (can_parse()) { + memcpy(header, &_message.header, sizeof(Header)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(DOP *message) const +{ + if (can_parse() && id() == BlockID::DOP) { + memcpy(message, _message.payload, sizeof(DOP)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(PVTGeodetic *message) const +{ + if (can_parse() && id() == BlockID::PVTGeodetic) { + memcpy(message, _message.payload, sizeof(PVTGeodetic)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(ReceiverStatus *message) const +{ + if (can_parse() && id() == BlockID::ReceiverStatus) { + memcpy(message, _message.payload, sizeof(ReceiverStatus)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(QualityInd *message) const +{ + if (can_parse() && id() == BlockID::QualityInd) { + // Safe to copy entire size of the message as it is smaller than the maximum expected SBF message size. + // It's up to the user of the parsed message to ignore the invalid fields. + memcpy(message, _message.payload, sizeof(QualityInd)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(RFStatus *message) const +{ + if (can_parse() && id() == BlockID::PVTGeodetic) { + memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band)); + + for (uint8_t i = 0; i < math::min(message->n, k_max_rfband_blocks); i++) { + memcpy(&message->rf_band[i], &_message.payload[sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i * + message->sb_length], sizeof(RFBand)); + } + + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(GALAuthStatus *message) const +{ + if (can_parse() && id() == BlockID::GALAuthStatus) { + memcpy(message, _message.payload, sizeof(GALAuthStatus)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(VelCovGeodetic *message) const +{ + if (can_parse() && id() == BlockID::VelCovGeodetic) { + memcpy(message, _message.payload, sizeof(VelCovGeodetic)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(GEOIonoDelay *message) const +{ + if (can_parse() && id() == BlockID::GEOIonoDelay) { + memcpy(message, _message.payload, sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc)); + + for (size_t i = 0; i < math::min(message->n, (uint8_t)4); i++) { + memcpy(&message->idc[i], &_message.payload[sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i * + message->sb_length], sizeof(IDC)); + } + + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(AttEuler *message) const +{ + if (can_parse() && id() == BlockID::AttEuler) { + memcpy(message, _message.payload, sizeof(AttEuler)); + return PX4_OK; + } + + return PX4_ERROR; +} + +int Decoder::parse(AttCovEuler *message) const +{ + if (can_parse() && id() == BlockID::AttCovEuler) { + memcpy(message, _message.payload, sizeof(AttCovEuler)); + return PX4_OK; + } + + return PX4_ERROR; +} + +void Decoder::reset() +{ + _current_index = 0; + _state = State::SearchingSync1; + memset(&_message, 0, sizeof(_message)); +} + +bool Decoder::done() const +{ + return (_current_index >= 14 && _current_index >= _message.header.length) || _current_index >= sizeof(_message); +} + +bool Decoder::can_parse() const +{ + return done() + && _message.header.crc == buffer_crc16(reinterpret_cast(&_message) + 4, _message.header.length - 4); +} + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf/decoder.h b/src/drivers/gnss/septentrio/sbf/decoder.h new file mode 100644 index 000000000000..78a62ebc0f42 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/decoder.h @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 decoder.h + * + * Decoding logic for the Septentrio Binary Format (SBF). + * + * @author Thomas Frans +*/ + +#pragma once + +#include + +#include "messages.h" + +namespace septentrio +{ + +namespace sbf +{ + +#pragma pack(push, 1) + +/// A complete SBF message with parsed header and unparsed body. +typedef struct { + Header header; + uint8_t payload[k_max_message_size]; +} message_t; + +#pragma pack(pop) + +/** + * @brief A decoder and parser for Septentrio Binary Format (SBF) data. + */ +class Decoder +{ +public: + /** + * @brief The current decoding state of the decoder. + */ + enum class State { + /// Searching for the first sync byte of an SBF message. + SearchingSync1, + + /// Searching for the second sync byte of an SBF message. + SearchingSync2, + + /// In the process of receiving an SBF message. + Busy, + + /// Done receiving an SBF message and ready to parse. + Done, + }; + + /** + * @brief Add one byte to the decoder. + * + * @param byte The byte to add. + * + * @return The decoding state after adding the byte. + */ + State add_byte(uint8_t byte); + + /** + * @brief Get the id of the current message. + * + * @return The SBF id of the current message. + */ + BlockID id() const; + + /** + * @brief Try to parse the current header. + * + * @param header The header data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(Header *header) const; + + /** + * @brief Parse a DOP SBF message. + * + * @param message The DOP data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(DOP *message) const; + + /** + * @brief Parse a PVTGeodetic SBF message. + * + * @param message The PVTGeodetic data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(PVTGeodetic *message) const; + + /** + * @brief Parse a ReceiverStatus SBF message. + * + * @param message The ReceiverStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(ReceiverStatus *message) const; + + /** + * @brief Parse a QualityInd SBF message. + * + * @param message The QualityInd data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(QualityInd *message) const; + + /** + * @brief Parse an RFSTatus SBF message. + * + * @param message The RFStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(RFStatus *message) const; + + /** + * @brief Parse a GALAuthStatus SBF message. + * + * @param message The GALAuthStatus data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(GALAuthStatus *message) const; + + /** + * @brief Parse a VelCovGeodetic SBF message. + * + * @param message The VelCovGeodetic data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(VelCovGeodetic *message) const; + + /** + * @brief Parse a GEOIonoDelay SBF message. + * + * @param message The GEOIonoDelay data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(GEOIonoDelay *message) const; // NOTE: This serves as an example of how to parse sub-blocks. + + /** + * @brief Parse an AttEuler SBF message. + * + * @param message The AttEuler data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(AttEuler *message) const; + + /** + * @brief Parse an AttCovEuler SBF message. + * + * @param message The AttCovEuler data structure to parse into. + * + * @return `PX4_OK` if success, or `PX4_ERROR` when an error occurs. + */ + int parse(AttCovEuler *message) const; + + /** + * @brief Reset the decoder to a clean state. + * + * If the decoder is in the process of decoding a message or contains a complete message, it will discard it and + * become ready for the next message. + */ + void reset(); +private: + /** + * @brief Check whether a full message has been received. + * + * Does not guarantee validity of the message, only that a complete message should be available. + * + * @return `true` if a message is ready, `false` if still decoding. + */ + bool done() const; + + /** + * @brief Check whether a full message has been received and is valid. + * + * @return `true` if the data can be parsed, `false` if the message is incomplete or not valid. + */ + bool can_parse() const; + + State _state{State::SearchingSync1}; + uint16_t _current_index; + message_t _message; +}; + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/sbf/messages.h b/src/drivers/gnss/septentrio/sbf/messages.h new file mode 100644 index 000000000000..487a40054c64 --- /dev/null +++ b/src/drivers/gnss/septentrio/sbf/messages.h @@ -0,0 +1,353 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 messages.h + * + * @brief Septentrio binary format (SBF) protocol message definitions. + * + * @author Thomas Frans + */ + +#pragma once + +#include +#include + +namespace septentrio +{ + +namespace sbf +{ + +constexpr char k_sync1 {'$'}; +constexpr char k_sync2 {'@'}; + +// Do-Not-Use values for fields in SBF blocks. The receiver can set certain fields to these values to signal that they are invalid. +// Most fields of a certain type will use these values (u2 representing a value often has DNU value of 65535). +// For the ones that do not use these, custom ones can be specified together with the blocks. +constexpr uint32_t k_dnu_u4_value {4294967295}; +constexpr uint32_t k_dnu_u4_bitfield {0}; +constexpr uint16_t k_dnu_u2_value {65535}; +constexpr uint16_t k_dnu_u2_bitfield {0}; +constexpr float k_dnu_f4_value {-2.0f * 10000000000.0f}; +constexpr double k_dnu_f8_value {-2.0 * 10000000000.0}; + +/// Maximum size of all expected messages. +/// This needs to be bigger than the maximum size of all declared SBF blocks so that `memcpy()` can safely copy from the decoding buffer using this value into messages. +constexpr size_t k_max_message_size {300}; + +/// Maximum expected number of sub-blocks. +constexpr uint8_t k_max_quality_indicators {9}; +constexpr uint8_t k_max_rfband_blocks {4}; + +/** + * IDs of all the available SBF messages that we care about. +*/ +enum class BlockID : uint16_t { + Invalid = 0, + DOP = 4001, + PVTGeodetic = 4007, + ReceiverStatus = 4014, + QualityInd = 4082, + RFStatus = 4092, + GALAuthStatus = 4245, + VelCovGeodetic = 5908, + EndOfPVT = 5921, + GEOIonoDelay = 5933, + AttEuler = 5938, + AttCovEuler = 5939, +}; + +#pragma pack(push, 1) + +/** + * Common SBF message header. + */ +struct Header { + uint8_t sync1; + uint8_t sync2; + uint16_t crc; + BlockID id_number: 13; + uint16_t id_revision: 3; + uint16_t length; + uint32_t tow; + uint16_t wnc; +}; + +struct DOP { + uint8_t nr_sv; + uint8_t reserved; + uint16_t p_dop; + uint16_t t_dop; + uint16_t h_dop; + uint16_t v_dop; + float hpl; + float vpl; +}; + +struct PVTGeodetic { + static constexpr uint8_t k_dnu_nr_sv {255}; + enum class ModeType : uint8_t { + NoPVT = 0, + StandAlonePVT = 1, + DifferentialPVT = 2, + FixedLocation = 3, + RTKFixed = 4, + RTKFloat = 5, + PVTWithSBAS = 6, + MovingBaseRTKFixed = 7, + MovingBaseRTKFloat = 8, + PrecisePointPositioning = 10, + }; + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + InsufficientEphemerides = 2, + DOPTooLarge = 3, + SquaredResidualSumTooLarge = 4, + NoConvergence = 5, + InsufficientMeasurementsAfterOutlierRejection = 6, + ExportLawsForbidPositionOutput = 7, + InsufficientDifferentialCorrections = 8, + MissingBaseStationCoordinates = 9, + MissingRequiredRTKFixedPosition = 10, + }; + ModeType mode_type: 4; + uint8_t mode_reserved: 2; + uint8_t mode_base_fixed: 1; + uint8_t mode_2d: 1; + Error error; + double latitude; + double longitude; + double height; + float undulation; + float vn; + float ve; + float vu; + float cog; + double rx_clk_bias; + float rx_clk_drift; + uint8_t time_system; + uint8_t datum; + uint8_t nr_sv; + uint8_t wa_corr_info; + uint16_t reference_id; + uint16_t mean_corr_age; + uint32_t signal_info; + uint8_t alert_flag; + uint8_t nr_bases; + uint16_t ppp_info; + uint16_t latency; + uint16_t h_accuracy; + uint16_t v_accuracy; +}; + +struct ReceiverStatus { + uint8_t cpu_load; + uint8_t ext_error_siserror: 1; + uint8_t ext_error_diff_corr_error: 1; + uint8_t ext_error_ext_sensor_error: 1; + uint8_t ext_error_setup_error: 1; + uint8_t ext_error_reserved: 4; + uint32_t uptime; + uint32_t rx_state_reserved1: 1; + uint32_t rx_state_active_antenna: 1; + uint32_t rx_state_ext_freq: 1; + uint32_t rx_state_ext_time: 1; + uint32_t rx_state_wn_set: 1; + uint32_t rx_state_tow_set: 1; + uint32_t rx_state_fine_time: 1; + uint32_t rx_state_internal_disk_activity: 1; + uint32_t rx_state_internal_disk_full: 1; + uint32_t rx_state_internal_disk_mounted: 1; + uint32_t rx_state_int_ant: 1; + uint32_t rx_state_refout_locked: 1; + uint32_t rx_state_reserved2: 1; + uint32_t rx_state_external_disk_activity: 1; + uint32_t rx_state_external_disk_full: 1; + uint32_t rx_state_external_disk_mounted: 1; + uint32_t rx_state_pps_in_cal: 1; + uint32_t rx_state_diff_corr_in: 1; + uint32_t rx_state_internet: 1; + uint32_t rx_state_reserved3: 13; + uint32_t rx_error_reserved1: 3; + uint32_t rx_error_software: 1; + uint32_t rx_error_watchdog: 1; + uint32_t rx_error_antenna: 1; + uint32_t rx_error_congestion: 1; + uint32_t rx_error_reserved2: 1; + uint32_t rx_error_missed_event: 1; + uint32_t rx_error_cpu_overload: 1; + uint32_t rx_error_invalid_config: 1; + uint32_t rx_error_out_of_geofence: 1; + uint32_t rx_error_reserved3: 22; + uint8_t n; + uint8_t sb_length; + uint8_t cmd_count; + uint8_t temperature; +}; + +struct QualityIndicator { + enum class Type : uint8_t { + Overall = 0, + GNSSSignalsMainAntenna = 1, + GNSSSignalsAuxAntenna = 2, + RFPowerMainAntenna = 11, + RFPowerAuxAntenna = 12, + CPUHeadroom = 21, + OCXOStability = 25, + BaseStationMeasurements = 30, + RTKPostProcessing = 31, + }; + Type type: 8; + uint16_t value: 4; + uint16_t reserved: 4; +}; + +struct QualityInd { + uint8_t n; + uint8_t reserved1; + // Only support the currently available indicators for now so we don't have to allocate. + QualityIndicator indicators[k_max_quality_indicators]; +}; + +struct RFBand { + uint32_t frequency; + uint16_t bandwidth; + uint8_t info_mode: 4; + uint8_t info_reserved: 2; + uint8_t info_antenna_id: 2; +}; + +struct RFStatus { + uint8_t n; + uint8_t sb_length; + uint8_t flags_inauthentic_gnss_signals: 1; + uint8_t flags_inauthentic_navigation_message: 1; + uint8_t flags_reserved: 6; + uint8_t reserved[3]; + RFBand rf_band[k_max_rfband_blocks]; +}; + +struct GALAuthStatus { + uint16_t osnma_status_status: 3; + uint16_t osnma_status_initialization_progress: 8; + uint16_t osnma_status_trusted_time_source: 3; + uint16_t osnma_status_merkle_tree_busy: 1; + uint16_t osnma_status_reserved: 1; + float trusted_time_delta; + uint64_t gal_active_mask; + uint64_t gal_authentic_mask; + uint64_t gps_active_mask; + uint64_t gps_authentic_mask; +}; + +struct VelCovGeodetic { + uint8_t mode_type: 4; + uint8_t mode_reserved: 2; + uint8_t mode_base_fixed: 1; + uint8_t mode_2d: 1; + uint8_t error; + float cov_vn_vn; + float cov_ve_ve; + float cov_vu_vu; + float cov_dt_dt; + float cov_vn_ve; + float cov_vn_vu; + float cov_vn_dt; + float cov_ve_vu; + float cov_ve_dt; + float cov_vu_dt; +}; + +struct IDC { + uint8_t igp_mask_no; + uint8_t givei; + uint8_t reserved[2]; + float vertical_delay; +}; + +struct GEOIonoDelay { + uint8_t prn; + uint8_t bandnbr; + uint8_t iodi; + uint8_t n; + uint8_t sb_length; + uint8_t reserved; + IDC idc[4]; +}; + +struct AttEuler { + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + }; + uint8_t nr_sv; + Error error_aux1: 2; + Error error_aux2: 2; + uint8_t error_reserved: 3; + uint8_t error_not_requested: 1; + uint16_t mode; + uint16_t reserved; + float heading; + float pitch; + float roll; + float pitch_dot; + float roll_dot; + float heading_dot; +}; + +struct AttCovEuler { + enum class Error : uint8_t { + None = 0, + InsufficientMeasurements = 1, + }; + uint8_t reserved; + Error error_aux1: 2; + Error error_aux2: 2; + uint8_t error_reserved: 3; + uint8_t error_not_requested: 1; + float cov_headhead; + float cov_pitchpitch; + float cov_rollroll; + float cov_headpitch; + float cov_headroll; + float cov_pitchroll; +}; + +#pragma pack(pop) + +} // namespace sbf + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/septentrio.cpp b/src/drivers/gnss/septentrio/septentrio.cpp new file mode 100644 index 000000000000..5c53db6098d5 --- /dev/null +++ b/src/drivers/gnss/septentrio/septentrio.cpp @@ -0,0 +1,1751 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 septentrio.cpp + * + * Septentrio GNSS receiver driver + * + * @author Matej Franceskin + * @author Seppe Geuens + * @author Thomas Frans +*/ + +#include "septentrio.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "util.h" +#include "sbf/messages.h" + +using namespace device; + +namespace septentrio +{ + +/** + * RTC drift time when time synchronization is needed (in seconds). +*/ +constexpr int k_max_allowed_clock_drift = 5; + +/** + * If silence from the receiver for this time (ms), assume full data received. +*/ +constexpr int k_receiver_read_timeout = 2; + +/** + * The maximum allowed time for reading from the receiver. + */ +constexpr int k_max_receiver_read_timeout = 50; + +/** + * Minimum amount of bytes we try to read at one time from the receiver. +*/ +constexpr size_t k_min_receiver_read_bytes = 32; + +/** + * The baud rate of Septentrio receivers with factory default configuration. +*/ +constexpr uint32_t k_septentrio_receiver_default_baud_rate = 115200; + +constexpr uint8_t k_max_command_size = 120; +constexpr uint16_t k_timeout_5hz = 500; +constexpr uint32_t k_read_buffer_size = 150; +constexpr time_t k_gps_epoch_secs = 1234567890ULL; // TODO: This seems wrong + +// Septentrio receiver commands +// - erst: exeResetReceiver +// - sso: setSBFOutput +// - ssu: setSatelliteUsage +// - scs: setCOMSettings +// - srd: setReceiverDynamics +// - sto: setAttitudeOffset +// - sdio: setDataInOut +// - gecm: getEchoMessage +// - sga: setGNSSAttitude +constexpr const char *k_command_force_input = "SSSSSSSSSS\n"; +constexpr const char *k_command_reset_hot = "erst,soft,none\n"; +constexpr const char *k_command_reset_warm = "erst,soft,PVTData\n"; +constexpr const char *k_command_reset_cold = "erst,hard,SatData\n"; +constexpr const char *k_command_sbf_output_pvt = + "sso,Stream%lu,%s,PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler+EndOfPVT+ReceiverStatus,%s\n"; +constexpr const char *k_command_set_sbf_output = + "sso,Stream%lu,%s,%s%s,%s\n"; +constexpr const char *k_command_clear_sbf = "sso,Stream%lu,%s,none,off\n"; +constexpr const char *k_command_set_baud_rate = + "scs,%s,baud%lu\n"; // The receiver sends the reply at the new baud rate! +constexpr const char *k_command_set_dynamics = "srd,%s,UAV\n"; +constexpr const char *k_command_set_attitude_offset = "sto,%.3f,%.3f\n"; +constexpr const char *k_command_set_data_io = "sdio,%s,Auto,%s\n"; +constexpr const char *k_command_set_satellite_usage = "ssu,%s\n"; +constexpr const char *k_command_ping = "gecm\n"; +constexpr const char *k_command_set_gnss_attitude = "sga,%s\n"; + +constexpr const char *k_gnss_attitude_source_moving_base = "MovingBase"; +constexpr const char *k_gnss_attitude_source_multi_antenna = "MultiAntenna"; + +constexpr const char *k_frequency_0_1hz = "sec10"; +constexpr const char *k_frequency_0_2hz = "sec5"; +constexpr const char *k_frequency_0_5hz = "sec2"; +constexpr const char *k_frequency_1_0hz = "sec1"; +constexpr const char *k_frequency_2_0hz = "msec500"; +constexpr const char *k_frequency_5_0hz = "msec200"; +constexpr const char *k_frequency_10_0hz = "msec100"; +constexpr const char *k_frequency_20_0hz = "msec50"; +constexpr const char *k_frequency_25_0hz = "msec40"; +constexpr const char *k_frequency_50_0hz = "msec20"; + +px4::atomic SeptentrioDriver::_secondary_instance {nullptr}; +uint32_t SeptentrioDriver::k_supported_baud_rates[] {0, 38400, 57600, 115200, 230400, 460800, 500000, 576000, 921600, 1000000, 1500000}; +uint32_t SeptentrioDriver::k_default_baud_rate {230400}; +orb_advert_t SeptentrioDriver::k_mavlink_log_pub {nullptr}; + +SeptentrioDriver::SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate) : + Device(MODULE_NAME), + _instance(instance), + _chosen_baud_rate(baud_rate) +{ + strncpy(_port, device_path, sizeof(_port) - 1); + // Enforce null termination. + _port[sizeof(_port) - 1] = '\0'; + + int32_t enable_sat_info {0}; + get_parameter("SEP_SAT_INFO", &enable_sat_info); + + if (enable_sat_info) { + _message_satellite_info = new satellite_info_s(); + } + + get_parameter("SEP_YAW_OFFS", &_heading_offset); + get_parameter("SEP_PITCH_OFFS", &_pitch_offset); + + int32_t dump_mode {0}; + get_parameter("SEP_DUMP_COMM", &dump_mode); + DumpMode mode = static_cast(dump_mode); + + if (mode != DumpMode::Disabled) { + initialize_communication_dump(mode); + } + + int32_t receiver_stream_main {k_default_main_stream}; + get_parameter("SEP_STREAM_MAIN", &receiver_stream_main); + _receiver_stream_main = receiver_stream_main; + int32_t receiver_stream_log {k_default_log_stream}; + get_parameter("SEP_STREAM_LOG", &receiver_stream_log); + _receiver_stream_log = receiver_stream_log; + + if (_receiver_stream_log == _receiver_stream_main) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Logging stream should be different from main stream"); + } + + int32_t automatic_configuration {true}; + get_parameter("SEP_AUTO_CONFIG", &automatic_configuration); + _automatic_configuration = static_cast(automatic_configuration); + + get_parameter("SEP_CONST_USAGE", &_receiver_constellation_usage); + + int32_t logging_frequency {static_cast(ReceiverLogFrequency::Hz1_0)}; + get_parameter("SEP_LOG_HZ", &logging_frequency); + _receiver_logging_frequency = static_cast(logging_frequency); + int32_t logging_level {static_cast(ReceiverLogLevel::Default)}; + get_parameter("SEP_LOG_LEVEL", &logging_level); + _receiver_logging_level = static_cast(logging_level); + int32_t logging_overwrite {false}; + get_parameter("SEP_LOG_FORCE", &logging_overwrite); + _receiver_logging_overwrite = logging_overwrite; + int32_t receiver_setup {static_cast(ReceiverSetup::Default)}; + get_parameter("SEP_HARDW_SETUP", &receiver_setup); + _receiver_setup = static_cast(receiver_setup); + int32_t sbf_output_frequency {static_cast(SBFOutputFrequency::Hz5_0)}; + get_parameter("SEP_OUTP_HZ", &sbf_output_frequency); + _sbf_output_frequency = static_cast(sbf_output_frequency); + + if (_instance == Instance::Secondary && _receiver_setup == ReceiverSetup::MovingBase) { + _rtcm_decoder = new rtcm::Decoder(); + } + + set_device_type(DRV_GPS_DEVTYPE_SBF); + + reset_gps_state_message(); +} + +SeptentrioDriver::~SeptentrioDriver() +{ + if (_instance == Instance::Main) { + if (await_second_instance_shutdown() == PX4_ERROR) { + SEP_ERR("Secondary instance shutdown timed out"); + } + } + + if (_message_data_from_receiver) { + delete _message_data_from_receiver; + } + + if (_message_data_to_receiver) { + delete _message_data_to_receiver; + } + + if (_message_satellite_info) { + delete _message_satellite_info; + } + + if (_rtcm_decoder) { + delete _rtcm_decoder; + } +} + +int SeptentrioDriver::print_status() +{ + SeptentrioDriver *secondary_instance = _secondary_instance.load(); + + switch (_instance) { + case Instance::Main: + PX4_INFO("Main GPS"); + break; + + case Instance::Secondary: + PX4_INFO(""); + PX4_INFO("Secondary GPS"); + break; + } + + PX4_INFO("health: %s, port: %s, baud rate: %lu", is_healthy() ? "OK" : "NOT OK", _port, _uart.getBaudrate()); + PX4_INFO("controller -> receiver data rate: %lu B/s", output_data_rate()); + PX4_INFO("receiver -> controller data rate: %lu B/s", input_data_rate()); + PX4_INFO("sat info: %s", (_message_satellite_info != nullptr) ? "enabled" : "disabled"); + + if (first_gps_uorb_message_created() && _state == State::ReceivingData) { + PX4_INFO("rate RTCM injection: %6.2f Hz", static_cast(rtcm_injection_frequency())); + print_message(ORB_ID(sensor_gps), _message_gps_state); + } + + if (_instance == Instance::Main && secondary_instance) { + secondary_instance->print_status(); + } + + return 0; +} + +void SeptentrioDriver::run() +{ + while (!should_exit()) { + switch (_state) { + case State::OpeningSerialPort: { + _uart.setPort(_port); + + if (_uart.open()) { + _state = State::DetectingBaudRate; + + } else { + // Failed to open port, so wait a bit before trying again. + px4_usleep(200000); + } + + break; + } + + case State::DetectingBaudRate: { + static uint32_t expected_baud_rates[] = {k_septentrio_receiver_default_baud_rate, 115200, 230400, 57600, 460800, 500000, 576000, 38400, 921600, 1000000, 1500000}; + expected_baud_rates[0] = _chosen_baud_rate != 0 ? _chosen_baud_rate : k_septentrio_receiver_default_baud_rate; + + if (detect_receiver_baud_rate(expected_baud_rates[_current_baud_rate_index], true)) { + if (set_baudrate(expected_baud_rates[_current_baud_rate_index]) == PX4_OK) { + _state = State::ConfiguringDevice; + + } else { + SEP_ERR("Setting local baud rate failed"); + } + + } else { + _current_baud_rate_index++; + + if (_current_baud_rate_index == sizeof(expected_baud_rates) / sizeof(expected_baud_rates[0])) { + _current_baud_rate_index = 0; + } + } + + break; + } + + case State::ConfiguringDevice: { + ConfigureResult result = configure(); + + if (!(static_cast(result) & static_cast(ConfigureResult::FailedCompletely))) { + if (static_cast(result) & static_cast(ConfigureResult::NoLogging)) { + mavlink_log_warning(&k_mavlink_log_pub, "Septentrio: Failed to configure receiver internal logging"); + } + + SEP_INFO("Automatic configuration finished"); + _state = State::ReceivingData; + + } else { + _state = State::DetectingBaudRate; + } + + break; + } + + case State::ReceivingData: { + int receive_result {0}; + + receive_result = receive(k_timeout_5hz); + + if (receive_result == -1) { + _state = State::DetectingBaudRate; + } + + if (_message_satellite_info && (receive_result & 2)) { + publish_satellite_info(); + } + + break; + } + + } + + reset_if_scheduled(); + + handle_inject_data_topic(); + + if (update_monitoring_interval_ended()) { + start_update_monitoring_interval(); + } + } + +} + +int SeptentrioDriver::task_spawn(int argc, char *argv[]) +{ + return task_spawn(argc, argv, Instance::Main); +} + +int SeptentrioDriver::task_spawn(int argc, char *argv[], Instance instance) +{ + px4_main_t entry_point; + static constexpr int k_task_stack_size = PX4_STACK_ADJUSTED(2048); + + if (instance == Instance::Main) { + entry_point = &run_trampoline; + + } else { + entry_point = &run_trampoline_secondary; + } + + px4_task_t task_id = px4_task_spawn_cmd("septentrio", + SCHED_DEFAULT, + SCHED_PRIORITY_SLOW_DRIVER, + k_task_stack_size, + entry_point, + (char *const *)argv); + + if (task_id < 0) { + // `_task_id` of module that hasn't been started before or has been stopped should already be -1. + // This is just to make sure. + _task_id = -1; + return -errno; + } + + if (instance == Instance::Main) { + _task_id = task_id; + } + + return 0; +} + +int SeptentrioDriver::run_trampoline_secondary(int argc, char *argv[]) +{ + // Get rid of the task name (first argument) + argc -= 1; + argv += 1; + + SeptentrioDriver *gps = instantiate(argc, argv, Instance::Secondary); + + if (gps) { + _secondary_instance.store(gps); + gps->run(); + _secondary_instance.store(nullptr); + delete gps; + + } else { + return -1; + } + + return 0; +} + +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[]) +{ + return instantiate(argc, argv, Instance::Main); +} + +SeptentrioDriver *SeptentrioDriver::instantiate(int argc, char *argv[], Instance instance) +{ + ModuleArguments arguments {}; + SeptentrioDriver *gps {nullptr}; + + if (parse_cli_arguments(argc, argv, arguments) == PX4_ERROR) { + return nullptr; + } + + if (arguments.device_path_main && arguments.device_path_secondary + && strcmp(arguments.device_path_main, arguments.device_path_secondary) == 0) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Device paths must be different"); + return nullptr; + } + + bool valid_chosen_baud_rate {false}; + + for (uint8_t i = 0; i < sizeof(k_supported_baud_rates) / sizeof(k_supported_baud_rates[0]); i++) { + switch (instance) { + case Instance::Main: + if (arguments.baud_rate_main == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + + case Instance::Secondary: + if (arguments.baud_rate_secondary == static_cast(k_supported_baud_rates[i])) { + valid_chosen_baud_rate = true; + } + + break; + } + } + + if (!valid_chosen_baud_rate) { + mavlink_log_critical(&k_mavlink_log_pub, "Septentrio: Baud rate %d is unsupported, falling back to default %lu", + instance == Instance::Main ? arguments.baud_rate_main : arguments.baud_rate_secondary, k_default_baud_rate); + } + + if (instance == Instance::Main) { + if (Serial::validatePort(arguments.device_path_main)) { + gps = new SeptentrioDriver(arguments.device_path_main, instance, + valid_chosen_baud_rate ? arguments.baud_rate_main : k_default_baud_rate); + + } else { + PX4_ERR("Invalid device (-d) %s", arguments.device_path_main ? arguments.device_path_main : ""); + } + + if (gps && arguments.device_path_secondary) { + task_spawn(argc, argv, Instance::Secondary); + + if (await_second_instance_startup() == PX4_ERROR) { + return nullptr; + } + } + + } else { + if (Serial::validatePort(arguments.device_path_secondary)) { + gps = new SeptentrioDriver(arguments.device_path_secondary, instance, + valid_chosen_baud_rate ? arguments.baud_rate_secondary : k_default_baud_rate); + + } else { + PX4_ERR("Invalid secondary device (-e) %s", arguments.device_path_secondary ? arguments.device_path_secondary : ""); + } + } + + return gps; +} + +// Called from outside driver thread. +// Return 0 on success, -1 otherwise. +int SeptentrioDriver::custom_command(int argc, char *argv[]) +{ + bool handled = false; + const char *failure_reason {"unknown command"}; + SeptentrioDriver *driver_instance; + + if (!is_running()) { + PX4_INFO("not running"); + return -1; + } + + driver_instance = get_instance(); + + if (argc >= 1 && strcmp(argv[0], "reset") == 0) { + if (argc == 2) { + ReceiverResetType type{ReceiverResetType::None}; + + if (strcmp(argv[1], "hot") == 0) { + type = ReceiverResetType::Hot; + + } else if (strcmp(argv[1], "warm") == 0) { + type = ReceiverResetType::Warm; + + } else if (strcmp(argv[1], "cold") == 0) { + type = ReceiverResetType::Cold; + + } else { + failure_reason = "unknown reset type"; + } + + if (type != ReceiverResetType::None) { + driver_instance->schedule_reset(type); + handled = true; + } + + } else { + failure_reason = "incorrect usage of reset command"; + } + } + + return handled ? 0 : print_usage(failure_reason); +} + +int SeptentrioDriver::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for Septentrio GNSS receivers. +It can automatically configure them and make their output available for the rest of the system. +A secondary receiver is supported for redundancy, logging and dual-receiver heading. +Septentrio receiver baud rates from 57600 to 1500000 are supported. +If others are used, the driver will use 230400 and give a warning. + +### Examples + +Use one receiver on port `/dev/ttyS0` and automatically configure it to use baud rate 230400: +$ septentrio start -d /dev/ttyS0 -b 230400 + +Use two receivers, the primary on port `/dev/ttyS3` and the secondary on `/dev/ttyS4`, +detect baud rate automatically and preserve them: +$ septentrio start -d /dev/ttyS3 -e /dev/ttyS4 + +Perform warm reset of the receivers: +$ gps reset warm +)DESCR_STR"); + PRINT_MODULE_USAGE_NAME("septentrio", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Primary receiver port", false); + PRINT_MODULE_USAGE_PARAM_INT('b', 0, 57600, 1500000, "Primary receiver baud rate", true); + PRINT_MODULE_USAGE_PARAM_STRING('e', nullptr, "", "Secondary receiver port", true); + PRINT_MODULE_USAGE_PARAM_INT('g', 0, 57600, 1500000, "Secondary receiver baud rate", true); + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "Reset connected receiver"); + PRINT_MODULE_USAGE_ARG("cold|warm|hot", "Specify reset type", false); + + return 0; +} + +int SeptentrioDriver::reset(ReceiverResetType type) +{ + bool res = false; + + force_input(); + + switch (type) { + case ReceiverResetType::Hot: + res = send_message_and_wait_for_ack(k_command_reset_hot, k_receiver_ack_timeout_fast); + break; + + case ReceiverResetType::Warm: + res = send_message_and_wait_for_ack(k_command_reset_warm, k_receiver_ack_timeout_fast); + break; + + case ReceiverResetType::Cold: + res = send_message_and_wait_for_ack(k_command_reset_cold, k_receiver_ack_timeout_fast); + break; + + default: + break; + } + + if (res) { + return PX4_OK; + } else { + return PX4_ERROR; + } +} + +int SeptentrioDriver::force_input() +{ + ssize_t written = write(reinterpret_cast(k_command_force_input), strlen(k_command_force_input)); + + if (written < 0) { + return PX4_ERROR; + } else { + // The receiver can't receive input right after forcing input. From testing, the duration seems to be 1 ms, so wait 10 ms to be sure. + px4_usleep(10000); + return PX4_OK; + } +} + +int SeptentrioDriver::parse_cli_arguments(int argc, char *argv[], ModuleArguments& arguments) +{ + int ch{'\0'}; + int myoptind{1}; + const char *myoptarg{nullptr}; + + while ((ch = px4_getopt(argc, argv, "d:e:b:g:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + if (px4_get_parameter_value(myoptarg, arguments.baud_rate_main) != 0) { + PX4_ERR("Baud rate parsing failed"); + return PX4_ERROR; + } + break; + case 'g': + if (px4_get_parameter_value(myoptarg, arguments.baud_rate_secondary) != 0) { + PX4_ERR("Baud rate parsing failed"); + return PX4_ERROR; + } + break; + case 'd': + arguments.device_path_main = myoptarg; + break; + + case 'e': + arguments.device_path_secondary = myoptarg; + break; + + case '?': + return PX4_ERROR; + + default: + PX4_WARN("unrecognized flag"); + return PX4_ERROR; + break; + } + } + + return PX4_OK; +} + +int SeptentrioDriver::await_second_instance_startup() +{ + uint32_t i = 0; + + do { + px4_usleep(2500); + } while (!_secondary_instance.load() && ++i < 400); + + if (!_secondary_instance.load()) { + SEP_ERR("Timed out while waiting for second instance to start"); + return PX4_ERROR; + } + + return PX4_OK; +} + +int SeptentrioDriver::await_second_instance_shutdown() +{ + if (_instance == Instance::Secondary) { + return PX4_OK; + } + + SeptentrioDriver* secondary_instance = _secondary_instance.load(); + + if (secondary_instance) { + secondary_instance->request_stop(); + + uint32_t i {0}; + + // Give the secondary instance 2 seconds at most to properly shut down. + while (_secondary_instance.load() && i < 100) { + px4_usleep(20000); + + ++i; + } + + return _secondary_instance.load() ? PX4_ERROR : PX4_OK; + } else { + return PX4_OK; + } +} + +void SeptentrioDriver::schedule_reset(ReceiverResetType reset_type) +{ + SeptentrioDriver *secondary_instance = _secondary_instance.load(); + + _scheduled_reset.store((int)reset_type); + + if (_instance == Instance::Main && secondary_instance) { + secondary_instance->schedule_reset(reset_type); + } +} + +bool SeptentrioDriver::detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input) { + if (set_baudrate(baud_rate) != PX4_OK) { + return false; + } + + if (forced_input) { + force_input(); + } + + // Make sure that any weird data is "flushed" in the receiver. + (void)send_message("\n"); + + if (send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_INFO("Detected baud rate: %lu", baud_rate); + return true; + } + + return false; +} + +int SeptentrioDriver::detect_serial_port(char* const port_name) { + // Read buffer to get the COM port + char buf[k_read_buffer_size]; + size_t buffer_offset = 0; // The offset into the string where the next data should be read to. + hrt_abstime timeout_time = hrt_absolute_time() + 5 * 1000 * k_receiver_ack_timeout_fast; + bool response_detected = false; + + // Receiver prints prompt after a message. + if (!send_message(k_command_ping)) { + return PX4_ERROR; + } + + do { + // Read at most the amount of available bytes in the buffer after the current offset, -1 because we need '\0' at the end for a valid string. + int read_result = read(reinterpret_cast(buf) + buffer_offset, sizeof(buf) - buffer_offset - 1, k_receiver_ack_timeout_fast); + + if (read_result < 0) { + SEP_WARN("SBF read error"); + return PX4_ERROR; + } + + // Sanitize the data so it doesn't contain any `0` values. + for (size_t i = buffer_offset; i < buffer_offset + read_result; i++) { + if (buf[i] == 0) { + buf[i] = 1; + } + } + + buffer_offset += read_result; + + // Make sure the current buffer is a valid string. + buf[buffer_offset] = '\0'; + + char* port_name_address = strstr(buf, ">"); + + // Check if we found a port candidate. + if (buffer_offset > 4 && port_name_address != nullptr) { + size_t port_name_offset = reinterpret_cast(port_name_address) - reinterpret_cast(buf) - 4; + for (size_t i = 0; i < 4; i++) { + port_name[i] = buf[port_name_offset + i]; + } + // NOTE: This limits the ports to serial and USB ports only. Otherwise the detection doesn't work correctly. + if (strstr(port_name, "COM") != nullptr || strstr(port_name, "USB") != nullptr) { + response_detected = true; + break; + } + } + + if (buffer_offset + 1 >= sizeof(buf)) { + // Copy the last 3 bytes such that a half port isn't lost. + for (int i = 0; i < 4; i++) { + buf[i] = buf[sizeof(buf) - 4 + i]; + } + buffer_offset = 3; + } + } while (timeout_time > hrt_absolute_time()); + + if (!response_detected) { + SEP_WARN("No valid serial port detected"); + return PX4_ERROR; + } else { + SEP_INFO("Serial port found: %s", port_name); + return PX4_OK; + } +} + +SeptentrioDriver::ConfigureResult SeptentrioDriver::configure() +{ + char msg[k_max_command_size] {}; + char com_port[5] {}; + ConfigureResult result {ConfigureResult::OK}; + + // Passively detect receiver port. + if (detect_serial_port(com_port) != PX4_OK) { + SEP_WARN("CONFIG: failed port detection"); + return ConfigureResult::FailedCompletely; + } + + // We should definitely match baud rates and detect used port, but don't do other configuration if not requested. + // This will force input on the receiver. That shouldn't be a problem as it's on our own connection. + if (!_automatic_configuration) { + return ConfigureResult::OK; + } + + // If user requested specific baud rate, set it now. Otherwise keep detected baud rate. + if (strstr(com_port, "COM") != nullptr && _chosen_baud_rate != 0) { + snprintf(msg, sizeof(msg), k_command_set_baud_rate, com_port, _chosen_baud_rate); + + if (!send_message(msg)) { + SEP_WARN("CONFIG: baud rate command write error"); + return ConfigureResult::FailedCompletely; + } + + // When sending a command and setting the baud rate right after, the controller could send the command at the new baud rate. + // From testing this could take some time. + px4_usleep(2000000); + + if (set_baudrate(_chosen_baud_rate) != PX4_OK) { + SEP_WARN("CONFIG: failed local baud rate setting"); + return ConfigureResult::FailedCompletely; + } + + if (!send_message_and_wait_for_ack(k_command_ping, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed remote baud rate setting"); + return ConfigureResult::FailedCompletely; + } + } + + // Delete all sbf outputs on current COM port to remove clutter data + snprintf(msg, sizeof(msg), k_command_clear_sbf, _receiver_stream_main, com_port); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: failed delete output"); + return ConfigureResult::FailedCompletely; // connection and/or baudrate detection failed + } + + // Set up the satellites used in PVT computation. + if (_receiver_constellation_usage != static_cast(SatelliteUsage::Default)) { + char requested_satellites[40] {}; + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GPS)) { + strcat(requested_satellites, "GPS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::GLONASS)) { + strcat(requested_satellites, "GLONASS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::Galileo)) { + strcat(requested_satellites, "GALILEO+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::SBAS)) { + strcat(requested_satellites, "SBAS+"); + } + if (_receiver_constellation_usage & static_cast(SatelliteUsage::BeiDou)) { + strcat(requested_satellites, "BEIDOU+"); + } + // Make sure to remove the trailing '+' if any. + requested_satellites[math::max(static_cast(strlen(requested_satellites)) - 1, 0)] = '\0'; + snprintf(msg, sizeof(msg), k_command_set_satellite_usage, requested_satellites); + // Use a longer timeout as the `setSatelliteUsage` command acknowledges a bit slower on mosaic-H-based receivers. + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_slow)) { + SEP_WARN("CONFIG: Failed to configure constellation usage"); + return ConfigureResult::FailedCompletely; + } + } + + // Internal logging on the receiver. + if (_receiver_logging_frequency != ReceiverLogFrequency::Disabled && _receiver_stream_log != _receiver_stream_main) { + const char *frequency {nullptr}; + const char *level {nullptr}; + + switch (_receiver_logging_frequency) { + case ReceiverLogFrequency::Hz0_1: + frequency = k_frequency_0_1hz; + break; + case ReceiverLogFrequency::Hz0_2: + frequency = k_frequency_0_2hz; + break; + case ReceiverLogFrequency::Hz0_5: + frequency = k_frequency_0_5hz; + break; + case ReceiverLogFrequency::Hz1_0: + default: + frequency = k_frequency_1_0hz; + break; + case ReceiverLogFrequency::Hz2_0: + frequency = k_frequency_2_0hz; + break; + case ReceiverLogFrequency::Hz5_0: + frequency = k_frequency_5_0hz; + break; + case ReceiverLogFrequency::Hz10_0: + frequency = k_frequency_10_0hz; + break; + case ReceiverLogFrequency::Hz20_0: + frequency = k_frequency_20_0hz; + break; + case ReceiverLogFrequency::Hz25_0: + frequency = k_frequency_25_0hz; + break; + case ReceiverLogFrequency::Hz50_0: + frequency = k_frequency_50_0hz; + break; + } + + switch (_receiver_logging_level) { + case ReceiverLogLevel::Lite: + level = "Comment+ReceiverStatus"; + break; + case ReceiverLogLevel::Basic: + level = "Comment+ReceiverStatus+PostProcess+Event"; + break; + case ReceiverLogLevel::Default: + default: + level = "Comment+ReceiverStatus+PostProcess+Event+Support"; + break; + case ReceiverLogLevel::Full: + level = "Comment+ReceiverStatus+PostProcess+Event+Support+BBSamples"; + break; + } + + snprintf(msg, sizeof(msg), k_command_set_sbf_output, _receiver_stream_log, "DSK1", _receiver_logging_overwrite ? "" : "+", level, frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); + } + } else if (_receiver_stream_log == _receiver_stream_main) { + result = static_cast(static_cast(result) | static_cast(ConfigureResult::NoLogging)); + } + + // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "SBF"); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + // Specify the offsets that the receiver applies to the computed attitude angles. + snprintf(msg, sizeof(msg), k_command_set_attitude_offset, static_cast(_heading_offset), static_cast(_pitch_offset)); + + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + snprintf(msg, sizeof(msg), k_command_set_dynamics, "high"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + return ConfigureResult::FailedCompletely; + } + + const char *sbf_frequency {k_frequency_10_0hz}; + switch (_sbf_output_frequency) { + case SBFOutputFrequency::Hz5_0: + sbf_frequency = k_frequency_5_0hz; + break; + case SBFOutputFrequency::Hz10_0: + sbf_frequency = k_frequency_10_0hz; + break; + case SBFOutputFrequency::Hz20_0: + sbf_frequency = k_frequency_20_0hz; + break; + case SBFOutputFrequency::Hz25_0: + sbf_frequency = k_frequency_25_0hz; + break; + } + + // Output a set of SBF blocks on a given connection at a regular interval. + snprintf(msg, sizeof(msg), k_command_sbf_output_pvt, _receiver_stream_main, com_port, sbf_frequency); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure SBF"); + return ConfigureResult::FailedCompletely; + } + + if (_receiver_setup == ReceiverSetup::MovingBase) { + if (_instance == Instance::Secondary) { + snprintf(msg, sizeof(msg), k_command_set_data_io, com_port, "+RTCMv3"); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure RTCM output"); + } + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_moving_base); + if (!send_message_and_wait_for_ack(msg, k_receiver_ack_timeout_fast)) { + SEP_WARN("CONFIG: Failed to configure attitude source"); + } + } + } else { + snprintf(msg, sizeof(msg), k_command_set_gnss_attitude, k_gnss_attitude_source_multi_antenna); + // This fails on single-antenna receivers, which is fine. Therefore don't check for acknowledgement. + if (!send_message(msg)) { + SEP_WARN("CONFIG: Failed to configure attitude source"); + return ConfigureResult::FailedCompletely; + } + } + + return result; +} + +int SeptentrioDriver::parse_char(const uint8_t byte) +{ + int result = 0; + + switch (_active_decoder) { + case DecodingStatus::Searching: + if (_sbf_decoder.add_byte(byte) != sbf::Decoder::State::SearchingSync1) { + _active_decoder = DecodingStatus::SBF; + } else if (_rtcm_decoder && _rtcm_decoder->add_byte(byte) != rtcm::Decoder::State::SearchingPreamble) { + _active_decoder = DecodingStatus::RTCMv3; + } + break; + case DecodingStatus::SBF: + if (_sbf_decoder.add_byte(byte) == sbf::Decoder::State::Done) { + if (process_message() == PX4_OK) { + result = 1; + } + _sbf_decoder.reset(); + _active_decoder = DecodingStatus::Searching; + } + break; + case DecodingStatus::RTCMv3: + if (_rtcm_decoder->add_byte(byte) == rtcm::Decoder::State::Done) { + if (process_message() == PX4_OK) { + result = 1; + } + _rtcm_decoder->reset(); + _active_decoder = DecodingStatus::Searching; + } + break; + } + + return result; +} + +int SeptentrioDriver::process_message() +{ + int result = PX4_ERROR; + + switch (_active_decoder) { + case DecodingStatus::Searching: { + SEP_ERR("Can't process incomplete message!"); + result = PX4_ERROR; + break; + } + case DecodingStatus::SBF: { + using namespace sbf; + + switch (_sbf_decoder.id()) { + case BlockID::Invalid: { + SEP_TRACE_PARSING("Tried to process invalid SBF message"); + break; + } + case BlockID::DOP: { + SEP_TRACE_PARSING("Processing DOP SBF message"); + _current_interval_messages.dop = true; + + DOP dop; + + if (_sbf_decoder.parse(&dop) == PX4_OK) { + _message_gps_state.hdop = dop.h_dop * 0.01f; + _message_gps_state.vdop = dop.v_dop * 0.01f; + result = PX4_OK; + } + + break; + } + case BlockID::PVTGeodetic: { + using ModeType = PVTGeodetic::ModeType; + using Error = PVTGeodetic::Error; + + SEP_TRACE_PARSING("Processing PVTGeodetic SBF message"); + _current_interval_messages.pvt_geodetic = true; + + Header header; + PVTGeodetic pvt_geodetic; + + if (_sbf_decoder.parse(&header) == PX4_OK && _sbf_decoder.parse(&pvt_geodetic) == PX4_OK) { + switch (pvt_geodetic.mode_type) { + case ModeType::NoPVT: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; + break; + case ModeType::PVTWithSBAS: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL; + break; + case ModeType::RTKFloat: + case ModeType::MovingBaseRTKFloat: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT; + break; + case ModeType::RTKFixed: + case ModeType::MovingBaseRTKFixed: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED; + break; + default: + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_3D; + break; + } + + // Check boundaries and invalidate GPS velocities + if (pvt_geodetic.vn <= k_dnu_f4_value || pvt_geodetic.ve <= k_dnu_f4_value || pvt_geodetic.vu <= k_dnu_f4_value) { + _message_gps_state.vel_ned_valid = false; + } + + if (pvt_geodetic.latitude > k_dnu_f8_value && pvt_geodetic.longitude > k_dnu_f8_value && pvt_geodetic.height > k_dnu_f8_value && pvt_geodetic.undulation > k_dnu_f4_value) { + _message_gps_state.latitude_deg = pvt_geodetic.latitude * M_RAD_TO_DEG; + _message_gps_state.longitude_deg = pvt_geodetic.longitude * M_RAD_TO_DEG; + _message_gps_state.altitude_msl_m = pvt_geodetic.height - static_cast(pvt_geodetic.undulation); + _message_gps_state.altitude_ellipsoid_m = pvt_geodetic.height; + } else { + _message_gps_state.fix_type = sensor_gps_s::FIX_TYPE_NONE; + } + + if (pvt_geodetic.nr_sv != PVTGeodetic::k_dnu_nr_sv) { + _message_gps_state.satellites_used = pvt_geodetic.nr_sv; + + if (_message_satellite_info) { + // Only fill in the satellite count for now (we could use the ChannelStatus message for the + // other data, but it's really large: >800B) + _message_satellite_info->timestamp = hrt_absolute_time(); + _message_satellite_info->count = _message_gps_state.satellites_used; + } + + } else { + _message_gps_state.satellites_used = 0; + } + + /* H and V accuracy are reported in 2DRMS, but based off the u-blox reporting we expect RMS. + * Divide by 100 from cm to m and in addition divide by 2 to get RMS. */ + _message_gps_state.eph = static_cast(pvt_geodetic.h_accuracy) / 200.0f; + _message_gps_state.epv = static_cast(pvt_geodetic.v_accuracy) / 200.0f; + + // Check fix and error code + _message_gps_state.vel_ned_valid = _message_gps_state.fix_type > sensor_gps_s::FIX_TYPE_NONE && pvt_geodetic.error == Error::None; + _message_gps_state.vel_n_m_s = pvt_geodetic.vn; + _message_gps_state.vel_e_m_s = pvt_geodetic.ve; + _message_gps_state.vel_d_m_s = -1.0f * pvt_geodetic.vu; + _message_gps_state.vel_m_s = sqrtf(_message_gps_state.vel_n_m_s * _message_gps_state.vel_n_m_s + + _message_gps_state.vel_e_m_s * _message_gps_state.vel_e_m_s); + + if (pvt_geodetic.cog > k_dnu_f4_value) { + _message_gps_state.cog_rad = pvt_geodetic.cog * M_DEG_TO_RAD_F; + } + _message_gps_state.c_variance_rad = M_DEG_TO_RAD_F; + + _message_gps_state.time_utc_usec = 0; +#ifndef __PX4_QURT // NOTE: Functionality isn't available on Snapdragon yet. + struct tm timeinfo; + time_t epoch; + + // Convert to unix timestamp + memset(&timeinfo, 0, sizeof(timeinfo)); + + timeinfo.tm_year = 1980 - 1900; + timeinfo.tm_mon = 0; + timeinfo.tm_mday = 6 + header.wnc * 7; + timeinfo.tm_hour = 0; + timeinfo.tm_min = 0; + timeinfo.tm_sec = header.tow / 1000; + + epoch = mktime(&timeinfo); + + if (epoch > k_gps_epoch_secs) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + memset(&ts, 0, sizeof(ts)); + ts.tv_sec = epoch; + ts.tv_nsec = (header.tow % 1000) * 1000 * 1000; + set_clock(ts); + + _message_gps_state.time_utc_usec = static_cast(epoch) * 1000000ULL; + _message_gps_state.time_utc_usec += (header.tow % 1000) * 1000; + } + +#endif + _message_gps_state.timestamp = hrt_absolute_time(); + result = PX4_OK; + } + + break; + } + + case BlockID::ReceiverStatus: { + SEP_TRACE_PARSING("Processing ReceiverStatus SBF message"); + + ReceiverStatus receiver_status; + + if (_sbf_decoder.parse(&receiver_status) == PX4_OK) { + _message_gps_state.rtcm_msg_used = receiver_status.rx_state_diff_corr_in ? sensor_gps_s::RTCM_MSG_USED_USED : sensor_gps_s::RTCM_MSG_USED_NOT_USED; + } + + break; + } + case BlockID::QualityInd: { + SEP_TRACE_PARSING("Processing QualityInd SBF message"); + break; + } + case BlockID::RFStatus: { + SEP_TRACE_PARSING("Processing RFStatus SBF message"); + break; + } + case BlockID::GALAuthStatus: { + SEP_TRACE_PARSING("Processing GALAuthStatus SBF message"); + break; + } + case BlockID::EndOfPVT: { + SEP_TRACE_PARSING("Processing EndOfPVT SBF message"); + + // EndOfPVT guarantees that all PVT blocks for this epoch have been sent, so it's safe to assume the uORB message contains all required data. + publish(); + break; + } + case BlockID::VelCovGeodetic: { + SEP_TRACE_PARSING("Processing VelCovGeodetic SBF message"); + _current_interval_messages.vel_cov_geodetic = true; + + VelCovGeodetic vel_cov_geodetic; + + if (_sbf_decoder.parse(&vel_cov_geodetic) == PX4_OK) { + if (vel_cov_geodetic.cov_ve_ve > k_dnu_f4_value && vel_cov_geodetic.cov_vn_vn > k_dnu_f4_value && vel_cov_geodetic.cov_vu_vu > k_dnu_f4_value) { + _message_gps_state.s_variance_m_s = math::max(math::max(vel_cov_geodetic.cov_ve_ve, vel_cov_geodetic.cov_vn_vn), vel_cov_geodetic.cov_vu_vu); + } + } + + break; + } + case BlockID::GEOIonoDelay: { + SEP_TRACE_PARSING("Processing GEOIonoDelay SBF message"); + break; + } + case BlockID::AttEuler: { + using Error = AttEuler::Error; + + SEP_TRACE_PARSING("Processing AttEuler SBF message"); + _current_interval_messages.att_euler = true; + + AttEuler att_euler; + + if (_sbf_decoder.parse(&att_euler) == PX4_OK && + !att_euler.error_not_requested && + att_euler.error_aux1 == Error::None && + att_euler.error_aux2 == Error::None && + att_euler.heading > k_dnu_f4_value) { + float heading = att_euler.heading * M_PI_F / 180.0f; // Range of degrees to range of radians in [0, 2PI]. + + // Ensure range is in [-PI, PI]. + if (heading > M_PI_F) { + heading -= 2.f * M_PI_F; + } + + _message_gps_state.heading = heading; + } + + break; + } + case BlockID::AttCovEuler: { + using Error = AttCovEuler::Error; + + SEP_TRACE_PARSING("Processing AttCovEuler SBF message"); + _current_interval_messages.att_cov_euler = true; + + AttCovEuler att_cov_euler; + + if (_sbf_decoder.parse(&att_cov_euler) == PX4_OK && + !att_cov_euler.error_not_requested && + att_cov_euler.error_aux1 == Error::None && + att_cov_euler.error_aux2 == Error::None && + att_cov_euler.cov_headhead > k_dnu_f4_value) { + _message_gps_state.heading_accuracy = att_cov_euler.cov_headhead * M_PI_F / 180.0f; // Convert range of degrees to range of radians in [0, 2PI] + } + + break; + } + } + + break; + } + case DecodingStatus::RTCMv3: { + SEP_TRACE_PARSING("Processing RTCMv3 message"); + publish_rtcm_corrections(_rtcm_decoder->message(), _rtcm_decoder->received_bytes()); + break; + } + } + + return result; +} + +bool SeptentrioDriver::send_message(const char *msg) +{ + PX4_DEBUG("Send MSG: %s", msg); + int length = strlen(msg); + + return (write(reinterpret_cast(msg), length) == length); +} + +bool SeptentrioDriver::send_message_and_wait_for_ack(const char *msg, const int timeout) +{ + if (!send_message(msg)) { + return false; + } + + // Wait for acknowledge + // For all valid set -, get - and exe -commands, the first line of the reply is an exact copy + // of the command as entered by the user, preceded with "$R: " + char buf[k_read_buffer_size]; + char expected_response[k_max_command_size+4]; + snprintf(expected_response, sizeof(expected_response), "$R: %s", msg); + uint16_t response_check_character = 0; + // Length of the message without the newline but including the preceding response part "$R: " + size_t response_len = strlen(msg) + 3; + hrt_abstime timeout_time = hrt_absolute_time() + 1000 * timeout; + + do { + int read_result = read(reinterpret_cast(buf), sizeof(buf), 50); + + if (read_result < 0) { + SEP_WARN("SBF read error"); + return false; + } + + for (int i = 0; i < read_result; i++) { + if (response_check_character == response_len) { + // We encountered the complete response + return true; + } else if (expected_response[response_check_character] == buf[i]) { + ++response_check_character; + } else if (buf[i] == '$') { + // Special case makes sure we don't miss start of new response if that happened to be the character we weren't expecting next (e.g., `$R: ge$R: gecm`) + response_check_character = 1; + } else { + response_check_character = 0; + } + } + } while (timeout_time > hrt_absolute_time()); + + SEP_WARN("Response: timeout"); + return false; +} + +int SeptentrioDriver::receive(unsigned timeout) +{ + int ret = 0; + int handled = 0; + uint8_t buf[k_read_buffer_size]; + + // timeout additional to poll + hrt_abstime time_started = hrt_absolute_time(); + + while (true) { + // Wait for only `k_receiver_read_timeout` if something already received. + ret = read(buf, sizeof(buf), handled ? k_receiver_read_timeout : timeout); + + if (ret < 0) { + // Something went wrong when polling or reading. + SEP_WARN("poll_or_read err"); + return -1; + + } else { + // Pass received bytes to the packet decoder. + for (int i = 0; i < ret; i++) { + handled |= parse_char(buf[i]); + } + } + + if (handled > 0) { + return handled; + } + + // abort after timeout if no useful packets received + if (time_started + timeout * 1000 < hrt_absolute_time()) { + PX4_DEBUG("timed out, returning"); + return -1; + } + } +} + +int SeptentrioDriver::read(uint8_t *buf, size_t buf_length, int timeout) +{ + int num_read = poll_or_read(buf, buf_length, timeout); + + if (num_read > 0) { + _current_interval_bytes_read += num_read; + if (should_dump_incoming()) { + dump_gps_data(buf, num_read, DataDirection::FromReceiver); + } + } + + return num_read; +} + +int SeptentrioDriver::poll_or_read(uint8_t *buf, size_t buf_length, int timeout) +{ + int read_timeout = math::min(k_max_receiver_read_timeout, timeout); + + return _uart.readAtLeast(buf, buf_length, math::min(k_min_receiver_read_bytes, buf_length), read_timeout); +} + +int SeptentrioDriver::write(const uint8_t* buf, size_t buf_length) +{ + ssize_t write_result = _uart.write(buf, buf_length); + + if (write_result >= 0) { + _current_interval_bytes_written += write_result; + if (should_dump_outgoing()) { + dump_gps_data(buf, write_result, DataDirection::ToReceiver); + } + } + + return write_result; +} + +int SeptentrioDriver::initialize_communication_dump(DumpMode mode) +{ + if (mode == DumpMode::FromReceiver || mode == DumpMode::Both) { + _message_data_from_receiver = new gps_dump_s(); + + if (!_message_data_from_receiver) { + SEP_ERR("Failed to allocate incoming dump buffer"); + return PX4_ERROR; + } + + memset(_message_data_from_receiver, 0, sizeof(*_message_data_from_receiver)); + } + if (mode == DumpMode::ToReceiver || mode == DumpMode::Both) { + _message_data_to_receiver = new gps_dump_s(); + + if (!_message_data_to_receiver) { + SEP_ERR("failed to allocated dump data"); + return PX4_ERROR; + } + + memset(_message_data_to_receiver, 0, sizeof(*_message_data_to_receiver)); + } + + if (mode != DumpMode::Disabled) { + _gps_dump_pub.advertise(); + } + + return PX4_OK; +} + +void SeptentrioDriver::reset_if_scheduled() +{ + ReceiverResetType reset_type = (ReceiverResetType)_scheduled_reset.load(); + + if (reset_type != ReceiverResetType::None) { + _scheduled_reset.store((int)ReceiverResetType::None); + int res = reset(reset_type); + + if (res == PX4_OK) { + SEP_INFO("Reset succeeded."); + } else { + SEP_INFO("Reset failed."); + } + } +} + +int SeptentrioDriver::set_baudrate(uint32_t baud) +{ + if (_uart.setBaudrate(baud)) { + SEP_INFO("baud controller: %lu", baud); + return PX4_OK; + } else { + return PX4_ERROR; + } +} + +void SeptentrioDriver::handle_inject_data_topic() +{ + // We don't want to call copy again further down if we have already done a copy in the selection process. + bool already_copied = false; + gps_inject_data_s msg; + + // If there has not been a valid RTCM message for a while, try to switch to a different RTCM link + if ((hrt_absolute_time() - _last_rtcm_injection_time) > 5_s) { + + for (int instance = 0; instance < _gps_inject_data_sub.size(); instance++) { + const bool exists = _gps_inject_data_sub[instance].advertised(); + + if (exists) { + if (_gps_inject_data_sub[instance].copy(&msg)) { + if ((hrt_absolute_time() - msg.timestamp) < 5_s) { + // Remember that we already did a copy on this instance. + already_copied = true; + _selected_rtcm_instance = instance; + break; + } + } + } + } + } + + bool updated = already_copied; + + // Limit maximum number of GPS injections to 8 since usually + // GPS injections should consist of 1-4 packets (GPS, GLONASS, BeiDou, Galileo). + // Looking at 8 packets thus guarantees, that at least a full injection + // data set is evaluated. + // Moving Base requires a higher rate, so we allow up to 8 packets. + const size_t max_num_injections = gps_inject_data_s::ORB_QUEUE_LENGTH; + size_t num_injections = 0; + + do { + if (updated) { + num_injections++; + + // Prevent injection of data from self or from ground if moving base and this is rover. + if ((_instance == Instance::Secondary && msg.device_id != get_device_id()) || (_instance == Instance::Main && msg.device_id == get_device_id()) || _receiver_setup != ReceiverSetup::MovingBase) { + /* Write the message to the gps device. Note that the message could be fragmented. + * But as we don't write anywhere else to the device during operation, we don't + * need to assemble the message first. + */ + write(msg.data, msg.len); + + ++_current_interval_rtcm_injections; + _last_rtcm_injection_time = hrt_absolute_time(); + } + } + + updated = _gps_inject_data_sub[_selected_rtcm_instance].update(&msg); + + } while (updated && num_injections < max_num_injections); +} + +void SeptentrioDriver::publish() +{ + _message_gps_state.device_id = get_device_id(); + _message_gps_state.selected_rtcm_instance = _selected_rtcm_instance; + _message_gps_state.rtcm_injection_rate = rtcm_injection_frequency(); + + _sensor_gps_pub.publish(_message_gps_state); + + if (_message_gps_state.spoofing_state != _spoofing_state) { + + if (_message_gps_state.spoofing_state > sensor_gps_s::SPOOFING_STATE_NONE) { + SEP_WARN("GPS spoofing detected! (state: %d)", _message_gps_state.spoofing_state); + } + + _spoofing_state = _message_gps_state.spoofing_state; + } + + if (_message_gps_state.jamming_state != _jamming_state) { + + if (_message_gps_state.jamming_state > sensor_gps_s::JAMMING_STATE_WARNING) { + SEP_WARN("GPS jamming detected! (state: %d) (indicator: %d)", _message_gps_state.jamming_state, + (uint8_t)_message_gps_state.jamming_indicator); + } + + _jamming_state = _message_gps_state.jamming_state; + } +} + +void SeptentrioDriver::publish_satellite_info() +{ + if (_message_satellite_info) { + _satellite_info_pub.publish(*_message_satellite_info); + } +} + +bool SeptentrioDriver::first_gps_uorb_message_created() const +{ + return _message_gps_state.timestamp != 0; +} + +void SeptentrioDriver::publish_rtcm_corrections(uint8_t *data, size_t len) +{ + gps_inject_data_s gps_inject_data{}; + + gps_inject_data.timestamp = hrt_absolute_time(); + gps_inject_data.device_id = get_device_id(); + + size_t capacity = (sizeof(gps_inject_data.data) / sizeof(gps_inject_data.data[0])); + + if (len > capacity) { + gps_inject_data.flags = 1; //LSB: 1=fragmented + + } else { + gps_inject_data.flags = 0; + } + + size_t written = 0; + + while (written < len) { + + gps_inject_data.len = len - written; + + if (gps_inject_data.len > capacity) { + gps_inject_data.len = capacity; + } + + memcpy(gps_inject_data.data, &data[written], gps_inject_data.len); + + _gps_inject_data_pub.publish(gps_inject_data); + + written = written + gps_inject_data.len; + } +} + +void SeptentrioDriver::dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction) +{ + gps_dump_s *dump_data = data_direction == DataDirection::FromReceiver ? _message_data_from_receiver : _message_data_to_receiver; + dump_data->instance = _instance == Instance::Main ? 0 : 1; + + while (len > 0) { + size_t write_len = len; + + if (write_len > sizeof(dump_data->data) - dump_data->len) { + write_len = sizeof(dump_data->data) - dump_data->len; + } + + memcpy(dump_data->data + dump_data->len, data, write_len); + data += write_len; + dump_data->len += write_len; + len -= write_len; + + if (dump_data->len >= sizeof(dump_data->data)) { + if (data_direction == DataDirection::ToReceiver) { + dump_data->len |= 1 << 7; + } + + dump_data->timestamp = hrt_absolute_time(); + _gps_dump_pub.publish(*dump_data); + dump_data->len = 0; + } + } +} + +bool SeptentrioDriver::should_dump_incoming() const +{ + return _message_data_from_receiver != 0; +} + +bool SeptentrioDriver::should_dump_outgoing() const +{ + return _message_data_to_receiver != 0; +} + +void SeptentrioDriver::start_update_monitoring_interval() +{ + PX4_DEBUG("Update monitoring interval started"); + _last_interval_rtcm_injections = _current_interval_rtcm_injections; + _last_interval_bytes_written = _current_interval_bytes_written; + _last_interval_bytes_read = _current_interval_bytes_read; + _last_interval_messages = _current_interval_messages; + _current_interval_rtcm_injections = 0; + _current_interval_bytes_written = 0; + _current_interval_bytes_read = 0; + _current_interval_messages = MessageTracker {}; + _current_interval_start_time = hrt_absolute_time(); +} + +bool SeptentrioDriver::update_monitoring_interval_ended() const +{ + return current_monitoring_interval_duration() > k_update_monitoring_interval_duration; +} + +hrt_abstime SeptentrioDriver::current_monitoring_interval_duration() const +{ + return hrt_absolute_time() - _current_interval_start_time; +} + +float SeptentrioDriver::rtcm_injection_frequency() const +{ + return _last_interval_rtcm_injections / us_to_s(static_cast(k_update_monitoring_interval_duration)); +} + +uint32_t SeptentrioDriver::output_data_rate() const +{ + return static_cast(_last_interval_bytes_written / us_to_s(static_cast(k_update_monitoring_interval_duration))); +} + +uint32_t SeptentrioDriver::input_data_rate() const +{ + return static_cast(_last_interval_bytes_read / us_to_s(static_cast(k_update_monitoring_interval_duration))); +} + +bool SeptentrioDriver::receiver_configuration_healthy() const +{ + return _last_interval_messages.dop && + _last_interval_messages.pvt_geodetic && + _last_interval_messages.vel_cov_geodetic && + _last_interval_messages.att_euler && + _last_interval_messages.att_cov_euler; +} + +float SeptentrioDriver::us_to_s(uint64_t us) +{ + return static_cast(us) / 1000000.0f; +} + +bool SeptentrioDriver::clock_needs_update(timespec real_time) +{ + timespec rtc_system_time; + + px4_clock_gettime(CLOCK_REALTIME, &rtc_system_time); + int drift_time = abs(rtc_system_time.tv_sec - real_time.tv_sec); + + return drift_time >= k_max_allowed_clock_drift; +} + +void SeptentrioDriver::set_clock(timespec rtc_gps_time) +{ + if (clock_needs_update(rtc_gps_time)) { + px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time); + } +} + +bool SeptentrioDriver::is_healthy() const +{ + if (_state == State::ReceivingData && receiver_configuration_healthy()) { + return true; + } + + return false; +} + +void SeptentrioDriver::reset_gps_state_message() +{ + memset(&_message_gps_state, 0, sizeof(_message_gps_state)); + _message_gps_state.heading = NAN; + _message_gps_state.heading_offset = matrix::wrap_pi(math::radians(_heading_offset)); +} + +uint32_t SeptentrioDriver::get_parameter(const char *name, int32_t *value) +{ + return _get_parameter(name, value); +} + +uint32_t SeptentrioDriver::get_parameter(const char *name, float *value) +{ + return _get_parameter(name, value); +} + +} // namespace septentrio + +extern "C" __EXPORT int septentrio_main(int argc, char *argv[]) +{ + return septentrio::SeptentrioDriver::main(argc, argv); +} diff --git a/src/drivers/gnss/septentrio/septentrio.h b/src/drivers/gnss/septentrio/septentrio.h new file mode 100644 index 000000000000..ec203cd86186 --- /dev/null +++ b/src/drivers/gnss/septentrio/septentrio.h @@ -0,0 +1,763 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 septentrio.h + * + * Septentrio GNSS receiver driver + * + * @author Matej Franceskin + * @author Seppe Geuens + * @author Thomas Frans +*/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "module.h" +#include "sbf/decoder.h" +#include "rtcm.h" + +using namespace time_literals; + +namespace septentrio +{ + +/** + * The parsed command line arguments for this module. + */ +struct ModuleArguments { + int baud_rate_main {0}; + int baud_rate_secondary {0}; + const char *device_path_main {nullptr}; + const char *device_path_secondary {nullptr}; +}; + +/** + * Which raw communication data to dump to the log file. +*/ +enum class DumpMode : int32_t { + Disabled = 0, + FromReceiver = 1, + ToReceiver = 2, + Both = 3, +}; + +/** + * Instance of the driver. +*/ +enum class Instance : uint8_t { + Main = 0, + Secondary, +}; + +/** + * Hardware setup and use of the connected receivers. +*/ +enum class ReceiverSetup { + /// Two receivers with the same purpose. + Default = 0, + + /// One rover and one moving base receiver, used for heading with two single-antenna receivers. + MovingBase = 1, +}; + +/** + * Type of reset to perform on the receiver. +*/ +enum class ReceiverResetType { + /** + * There is no pending GPS reset. + */ + None, + + /** + * In hot start mode, the receiver was powered down only for a short time (4 hours or less), + * so that its ephemeris is still valid. Since the receiver doesn't need to download ephemeris + * again, this is the fastest startup method. + */ + Hot, + + /** + * In warm start mode, the receiver has approximate information for time, position, and coarse + * satellite position data (Almanac). In this mode, after power-up, the receiver normally needs + * to download ephemeris before it can calculate position and velocity data. + */ + Warm, + + /** + * In cold start mode, the receiver has no information from the last position at startup. + * Therefore, the receiver must search the full time and frequency space, and all possible + * satellite numbers. If a satellite signal is found, it is tracked to decode the ephemeris, + * whereas the other channels continue to search satellites. Once there is a sufficient number + * of satellites with valid ephemeris, the receiver can calculate position and velocity data. + */ + Cold +}; + +/** + * Direction of raw data. +*/ +enum class DataDirection { + /// Data sent by the flight controller to the receiver. + ToReceiver, + + /// Data sent by the receiver to the flight controller. + FromReceiver, +}; + +/** + * Which satellites the receiver should use for PVT computation. +*/ +enum class SatelliteUsage : int32_t { + Default = 0, + GPS = 1 << 0, + GLONASS = 1 << 1, + Galileo = 1 << 2, + SBAS = 1 << 3, + BeiDou = 1 << 4, +}; + +/** + * General logging level of the receiver that determines the blocks to log. +*/ +enum class ReceiverLogLevel : int32_t { + Lite = 0, + Basic = 1, + Default = 2, + Full = 3, +}; + +/** + * Logging frequency of the receiver that determines SBF output frequency. +*/ +enum class ReceiverLogFrequency : int32_t { + Disabled = 0, + Hz0_1 = 1, + Hz0_2 = 2, + Hz0_5 = 3, + Hz1_0 = 4, + Hz2_0 = 5, + Hz5_0 = 6, + Hz10_0 = 7, + Hz20_0 = 8, + Hz25_0 = 9, + Hz50_0 = 10, +}; + +/** + * Output frequency for the main SBF blocks required for PVT computation. +*/ +enum class SBFOutputFrequency : int32_t { + Hz5_0 = 0, + Hz10_0 = 1, + Hz20_0 = 2, + Hz25_0 = 3, +}; + +/** + * Tracker for messages received by the driver. +*/ +struct MessageTracker { + bool dop {false}; + bool pvt_geodetic {false}; + bool vel_cov_geodetic {false}; + bool att_euler {false}; + bool att_cov_euler {false}; +}; + +/** + * Used for a bitmask to keep track of received messages to know when we need to broadcast them and to monitor receiver health. +*/ +enum class ReceiverOutputTracker { + None = 0, + DOP = 1 << 0, + PVTGeodetic = 1 << 1, + VelCovGeodetic = 1 << 2, + AttEuler = 1 << 3, + AttCovEuler = 1 << 4, + HeadingMessages = AttEuler + AttCovEuler, + RequiredPositionMessages = DOP + PVTGeodetic + VelCovGeodetic + AttCovEuler, + PositioningMessages = DOP + PVTGeodetic + VelCovGeodetic + AttEuler + AttCovEuler, +}; + +class SeptentrioDriver : public ModuleBase, public device::Device +{ +public: + SeptentrioDriver(const char *device_path, Instance instance, uint32_t baud_rate); + ~SeptentrioDriver() override; + + /** @see ModuleBase */ + int print_status() override; + + /** @see ModuleBase */ + void run() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + static int task_spawn(int argc, char *argv[], Instance instance); + + /** + * @brief Secondary run trampoline to support two driver instances. + */ + static int run_trampoline_secondary(int argc, char *argv[]); + + /** @see ModuleBase */ + static SeptentrioDriver *instantiate(int argc, char *argv[]); + + static SeptentrioDriver *instantiate(int argc, char *argv[], Instance instance); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** + * @brief Reset the connected GPS receiver. + * + * @return `PX4_OK` on success, `PX4_ERROR` on otherwise + */ + int reset(ReceiverResetType type); + + /** + * @brief Force command input on the currently used port on the receiver. + * + * @return `PX4_OK` on success, `PX4_ERROR` otherwise + */ + int force_input(); + + /** + * Standard baud rates the driver can be started with. `0` means the driver matches baud rates but does not change them. + */ + static uint32_t k_supported_baud_rates[]; + + /** + * Default baud rate, used when the user requested an invalid baud rate. + */ + static uint32_t k_default_baud_rate; +private: + enum class State { + OpeningSerialPort, + DetectingBaudRate, + ConfiguringDevice, + ReceivingData, + }; + + /** + * The current decoder that data has to be fed to. + */ + enum class DecodingStatus { + Searching, + SBF, + RTCMv3, + }; + + enum class ReceiveResult { + ReadError, + Timeout, + Receiving, + MessageAvailable, + }; + + /** + * The result of trying to configure the receiver. + */ + enum class ConfigureResult : int32_t { + OK = 0, + FailedCompletely = 1 << 0, + NoLogging = 1 << 1, + }; + + /** + * Maximum timeout to wait for fast command acknowledgement by the receiver. + */ + static constexpr uint16_t k_receiver_ack_timeout_fast = 200; + + /** + * Maximum timeout to wait for slow command acknowledgement by the receiver. + * Might be the case for commands that send more output back as reply. + */ + static constexpr uint16_t k_receiver_ack_timeout_slow = 400; + + /** + * Duration of one update monitoring interval in us. + * This should be longer than the time it takes for all *positioning* SBF messages to be sent once by the receiver! + * Otherwise the driver will assume the receiver configuration isn't healthy because it doesn't see all blocks in time. + */ + static constexpr hrt_abstime k_update_monitoring_interval_duration = 5_s; + + /** + * uORB type to send messages to ground control stations. + */ + static orb_advert_t k_mavlink_log_pub; + + /** + * The default stream for output of PVT data. + */ + static constexpr uint8_t k_default_main_stream = 1; + + /** + * The default stream for output of logging data. + */ + static constexpr uint8_t k_default_log_stream = 2; + + /** + * @brief Parse command line arguments for this module. + * + * @param argc Number of arguments. + * @param argv The arguments. + * @param arguments The parsed arguments. + * + * @return `PX4_OK` on success, `PX4_ERROR` on a parsing error. + */ + static int parse_cli_arguments(int argc, char *argv[], ModuleArguments &arguments); + + /** + * @brief Wait for the second instance to properly start up. + * + * @return `PX4_OK` once the second instance has started, or `PX4_ERROR` if timed out waiting. + */ + static int await_second_instance_startup(); + + /** + * @brief Wait for the second instance to properly shut down. + * + * @return `PX4_OK` once the second instance has shut down, or `PX4_ERROR` if timed out waiting. + */ + int await_second_instance_shutdown(); + + /** + * @brief Schedule a reset of the connected receiver. + */ + void schedule_reset(ReceiverResetType type); + + /** + * @brief Detect whether the receiver is running at the given baud rate. + * Does not preserve local baud rate! + * + * @param baud_rate The baud rate to check. + * @param force_input Choose whether the receiver forces input on the port. + * + * @return `true` if running at the baud rate, or `false` on error. + */ + bool detect_receiver_baud_rate(const uint32_t &baud_rate, bool forced_input); + + /** + * @brief Try to detect the serial port used on the receiver side. + * + * @param port_name A string with a length of 5 to store the result + * + * @return `PX4_OK` on success, `PX4_ERROR` on error + */ + int detect_serial_port(char *const port_name); + + /** + * @brief Configure the attached receiver based on the user's parameters. + * + * If the user has disabled automatic configuration, only execute the steps that do not touch the receiver (e.g., baud rate detection, port detection...). + * + * @return `ConfigureResult::OK` if configured, or error. + */ + ConfigureResult configure(); + + /** + * @brief Parse the next byte of a received message from the receiver. + * + * @return 0 = decoding, 1 = message handled, 2 = sat info message handled + */ + int parse_char(const uint8_t byte); + + /** + * @brief Process a fully received message from the receiver. + * + * @return `PX4_OK` on message handled, `PX4_ERROR` on error. + */ + int process_message(); + + /** + * @brief Add payload rx byte. + * + * @return -1 = error, 0 = ok, 1 = payload received completely + */ + int payload_rx_add(const uint8_t byte); + + /** + * @brief Parses incoming SBF blocks. + * + * @return bitfield: all 0 = no message handled, 1 = position handled, 2 = satellite info handled + */ + int payload_rx_done(); + + /** + * @brief Send a message. + * + * @return true on success, false on write error (errno set) + */ + [[nodiscard]] bool send_message(const char *msg); + + /** + * @brief Send a message and waits for acknowledge. + * + * @param msg The message to send to the receiver + * @param timeout The time before sending the message times out + * + * @return true on success, false on write error (errno set) or ack wait timeout + */ + [[nodiscard]] bool send_message_and_wait_for_ack(const char *msg, const int timeout); + + /** + * @brief Receive incoming messages. + * + * @param timeout Maximum time to wait for new data in ms, after which we return. + * + * @return -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled + */ + int receive(unsigned timeout); + + /** + * @brief Read from the receiver. + * + * @param buf Data that is read + * @param buf_length Size of the buffer + * @param timeout Reading timeout + * + * @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes) + */ + int read(uint8_t *buf, size_t buf_length, int timeout); + + /** + * @brief This is an abstraction for the poll on serial used. + * + * @param buf The read buffer + * @param buf_length Size of the read buffer + * @param timeout Read timeout in ms + * + * @return 0 on nothing read or poll timeout, <0 on error and >0 on bytes read (nr of bytes) + */ + int poll_or_read(uint8_t *buf, size_t buf_length, int timeout); + + /** + * @brief Write to the receiver. + * + * @param buf Data to be written + * @param buf_length Amount of bytes to be written + * + * @return the number of bytes written on success, or -1 otherwise + */ + int write(const uint8_t *buf, size_t buf_length); + + /** + * @brief Initialize uORB GPS logging and advertise the topic. + * + * @return `PX4_OK` on success, `PX4_ERROR` otherwise + */ + int initialize_communication_dump(DumpMode mode); + + /** + * @brief Reset the receiver if it was requested by the user. + */ + void reset_if_scheduled(); + + /** + * @brief Set the baudrate of the serial connection. + * + * @param baud The baud rate of the connection + * + * @return `PX4_OK` on success, `PX4_ERROR` on otherwise + */ + int set_baudrate(uint32_t baud); + + /** + * @brief Handle incoming messages on the "inject data" uORB topic and send them to the receiver. + */ + void handle_inject_data_topic(); + + /** + * @brief Send data to the receiver, such as RTCM injections. + * + * @param data The raw data to send to the device + * @param len The size of `data` + * + * @return `true` if all the data was written correctly, `false` otherwise + */ + inline bool inject_data(uint8_t *data, size_t len); + + /** + * @brief Publish new GPS data with uORB. + */ + void publish(); + + /** + * @brief Publish new GPS satellite data with uORB. + */ + void publish_satellite_info(); + + /** + * @brief Check whether the driver has created its first complete `SensorGPS` uORB message. + * + * @return `true` if the driver has created its first complete `SensorGPS` uORB message, `false` if still waiting. + */ + bool first_gps_uorb_message_created() const; + + /** + * @brief Publish RTCM corrections. + * + * @param data: The raw data to publish + * @param len: The size of `data` + */ + void publish_rtcm_corrections(uint8_t *data, size_t len); + + /** + * @brief Dump gps communication. + * + * @param data The raw data of the message. + * @param len The size of `data`. + * @param data_direction The type of data, either incoming or outgoing. + */ + void dump_gps_data(const uint8_t *data, size_t len, DataDirection data_direction); + + /** + * @brief Check whether we should dump incoming data. + * + * @return `true` when we should dump incoming data, `false` otherwise. + */ + bool should_dump_incoming() const; + + /** + * @brief Check whether we should dump outgoing data. + * + * @return `true` when we should dump outgoing data, `false` otherwise. + */ + bool should_dump_outgoing() const; + + /** + * @brief Start a new update frequency monitoring interval. + */ + void start_update_monitoring_interval(); + + /** + * @brief Check whether the current update monitoring interval should end. + * + * @return `true` if a new interval should be started, or `false` if the current interval is still valid. + */ + bool update_monitoring_interval_ended() const; + + /** + * @brief Get the duration of the current update frequency monitoring interval. + * + * @return The duration of the current interval in us. + */ + hrt_abstime current_monitoring_interval_duration() const; + + /** + * @brief Calculate RTCM message injection frequency for the current measurement interval. + * + * @return The RTCM message injection frequency for the current measurement interval in Hz. + */ + float rtcm_injection_frequency() const; + + /** + * @brief Calculate output data rate to the receiver for the current measurement interval. + * + * @return The output data rate for the current measurement interval in B/s. + */ + uint32_t output_data_rate() const; + + /** + * @brief Calculate input data rate from the receiver for the current measurement interval. + * + * @return The input data rate for the current measurement interval in B/s. + */ + uint32_t input_data_rate() const; + + /** + * @brief Check whether the current receiver configuration is likely healthy. + * + * This is checked by passively monitoring output from the receiver and validating whether it is what is + * expected. + * + * @return `true` if the receiver is operating correctly, `false` if it needs to be reconfigured. + */ + bool receiver_configuration_healthy() const; + + /** + * @brief Convert from microseconds to seconds. + * + * @return `us` converted into seconds. + */ + static float us_to_s(uint64_t us); + + /** + * @brief Check if the system clock needs to be updated with new time obtained from the receiver. + * + * Setting the clock on Nuttx temporarily pauses interrupts. Therefore it should only be set if it is absolutely necessary. + * + * @return `true` if the clock should be update, `false` if the clock is still accurate enough. + */ + static bool clock_needs_update(timespec real_time); + + /** + * @brief Used to set the system clock accurately. + * + * This does not guarantee that the clock will be set. + * + * @param time The current time. + */ + static void set_clock(timespec rtc_gps_time); + + /** + * @brief Check whether the driver is operating correctly. + * + * The driver is operating correctly when it has fully configured the receiver and is actively receiving all the + * expected data. + * + * @return `true` if the driver is working as expected, `false` otherwise. + */ + bool is_healthy() const; + + /** + * @brief Reset the GPS state uORB message for reuse. + */ + void reset_gps_state_message(); + + /** + * @brief Get the parameter with the given name into `value`. + * + * @param name The name of the parameter. + * @param value The value in which to store the parameter. + * + * @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found. + */ + static uint32_t get_parameter(const char *name, int32_t *value); + + /** + * @brief Get the parameter with the given name into `value`. + * + * @param name The name of the parameter. + * @param value The value in which to store the parameter. + * + * @return `PX4_OK` on success, or `PX4_ERROR` if the parameter wasn't found. + */ + static uint32_t get_parameter(const char *name, float *value); + + /** + * @brief Don't use this, use the other parameter functions instead! + */ + template + static uint32_t _get_parameter(const char *name, T *value) + { + param_t handle = param_find(name); + + if (handle == PARAM_INVALID || param_get(handle, value) == PX4_ERROR) { + SEP_ERR("Failed to get parameter %s", name); + return PX4_ERROR; + } + + return PX4_OK; + } + + State _state {State::OpeningSerialPort}; ///< Driver state which allows for single run loop + px4::atomic _scheduled_reset {static_cast(ReceiverResetType::None)}; ///< The type of receiver reset that is scheduled + DumpMode _dump_communication_mode {DumpMode::Disabled}; ///< GPS communication dump mode + device::Serial _uart {}; ///< Serial UART port for communication with the receiver + char _port[20] {}; ///< The path of the used serial device + hrt_abstime _last_rtcm_injection_time {0}; ///< Time of last RTCM injection + uint8_t _selected_rtcm_instance {0}; ///< uORB instance that is being used for RTCM corrections + uint8_t _spoofing_state {0}; ///< Receiver spoofing state + uint8_t _jamming_state {0}; ///< Receiver jamming state + const Instance _instance {Instance::Main}; ///< The receiver that this instance of the driver controls + uint32_t _chosen_baud_rate {0}; ///< The baud rate requested by the user + static px4::atomic _secondary_instance; + hrt_abstime _sleep_end {0}; ///< End time for sleeping + State _resume_state {State::OpeningSerialPort}; ///< Resume state after sleep + + // Module configuration + float _heading_offset {0.0f}; ///< The heading offset given by the `SEP_YAW_OFFS` parameter + float _pitch_offset {0.0f}; ///< The pitch offset given by the `SEP_PITCH_OFFS` parameter + uint32_t _receiver_stream_main {k_default_main_stream}; ///< The main output stream for the receiver given by the `SEP_STREAM_MAIN` parameter + uint32_t _receiver_stream_log {k_default_log_stream}; ///< The log output stream for the receiver given by the `SEP_STREAM_LOG` parameter + SBFOutputFrequency _sbf_output_frequency {SBFOutputFrequency::Hz5_0}; ///< Output frequency of the main SBF blocks given by the `SEP_OUTP_HZ` parameter + ReceiverLogFrequency _receiver_logging_frequency {ReceiverLogFrequency::Hz1_0}; ///< Logging frequency of the receiver given by the `SEP_LOG_HZ` parameter + ReceiverLogLevel _receiver_logging_level {ReceiverLogLevel::Default}; ///< Logging level of the receiver given by the `SEP_LOG_LEVEL` parameter + bool _receiver_logging_overwrite {0}; ///< Logging overwrite behavior, given by the `SEP_LOG_FORCE` parameter + bool _automatic_configuration {true}; ///< Automatic configuration of the receiver given by the `SEP_AUTO_CONFIG` parameter + ReceiverSetup _receiver_setup {ReceiverSetup::Default}; ///< Purpose of the receivers, given by the `SEP_HARDW_SETUP` parameter + int32_t _receiver_constellation_usage {0}; ///< Constellation usage in PVT computation given by the `SEP_CONST_USAGE` parameter + uint8_t _current_baud_rate_index {0}; ///< Index of the current baud rate to check + + // Decoding and parsing + DecodingStatus _active_decoder {DecodingStatus::Searching}; ///< Currently active decoder that new data has to be fed into + sbf::Decoder _sbf_decoder {}; ///< SBF message decoder + rtcm::Decoder *_rtcm_decoder {nullptr}; ///< RTCM message decoder + + // uORB topics and subscriptions + sensor_gps_s _message_gps_state {}; ///< uORB topic for position + gps_dump_s *_message_data_to_receiver {nullptr}; ///< uORB topic for dumping data to the receiver + gps_dump_s *_message_data_from_receiver {nullptr}; ///< uORB topic for dumping data from the receiver + satellite_info_s *_message_satellite_info {nullptr}; ///< uORB topic for satellite info + uORB::PublicationMulti _sensor_gps_pub {ORB_ID(sensor_gps)}; ///< uORB publication for gps position + uORB::Publication _gps_dump_pub {ORB_ID(gps_dump)}; ///< uORB publication for dump GPS data + uORB::Publication _gps_inject_data_pub {ORB_ID(gps_inject_data)}; ///< uORB publication for injected data to the receiver + uORB::PublicationMulti _satellite_info_pub {ORB_ID(satellite_info)}; ///< uORB publication for satellite info + uORB::SubscriptionMultiArray _gps_inject_data_sub {ORB_ID::gps_inject_data}; ///< uORB subscription about data to inject to the receiver + + // Data about update frequencies of various bits of information like RTCM message injection frequency, received data rate... + hrt_abstime _current_interval_start_time {0}; ///< Start time of the current update measurement interval in us + uint16_t _last_interval_rtcm_injections {0}; ///< Nr of RTCM message injections in the last measurement interval + uint16_t _current_interval_rtcm_injections {0}; ///< Nr of RTCM message injections in the current measurement interval + uint32_t _last_interval_bytes_written {0}; ///< Nr of bytes written to the receiver in the last measurement interval + uint32_t _current_interval_bytes_written {0}; ///< Nr of bytes written to the receiver in the current measurement interval + uint32_t _last_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the last measurement interval + uint32_t _current_interval_bytes_read {0}; ///< Nr of bytes read from the receiver in the current measurement interval + MessageTracker _last_interval_messages {}; ///< Messages encountered in the last measurement interval + MessageTracker _current_interval_messages {}; ///< Messages encountered in the current measurement interval +}; + +} // namespace septentrio diff --git a/src/drivers/gnss/septentrio/util.cpp b/src/drivers/gnss/septentrio/util.cpp new file mode 100644 index 000000000000..34bf02f58b89 --- /dev/null +++ b/src/drivers/gnss/septentrio/util.cpp @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 util.cpp + * + * @author Thomas Frans +*/ + +#include "util.h" + +namespace septentrio +{ + +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length) +{ + uint8_t x; + uint16_t crc = 0; + + while (length--) { + x = crc >> 8 ^ *data_p++; + x ^= x >> 4; + crc = static_cast((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); + } + + return crc; +} + +} // namespace septentrio diff --git a/src/modules/ekf2/ekf2_params_multi.c b/src/drivers/gnss/septentrio/util.h similarity index 76% rename from src/modules/ekf2/ekf2_params_multi.c rename to src/drivers/gnss/septentrio/util.h index cd06198dc161..aeb9369ad5ac 100644 --- a/src/modules/ekf2/ekf2_params_multi.c +++ b/src/drivers/gnss/septentrio/util.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -32,27 +32,21 @@ ****************************************************************************/ /** - * Multi-EKF IMUs + * @file util.h * - * Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. - * Requires SENS_IMU_MODE 0. - * - * @group EKF2 - * @reboot_required true - * @min 0 - * @max 4 + * @author Thomas Frans */ -PARAM_DEFINE_INT32(EKF2_MULTI_IMU, 0); + +#pragma once + +#include + +namespace septentrio +{ /** - * Multi-EKF Magnetometers. - * - * Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable. - * Requires SENS_MAG_MODE 0. - * - * @group EKF2 - * @reboot_required true - * @min 0 - * @max 4 + * @brief Calculate buffer CRC16 */ -PARAM_DEFINE_INT32(EKF2_MULTI_MAG, 0); +uint16_t buffer_crc16(const uint8_t *data_p, uint32_t length); + +} // namespace septentrio diff --git a/src/drivers/gps/CMakeLists.txt b/src/drivers/gps/CMakeLists.txt index 9796d24fb3bc..4719e0e34e14 100644 --- a/src/drivers/gps/CMakeLists.txt +++ b/src/drivers/gps/CMakeLists.txt @@ -51,7 +51,6 @@ px4_add_module( devices/src/nmea.cpp devices/src/unicore.cpp devices/src/crc.cpp - devices/src/sbf.cpp MODULE_CONFIG module.yaml DEPENDS diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index 17c0e2bfad1e..a41210ede8c2 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 17c0e2bfad1e544c4b11329c742630a765c7537f +Subproject commit a41210ede8c2d22dd8e9fdcf388fca927c1fc5e1 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4ed29a8c6e35..004be4cc8b90 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -45,7 +45,6 @@ #include #endif -#include #include #include @@ -57,6 +56,8 @@ #include #include #include +#include +#include #include #include #include @@ -72,7 +73,6 @@ # include "devices/src/mtk.h" # include "devices/src/femtomes.h" # include "devices/src/nmea.h" -# include "devices/src/sbf.h" #endif // CONSTRAINED_FLASH #include "devices/src/ubx.h" @@ -81,6 +81,7 @@ #include #endif /* __PX4_LINUX */ +using namespace device; using namespace time_literals; #define TIMEOUT_1HZ 1300 //!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error @@ -95,7 +96,6 @@ enum class gps_driver_mode_t { EMLIDREACH, FEMTOMES, NMEA, - SBF }; enum class gps_dump_comm_mode_t : int32_t { @@ -169,7 +169,10 @@ class GPS : public ModuleBase, public device::Device void reset_if_scheduled(); private: - int _serial_fd{-1}; ///< serial interface to GPS +#ifdef __PX4_LINUX + int _spi_fd {-1}; ///< SPI interface to GPS +#endif + Serial _uart {}; ///< UART interface to GPS unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) char _port[20] {}; ///< device / serial port path @@ -329,8 +332,11 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2) set_device_bus(c - 48); // sub 48 to convert char to integer +#ifdef __PX4_LINUX + } else if (_interface == GPSHelper::Interface::SPI) { set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI); +#endif } if (_mode == gps_driver_mode_t::None) { @@ -353,8 +359,6 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac case 5: _mode = gps_driver_mode_t::FEMTOMES; break; case 6: _mode = gps_driver_mode_t::NMEA; break; - - case 7: _mode = gps_driver_mode_t::SBF; break; #endif // CONSTRAINED_FLASH } } @@ -403,10 +407,23 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) return num_read; } - case GPSCallbackType::writeDeviceData: - gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); + case GPSCallbackType::writeDeviceData: { + gps->dumpGpsData((uint8_t *)data1, (size_t)data2, gps_dump_comm_mode_t::Full, true); - return ::write(gps->_serial_fd, data1, (size_t)data2); + int ret = 0; + + if (gps->_interface == GPSHelper::Interface::UART) { + ret = gps->_uart.write((void *) data1, (size_t) data2); + +#ifdef __PX4_LINUX + + } else if (gps->_spi_fd >= 0) { + ret = ::write(gps->_spi_fd, data1, (size_t)data2); +#endif + } + + return ret; + } case GPSCallbackType::setBaudrate: return gps->setBaudrate(data2); @@ -449,72 +466,64 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) { - handleInjectDataTopic(); - -#if !defined(__PX4_QURT) - - /* For non QURT, use the usual polling. */ - - //Poll only for the serial data. In the same thread we also need to handle orb messages, - //so ideally we would poll on both, the serial fd and orb subscription. Unfortunately the - //two pollings use different underlying mechanisms (at least under posix), which makes this - //impossible. Instead we limit the maximum polling interval and regularly check for new orb - //messages. - //FIXME: add a unified poll() API + int ret = 0; + const size_t character_count = 32; // minimum bytes that we want to read const int max_timeout = 50; + int timeout_adjusted = math::min(max_timeout, timeout); - pollfd fds[1]; - fds[0].fd = _serial_fd; - fds[0].events = POLLIN; - - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), math::min(max_timeout, timeout)); - - if (ret > 0) { - /* if we have new data from GPS, go handle it */ - if (fds[0].revents & POLLIN) { - /* - * We are here because poll says there is some data, so this - * won't block even on a blocking device. But don't read immediately - * by 1-2 bytes, wait for some more data to save expensive read() calls. - * If we have all requested data available, read it without waiting. - * If more bytes are available, we'll go back to poll() again. - */ - const unsigned character_count = 32; // minimum bytes that we want to read - unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; - const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); + handleInjectDataTopic(); -#ifdef __PX4_NUTTX - int err = 0; - int bytes_available = 0; - err = ::ioctl(_serial_fd, FIONREAD, (unsigned long)&bytes_available); + if (_interface == GPSHelper::Interface::UART) { + ret = _uart.readAtLeast(buf, buf_length, math::min(character_count, buf_length), timeout_adjusted); + +// SPI is only supported on LInux +#if defined(__PX4_LINUX) + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + + //Poll only for the SPI data. In the same thread we also need to handle orb messages, + //so ideally we would poll on both, the SPI fd and orb subscription. Unfortunately the + //two pollings use different underlying mechanisms (at least under posix), which makes this + //impossible. Instead we limit the maximum polling interval and regularly check for new orb + //messages. + //FIXME: add a unified poll() API + + pollfd fds[1]; + fds[0].fd = _spi_fd; + fds[0].events = POLLIN; + + ret = poll(fds, sizeof(fds) / sizeof(fds[0]), timeout_adjusted); + + if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If we have all requested data available, read it without waiting. + * If more bytes are available, we'll go back to poll() again. + */ + unsigned baudrate = _baudrate == 0 ? 115200 : _baudrate; + const unsigned sleeptime = character_count * 1000000 / (baudrate / 10); - if (err != 0 || bytes_available < (int)character_count) { px4_usleep(sleeptime); - } -#else - px4_usleep(sleeptime); -#endif + ret = ::read(_spi_fd, buf, buf_length); - ret = ::read(_serial_fd, buf, buf_length); + if (ret > 0) { + _num_bytes_read += ret; + } - if (ret > 0) { - _num_bytes_read += ret; + } else { + ret = -1; } - - } else { - ret = -1; } + +#endif } return ret; - -#else - /* For QURT, just use read for now, since this doesn't block, we need to slow it down - * just a bit. */ - px4_usleep(10000); - return ::read(_serial_fd, buf, buf_length); -#endif } void GPS::handleInjectDataTopic() @@ -583,105 +592,38 @@ bool GPS::injectData(uint8_t *data, size_t len) { dumpGpsData(data, len, gps_dump_comm_mode_t::Full, true); - size_t written = ::write(_serial_fd, data, len); - ::fsync(_serial_fd); - return written == len; -} - -int GPS::setBaudrate(unsigned baud) -{ - /* process baud rate */ - int speed; - - switch (baud) { - case 9600: speed = B9600; break; - - case 19200: speed = B19200; break; - - case 38400: speed = B38400; break; - - case 57600: speed = B57600; break; - - case 115200: speed = B115200; break; - - case 230400: speed = B230400; break; + size_t written = 0; -#ifndef B460800 -#define B460800 460800 -#endif + if (_interface == GPSHelper::Interface::UART) { + written = _uart.write((const void *) data, len); - case 460800: speed = B460800; break; +#ifdef __PX4_LINUX -#ifndef B921600 -#define B921600 921600 + } else if (_interface == GPSHelper::Interface::SPI) { + written = ::write(_spi_fd, data, len); + ::fsync(_spi_fd); #endif - - case 921600: speed = B921600; break; - - default: - PX4_ERR("ERR: unknown baudrate: %d", baud); - return -EINVAL; } - struct termios uart_config; - - int termios_state; - - /* fill the struct for the new configuration */ - tcgetattr(_serial_fd, &uart_config); - - /* properly configure the terminal (see also https://en.wikibooks.org/wiki/Serial_Programming/termios ) */ - - // - // Input flags - Turn off input processing - // - // convert break to null byte, no CR to NL translation, - // no NL to CR translation, don't mark parity errors or breaks - // no input parity check, don't strip high bit off, - // no XON/XOFF software flow control - // - uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | - INLCR | PARMRK | INPCK | ISTRIP | IXON); - // - // Output flags - Turn off output processing - // - // no CR to NL translation, no NL to CR-NL translation, - // no NL to CR translation, no column 0 CR suppression, - // no Ctrl-D suppression, no fill characters, no case mapping, - // no local output processing - // - // config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | - // ONOCR | ONOEOT| OFILL | OLCUC | OPOST); - uart_config.c_oflag = 0; - - // - // No line processing - // - // echo off, echo newline off, canonical mode off, - // extended input processing off, signal chars off - // - uart_config.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - - /* no parity, one stop bit, disable flow control */ - uart_config.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - - /* set baud rate */ - if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetispeed)", termios_state); - return -1; - } + return written == len; +} - if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - GPS_ERR("ERR: %d (cfsetospeed)", termios_state); - return -1; - } +int GPS::setBaudrate(unsigned baud) +{ + if (_interface == GPSHelper::Interface::UART) { + if (_uart.setBaudrate(baud)) { + return 0; + } + +#ifdef __PX4_LINUX - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { - GPS_ERR("ERR: %d (tcsetattr)", termios_state); - return -1; + } else if (_interface == GPSHelper::Interface::SPI) { + // Can't set the baudrate on a SPI port but just return a success + return 0; +#endif } - return 0; + return -1; } void GPS::initializeCommunicationDump() @@ -760,13 +702,6 @@ GPS::run() heading_offset = matrix::wrap_pi(math::radians(heading_offset)); } - handle = param_find("GPS_PITCH_OFFSET"); - float pitch_offset = 0.f; - - if (handle != PARAM_INVALID) { - param_get(handle, &pitch_offset); - } - int32_t gps_ubx_dynmodel = 7; // default to 7: airborne with <2g acceleration handle = param_find("GPS_UBX_DYNMODEL"); @@ -782,7 +717,8 @@ GPS::run() int32_t gps_ubx_mode = 0; param_get(handle, &gps_ubx_mode); - if (gps_ubx_mode == 1) { // heading + switch (gps_ubx_mode) { + case 1: // heading if (_instance == Instance::Main) { ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase; @@ -790,10 +726,13 @@ GPS::run() ubx_mode = GPSDriverUBX::UBXMode::MovingBase; } - } else if (gps_ubx_mode == 2) { + break; + + case 2: ubx_mode = GPSDriverUBX::UBXMode::MovingBase; + break; - } else if (gps_ubx_mode == 3) { + case 3: if (_instance == Instance::Main) { ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1; @@ -801,11 +740,18 @@ GPS::run() ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; } - } else if (gps_ubx_mode == 4) { + break; + + case 4: ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1; + break; - } else if (gps_ubx_mode == 5) { // rover with static base on Uart2 + case 5: // rover with static base on Uart2 ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2; + break; + + default: + break; } } @@ -840,31 +786,54 @@ GPS::run() _helper = nullptr; } - if (_serial_fd < 0) { - /* open the serial port */ - _serial_fd = ::open(_port, O_RDWR | O_NOCTTY); + if ((_interface == GPSHelper::Interface::UART) && (! _uart.isOpen())) { - if (_serial_fd < 0) { - PX4_ERR("failed to open %s err: %d", _port, errno); + // Configure UART port + if (!_uart.setPort(_port)) { + PX4_ERR("Error configuring serial device on port %s", _port); + px4_sleep(1); + continue; + } + + // Configure the desired baudrate if one was specified by the user. + // Otherwise the default baudrate will be used. + if (_configured_baudrate) { + if (! _uart.setBaudrate(_configured_baudrate)) { + PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); + px4_sleep(1); + continue; + } + } + + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart.open()) { + PX4_ERR("Error opening serial device %s", _port); px4_sleep(1); continue; } #ifdef __PX4_LINUX - if (_interface == GPSHelper::Interface::SPI) { - int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) - int status_value = ::ioctl(_serial_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd < 0)) { + _spi_fd = ::open(_port, O_RDWR | O_NOCTTY); - if (status_value < 0) { - PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (_spi_fd < 0) { + PX4_ERR("failed to open SPI port %s err: %d", _port, errno); + px4_sleep(1); + continue; + } - status_value = ::ioctl(_serial_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + int spi_speed = 1000000; // make sure the bus speed is not too high (required on RPi) + int status_value = ::ioctl(_spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed); - if (status_value < 0) { - PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); - } + if (status_value < 0) { + PX4_ERR("SPI_IOC_WR_MAX_SPEED_HZ failed for %s (%d)", _port, errno); + } + + status_value = ::ioctl(_spi_fd, SPI_IOC_RD_MAX_SPEED_HZ, &spi_speed); + + if (status_value < 0) { + PX4_ERR("SPI_IOC_RD_MAX_SPEED_HZ failed for %s (%d)", _port, errno); } #endif /* __PX4_LINUX */ @@ -906,11 +875,6 @@ GPS::run() _helper = new GPSDriverNMEA(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset); set_device_type(DRV_GPS_DEVTYPE_NMEA); break; - - case gps_driver_mode_t::SBF: - _helper = new GPSDriverSBF(&GPS::callback, this, &_report_gps_pos, _p_report_sat_info, heading_offset, pitch_offset); - set_device_type(DRV_GPS_DEVTYPE_SBF); - break; #endif // CONSTRAINED_FLASH default: @@ -970,7 +934,8 @@ GPS::run() set_device_type(DRV_GPS_DEVTYPE_UBX_9); break; - case GPSDriverUBX::Board::u_blox9_F9P: + case GPSDriverUBX::Board::u_blox9_F9P_L1L2: + case GPSDriverUBX::Board::u_blox9_F9P_L1L5: set_device_type(DRV_GPS_DEVTYPE_UBX_F9P); break; @@ -1056,9 +1021,15 @@ GPS::run() } } - if (_serial_fd >= 0) { - ::close(_serial_fd); - _serial_fd = -1; + if (_interface == GPSHelper::Interface::UART) { + (void) _uart.close(); + +#ifdef __PX4_LINUX + + } else if ((_interface == GPSHelper::Interface::SPI) && (_spi_fd >= 0)) { + ::close(_spi_fd); + _spi_fd = -1; +#endif } if (_mode_auto) { @@ -1081,11 +1052,8 @@ GPS::run() break; case gps_driver_mode_t::FEMTOMES: - _mode = gps_driver_mode_t::SBF; - break; - - case gps_driver_mode_t::SBF: case gps_driver_mode_t::NMEA: // skip NMEA for auto-detection to avoid false positive matching + #endif // CONSTRAINED_FLASH _mode = gps_driver_mode_t::UBX; px4_usleep(500000); // tried all possible drivers. Wait a bit before next round @@ -1145,10 +1113,6 @@ GPS::print_status() case gps_driver_mode_t::NMEA: PX4_INFO("protocol: NMEA"); - break; - - case gps_driver_mode_t::SBF: - PX4_INFO("protocol: SBF"); #endif // CONSTRAINED_FLASH default: @@ -1250,11 +1214,13 @@ GPS::publish() void GPS::publishSatelliteInfo() { - if (_instance == Instance::Main) { + if (_instance == Instance::Main || _is_gps_main_advertised.load()) { if (_p_report_sat_info != nullptr) { _report_sat_info_pub.publish(*_p_report_sat_info); } + _is_gps_main_advertised.store(true); + } else { //we don't publish satellite info for the secondary gps } @@ -1406,7 +1372,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance) entry_point, (char *const *)argv); if (task_id < 0) { - task_id = -1; + _task_id = -1; return -errno; } @@ -1477,12 +1443,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'i': - if (!strcmp(myoptarg, "spi")) { - interface = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface: %s", myoptarg); error_flag = true; @@ -1490,12 +1456,12 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) break; case 'j': - if (!strcmp(myoptarg, "spi")) { - interface_secondary = GPSHelper::Interface::SPI; - - } else if (!strcmp(myoptarg, "uart")) { + if (!strcmp(myoptarg, "uart")) { interface_secondary = GPSHelper::Interface::UART; - +#ifdef __PX4_LINUX + } else if (!strcmp(myoptarg, "spi")) { + interface_secondary = GPSHelper::Interface::SPI; +#endif } else { PX4_ERR("unknown interface for secondary: %s", myoptarg); error_flag = true; @@ -1521,8 +1487,6 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } else if (!strcmp(myoptarg, "nmea")) { mode = gps_driver_mode_t::NMEA; - } else if (!strcmp(myoptarg, "sbf")) { - mode = gps_driver_mode_t::SBF; #endif // CONSTRAINED_FLASH } else { PX4_ERR("unknown protocol: %s", myoptarg); @@ -1547,7 +1511,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) GPS *gps = nullptr; if (instance == Instance::Main) { - if (device_name && (access(device_name, R_OK|W_OK) == 0)) { + if (Serial::validatePort(device_name)) { gps = new GPS(device_name, mode, interface, instance, baudrate_main); } else { @@ -1570,7 +1534,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } } } else { // secondary instance - if (device_name_secondary && (access(device_name_secondary, R_OK|W_OK) == 0)) { + if (Serial::validatePort(device_name_secondary)) { gps = new GPS(device_name_secondary, mode, interface_secondary, instance, baudrate_secondary); } else { diff --git a/src/drivers/gps/module.yaml b/src/drivers/gps/module.yaml index 484e434d5881..1424864277fc 100644 --- a/src/drivers/gps/module.yaml +++ b/src/drivers/gps/module.yaml @@ -13,4 +13,3 @@ serial_config: group: GPS default: GPS1 label: Main GPS - diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c index 49b4da7065bf..512a5131e1c5 100644 --- a/src/drivers/gps/params.c +++ b/src/drivers/gps/params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2024 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 @@ -152,8 +152,9 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); * * The offset angle increases clockwise. * - * Set this to 90 if the rover (or Unicore primary) antenna is placed on the - * right side of the vehicle and the moving base antenna is on the left side. + * Set this to 90 if the rover (or Unicore primary, or Septentrio Mosaic Aux) + * antenna is placed on the right side of the vehicle and the moving base + * antenna is on the left side. * * (Note: the Unicore primary antenna is the one connected on the right as seen * from the top). @@ -168,24 +169,6 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0); */ PARAM_DEFINE_FLOAT(GPS_YAW_OFFSET, 0.f); -/** - * Pitch offset for dual antenna GPS - * - * Vertical offsets can be compensated for by adjusting the Pitch offset (Septentrio). - * - * Note that this can be interpreted as the "roll" angle in case the antennas are aligned along the perpendicular axis. This occurs in situations where the two antenna ARPs may not be exactly at the same height in the vehicle reference frame. Since pitch is defined as the right-handed rotation about the vehicle Y axis, a situation where the main antenna is mounted lower than the aux antenna (assuming the default antenna setup) will result in a positive pitch. - * - * - * @min -90 - * @max 90 - * @unit deg - * @reboot_required true - * @decimal 3 - * - * @group GPS - */ -PARAM_DEFINE_FLOAT(GPS_PITCH_OFFSET, 0.f); - /** * Protocol for Main GPS * @@ -202,7 +185,6 @@ PARAM_DEFINE_FLOAT(GPS_PITCH_OFFSET, 0.f); * @value 4 Emlid Reach * @value 5 Femtomes * @value 6 NMEA (generic) - * @value 7 Septentrio (SBF) * * @reboot_required true * @group GPS @@ -247,14 +229,16 @@ PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1); * 2 : Use Galileo * 3 : Use BeiDou * 4 : Use GLONASS + * 5 : Use NAVIC * * @min 0 - * @max 31 + * @max 63 * @bit 0 GPS (with QZSS) * @bit 1 SBAS * @bit 2 Galileo * @bit 3 BeiDou * @bit 4 GLONASS + * @bit 5 NAVIC * * @reboot_required true * @group GPS @@ -277,14 +261,16 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0); * 2 : Use Galileo * 3 : Use BeiDou * 4 : Use GLONASS + * 5 : Use NAVIC * * @min 0 - * @max 31 + * @max 63 * @bit 0 GPS (with QZSS) * @bit 1 SBAS * @bit 2 Galileo * @bit 3 BeiDou * @bit 4 GLONASS + * @bit 5 NAVIC * * @reboot_required true * @group GPS diff --git a/src/drivers/imu/Kconfig b/src/drivers/imu/Kconfig index 3b084a580169..ee0978d39cd9 100644 --- a/src/drivers/imu/Kconfig +++ b/src/drivers/imu/Kconfig @@ -8,6 +8,7 @@ menu "IMU" select DRIVERS_IMU_ANALOG_DEVICES_ADIS16470 select DRIVERS_IMU_BOSCH_BMI055 select DRIVERS_IMU_BOSCH_BMI088 + select DRIVERS_IMU_MURATA_SCH16T select DRIVERS_IMU_NXP_FXAS21002C select DRIVERS_IMU_NXP_FXOS8701CQ select DRIVERS_IMU_INVENSENSE_ICM20602 diff --git a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp index c12e8d356d34..dff2c7f4f8a2 100644 --- a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp +++ b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp @@ -172,7 +172,9 @@ void ADIS16507::RunImpl() const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); if (DIAG_STAT != 0) { - PX4_ERR("DIAG_STAT: %#X", DIAG_STAT); + PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT); + _state = STATE::RESET; + ScheduleDelayed(3_s); } else { PX4_DEBUG("self test passed"); diff --git a/src/drivers/imu/bosch/bmi088_i2c/Kconfig b/src/drivers/imu/bosch/bmi088_i2c/Kconfig index b4e48f2da9e8..763070403088 100644 --- a/src/drivers/imu/bosch/bmi088_i2c/Kconfig +++ b/src/drivers/imu/bosch/bmi088_i2c/Kconfig @@ -1,5 +1,6 @@ menuconfig DRIVERS_IMU_BOSCH_BMI088_I2C bool "bosch bmi088_i2c" default n + depends on !DRIVERS_IMU_BOSCH_BMI088 ---help--- Enable support for bosch bmi088_i2c diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 8dc0f4b79512..28fce0691ce9 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -203,57 +203,45 @@ void ICM40609D::RunImpl() case STATE::FIFO_READ: { hrt_abstime timestamp_sample = now; - uint8_t samples = 0; + uint8_t samples = FIFOReadCount(); - if (_data_ready_interrupt_enabled) { - // scheduled from interrupt if _drdy_timestamp_sample was set as expected - const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - - if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { - timestamp_sample = drdy_timestamp_sample; - samples = _fifo_gyro_samples; + if (samples == 0) { + perf_count(_fifo_empty_perf); - } else { - perf_count(_drdy_missed_perf); + } else { + // tolerate minor jitter, leave sample to next iteration if behind by only 1 + if (samples == _fifo_gyro_samples + 1) { + timestamp_sample -= static_cast(FIFO_SAMPLE_DT); + samples--; } - // push backup schedule back - ScheduleDelayed(_fifo_empty_interval_us * 2); - } - - if (samples == 0) { - // check current FIFO count - const uint16_t fifo_count = FIFOReadCount(); - - if (fifo_count >= FIFO::SIZE) { + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish FIFOReset(); perf_count(_fifo_overflow_perf); + samples = 0; + } + } - } else if (fifo_count == 0) { - perf_count(_fifo_empty_perf); + bool success = false; - } else { - // FIFO count (size in bytes) - samples = (fifo_count / sizeof(FIFO::DATA)); + if (samples > 0) { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); - // tolerate minor jitter, leave sample to next iteration if behind by only 1 - if (samples == _fifo_gyro_samples + 1) { - timestamp_sample -= static_cast(FIFO_SAMPLE_DT); - samples--; - } + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; - if (samples > FIFO_MAX_SAMPLES) { - // not technically an overflow, but more samples than we expected or can publish - FIFOReset(); - perf_count(_fifo_overflow_perf); - samples = 0; + } else { + perf_count(_drdy_missed_perf); } - } - } - bool success = false; + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } - if (samples >= 1) { if (FIFORead(timestamp_sample, samples)) { success = true; @@ -374,17 +362,11 @@ void ICM40609D::ConfigureSampleRate(int sample_rate) void ICM40609D::ConfigureFIFOWatermark(uint8_t samples) { - // FIFO watermark threshold in number of bytes - const uint16_t fifo_watermark_threshold = samples * sizeof(FIFO::DATA); - for (auto &r : _register_bank0_cfg) { if (r.reg == Register::BANK_0::FIFO_CONFIG2) { // FIFO_WM[7:0] FIFO_CONFIG2 - r.set_bits = fifo_watermark_threshold & 0xFF; + r.set_bits = samples & 0xFF; - } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { - // FIFO_WM[11:8] FIFO_CONFIG3 - r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; } } } @@ -537,25 +519,10 @@ bool ICM40609D::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) return false; } - const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL); - - if (fifo_count_bytes >= FIFO::SIZE) { - perf_count(_fifo_overflow_perf); - FIFOReset(); - return false; - } - - const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA); - - if (fifo_count_samples == 0) { - perf_count(_fifo_empty_perf); - return false; - } - // check FIFO header in every sample uint8_t valid_samples = 0; - for (int i = 0; i < math::min(samples, fifo_count_samples); i++) { + for (int i = 0; i < samples; i++) { bool valid = true; // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx @@ -599,6 +566,9 @@ void ICM40609D::FIFOReset() // SIGNAL_PATH_RESET: FIFO flush RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + // Read INT_STATUS to clear + RegisterRead(Register::BANK_0::INT_STATUS); + // reset while FIFO is disabled _drdy_timestamp_sample.store(0); } diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index ab6fcfa1f27b..8dee6c2fb793 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -160,10 +160,11 @@ class ICM40609D : 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{10}; + static constexpr uint8_t size_register_bank0_cfg{11}; 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::INTF_CONFIG0, INTF_CONFIG0_BIT::FIFO_COUNT_REC | INTF_CONFIG0_BIT::UI_SIFS_CFG_DISABLE_I2C, 0}, { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, { 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_ODR_8kHz, Bit7 | Bit6 | Bit5 | Bit3 | Bit2 }, diff --git a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp index 4d359526ba92..e9880ec8cdd0 100644 --- a/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp +++ b/src/drivers/imu/invensense/icm40609d/InvenSense_ICM40609D_registers.hpp @@ -82,6 +82,8 @@ enum class BANK_0 : uint8_t { SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + PWR_MGMT0 = 0x4E, GYRO_CONFIG0 = 0x4F, ACCEL_CONFIG0 = 0x50, @@ -132,6 +134,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_SPI = Bit1, + UI_SIFS_CFG_DISABLE_I2C = Bit1 | Bit0 +}; + // PWR_MGMT0 enum PWR_MGMT0_BIT : uint8_t { GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode diff --git a/src/drivers/imu/murata/Kconfig b/src/drivers/imu/murata/Kconfig new file mode 100644 index 000000000000..36d4534078aa --- /dev/null +++ b/src/drivers/imu/murata/Kconfig @@ -0,0 +1,3 @@ +menu "Murata" +rsource "*/Kconfig" +endmenu #Murata diff --git a/src/drivers/imu/murata/sch16t/CMakeLists.txt b/src/drivers/imu/murata/sch16t/CMakeLists.txt new file mode 100644 index 000000000000..73b90a12fb45 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2022 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_module( + MODULE drivers__imu__murata__sch16t + MAIN sch16t + COMPILE_FLAGS + SRCS + SCH16T.cpp + SCH16T.hpp + sch16t_main.cpp + Murata_SCH16T_registers.hpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/murata/sch16t/Kconfig b/src/drivers/imu/murata/sch16t/Kconfig new file mode 100644 index 000000000000..ef0e906edd25 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_MURATA_SCH16T + bool "SCH16T" + default n + ---help--- + Enable support for murata SCH16T diff --git a/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp b/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp new file mode 100644 index 000000000000..3d85c6eb012c --- /dev/null +++ b/src/drivers/imu/murata/sch16t/Murata_SCH16T_registers.hpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +namespace Murata_SCH16T +{ +// General definitions +static constexpr uint16_t EOI = (1 << 1); // End of Initialization +static constexpr uint16_t EN_SENSOR = (1 << 0); // Enable RATE and ACC measurement +static constexpr uint16_t DRY_DRV_EN = (1 << 5); // Enables Data ready function +static constexpr uint16_t SPI_SOFT_RESET = (0b1010); + +// Filter settings +static constexpr uint8_t FILTER_13_HZ = (0b010); +static constexpr uint8_t FILTER_30_HZ = (0b001); +static constexpr uint8_t FILTER_68_HZ = (0b000); +static constexpr uint8_t FILTER_280_HZ = (0b011); +static constexpr uint8_t FILTER_370_HZ = (0b100); +static constexpr uint8_t FILTER_235_HZ = (0b101); +static constexpr uint8_t FILTER_BYPASS = (0b111); + +// Dynamic range settings +static constexpr uint8_t RATE_RANGE_300 = (0b001); +static constexpr uint8_t ACC12_RANGE_80 = (0b001); +static constexpr uint8_t ACC3_RANGE_260 = (0b000); + +// Decimation ratio settings +static constexpr uint8_t DECIMATION_NONE = (0b000); +static constexpr uint8_t DECIMATION_5900_HZ = (0b001); +static constexpr uint8_t DECIMATION_2950_HZ = (0b010); +static constexpr uint8_t DECIMATION_1475_HZ = (0b011); +static constexpr uint8_t DECIMATION_738_HZ = (0b100); + +union CTRL_FILT_RATE_Register { + struct { + uint16_t FILT_SEL_RATE_X : 3; + uint16_t FILT_SEL_RATE_Y : 3; + uint16_t FILT_SEL_RATE_Z : 3; + uint16_t reserved : 7; + } bits; + + uint16_t value; +}; + +union CTRL_FILT_ACC12_Register { + struct { + uint16_t FILT_SEL_ACC_X12 : 3; + uint16_t FILT_SEL_ACC_Y12 : 3; + uint16_t FILT_SEL_ACC_Z12 : 3; + uint16_t reserved : 7; + } bits; + + uint16_t value; +}; + +union CTRL_FILT_ACC3_Register { + struct { + uint16_t FILT_SEL_ACC_X3 : 3; + uint16_t FILT_SEL_ACC_Y3 : 3; + uint16_t FILT_SEL_ACC_Z3 : 3; + uint16_t reserved : 7; + } bits; + + uint16_t value; +}; + +union RATE_CTRL_Register { + struct { + uint16_t DEC_RATE_X2 : 3; + uint16_t DEC_RATE_Y2 : 3; + uint16_t DEC_RATE_Z2 : 3; + uint16_t DYN_RATE_XYZ2: 3; + uint16_t DYN_RATE_XYZ1: 3; + uint16_t reserved : 1; + } bits; + + uint16_t value; +}; + +union ACC12_CTRL_Register { + struct { + uint16_t DEC_ACC_X2 : 3; + uint16_t DEC_ACC_Y2 : 3; + uint16_t DEC_ACC_Z2 : 3; + uint16_t DYN_ACC_XYZ2: 3; + uint16_t DYN_ACC_XYZ1: 3; + uint16_t reserved : 1; + } bits; + + uint16_t value; +}; + +union ACC3_CTRL_Register { + struct { + uint16_t DYN_ACC_XYZ3 : 3; + uint16_t reserved : 13; + } bits; + + uint16_t value; +}; + +// Data registers +#define RATE_X1 0x01 // 20 bit +#define RATE_Y1 0x02 // 20 bit +#define RATE_Z1 0x03 // 20 bit +#define ACC_X1 0x04 // 20 bit +#define ACC_Y1 0x05 // 20 bit +#define ACC_Z1 0x06 // 20 bit +#define ACC_X3 0x07 // 20 bit +#define ACC_Y3 0x08 // 20 bit +#define ACC_Z3 0x09 // 20 bit +#define RATE_X2 0x0A // 20 bit +#define RATE_Y2 0x0B // 20 bit +#define RATE_Z2 0x0C // 20 bit +#define ACC_X2 0x0D // 20 bit +#define ACC_Y2 0x0E // 20 bit +#define ACC_Z2 0x0F // 20 bit +#define TEMP 0x10 // 16 bit +// Status registers +#define STAT_SUM 0x14 // 16 bit +#define STAT_SUM_SAT 0x15 // 16 bit +#define STAT_COM 0x16 // 16 bit +#define STAT_RATE_COM 0x17 // 16 bit +#define STAT_RATE_X 0x18 // 16 bit +#define STAT_RATE_Y 0x19 // 16 bit +#define STAT_RATE_Z 0x1A // 16 bit +#define STAT_ACC_X 0x1B // 16 bit +#define STAT_ACC_Y 0x1C // 16 bit +#define STAT_ACC_Z 0x1D // 16 bit +// Control registers +#define CTRL_FILT_RATE 0x25 // 9 bit +#define CTRL_FILT_ACC12 0x26 // 9 bit +#define CTRL_FILT_ACC3 0x27 // 9 bit +#define CTRL_RATE 0x28 // 15 bit +#define CTRL_ACC12 0x29 // 15 bit +#define CTRL_ACC3 0x2A // 3 bit +#define CTRL_USER_IF 0x33 // 16 bit +#define CTRL_ST 0x34 // 13 bit +#define CTRL_MODE 0x35 // 4 bit +#define CTRL_RESET 0x36 // 4 bit +// Misc registers +#define ASIC_ID 0x3B // 12 bit +#define COMP_ID 0x3C // 16 bit +#define SN_ID1 0x3D // 16 bit +#define SN_ID2 0x3E // 16 bit +#define SN_ID3 0x3F // 16 bit + +// STAT_SUM_SAT bits +#define STAT_SUM_SAT_RSVD (1 << 15) +#define STAT_SUM_SAT_RATE_X1 (1 << 14) +#define STAT_SUM_SAT_RATE_Y1 (1 << 13) +#define STAT_SUM_SAT_RATE_Z1 (1 << 12) +#define STAT_SUM_SAT_ACC_X1 (1 << 11) +#define STAT_SUM_SAT_ACC_Y1 (1 << 10) +#define STAT_SUM_SAT_ACC_Z1 (1 << 9) +#define STAT_SUM_SAT_ACC_X3 (1 << 8) +#define STAT_SUM_SAT_ACC_Y3 (1 << 7) +#define STAT_SUM_SAT_ACC_Z3 (1 << 6) +#define STAT_SUM_SAT_RATE_X2 (1 << 5) +#define STAT_SUM_SAT_RATE_Y2 (1 << 4) +#define STAT_SUM_SAT_RATE_Z2 (1 << 3) +#define STAT_SUM_SAT_ACC_X2 (1 << 2) +#define STAT_SUM_SAT_ACC_Y2 (1 << 1) +#define STAT_SUM_SAT_ACC_Z2 (1 << 0) + +} // namespace Murata_SCH16T diff --git a/src/drivers/imu/murata/sch16t/SCH16T.cpp b/src/drivers/imu/murata/sch16t/SCH16T.cpp new file mode 100644 index 000000000000..d7ed53f27c7a --- /dev/null +++ b/src/drivers/imu/murata/sch16t/SCH16T.cpp @@ -0,0 +1,713 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SCH16T.hpp" + +using namespace time_literals; + +#define SPI48_DATA_INT32(a) (((int32_t)(((a) << 4) & 0xfffff000UL)) >> 12) +#define SPI48_DATA_UINT32(a) ((uint32_t)(((a) >> 8) & 0x000fffffUL)) +#define SPI48_DATA_UINT16(a) ((uint16_t)(((a) >> 8) & 0x0000ffffUL)) + +static constexpr uint32_t POWER_ON_TIME = 250_ms; + +SCH16T::SCH16T(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + ModuleParams(nullptr), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation), + _drdy_gpio(config.drdy_gpio) +{ + if (_drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + +#if defined(SPI6_nRESET_EXTERNAL1) + _hardware_reset_available = true; +#endif +} + +SCH16T::~SCH16T() +{ + perf_free(_reset_perf); + perf_free(_bad_transfer_perf); + perf_free(_perf_crc_bad); + perf_free(_drdy_missed_perf); + perf_free(_perf_general_error); + perf_free(_perf_command_error); + perf_free(_perf_saturation_error); + perf_free(_perf_doing_initialization); +} + +int SCH16T::init() +{ + px4_usleep(POWER_ON_TIME); + + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + Reset(); + + return PX4_OK; +} + +int SCH16T::probe() +{ + if (hrt_absolute_time() < POWER_ON_TIME) { + PX4_WARN("Required Power-On Start-Up Time %" PRIu32 " ms", POWER_ON_TIME); + } + + RegisterRead(COMP_ID); + uint16_t comp_id = SPI48_DATA_UINT16(RegisterRead(ASIC_ID)); + uint16_t asic_id = SPI48_DATA_UINT16(RegisterRead(ASIC_ID)); + + RegisterRead(SN_ID1); + uint16_t sn_id1 = SPI48_DATA_UINT16(RegisterRead(SN_ID2)); + uint16_t sn_id2 = SPI48_DATA_UINT16(RegisterRead(SN_ID3)); + uint16_t sn_id3 = SPI48_DATA_UINT16(RegisterRead(SN_ID3)); + + char serial_str[14]; + snprintf(serial_str, 14, "%05d%01X%04X", sn_id2, sn_id1 & 0x000F, sn_id3); + + PX4_INFO("Serial:\t %s", serial_str); + PX4_INFO("COMP_ID:\t 0x%0x", comp_id); + PX4_INFO("ASIC_ID:\t 0x%0x", asic_id); + + bool success = asic_id == 0x21 && comp_id == 0x23; + + return success ? PX4_OK : PX4_ERROR; +} + +void SCH16T::Reset() +{ + if (_drdy_gpio) { + DataReadyInterruptDisable(); + } + + ScheduleClear(); + + _state = STATE::RESET_INIT; + ScheduleNow(); +} + +void SCH16T::ResetSpi6(bool reset) +{ +#if defined(SPI6_RESET) + SPI6_RESET(reset); +#endif +} + +void SCH16T::exit_and_cleanup() +{ + if (_drdy_gpio) { + DataReadyInterruptDisable(); + } + + I2CSPIDriverBase::exit_and_cleanup(); +} + +void SCH16T::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_reset_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_perf_crc_bad); + perf_print_counter(_drdy_missed_perf); + perf_print_counter(_perf_general_error); + perf_print_counter(_perf_command_error); + perf_print_counter(_perf_saturation_error); + perf_print_counter(_perf_doing_initialization); +} + +void SCH16T::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET_INIT: { + perf_count(_reset_perf); + + _failure_count = 0; + + if (_hardware_reset_available) { + PX4_INFO("Resetting (hard)"); + ResetSpi6(true); + _state = STATE::RESET_HARD; + ScheduleDelayed(2_ms); + + } else { + PX4_INFO("Resetting (soft)"); + SoftwareReset(); + _state = STATE::CONFIGURE; + ScheduleDelayed(POWER_ON_TIME); + } + + break; + } + + case STATE::RESET_HARD: { + if (_hardware_reset_available) { + ResetSpi6(false); + } + + _state = STATE::CONFIGURE; + ScheduleDelayed(POWER_ON_TIME); + break; + } + + case STATE::CONFIGURE: { + ConfigurationFromParameters(); + Configure(); + + _state = STATE::LOCK_CONFIGURATION; + ScheduleDelayed(POWER_ON_TIME); + break; + } + + case STATE::LOCK_CONFIGURATION: { + ReadStatusRegisters(); // Read all status registers once + RegisterWrite(CTRL_MODE, (EOI | EN_SENSOR)); // Write EOI and EN_SENSOR + + _state = STATE::VALIDATE; + ScheduleDelayed(5_ms); + break; + } + + case STATE::VALIDATE: { + ReadStatusRegisters(); // Read all status registers twice + ReadStatusRegisters(); + + // Check that registers are configured properly and that the sensor status is OK + if (ValidateSensorStatus() && ValidateRegisterConfiguration()) { + _state = STATE::READ; + + if (_drdy_gpio) { + DataReadyInterruptConfigure(); + ScheduleDelayed(100_ms); // backup schedule as a watchdog timeout + + } else { + ScheduleOnInterval(_sample_interval_us, _sample_interval_us); + } + + } else { + _state = STATE::RESET_INIT; + ScheduleDelayed(100_ms); + } + + break; + } + + case STATE::READ: { + hrt_abstime timestamp_sample = now; + + if (_drdy_gpio) { + // scheduled from interrupt if _drdy_timestamp_sample was set as expected + const hrt_abstime drdy_timestamp_sample = _drdy_timestamp_sample.fetch_and(0); + + if ((now - drdy_timestamp_sample) < _sample_interval_us) { + timestamp_sample = drdy_timestamp_sample; + + } else { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_sample_interval_us * 2); + } + + // Collect the data + SensorData data = {}; + + if (ReadData(&data)) { + _px4_accel.set_temperature(float(data.temp) / 100.f); // Temperature signal sensitivity is 100 LSB/°C + _px4_gyro.set_temperature(float(data.temp) / 100.f); + _px4_accel.update(timestamp_sample, data.acc_x, data.acc_y, data.acc_z); + _px4_gyro.update(timestamp_sample, data.gyro_x, data.gyro_y, data.gyro_z); + + if (_failure_count > 0) { + _failure_count--; + } + + } else { + perf_count(_bad_transfer_perf); + _failure_count++; + } + + // Reset if successive failures + if (_failure_count > 10) { + PX4_INFO("Failure count high, resetting"); + Reset(); + return; + } + + break; + } + + default: + break; + } // end switch/case +} + +bool SCH16T::ReadData(SensorData *data) +{ + // Register reads return 48bits. See SafeSpi 48bit out-of-frame protocol. + RegisterRead(RATE_X2); + uint64_t gyro_x = RegisterRead(RATE_Y2); + uint64_t gyro_y = RegisterRead(RATE_Z2); + uint64_t gyro_z = RegisterRead(ACC_X2); + uint64_t acc_x = RegisterRead(ACC_Y2); + uint64_t acc_y = RegisterRead(ACC_Z2); + uint64_t acc_z = RegisterRead(TEMP); + uint64_t temp = RegisterRead(TEMP); + + static constexpr uint64_t MASK48_GENERAL_ERROR = 0b00000000'00010000'00000000'00000000'00000000'00000000; + static constexpr uint64_t MASK48_COMMAND_ERROR = 0b00000000'00001000'00000000'00000000'00000000'00000000; + static constexpr uint64_t MASK48_SATURATION_ERROR = 0b00000000'00000100'00000000'00000000'00000000'00000000; + static constexpr uint64_t MASK48_DOING_INIT = 0b00000000'00000110'00000000'00000000'00000000'00000000; + + uint64_t values[] = { gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z, temp }; + + for (auto v : values) { + // [1b ][1b][ 2b ] + // [IDS][CE][S1:0] + // IDS: Internal Data Status indication. SCH16T uses this field to indicate common cause error. This is redundant, more accurate info + // is seen from sensor status (S1:S0). + // CE: Command Error indication. SCH16T reports only semantically invalid frame content using this field. SPI protocol + // level errors are indicated with High-Z on MISO pin. + // S1,0: Sensor status indication. + // 00: Normal operation + // 01: Error status + // 10: Saturation error + // 11: Initialization running + + if (v & MASK48_GENERAL_ERROR) { + perf_count(_perf_general_error); + return false; + + } else if (v & MASK48_COMMAND_ERROR) { + perf_count(_perf_command_error); + return false; + + } else if ((v & MASK48_DOING_INIT) == MASK48_DOING_INIT) { + perf_count(_perf_doing_initialization); + return false; + + } else if (v & MASK48_SATURATION_ERROR) { + perf_count(_perf_saturation_error); + // Don't consider saturation an error + } + + // Validate the CRC + if (uint8_t(v & 0xff) != CalculateCRC8(v)) { + perf_count(_perf_crc_bad); + return false; + } + } + + // Data registers are 20bit 2s complement + data->acc_x = SPI48_DATA_INT32(acc_x); + data->acc_y = SPI48_DATA_INT32(acc_y); + data->acc_z = SPI48_DATA_INT32(acc_z); + data->gyro_x = SPI48_DATA_INT32(gyro_x); + data->gyro_y = SPI48_DATA_INT32(gyro_y); + data->gyro_z = SPI48_DATA_INT32(gyro_z); + // Temperature data is always 16 bits wide. Drop 4 LSBs as they are not used. + data->temp = SPI48_DATA_INT32(temp) >> 4; + // Conver to PX4 coordinate system (FLU to FRD) + data->acc_x = data->acc_x; + data->acc_y = -data->acc_y; + data->acc_z = -data->acc_z; + data->gyro_x = data->gyro_x; + data->gyro_y = -data->gyro_y; + data->gyro_z = -data->gyro_z; + return true; +} + +void SCH16T::ConfigurationFromParameters() +{ + // NOTE: We use ACC2 and RATE2 which are both decimated without interpolation + CTRL_FILT_RATE_Register filt_rate; + CTRL_FILT_ACC12_Register filt_acc12; + CTRL_FILT_ACC3_Register filt_acc3; + RATE_CTRL_Register rate_ctrl; + ACC12_CTRL_Register acc12_ctrl; + ACC3_CTRL_Register acc3_ctrl; + + // We always use the maximum dynamic range for gyro and accel + rate_ctrl.bits.DYN_RATE_XYZ1 = RATE_RANGE_300; + rate_ctrl.bits.DYN_RATE_XYZ2 = RATE_RANGE_300; + acc12_ctrl.bits.DYN_ACC_XYZ1 = ACC12_RANGE_80; + acc12_ctrl.bits.DYN_ACC_XYZ2 = ACC12_RANGE_80; + acc3_ctrl.bits.DYN_ACC_XYZ3 = ACC3_RANGE_260; + + _px4_gyro.set_range(math::radians(327.5f)); // 327.5 °/sec + _px4_gyro.set_scale(math::radians(1.f / 1600.f)); // 1600 LSB/(°/sec) + _px4_accel.set_range(163.4f); // 163.4 m/s2 + _px4_accel.set_scale(1.f / 3200.f); // 3200 LSB/(m/s2) + + // Gyro filter + switch (_sch16t_gyro_filt.get()) { + case 0: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_13_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_13_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_13_HZ; + break; + + case 1: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_30_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_30_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_30_HZ; + break; + + case 2: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_68_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_68_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_68_HZ; + break; + + case 3: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_235_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_235_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_235_HZ; + break; + + case 4: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_280_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_280_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_280_HZ; + break; + + case 5: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_370_HZ; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_370_HZ; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_370_HZ; + break; + + case 6: + filt_rate.bits.FILT_SEL_RATE_X = FILTER_BYPASS; + filt_rate.bits.FILT_SEL_RATE_Y = FILTER_BYPASS; + filt_rate.bits.FILT_SEL_RATE_Z = FILTER_BYPASS; + break; + } + + // ACC12 / ACC3 filter + switch (_sch16t_acc_filt.get()) { + case 0: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_13_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_13_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_13_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_13_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_13_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_13_HZ; + break; + + case 1: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_30_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_30_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_30_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_30_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_30_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_30_HZ; + break; + + case 2: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_68_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_68_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_68_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_68_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_68_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_68_HZ; + break; + + case 3: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_235_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_235_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_235_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_235_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_235_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_235_HZ; + break; + + case 4: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_280_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_280_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_280_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_280_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_280_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_280_HZ; + break; + + case 5: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_370_HZ; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_370_HZ; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_370_HZ; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_370_HZ; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_370_HZ; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_370_HZ; + break; + + case 6: + filt_acc12.bits.FILT_SEL_ACC_X12 = FILTER_BYPASS; + filt_acc12.bits.FILT_SEL_ACC_Y12 = FILTER_BYPASS; + filt_acc12.bits.FILT_SEL_ACC_Z12 = FILTER_BYPASS; + + filt_acc3.bits.FILT_SEL_ACC_X3 = FILTER_BYPASS; + filt_acc3.bits.FILT_SEL_ACC_Y3 = FILTER_BYPASS; + filt_acc3.bits.FILT_SEL_ACC_Z3 = FILTER_BYPASS; + break; + } + + // Gyro decimation (only affects channel 2, ie RATE2) + switch (_sch16t_decim.get()) { + case 0: + _sample_interval_us = 85; + rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_NONE; + rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_NONE; + rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_NONE; + acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_NONE; + acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_NONE; + acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_NONE; + break; + + case 1: + _sample_interval_us = 169; + rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_5900_HZ; + rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_5900_HZ; + rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_5900_HZ; + acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_5900_HZ; + acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_5900_HZ; + acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_5900_HZ; + break; + + case 2: + _sample_interval_us = 338; + rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_2950_HZ; + rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_2950_HZ; + rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_2950_HZ; + acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_2950_HZ; + acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_2950_HZ; + acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_2950_HZ; + break; + + case 3: + _sample_interval_us = 678; + rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_1475_HZ; + rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_1475_HZ; + rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_1475_HZ; + acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_1475_HZ; + acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_1475_HZ; + acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_1475_HZ; + break; + + case 4: + _sample_interval_us = 1355; + rate_ctrl.bits.DEC_RATE_X2 = DECIMATION_738_HZ; + rate_ctrl.bits.DEC_RATE_Y2 = DECIMATION_738_HZ; + rate_ctrl.bits.DEC_RATE_Z2 = DECIMATION_738_HZ; + acc12_ctrl.bits.DEC_ACC_X2 = DECIMATION_738_HZ; + acc12_ctrl.bits.DEC_ACC_Y2 = DECIMATION_738_HZ; + acc12_ctrl.bits.DEC_ACC_Z2 = DECIMATION_738_HZ; + break; + } + + _registers[0] = RegisterConfig(CTRL_FILT_RATE, filt_rate.value); + _registers[1] = RegisterConfig(CTRL_FILT_ACC12, filt_acc12.value); + _registers[2] = RegisterConfig(CTRL_FILT_ACC3, filt_acc3.value); + _registers[3] = RegisterConfig(CTRL_RATE, rate_ctrl.value); + _registers[4] = RegisterConfig(CTRL_ACC12, acc12_ctrl.value); + _registers[5] = RegisterConfig(CTRL_ACC3, acc3_ctrl.value); +} + +void SCH16T::Configure() +{ + for (auto &r : _registers) { + RegisterWrite(r.addr, r.value); + } + + RegisterWrite(CTRL_USER_IF, DRY_DRV_EN); // Enable data ready + RegisterWrite(CTRL_MODE, EN_SENSOR); // Enable the sensor +} + +bool SCH16T::ValidateRegisterConfiguration() +{ + bool success = true; + + for (auto &r : _registers) { + RegisterRead(r.addr); // double read, wasteful but makes the code cleaner, not high rate so doesn't matter anyway + auto value = SPI48_DATA_UINT16(RegisterRead(r.addr)); + + if (value != r.value) { + PX4_INFO("Register 0x%0x misconfigured: 0x%0x", r.addr, value); + success = false; + } + } + + return success; +} + +void SCH16T::ReadStatusRegisters() +{ + RegisterRead(STAT_SUM); + _sensor_status.summary = SPI48_DATA_UINT16(RegisterRead(STAT_SUM_SAT)); + _sensor_status.saturation = SPI48_DATA_UINT16(RegisterRead(STAT_COM)); + _sensor_status.common = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_COM)); + _sensor_status.rate_common = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_X)); + _sensor_status.rate_x = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_Y)); + _sensor_status.rate_y = SPI48_DATA_UINT16(RegisterRead(STAT_RATE_Z)); + _sensor_status.rate_z = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_X)); + _sensor_status.acc_x = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Y)); + _sensor_status.acc_y = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Z)); + _sensor_status.acc_z = SPI48_DATA_UINT16(RegisterRead(STAT_ACC_Z)); +} + +bool SCH16T::ValidateSensorStatus() +{ + auto &s = _sensor_status; + uint16_t values[] = { s.summary, s.saturation, s.common, s.rate_common, s.rate_x, s.rate_y, s.rate_z, s.acc_x, s.acc_y, s.acc_z }; + + for (auto v : values) { + if (v != 0xFFFF) { + PX4_INFO("Sensor status failed"); + return false; + } + } + + return true; +} + +void SCH16T::SoftwareReset() +{ + RegisterWrite(CTRL_RESET, SPI_SOFT_RESET); +} + +uint64_t SCH16T::RegisterRead(uint8_t addr) +{ + uint64_t frame = {}; + frame |= uint64_t(addr) << 38; // Target address offset + frame |= uint64_t(1) << 35; // FrameType: SPI48BF + frame |= uint64_t(CalculateCRC8(frame)); + + return TransferSpiFrame(frame); +} + +// Non-data registers are the only writable ones and are 16 bit or less +void SCH16T::RegisterWrite(uint8_t addr, uint16_t value) +{ + uint64_t frame = {}; + frame |= uint64_t(1) << 37; // Write bit + frame |= uint64_t(addr) << 38; // Target address offset + frame |= uint64_t(1) << 35; // FrameType: SPI48BF + frame |= uint64_t(value) << 8; + frame |= uint64_t(CalculateCRC8(frame)); + + // We don't care about the return frame on a write + (void)TransferSpiFrame(frame); +} + +// The SPI protocol (SafeSPI) is 48bit out-of-frame. This means read return frames will be received on the next transfer. +uint64_t SCH16T::TransferSpiFrame(uint64_t frame) +{ + uint16_t buf[3]; + + for (int index = 0; index < 3; index++) { + buf[3 - index - 1] = (frame >> (index << 4)) & 0xFFFF; + } + + transferhword(buf, buf, 3); + +#if defined(DEBUG_BUILD) + PX4_INFO("TransferSpiFrame: 0x%llx", frame); + + PX4_INFO("RECEIVED"); + + for (auto r : buf) { + PX4_INFO("%u", r); + } + +#endif + + uint64_t value = {}; + + for (int index = 0; index < 3; index++) { + value |= (uint64_t)buf[index] << ((3 - index - 1) << 4); + } + + return value; +} + +int SCH16T::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void SCH16T::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool SCH16T::DataReadyInterruptConfigure() +{ + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, true, false, false, &DataReadyInterruptCallback, this) == 0; +} + +bool SCH16T::DataReadyInterruptDisable() +{ + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +uint8_t SCH16T::CalculateCRC8(uint64_t frame) +{ + uint64_t data = frame & 0xFFFFFFFFFF00LL; + uint8_t crc = 0xFF; + + for (int i = 47; i >= 0; i--) { + uint8_t data_bit = data >> i & 0x01; + crc = crc & 0x80 ? (uint8_t)((crc << 1) ^ 0x2F) ^ data_bit : (uint8_t)(crc << 1) | data_bit; + } + + return crc; +} diff --git a/src/drivers/imu/murata/sch16t/SCH16T.hpp b/src/drivers/imu/murata/sch16t/SCH16T.hpp new file mode 100644 index 000000000000..3487bf115e0e --- /dev/null +++ b/src/drivers/imu/murata/sch16t/SCH16T.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "Murata_SCH16T_registers.hpp" +#include +#include +#include +#include + +using namespace Murata_SCH16T; + +class SCH16T : public device::SPI, public I2CSPIDriver, public ModuleParams +{ +public: + SCH16T(const I2CSPIDriverConfig &config); + ~SCH16T() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct SensorData { + int32_t acc_x; + int32_t acc_y; + int32_t acc_z; + int32_t gyro_x; + int32_t gyro_y; + int32_t gyro_z; + int32_t temp; + }; + + struct SensorStatus { + uint16_t summary; + uint16_t saturation; + uint16_t common; + uint16_t rate_common; + uint16_t rate_x; + uint16_t rate_y; + uint16_t rate_z; + uint16_t acc_x; + uint16_t acc_y; + uint16_t acc_z; + }; + + struct RegisterConfig { + RegisterConfig(uint16_t a = 0, uint16_t v = 0) + : addr(a) + , value(v) + {}; + uint8_t addr; + uint16_t value; + }; + + int probe() override; + void exit_and_cleanup() override; + + bool ValidateSensorStatus(); + bool ValidateRegisterConfiguration(); + void Reset(); + void ResetSpi6(bool reset); + uint8_t CalculateCRC8(uint64_t frame); + + bool ReadData(SensorData *data); + void ReadStatusRegisters(); + + void Configure(); + void ConfigurationFromParameters(); + + void SoftwareReset(); + + void RegisterWrite(uint8_t addr, uint16_t value); + uint64_t RegisterRead(uint8_t addr); + uint64_t TransferSpiFrame(uint64_t frame); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); +private: + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + SensorStatus _sensor_status{}; + + int _failure_count{0}; + + px4::atomic _drdy_timestamp_sample{0}; + const spi_drdy_gpio_t _drdy_gpio; + bool _hardware_reset_available{false}; + + enum class STATE : uint8_t { + RESET_INIT, + RESET_HARD, + CONFIGURE, + LOCK_CONFIGURATION, + VALIDATE, + READ, + } _state{STATE::RESET_INIT}; + + RegisterConfig _registers[6]; + + uint32_t _sample_interval_us = 678; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC8 bad"))}; + perf_counter_t _drdy_missed_perf{nullptr}; + perf_counter_t _perf_general_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": general error"))}; + perf_counter_t _perf_command_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": command error"))}; + perf_counter_t _perf_saturation_error{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": saturation error"))}; + perf_counter_t _perf_doing_initialization{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": re-initializing"))}; + + DEFINE_PARAMETERS( + (ParamInt) _sch16t_gyro_filt, + (ParamInt) _sch16t_acc_filt, + (ParamInt) _sch16t_decim + ) +}; diff --git a/src/lib/battery/battery_params_deprecated.c b/src/drivers/imu/murata/sch16t/parameters.c similarity index 65% rename from src/lib/battery/battery_params_deprecated.c rename to src/drivers/imu/murata/sch16t/parameters.c index 829e636c2c27..85e6728f6461 100644 --- a/src/lib/battery/battery_params_deprecated.c +++ b/src/drivers/imu/murata/sch16t/parameters.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -32,39 +32,59 @@ ****************************************************************************/ /** - * @file battery_params_deprecated.c + * Murata SCH16T IMU (external SPI) * - * Defines the deprcated single battery configuration which are temporarily kept for backwards compatibility with QGC. - */ + * @reboot_required true + * @min 0 + * @max 1 + * @group Sensors + * @value 0 Disabled + * @value 1 Enabled + */ +PARAM_DEFINE_INT32(SENS_EN_SCH16T, 0); /** - * This parameter is deprecated. Please use BAT1_V_EMPTY instead. + * Gyro filter settings * - * @group Battery Calibration - * @category system - */ -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.6f); - -/** - * This parameter is deprecated. Please use BAT1_V_CHARGED instead. + * @value 0 13 Hz + * @value 1 30 Hz + * @value 2 68 Hz + * @value 3 235 Hz + * @value 4 280 Hz + * @value 5 370 Hz + * @value 6 No filter + * + * @reboot_required true * - * @group Battery Calibration - * @category system */ -PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.05f); +PARAM_DEFINE_INT32(SCH16T_GYRO_FILT, 2); /** - * This parameter is deprecated. Please use BAT1_V_LOAD_DROP instead. + * Accel filter settings + * + * @value 0 13 Hz + * @value 1 30 Hz + * @value 2 68 Hz + * @value 3 235 Hz + * @value 4 280 Hz + * @value 5 370 Hz + * @value 6 No filter + * + * @reboot_required true * - * @group Battery Calibration - * @category system */ -PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.3f); +PARAM_DEFINE_INT32(SCH16T_ACC_FILT, 6); /** - * This parameter is deprecated. Please use BAT1_N_CELLS instead. + * Gyro and Accel decimation settings + * + * @value 0 None + * @value 1 5900 Hz + * @value 2 2950 Hz + * @value 3 1475 Hz + * @value 4 738 Hz + * + * @reboot_required true * - * @group Battery Calibration - * @category system */ -PARAM_DEFINE_INT32(BAT_N_CELLS, 3); +PARAM_DEFINE_INT32(SCH16T_DECIM, 4); diff --git a/src/drivers/imu/murata/sch16t/sch16t_main.cpp b/src/drivers/imu/murata/sch16t/sch16t_main.cpp new file mode 100644 index 000000000000..0f2341abd8b8 --- /dev/null +++ b/src/drivers/imu/murata/sch16t/sch16t_main.cpp @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "SCH16T.hpp" + +#include + +void SCH16T::print_usage() +{ + PRINT_MODULE_USAGE_NAME("sch16t", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int sch16t_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = SCH16T; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = 5000000; + cli.spi_mode = SPIDEV_MODE0; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_SCH16T); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/imu/nxp/fxos8701cq/fxos8701cq_spi.cpp b/src/drivers/imu/nxp/fxos8701cq/fxos8701cq_spi.cpp index 28c12b3f652a..795afdea006e 100644 --- a/src/drivers/imu/nxp/fxos8701cq/fxos8701cq_spi.cpp +++ b/src/drivers/imu/nxp/fxos8701cq/fxos8701cq_spi.cpp @@ -213,4 +213,3 @@ int FXOS8701CQ_SPI::write(unsigned reg, void *data, unsigned count) return transfer(cmd, nullptr, sizeof(cmd)); } - diff --git a/src/drivers/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 0278f63abdcb..85dd4c1846af 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -62,10 +62,6 @@ VectorNav::VectorNav(const char *port) : v = 0; param_set(param_find("EKF2_EN"), &v); - // SYS_MC_EST_GROUP 0 (disabled) - v = 0; - param_set(param_find("SYS_MC_EST_GROUP"), &v); - // SENS_IMU_MODE (VN handles sensor selection) v = 0; param_set(param_find("SENS_IMU_MODE"), &v); @@ -372,7 +368,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) test_ratio = 0.1f; } - estimator_status.mag_test_ratio = test_ratio; + estimator_status.hdg_test_ratio = test_ratio; estimator_status.vel_test_ratio = test_ratio; estimator_status.pos_test_ratio = test_ratio; estimator_status.hgt_test_ratio = test_ratio; @@ -445,10 +441,11 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.altitude_msl_m = positionGpsLla.c[2]; sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m; - sensor_gps.vel_ned_valid = true; + sensor_gps.vel_m_s = matrix::Vector3f(velocityGpsNed.c).length(); sensor_gps.vel_n_m_s = velocityGpsNed.c[0]; sensor_gps.vel_e_m_s = velocityGpsNed.c[1]; sensor_gps.vel_d_m_s = velocityGpsNed.c[2]; + sensor_gps.vel_ned_valid = true; sensor_gps.hdop = dop.hDOP; sensor_gps.vdop = dop.vDOP; @@ -835,7 +832,7 @@ Serial bus driver for the VectorNav VN-100, VN-200, VN-300. Most boards are configured to enable/start the driver on a specified UART using the SENS_VN_CFG parameter. -Setup/usage information: https://docs.px4.io/master/en/sensor/vectornav.html +Setup/usage information: https://docs.px4.io/main/en/sensor/vectornav.html ### Examples diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h b/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h index 6bbf62b97c80..959d42314d88 100644 --- a/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h +++ b/src/drivers/ins/vectornav/libvnc/include/vn/math/matrix.h @@ -84,4 +84,3 @@ mat3f vnm_negative_mat3f(mat3f m); #endif #endif - diff --git a/src/drivers/ins/vectornav/libvnc/include/vn/util.h b/src/drivers/ins/vectornav/libvnc/include/vn/util.h index 02388f41f08a..6da16dfd1c31 100644 --- a/src/drivers/ins/vectornav/libvnc/include/vn/util.h +++ b/src/drivers/ins/vectornav/libvnc/include/vn/util.h @@ -11,6 +11,10 @@ #include "vn/util/export.h" #include "vn/types.h" +#ifndef UNUSED +#define UNUSED(x) (void)(sizeof(x)) +#endif + #ifdef __cplusplus extern "C" { #endif diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c index d162a2f9aa73..fefce9ba515b 100644 --- a/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c +++ b/src/drivers/ins/vectornav/libvnc/src/vn/protocol/spi.c @@ -2,8 +2,6 @@ #include #include "vn/util.h" -//#define UNUSED(x) (void)(sizeof(x)) - VnError VnSpi_genGenericCommand( char cmdId, char* buffer, diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c index 958315b0421c..996fa4cd1baf 100644 --- a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c +++ b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/event.c @@ -1,7 +1,9 @@ /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */ #if defined(__linux__) || defined(__NUTTX__) /* Works for Ubuntu 15.10 */ +#ifndef _POSIX_C_SOURCE #define _POSIX_C_SOURCE 199309L +#endif #elif defined __CYGWIN__ /* Works for Cygwin 2.4.0 64-bit */ #define _POSIX_TIMERS 1 diff --git a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c index f06d415de0c7..9548f8c65341 100644 --- a/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c +++ b/src/drivers/ins/vectornav/libvnc/src/vn/xplat/time.c @@ -1,7 +1,9 @@ /* Enable IEEE Std 1003.1b-1993 functionality required for clock_gettime. */ #if defined(__linux__) || defined(__NUTTX__) /* Works for Ubuntu 15.10 */ +#ifndef _POSIX_C_SOURCE #define _POSIX_C_SOURCE 199309L +#endif #elif defined __CYGWIN__ /* Works for Cygwin 2.4.0 64-bit */ #define _POSIX_TIMERS 1 diff --git a/src/drivers/linux_pwm_out/Kconfig b/src/drivers/linux_pwm_out/Kconfig index d5728454da9e..ac7b54b2045a 100644 --- a/src/drivers/linux_pwm_out/Kconfig +++ b/src/drivers/linux_pwm_out/Kconfig @@ -1,5 +1,6 @@ menuconfig DRIVERS_LINUX_PWM_OUT bool "linux_pwm_out" default n + depends on PLATFORM_POSIX && !BOARD_TESTING ---help--- Enable support for linux_pwm_out diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 7255574717b0..07bf93cddc2b 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -39,5 +39,7 @@ add_subdirectory(isentek) add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) +add_subdirectory(memsic) add_subdirectory(rm3100) +add_subdirectory(st) add_subdirectory(vtrantech) diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig index 12859150806c..018b89502b26 100644 --- a/src/drivers/magnetometer/Kconfig +++ b/src/drivers/magnetometer/Kconfig @@ -9,11 +9,12 @@ menu "Magnetometer" select DRIVERS_MAGNETOMETER_QMC5883L select DRIVERS_MAGNETOMETER_ISENTEK_IST8308 select DRIVERS_MAGNETOMETER_ISENTEK_IST8310 - select DRIVERS_MAGNETOMETER_LIS2MDL select DRIVERS_MAGNETOMETER_LIS3MDL select DRIVERS_MAGNETOMETER_LSM303AGR select DRIVERS_MAGNETOMETER_RM3100 select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L + select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA + select DRIVERS_MAGNETOMETER_ST_IIS2MDC ---help--- Enable default set of magnetometer drivers rsource "*/Kconfig" diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt index b814e7224fb1..d7e848ec86a4 100644 --- a/src/drivers/magnetometer/bosch/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(bmm150) +add_subdirectory(bmm350) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp new file mode 100755 index 000000000000..e4e8e19f9c5e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -0,0 +1,773 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "BMM350.hpp" +using namespace time_literals; + +BMM350::BMM350(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + ModuleParams(nullptr), + _px4_mag(get_device_id(), config.rotation) + +{ +} + +BMM350::~BMM350() +{ + perf_free(_reset_perf); + perf_free(_bad_read_perf); + perf_free(_self_test_failed_perf); +} + +int BMM350::init() +{ + ModuleParams::updateParams(); + ParametersUpdate(true); + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMM350::Reset() +{ + RegisterWrite(Register::CMD, SOFT_RESET); + _state = STATE::RESET; + ScheduleClear(); + ScheduleDelayed(1_ms); + return true; +} + +void BMM350::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_read_perf); + perf_print_counter(_self_test_failed_perf); +} + +void BMM350::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); + UpdateMagParams(); + } +} + +void BMM350::UpdateMagParams() +{ + uint8_t odr = GetODR(_param_bmm350_odr.get()); + uint8_t avg = GetAVG(_param_bmm350_avg.get()); + + _mag_odr_mode = odr; + _mag_avg_mode = avg; + _mag_pad_drive = static_cast(_param_bmm350_drive.get()); + PX4_DEBUG("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive); +} + +uint8_t BMM350::GetODR(int value) +{ + switch (value) { + case 0: return ODR_400HZ; + + case 1: return ODR_200HZ; + + case 2: return ODR_100HZ; + + case 3: return ODR_50HZ; + + case 4: return ODR_25HZ; + + case 5: return ODR_12_5HZ; + + case 6: return ODR_6_25HZ; + + case 7: return ODR_3_125HZ; + + case 8: return ODR_1_5625HZ; + + default: return ODR_200HZ; + } +} + +hrt_abstime BMM350::OdrToUs(uint8_t odr) +{ + switch (odr) { + case ODR_400HZ: + return 2500_us; + + case ODR_200HZ: + return 5000_us; + + case ODR_100HZ: + return 10000_us; + + case ODR_50HZ: + return 20000_us; + + case ODR_25HZ: + return 40000_us; + + case ODR_12_5HZ: + return 80000_us; + + case ODR_6_25HZ: + return 160000_us; + + case ODR_3_125HZ: + return 320000_us; + + case ODR_1_5625HZ: + return 640000_us; + + default: + return 5000_us; + } +} + +uint8_t BMM350::GetAVG(int value) +{ + switch (value) { + case 0: return AVG_NO_AVG; + + case 1: return AVG_2; + + case 2: return AVG_4; + + case 3: return AVG_8; + + default: return AVG_2; + } +} + + +int BMM350::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t chip_id; + + if (PX4_OK == RegisterRead(Register::CHIP_ID, &chip_id)) { + PX4_DEBUG("CHIP_ID: 0x%02hhX", chip_id); + + if (chip_id == chip_identification_number) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void BMM350::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + int ret = PX4_OK; + ParametersUpdate(); + + switch (_state) { + case STATE::RESET: { + if (RegisterWrite(Register::CMD, SOFT_RESET) == PX4_OK) { + _reset_timestamp = now; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + } + + ScheduleDelayed(10_ms); + } + break; + + case STATE::WAIT_FOR_RESET: { + ret = probe(); + + if (ret == PX4_OK) { + _state = STATE::RESET; + uint8_t status_0; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { + ret = PX4_ERROR; + } + + if (ret == PX4_OK) { + ret = UpdateMagOffsets(); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to FGR"); + _state = STATE::FGR; + + } + + ScheduleDelayed(10_ms); + + } else { + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + } + break; + + case STATE::FGR: { + _state = STATE::RESET; + uint8_t odr_reg_data = (ODR_100HZ & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); + } + + if (ret == PX4_OK) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + } + + if (ret == PX4_OK) { + PX4_DEBUG("Going to BR"); + _state = STATE::BR; + } + + ScheduleDelayed(30_ms); + } + break; + + case STATE::BR: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_FGR) { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); + + if (ret == PX4_OK) { + PX4_DEBUG("Going to after reset"); + _state = STATE::AFTER_RESET; + } + } + + ScheduleDelayed(4_ms); + } + break; + + case STATE::AFTER_RESET: { + uint8_t status_0; + _state = STATE::RESET; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + + if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_BR_FAST) { + _state = STATE::MEASURE_FORCED; + _self_test_state = SELF_TEST_STATE::INIT; + PX4_DEBUG("Going to fast FM"); + } + + ScheduleNow(); + } + break; + + case STATE::MEASURE_FORCED: { + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + + if (ret == PX4_OK) { + _state = STATE::SELF_TEST_CHECK; + + } else { + _state = STATE::RESET; + } + + ScheduleDelayed(OdrToUs(_mag_odr_mode)); + break; + } + + case STATE::SELF_TEST_CHECK: { + + float xyzt[4]; + _state = STATE::RESET; + + if (ReadOutRawData(xyzt) != PX4_OK) { + perf_count(_self_test_failed_perf); + ScheduleNow(); + break; + } + + switch (_self_test_state) { + case SELF_TEST_STATE::INIT: + memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values)); + + if (RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_X: + if (xyzt[0] - _initial_self_test_values[0] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_X; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_X: + if (xyzt[0] - _initial_self_test_values[0] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::POS_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::POS_Y: + if (xyzt[1] - _initial_self_test_values[1] >= 130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y) == PX4_OK) { + _self_test_state = SELF_TEST_STATE::NEG_Y; + _state = STATE::MEASURE_FORCED; + } + + break; + + case SELF_TEST_STATE::NEG_Y: + if (xyzt[1] - _initial_self_test_values[1] <= -130.0f && + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_DISABLE) == PX4_OK) { + PX4_DEBUG("Self test good, going to configure"); + _state = STATE::CONFIGURE; + } + + break; + + } + + if (_state == STATE::RESET) { + PX4_DEBUG("Self test failed"); + perf_count(_self_test_failed_perf); + } + + ScheduleDelayed(1_ms); + } + break; + + + case STATE::CONFIGURE: + if (Configure() == PX4_OK) { + _state = STATE::READ; + PX4_DEBUG("Configure went fine"); + ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + // -- start get_compensated_mag_xyz_temp_data + float out_data[4] = {0.0f}; + float dut_offset_coeff[3], dut_sensit_coeff[3], dut_tcos[3], dut_tcss[3]; + float cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z; + + ret = ReadOutRawData(out_data); + + if (ret == PX4_OK) { + // Apply compensation to temperature reading + out_data[3] = (1 + _mag_comp_vals.dut_sensit_coef.t_sens) * out_data[3] + + _mag_comp_vals.dut_offset_coef.t_offs; + + // Store magnetic compensation structure to an array + dut_offset_coeff[0] = _mag_comp_vals.dut_offset_coef.offset_x; + dut_offset_coeff[1] = _mag_comp_vals.dut_offset_coef.offset_y; + dut_offset_coeff[2] = _mag_comp_vals.dut_offset_coef.offset_z; + + dut_sensit_coeff[0] = _mag_comp_vals.dut_sensit_coef.sens_x; + dut_sensit_coeff[1] = _mag_comp_vals.dut_sensit_coef.sens_y; + dut_sensit_coeff[2] = _mag_comp_vals.dut_sensit_coef.sens_z; + + dut_tcos[0] = _mag_comp_vals.dut_tco.tco_x; + dut_tcos[1] = _mag_comp_vals.dut_tco.tco_y; + dut_tcos[2] = _mag_comp_vals.dut_tco.tco_z; + + dut_tcss[0] = _mag_comp_vals.dut_tcs.tcs_x; + dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y; + dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z; + + for (int idx = 0; idx < 3; idx++) { + out_data[idx] *= 1 + dut_sensit_coeff[idx]; + out_data[idx] += dut_offset_coeff[idx]; + out_data[idx] += dut_tcos[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + out_data[idx] /= 1 + dut_tcss[idx] * (out_data[3] - _mag_comp_vals.dut_t0); + } + + cr_ax_comp_x = (out_data[0] - _mag_comp_vals.cross_axis.cross_x_y * out_data[1]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_y = (out_data[1] - _mag_comp_vals.cross_axis.cross_y_x * out_data[0]) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y); + cr_ax_comp_z = + (out_data[2] + + (out_data[0] * + (_mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_z_y - + _mag_comp_vals.cross_axis.cross_z_x) - + out_data[1] * + (_mag_comp_vals.cross_axis.cross_z_y - _mag_comp_vals.cross_axis.cross_x_y * + _mag_comp_vals.cross_axis.cross_z_x)) / + (1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y)); + + out_data[0] = cr_ax_comp_x; + out_data[1] = cr_ax_comp_y; + out_data[2] = cr_ax_comp_z; + _px4_mag.set_error_count(perf_event_count(_bad_read_perf) + perf_event_count(_self_test_failed_perf)); + _px4_mag.set_temperature(out_data[3]); + _px4_mag.update(now, out_data[0], out_data[1], out_data[2]); + + } else { + perf_count(_bad_read_perf); + } + } + + break; + } +} + +int BMM350::Configure() +{ + PX4_DEBUG("Configuring"); + uint8_t readData = 0; + int ret; + + // Set pad drive + ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + return ret; + } + + // Set PMU data aggregation + uint8_t odr = _mag_odr_mode; + uint8_t avg = _mag_avg_mode; + + if (odr == ODR_400HZ && avg >= AVG_2) { + avg = AVG_NO_AVG; + + } else if (odr == ODR_200HZ && avg >= AVG_4) { + avg = AVG_2; + + } else if (odr == ODR_100HZ && avg >= AVG_8) { + avg = AVG_4; + } + + uint8_t odr_reg_data = (odr & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30)); + + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Cannot set PMU AGG REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU AGGR REG"); + return ret; + } + + odr_reg_data = PMU_CMD_UPDATE_OAE; + ret = RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write PMU CMD REG"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU CMD REG"); + return ret; + } + + // Enable AXIS + uint8_t axis_data = EN_X | EN_Y | EN_Z; + // PMU_CMD_AXIS_EN + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write AXIS data"); + return ret; + } + + ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData); + + if (ret != PX4_OK || readData != axis_data) { + PX4_DEBUG("Couldn't cross check the set AXIS"); + return ret; + } + + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + if (ret != PX4_OK) { + PX4_DEBUG("Failed to start mag in normal mode"); + return ret; + } + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return ret; +} + +int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) +{ + int32_t power = 1 << (num_bits - 1); // Calculate 2^(num_bits - 1) + int32_t retval = static_cast(inval); + + if (retval >= power) { + retval -= (power << 1); // Equivalent to retval = retval - (power * 2) + } + + return retval; +} + +int BMM350::ReadOutRawData(float *out_data) +{ + if (out_data == NULL) { + return -1; + } + + float temp = 0.0; + struct BMM350::raw_mag_data raw_data = {0}; + + // --- Start read_uncomp_mag_temp_data + uint8_t mag_data[14] = {0}; + + uint32_t raw_mag_x, raw_mag_y, raw_mag_z, raw_temp; + uint8_t cmd = static_cast(Register::DATAX_XLSB); + + uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data)); + + if (res != PX4_OK) { + return -1; + } + + raw_mag_x = mag_data[2] + ((uint32_t)mag_data[3] << 8) + ((uint32_t)mag_data[4] << 16); + raw_mag_y = mag_data[5] + ((uint32_t)mag_data[6] << 8) + ((uint32_t)mag_data[7] << 16); + raw_mag_z = mag_data[8] + ((uint32_t)mag_data[9] << 8) + ((uint32_t)mag_data[10] << 16); + raw_temp = mag_data[11] + ((uint32_t)mag_data[12] << 8) + ((uint32_t)mag_data[13] << 16); + + raw_data.raw_x = FixSign(raw_mag_x, 24); + raw_data.raw_y = FixSign(raw_mag_y, 24); + raw_data.raw_z = FixSign(raw_mag_z, 24); + raw_data.raw_t = FixSign(raw_temp, 24); + // --- End read_uncomp_mag_temp_data + + // --- Start read_out_raw_data + out_data[0] = (float)raw_data.raw_x * lsb_to_utc_degc[0]; + out_data[1] = (float)raw_data.raw_y * lsb_to_utc_degc[1]; + out_data[2] = (float)raw_data.raw_z * lsb_to_utc_degc[2]; + out_data[3] = (float)raw_data.raw_t * lsb_to_utc_degc[3]; + + // Adjust temperature + if (out_data[3] > 0.0f) { + temp = (float)(out_data[3] - (1 * 25.49f)); + + } else if (out_data[3] < 0.0f) { + temp = (float)(out_data[3] - (-1 * 25.49f)); + + } else { + temp = (float)(out_data[3]); + } + + out_data[3] = temp; + + return res; +} + +int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +{ + uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); + int ret = RegisterWrite(Register::OTP_CMD, otp_cmd); + uint8_t otp_status = 0; + + if (ret == PX4_OK) { + do { + ret = RegisterRead(Register::OTP_STATUS, &otp_status); + + if (ret != PX4_OK) { + return PX4_ERROR; + } + } while (!(otp_status & 0x01)); + + uint8_t msb = 0, lsb = 0; + ret = RegisterRead(Register::OTP_DATA_MSB, &msb); + + if (ret == PX4_OK) { + ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); + + if (ret == PX4_OK) { + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } + } + } + + return ret; +} + +int BMM350::UpdateMagOffsets() +{ + PX4_DEBUG("DUMPING OTP"); + uint16_t otp_data[32] = {0}; + + for (int idx = 0; idx < 32; idx++) { + if (ReadOTPWord(idx, &otp_data[idx]) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); + } + + if (RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP) != PX4_OK) { + return PX4_ERROR; + } + + PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + + PX4_DEBUG("UPDATING OFFSETS"); + uint16_t off_x_lsb_msb, off_y_lsb_msb, off_z_lsb_msb, t_off; + uint8_t sens_x, sens_y, sens_z, t_sens; + uint8_t tco_x, tco_y, tco_z; + uint8_t tcs_x, tcs_y, tcs_z; + uint8_t cross_x_y, cross_y_x, cross_z_x, cross_z_y; + + off_x_lsb_msb = otp_data[0x0E] & 0x0FFF; + off_y_lsb_msb = ((otp_data[0x0E] & 0xF000) >> 4) + + (otp_data[0x0F] & 0x00FF); + off_z_lsb_msb = (otp_data[0x0F] & 0x0F00) + + (otp_data[0x10] & 0x00FF); + t_off = otp_data[0x0D] & 0x00FF; + + _mag_comp_vals.dut_offset_coef.offset_x = FixSign(off_x_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_y = FixSign(off_y_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.offset_z = FixSign(off_z_lsb_msb, 12); + _mag_comp_vals.dut_offset_coef.t_offs = FixSign(t_off, 8) / 5.0f; + + sens_x = (otp_data[0x10] & 0xFF00) >> 8; + sens_y = (otp_data[0x11] & 0x00FF); + sens_z = (otp_data[0x11] & 0xFF00) >> 8; + t_sens = (otp_data[0x0D] & 0xFF00) >> 8; + + _mag_comp_vals.dut_sensit_coef.sens_x = FixSign(sens_x, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.sens_y = (FixSign(sens_y, 8) / 256.0f) + 0.01f; + _mag_comp_vals.dut_sensit_coef.sens_z = FixSign(sens_z, 8) / 256.0f; + _mag_comp_vals.dut_sensit_coef.t_sens = FixSign(t_sens, 8) / 512.0f; + + tco_x = (otp_data[0x12] & 0x00FF); + tco_y = (otp_data[0x13] & 0x00FF); + tco_z = (otp_data[0x14] & 0x00FF); + + _mag_comp_vals.dut_tco.tco_x = FixSign(tco_x, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_y = FixSign(tco_y, 8) / 32.0f; + _mag_comp_vals.dut_tco.tco_z = FixSign(tco_z, 8) / 32.0f; + + tcs_x = (otp_data[0x12] & 0xFF00) >> 8; + tcs_y = (otp_data[0x13] & 0xFF00) >> 8; + tcs_z = (otp_data[0x14] & 0xFF00) >> 8; + + _mag_comp_vals.dut_tcs.tcs_x = FixSign(tcs_x, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_y = FixSign(tcs_y, 8) / 16384.0f; + _mag_comp_vals.dut_tcs.tcs_z = (FixSign(tcs_z, 8) / 16384.0f) - 0.0001f; + + _mag_comp_vals.dut_t0 = (FixSign(otp_data[0x18], 16) / 512.0f) + 23.0f; + + cross_x_y = (otp_data[0x15] & 0x00FF); + cross_y_x = (otp_data[0x15] & 0xFF00) >> 8; + cross_z_x = (otp_data[0x16] & 0x00FF); + cross_z_y = (otp_data[0x16] & 0xFF00) >> 8; + + _mag_comp_vals.cross_axis.cross_x_y = FixSign(cross_x_y, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_y_x = FixSign(cross_y_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_x = FixSign(cross_z_x, 8) / 800.0f; + _mag_comp_vals.cross_axis.cross_z_y = FixSign(cross_z_y, 8) / 800.0f; + return PX4_OK; +} + +int BMM350::RegisterRead(Register reg, uint8_t *value) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer[3] = {0}; + int ret = transfer(&cmd, 1, buffer, 3); + + if (ret != PX4_OK) { + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); + + } else { + *value = buffer[2]; + } + + return ret; +} + +int BMM350::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] {(uint8_t)reg, value}; + int ret = transfer(buffer, sizeof(buffer), nullptr, 0); + + if (ret != PX4_OK) { + PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); + } + + return ret; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp new file mode 100644 index 000000000000..4ba7f535dbd9 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 BMM350.hpp + * + * Driver for the Bosch BMM350 connected via I2C. + * + */ + +#pragma once + +#include "Bosch_BMM350_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMM350; +using namespace time_literals; + +class BMM350 : public device::I2C, public I2CSPIDriver, public ModuleParams +{ +public: + BMM350(const I2CSPIDriverConfig &config); + ~BMM350() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + struct mag_temp_data { + float x; + float y; + float z; + float temp; + }; + + struct raw_mag_data { + int32_t raw_x; + int32_t raw_y; + int32_t raw_z; + int32_t raw_t; + }; + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct dut_offset_coef { + float t_offs; + float offset_x; + float offset_y; + float offset_z; + }; + struct dut_sensit_coef { + float t_sens; + float sens_x; + float sens_y; + float sens_z; + }; + + struct dut_tco { + float tco_x; + float tco_y; + float tco_z; + }; + + struct dut_tcs { + float tcs_x; + float tcs_y; + float tcs_z; + }; + + struct cross_axis { + float cross_x_y; + float cross_y_x; + float cross_z_x; + float cross_z_y; + }; + + struct mag_compensate_vals { + struct dut_offset_coef dut_offset_coef; + struct dut_sensit_coef dut_sensit_coef; + struct dut_tco dut_tco; + struct dut_tcs dut_tcs; + float dut_t0; + struct cross_axis cross_axis; + }; + + int probe() override; + bool Reset(); + int Configure(); + + int RegisterRead(Register reg, uint8_t *value); + int RegisterWrite(Register reg, uint8_t value); + + int8_t CompensateAxisAndTemp(); + int ReadOutRawData(float *out_data); + int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int32_t FixSign(uint32_t inval, int8_t num_bits); + + int UpdateMagOffsets(); + void ParametersUpdate(bool force = false); + void UpdateMagParams(); + uint8_t GetODR(int value); + hrt_abstime OdrToUs(uint8_t value); + uint8_t GetAVG(int value); + + PX4Magnetometer _px4_mag; + + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")}; + perf_counter_t _bad_read_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad read")}; + perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + mag_compensate_vals _mag_comp_vals{0}; + + float _initial_self_test_values[4]; + + uint8_t _mag_odr_mode = ODR_200HZ; + uint8_t _mag_avg_mode = AVG_2; + uint8_t _mag_pad_drive = 7; + + + static constexpr float BXY_SENS = 14.55f; + static constexpr float BZ_SENS = 9.0f; + static constexpr float TEMP_SENS = 0.00204f; + static constexpr float INA_XY_GAIN_TRT = 19.46f; + static constexpr float INA_Z_GAIN_TRT = 31.0f; + static constexpr float ADC_GAIN = 1 / 1.5f; + static constexpr float LUT_GAIN = 0.714607238769531f; + static constexpr float POWER = 1000000.0f / 1048576.0f; + float lsb_to_utc_degc[4] = { + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + (POWER / (BZ_SENS *INA_Z_GAIN_TRT *ADC_GAIN * LUT_GAIN)), + 1 / (TEMP_SENS *ADC_GAIN *LUT_GAIN * 1048576) + }; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + FGR, + BR, + AFTER_RESET, + MEASURE_FORCED, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + enum class SELF_TEST_STATE : uint8_t { + INIT, + POS_X, + NEG_X, + POS_Y, + NEG_Y + } _self_test_state{SELF_TEST_STATE::INIT}; + + DEFINE_PARAMETERS( + (ParamInt) _param_bmm350_odr, + (ParamInt) _param_bmm350_avg, + (ParamInt) _param_bmm350_drive + ) + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp new file mode 100644 index 000000000000..dc391f8824aa --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -0,0 +1,159 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2024 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 Bosch_BMM350_registers.hpp + * + * Bosch BMM350 registers. + * + */ + +#pragma once + +#include + +namespace Bosch_BMM350 +{ +#define ENABLE_X_AXIS(axis_data) (axis_data = (axis_data & ~(0x01)) | (1 & 0x01)) +#define ENABLE_Y_AXIS(axis_data) (axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02))) +#define ENABLE_Z_AXIS(axis_data) (axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04))) + +static constexpr uint32_t I2C_SPEED = 400 * 1000; +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x14; +static constexpr uint8_t chip_identification_number = 0x33; + +enum class Register : uint8_t { + CHIP_ID = 0x00, + + PAD_CTRL = 0x03, + PMU_CMD_AGGR_SET = 0x04, + PMU_CMD_AXIS_EN = 0x05, + PMU_CMD = 0x06, + PMU_STATUS_0 = 0x07, + PMU_STATUS_1 = 0x08, + + I2C_WDT_SET = 0x0a, + + DATAX_XLSB = 0x31, + + OTP_CMD = 0x50, + OTP_DATA_MSB = 0x52, + OTP_DATA_LSB = 0x53, + OTP_STATUS = 0x55, + + TMR_SELF_TEST_USER = 0x60, + CMD = 0x7e +}; + +enum CONTROL_CMD : uint8_t { + SOFT_RESET = 0xb6, + EN_XYZ = 0x7 +}; + +enum PMU_CONTROL_CMD : uint8_t { + PMU_CMD_SUSPEND = 0x00, + PMU_CMD_NM = 0x01, + PMU_CMD_UPDATE_OAE = 0x02, + PMU_CMD_FM = 0x03, + PMU_CMD_FAST_FM = 0x04, + PMU_CMD_FGR = 0x05, + PMU_CMD_FAST_FGR = 0x06, + PMU_CMD_BR = 0x07, + PMU_CMD_BR_FAST = 0x08, + PMU_CMD_NM_TC = 0x09 +}; + +static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val) +{ + return (val >> 5) & 0x7; +}; + +enum PMU_STATUS_0_BIT : uint8_t { + PMU_BUSY = (1 << 0), + ODR_OVWR = (1 << 1), + AVG_OVWR = (1 << 2), + PWR_NORMAL = (1 << 3), + ILLEGAL_CMD = (1 << 4) +}; + +enum PMU_STATUS_VALUE { + PMU_STATUS_SUS = 0x0, + PMU_STATUS_NM = 0x1, + PMU_STATUS_UPDATE_OAE = 0x2, + PMU_STATUS_FM = 0x3, + PMU_STATUS_FM_FAST = 0x4, + PMU_STATUS_FGR = 0x5, + PMU_STATUS_FGR_FAST = 0x6, + PMU_STATUS_BR = 0x7, + PMU_STATUS_BR_FAST = 0x7, +}; + +enum ODR_CONTROL_CMD : uint8_t { + ODR_400HZ = 0x2, + ODR_200HZ = 0x3, + ODR_100HZ = 0x4, + ODR_50HZ = 0x5, + ODR_25HZ = 0x6, + ODR_12_5HZ = 0x7, + ODR_6_25HZ = 0x8, + ODR_3_125HZ = 0x9, + ODR_1_5625HZ = 0xa +}; + +enum AVG_CONTROL_CMD : uint8_t { + AVG_NO_AVG = 0x0, + AVG_2 = 0x1, + AVG_4 = 0x2, + AVG_8 = 0x3 +}; + +enum ENABLE_AXIS_BIT : uint8_t { + EN_X = (1 << 0), + EN_Y = (1 << 1), + EN_Z = (1 << 2) +}; + +enum OTP_CONTROL_CMD : uint8_t { + PWR_OFF_OTP = 0x80, + OTP_DIR_READ = 0x20, + OTP_WORD_MSK = 0x1F, +}; + +enum SELF_TEST_CMD : uint8_t { + SELF_TEST_DISABLE = 0x00, + SELF_TEST_POS_X = 0x0d, + SELF_TEST_NEG_X = 0x0b, + SELF_TEST_POS_Y = 0x15, + SELF_TEST_NEG_Y = 0x13, +}; +} // namespace Bosch_BMM350 diff --git a/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt new file mode 100644 index 000000000000..ca253077287e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt @@ -0,0 +1,48 @@ +############################################################################ +# +# 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. +# +############################################################################ + +px4_add_module( + MODULE drivers__magnetometer__bosch__bmm350 + MAIN bmm350 + COMPILE_FLAGS + SRCS + BMM350.cpp + BMM350.hpp + bmm350_main.cpp + Bosch_BMM350_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + MODULE_CONFIG + module.yaml + ) diff --git a/src/drivers/magnetometer/bosch/bmm350/Kconfig b/src/drivers/magnetometer/bosch/bmm350/Kconfig new file mode 100644 index 000000000000..834a2c694780 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350 + bool "bmm350" + default n + ---help--- + Enable support for bosch bmm350 diff --git a/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp new file mode 100644 index 000000000000..c707d7957b6b --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2020-2022 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 "BMM350.hpp" + +#include +#include + +void BMM350::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmm350", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x14); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmm350_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMM350; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM350); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml new file mode 100644 index 000000000000..140c5c321a8a --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -0,0 +1,44 @@ +module_name: BMM350 +parameters: + - group: Magnetometer + definitions: + BMM350_ODR: + description: + short: BMM350 ODR rate + long: | + Defines which ODR rate to use during data polling. + type: enum + values: + 0: 400 Hz + 1: 200 Hz + 2: 100 Hz + 3: 50 Hz + 4: 25 Hz + 5: 12.5 Hz + 6: 6.25 Hz + 7: 3.125 Hz + 8: 1.5625 Hz + reboot_required: true + default: [3] + BMM350_AVG: + description: + short: BMM350 data averaging + long: | + Defines which averaging mode to use during data polling. + type: enum + values: + 0: No averaging + 1: 2 sample averaging + 2: 4 sample averaging + 3: 8 sample averaging + reboot_required: true + default: [1] + BMM350_DRIVE: + description: + short: BMM350 pad drive strength setting + long: | + This setting helps avoid signal problems like overshoot or undershoot. + type: int32 + min: 0 + max: 7 + default: [7] diff --git a/src/drivers/magnetometer/hmc5883/CMakeLists.txt b/src/drivers/magnetometer/hmc5883/CMakeLists.txt index 77b8784629dc..d8a4998eb376 100644 --- a/src/drivers/magnetometer/hmc5883/CMakeLists.txt +++ b/src/drivers/magnetometer/hmc5883/CMakeLists.txt @@ -44,4 +44,3 @@ px4_add_module( drivers_magnetometer px4_work_queue ) - diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp index b7d8512ce931..e5baf14ac59c 100644 --- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp +++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp @@ -85,10 +85,27 @@ void IST8310::print_status() int IST8310::probe() { - const uint8_t WAI = RegisterRead(Register::WAI); + uint8_t id = RegisterRead(Register::WAI); + + if (id != Device_ID) { + DEVICE_DEBUG("unexpected WAI 0x%02x", id); + + // Apparently, the IST8310's WHOAMI register is writeable. Presumably, + // this can get corrupted by bus noise. It is only reset if powered off + // for 30s or by a reset. + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + + auto start_time = hrt_absolute_time(); + + while (hrt_elapsed_time(&start_time) < 50_ms) { + px4_usleep(10'000); + id = RegisterRead(Register::WAI); + + if (id == Device_ID) { + return PX4_OK; + } + } - if (WAI != Device_ID) { - DEVICE_DEBUG("unexpected WAI 0x%02x", WAI); return PX4_ERROR; } diff --git a/src/drivers/magnetometer/memsic/Kconfig b/src/drivers/magnetometer/memsic/Kconfig new file mode 100644 index 000000000000..22e5aa4175e5 --- /dev/null +++ b/src/drivers/magnetometer/memsic/Kconfig @@ -0,0 +1,3 @@ +menu "Memsic" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt new file mode 100644 index 000000000000..3bba9619d707 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2024 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_module( + MODULE drivers__magnetometer__memsic__mmc5983ma + MAIN mmc5983ma + COMPILE_FLAGS + SRCS + mmc5983ma_i2c.cpp + mmc5983ma_main.cpp + mmc5983ma.cpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig b/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig new file mode 100644 index 000000000000..f3d1f3145793 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA + bool "mmc5983ma" + default n + ---help--- + Enable support for mmc5983ma diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp new file mode 100644 index 000000000000..11ec0ea7de19 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "mmc5983ma.h" + +using namespace time_literals; + +MMC5983MA::MMC5983MA(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _interface(interface), + _px4_mag(interface->get_device_id(), config.rotation), + _sample_count(perf_alloc(PC_COUNT, "mmc5983ma_read")), + _comms_errors(perf_alloc(PC_COUNT, "mmc5983ma_comms_errors")) +{} + +MMC5983MA::~MMC5983MA() +{ + perf_free(_sample_count); + perf_free(_comms_errors); + delete _interface; +} + +int MMC5983MA::init() +{ + // Start with a reset of the chip + write_register(MMC5983MA_ADDR_CTRL_REG2, MMC5983MA_CTRL_REG1_SW_RESET); + px4_usleep(20_ms); + + // Set measurement BW to 200HZ + write_register(MMC5983MA_ADDR_CTRL_REG1, MMC5983MA_CTRL_REG1_BW_200HZ); + + ScheduleNow(); + + return PX4_OK; +} + +void MMC5983MA::RunImpl() +{ + // The measure/collect loop uses the set/reset functionality of the chip to eliminate temperature related bias + // by reversing the polarity of the sensing element and taking the difference of the two measurements to eliminate + // the offset and then dividing by 2 to arrive at the true value of the field measurement. + // + // The measurement will contain not only the sensors response to the external magnetic field, H, but also the Offset. + // + // Output1 = +H + Offset + // Output2 = -H + Offset + // Measurment = (Output1 - Output2) / 2 + // + // Please refer to Page 18 of the datasheet + // https://www.memsic.com/Public/Uploads/uploadfile/files/20220119/MMC5983MADatasheetRevA.pdf + + switch (_state) { + case State::Measure: { + + uint8_t set_reset_flag = _sample_index == 0 ? MMC5983MA_CTRL_REG0_SET : MMC5983MA_CTRL_REG0_RESET; + write_register(MMC5983MA_ADDR_CTRL_REG0, MMC5983MA_CTRL_REG0_TM_M | set_reset_flag); + + _collect_retries = 0; + _state = State::Collect; + + // 200Hz BW is 4ms measurement time + ScheduleDelayed(5_ms); + return; + } + + case State::Collect: { + + uint8_t status = read_register(MMC5983MA_ADDR_STATUS_REG); + + if (status & MMC5983MA_STATUS_REG_MEAS_M_DONE) { + SensorData data = {}; + + if (read_register_block(&data) != PX4_OK) { + PX4_DEBUG("read failed"); + perf_count(_comms_errors); + _state = State::Measure; + _sample_index = 0; + ScheduleDelayed(100_ms); + return; + } + + // Measurement available + _measurements[_sample_index] = data; + _sample_index++; + + if (_sample_index > 1) { + publish_data(); + _sample_index = 0; + perf_count(_sample_count); + } + + _state = State::Measure; + + // Immediately schedule next measurement + ScheduleNow(); + return; + + } else { + PX4_DEBUG("not ready"); + perf_count(_comms_errors); + _collect_retries++; + _state = _collect_retries > 3 ? State::Measure : State::Collect; + ScheduleDelayed(5_ms); + return; + } + } + } // end switch/case +} + +void MMC5983MA::publish_data() +{ + uint32_t xraw_1 = (_measurements[0].xout0 << 10) | (_measurements[0].xout1 << 2) | (( + _measurements[0].xyzout2 & 0b11000000) >> 6); + uint32_t yraw_1 = (_measurements[0].yout0 << 10) | (_measurements[0].yout1 << 2) | (( + _measurements[0].xyzout2 & 0b00110000) >> 4); + uint32_t zraw_1 = (_measurements[0].zout0 << 10) | (_measurements[0].zout1 << 2) | (( + _measurements[0].xyzout2 & 0b00001100) >> 2); + + uint32_t xraw_2 = (_measurements[1].xout0 << 10) | (_measurements[1].xout1 << 2) | (( + _measurements[1].xyzout2 & 0b11000000) >> 6); + uint32_t yraw_2 = (_measurements[1].yout0 << 10) | (_measurements[1].yout1 << 2) | (( + _measurements[1].xyzout2 & 0b00110000) >> 4); + uint32_t zraw_2 = (_measurements[1].zout0 << 10) | (_measurements[1].zout1 << 2) | (( + _measurements[1].xyzout2 & 0b00001100) >> 2); + + // NOTE: Temperature conversions did not work + // float trawf = float(_measurements[0].tout + _measurements[1].tout) / 2.f; + // float temp_c = trawf * 0.8f - 75.f; + // _px4_mag.set_temperature(temp_c); + + // +/- 8 Gauss full scale range + // 18-bit mode scaling factor: 0.0625 mG/LSB + float x1 = -8.f + (float(xraw_1) * 0.0625f) / 1e3f; + float x2 = -8.f + (float(xraw_2) * 0.0625f) / 1e3f; + float y1 = -8.f + (float(yraw_1) * 0.0625f) / 1e3f; + float y2 = -8.f + (float(yraw_2) * 0.0625f) / 1e3f; + float z1 = -8.f + (float(zraw_1) * 0.0625f) / 1e3f; + float z2 = -8.f + (float(zraw_2) * 0.0625f) / 1e3f; + + // Remove the offset from the measurements (SET/RESET) + float x = (x1 - x2) / 2.f; + float y = -1.f * (y1 - y2) / 2.f; // Y axis is inverted to convert from LH to RH + float z = (z1 - z2) / 2.f; + + _px4_mag.update(hrt_absolute_time(), x, y, z); + _px4_mag.set_error_count(perf_event_count(_comms_errors)); +} + +uint8_t MMC5983MA::read_register_block(SensorData *data) +{ + uint8_t reg = MMC5983MA_ADDR_XOUT_0; + + if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) { + perf_count(_comms_errors); + + return PX4_ERROR; + } + + return PX4_OK; +} + +uint8_t MMC5983MA::read_register(uint8_t reg) +{ + uint8_t value = 0; + + if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } + + return value; +} + +void MMC5983MA::write_register(uint8_t reg, uint8_t value) +{ + if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } +} + +void MMC5983MA::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_count); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h new file mode 100644 index 000000000000..afa526dd1a60 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// MMC5983MA Registers +#define MMC5983MA_ADDR_XOUT_0 0x00 +#define MMC5983MA_ADDR_STATUS_REG 0x08 +#define MMC5983MA_ADDR_CTRL_REG0 0x09 +#define MMC5983MA_ADDR_CTRL_REG1 0x0A +#define MMC5983MA_ADDR_CTRL_REG2 0x0B +#define MMC5983MA_ADDR_PRODUCT_ID 0x2F +// MMC5983MA Definitions +#define MMC5983MA_PRODUCT_ID 0x30 +#define MMC5983MA_STATUS_REG_MEAS_M_DONE (1 << 0) +#define MMC5983MA_CTRL_REG0_TM_M (1 << 0) +#define MMC5983MA_CTRL_REG0_SET (1 << 3) +#define MMC5983MA_CTRL_REG0_RESET (1 << 4) + +#define MMC5983MA_CTRL_REG1_BW_200HZ (0b00000001) +#define MMC5983MA_CTRL_REG1_SW_RESET (1 << 7) + +extern device::Device *MMC5983MA_I2C_interface(const I2CSPIDriverConfig &config); + +class MMC5983MA : public I2CSPIDriver +{ +public: + MMC5983MA(device::Device *interface, const I2CSPIDriverConfig &config); + virtual ~MMC5983MA(); + + struct SensorData { + uint8_t xout0; + uint8_t xout1; + uint8_t yout0; + uint8_t yout1; + uint8_t zout0; + uint8_t zout1; + uint8_t xyzout2; + uint8_t tout; + }; + + enum class State { + Measure, + Collect, + }; + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + int init(); + void print_status() override; + + void RunImpl(); + +private: + void publish_data(); + + // Read data + uint8_t read_register_block(SensorData *data); + uint8_t read_register(uint8_t reg); + void write_register(uint8_t reg, uint8_t value); + + device::Device *_interface; + PX4Magnetometer _px4_mag; + State _state = State::Measure; + int _sample_index = 0; + int _collect_retries = 0; + SensorData _measurements[2]; + perf_counter_t _sample_count; + perf_counter_t _comms_errors; +}; diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp new file mode 100644 index 000000000000..294fcb27af67 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "mmc5983ma.h" +#include + +class MMC5983MA_I2C : public device::I2C +{ +public: + MMC5983MA_I2C(const I2CSPIDriverConfig &config); + virtual ~MMC5983MA_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count) override; + virtual int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); +}; + +MMC5983MA_I2C::MMC5983MA_I2C(const I2CSPIDriverConfig &config) : + I2C(config) +{ +} + +int MMC5983MA_I2C::probe() +{ + uint8_t data = 0; + + if (read(MMC5983MA_ADDR_PRODUCT_ID, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + if (data != MMC5983MA_PRODUCT_ID) { + DEVICE_DEBUG("MMC5983MA bad ID: %02x", data); + return -EIO; + } + + _retries = 1; + + return OK; +} + +int MMC5983MA_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int MMC5983MA_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +device::Device *MMC5983MA_I2C_interface(const I2CSPIDriverConfig &config) +{ + return new MMC5983MA_I2C(config); +} diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp new file mode 100644 index 000000000000..b8d6e81e3471 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "mmc5983ma.h" +#include + +I2CSPIDriverBase *MMC5983MA::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + device::Device *interface = MMC5983MA_I2C_interface(config); + + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%x)", config.bus, config.spi_devid); + return nullptr; + } + + MMC5983MA *dev = new MMC5983MA(interface, config); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +void MMC5983MA::print_usage() +{ + PRINT_MODULE_USAGE_NAME("mmc5983ma", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int mmc5983ma_main(int argc, char *argv[]) +{ + using ThisDriver = MMC5983MA; + int ch; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x30; + cli.default_i2c_frequency = 400000; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_MMC5983MA); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/magnetometer/st/Kconfig b/src/drivers/magnetometer/st/Kconfig new file mode 100644 index 000000000000..f5acb5b61698 --- /dev/null +++ b/src/drivers/magnetometer/st/Kconfig @@ -0,0 +1,3 @@ +menu "ST" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt b/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt new file mode 100644 index 000000000000..041f142ca1a3 --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2024 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_module( + MODULE drivers__magnetometer__st__iis2mdc + MAIN iis2mdc + COMPILE_FLAGS + # -DDEBUG_BUILD + SRCS + iis2mdc_i2c.cpp + iis2mdc_main.cpp + iis2mdc.cpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/st/iis2mdc/Kconfig b/src/drivers/magnetometer/st/iis2mdc/Kconfig new file mode 100644 index 000000000000..fdd140c17eb6 --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC + bool "iis2mdc" + default n + ---help--- + Enable support for iis2mdc diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp new file mode 100644 index 000000000000..23ce5ac5390b --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.cpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "iis2mdc.h" + +using namespace time_literals; + +IIS2MDC::IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _interface(interface), + _px4_mag(interface->get_device_id(), config.rotation), + _sample_count(perf_alloc(PC_COUNT, "iis2mdc_read")), + _comms_errors(perf_alloc(PC_COUNT, "iis2mdc_comms_errors")) +{} + +IIS2MDC::~IIS2MDC() +{ + perf_free(_sample_count); + perf_free(_comms_errors); + delete _interface; +} + +int IIS2MDC::init() +{ + if (hrt_absolute_time() < 20_ms) { + px4_usleep(20_ms); // ~10ms power-on time + } + + write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN); + write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC); + write_register(IIS2MDC_ADDR_CFG_REG_C, BDU); + + _px4_mag.set_scale(100.f / 65535.f); // +/- 50 Gauss, 16bit + + ScheduleDelayed(20_ms); + + return PX4_OK; +} + +void IIS2MDC::RunImpl() +{ + uint8_t status = read_register(IIS2MDC_ADDR_STATUS_REG); + + if (status & IIS2MDC_STATUS_REG_READY) { + SensorData data = {}; + + if (read_register_block(&data) == PX4_OK) { + int16_t x = int16_t((data.xout1 << 8) | data.xout0); + int16_t y = int16_t((data.yout1 << 8) | data.yout0); + int16_t z = -int16_t((data.zout1 << 8) | data.zout0); + int16_t t = int16_t((data.tout1 << 8) | data.tout0); + // 16 bits twos complement with a sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C. + _px4_mag.set_temperature(float(t) / 8.f + 25.f); + _px4_mag.update(hrt_absolute_time(), x, y, z); + _px4_mag.set_error_count(perf_event_count(_comms_errors)); + perf_count(_sample_count); + + } else { + PX4_DEBUG("read failed"); + perf_count(_comms_errors); + } + + } else { + PX4_DEBUG("not ready: %u", status); + perf_count(_comms_errors); + } + + ScheduleDelayed(10_ms); +} + +uint8_t IIS2MDC::read_register_block(SensorData *data) +{ + uint8_t reg = IIS2MDC_ADDR_OUTX_L_REG; + + if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) { + perf_count(_comms_errors); + + return PX4_ERROR; + } + + return PX4_OK; +} + +uint8_t IIS2MDC::read_register(uint8_t reg) +{ + uint8_t value = 0; + + if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } + + return value; +} + +void IIS2MDC::write_register(uint8_t reg, uint8_t value) +{ + if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } +} + +void IIS2MDC::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_count); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h new file mode 100644 index 000000000000..40da40469d69 --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// IIS2MDC Registers +#define IIS2MDC_ADDR_CFG_REG_A 0x60 +#define IIS2MDC_ADDR_CFG_REG_B 0x61 +#define IIS2MDC_ADDR_CFG_REG_C 0x62 +#define IIS2MDC_ADDR_STATUS_REG 0x67 +#define IIS2MDC_ADDR_OUTX_L_REG 0x68 +#define IIS2MDC_ADDR_WHO_AM_I 0x4F + +// IIS2MDC Definitions +#define IIS2MDC_WHO_AM_I 0b01000000 +#define IIS2MDC_STATUS_REG_READY 0b00001111 +// CFG_REG_A +#define COMP_TEMP_EN (1 << 7) +#define MD_CONTINUOUS (0 << 0) +#define ODR_100 ((1 << 3) | (1 << 2)) +// CFG_REG_B +#define OFF_CANC (1 << 1) +// CFG_REG_C +#define BDU (1 << 4) + +extern device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config); + +class IIS2MDC : public I2CSPIDriver +{ +public: + IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config); + virtual ~IIS2MDC(); + + struct SensorData { + uint8_t xout0; + uint8_t xout1; + uint8_t yout0; + uint8_t yout1; + uint8_t zout0; + uint8_t zout1; + uint8_t tout0; + uint8_t tout1; + }; + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + int init(); + void print_status() override; + + void RunImpl(); + +private: + uint8_t read_register_block(SensorData *data); + uint8_t read_register(uint8_t reg); + void write_register(uint8_t reg, uint8_t value); + + device::Device *_interface; + PX4Magnetometer _px4_mag; + perf_counter_t _sample_count; + perf_counter_t _comms_errors; +}; diff --git a/src/modules/ekf2/Utility/InnovationLpf.hpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp similarity index 57% rename from src/modules/ekf2/Utility/InnovationLpf.hpp rename to src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp index 89964b0d7161..c3cd02813c19 100644 --- a/src/modules/ekf2/Utility/InnovationLpf.hpp +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_i2c.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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 @@ -31,49 +31,67 @@ * ****************************************************************************/ -/** - * First order "alpha" IIR digital filter with input saturation - */ +#include "iis2mdc.h" +#include -#include - -class InnovationLpf final +class IIS2MDC_I2C : public device::I2C { public: - InnovationLpf() = default; - ~InnovationLpf() = default; - - void reset(float val = 0.f) { _x = val; } - - /** - * Update the filter with a new value and returns the filtered state - * The new value is constained by the limit set in setSpikeLimit - * @param val new input - * @param alpha normalized weight of the new input - * @param spike_limit the amplitude of the saturation at the input of the filter - * @return filtered output - */ - float update(float val, float alpha, float spike_limit) - { - float val_constrained = math::constrain(val, -spike_limit, spike_limit); - float beta = 1.f - alpha; - - _x = beta * _x + alpha * val_constrained; - - return _x; + IIS2MDC_I2C(const I2CSPIDriverConfig &config); + virtual ~IIS2MDC_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count) override; + virtual int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); +}; + +IIS2MDC_I2C::IIS2MDC_I2C(const I2CSPIDriverConfig &config) : + I2C(config) +{ +} + +int IIS2MDC_I2C::probe() +{ + uint8_t data = 0; + + if (read(IIS2MDC_ADDR_WHO_AM_I, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; } - /** - * Helper function to compute alpha from dt and the inverse of tau - * @param dt sampling time in seconds - * @param tau_inv inverse of the time constant of the filter - * @return alpha, the normalized weight of a new measurement - */ - static float computeAlphaFromDtAndTauInv(float dt, float tau_inv) - { - return math::constrain(dt * tau_inv, 0.f, 1.f); + if (data != IIS2MDC_WHO_AM_I) { + DEVICE_DEBUG("IIS2MDC bad ID: %02x", data); + return -EIO; } -private: - float _x{}; ///< current state of the filter -}; + _retries = 1; + + return OK; +} + +int IIS2MDC_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int IIS2MDC_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config) +{ + return new IIS2MDC_I2C(config); +} diff --git a/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp new file mode 100644 index 000000000000..09ff5eceff86 --- /dev/null +++ b/src/drivers/magnetometer/st/iis2mdc/iis2mdc_main.cpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "iis2mdc.h" +#include + +I2CSPIDriverBase *IIS2MDC::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + device::Device *interface = IIS2MDC_I2C_interface(config); + + if (interface == nullptr) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (interface->init() != OK) { + delete interface; + PX4_DEBUG("no device on bus %i (devid 0x%lx)", config.bus, config.spi_devid); + return nullptr; + } + + IIS2MDC *dev = new IIS2MDC(interface, config); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +void IIS2MDC::print_usage() +{ + PRINT_MODULE_USAGE_NAME("iis2mdc", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int iis2mdc_main(int argc, char *argv[]) +{ + using ThisDriver = IIS2MDC; + int ch; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x1E; + cli.default_i2c_frequency = 400000; + + while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optArg()); + break; + } + } + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IIS2MDC); + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + } + + if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + } + + if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index f5e5facd1e24..e99621f27bca 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-2024 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 @@ -714,7 +714,7 @@ uint8_t PAA3905::RegisterRead(uint8_t reg) // tSRW/tSRR SPI Time Between Read And Subsequent Commands const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); - if (elapsed_last_write < TIME_TSRW_TSRR_us) { + if (elapsed_last_read < TIME_TSRW_TSRR_us) { px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 05a8092a3d75..af70f9a5e32a 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2024 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 @@ -918,7 +918,7 @@ uint8_t PAW3902::RegisterRead(uint8_t reg) // tSRW/tSRR SPI Time Between Read And Subsequent Commands const hrt_abstime elapsed_last_read = hrt_elapsed_time(&_last_read_time); - if (elapsed_last_write < TIME_TSRW_TSRR_us) { + if (elapsed_last_read < TIME_TSRW_TSRR_us) { px4_udelay(TIME_TSRW_TSRR_us - elapsed_last_read); } diff --git a/src/drivers/osd/atxxxx/atxxxx.cpp b/src/drivers/osd/atxxxx/atxxxx.cpp index b504b943c79a..a5acabd3919d 100644 --- a/src/drivers/osd/atxxxx/atxxxx.cpp +++ b/src/drivers/osd/atxxxx/atxxxx.cpp @@ -241,7 +241,7 @@ OSDatxxxx::add_battery_info(uint8_t pos_x, uint8_t pos_y) int ret = PX4_OK; // TODO: show battery symbol based on battery fill level - snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_filtered_v); + snprintf(buf, sizeof(buf), "%c%5.2f", OSD_SYMBOL_BATT_3, (double)_battery_voltage_v); buf[sizeof(buf) - 1] = '\0'; for (int i = 0; buf[i] != '\0'; i++) { @@ -330,7 +330,7 @@ OSDatxxxx::update_topics() _battery_sub.copy(&battery); if (battery.connected) { - _battery_voltage_filtered_v = battery.voltage_filtered_v; + _battery_voltage_v = battery.voltage_v; _battery_discharge_mah = battery.discharged_mah; _battery_valid = true; diff --git a/src/drivers/osd/atxxxx/atxxxx.h b/src/drivers/osd/atxxxx/atxxxx.h index e821e65deb32..10685eec9b98 100644 --- a/src/drivers/osd/atxxxx/atxxxx.h +++ b/src/drivers/osd/atxxxx/atxxxx.h @@ -111,7 +111,7 @@ class OSDatxxxx : public device::SPI, public ModuleParams, public I2CSPIDriver, public ModuleParams, public px4::Sched char _device[64] {}; PerformanceData _performance_data{}; }; - diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp index 97c89bea611d..baa54bcd7038 100644 --- a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp @@ -87,14 +87,6 @@ void PowerMonitorSelectorAuterion::Run() int ret_val = ina226_probe(i); if (ret_val == PX4_OK) { - - float current_shunt_value = 0.0f; - param_get(param_find("INA226_SHUNT"), ¤t_shunt_value); - - if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) { - param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value)); - } - char bus_number[4] = {0}; itoa(_sensors[i].bus_number, bus_number, 10); const char *start_argv[] { diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index a55af5fae278..c63b4195d29a 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -30,4 +30,3 @@ actuator_output: 200: PWM 200 Hz 400: PWM 400 Hz reboot_required: true - diff --git a/src/drivers/px4io/Kconfig b/src/drivers/px4io/Kconfig index dd9b402249fa..052f088d701f 100644 --- a/src/drivers/px4io/Kconfig +++ b/src/drivers/px4io/Kconfig @@ -1,5 +1,6 @@ menuconfig DRIVERS_PX4IO bool "px4io" default n + depends on PLATFORM_NUTTX ---help--- Enable support for px4io diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bd6b5d1f3192..52ad8325144e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -335,8 +335,7 @@ class PX4IO : public cdev::CDev, public ModuleBase, public OutputModuleIn (ParamInt) _param_rc_rssi_pwm_chan, (ParamInt) _param_rc_rssi_pwm_max, (ParamInt) _param_rc_rssi_pwm_min, - (ParamInt) _param_sens_en_themal, - (ParamInt) _param_sys_hitl + (ParamInt) _param_sens_en_themal ) }; @@ -364,6 +363,13 @@ PX4IO::~PX4IO() bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { + for (size_t i = 0; i < num_outputs; i++) { + if (!_mixing_output.isFunctionSet(i)) { + // do not run any signal on disabled channels + outputs[i] = 0; + } + } + if (!_test_fmu_fail) { /* output to the servos */ io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs); @@ -467,9 +473,7 @@ int PX4IO::init() } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ - if (_param_sys_hitl.get() <= 0) { - _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); - } + _mixing_output.setMaxTopicUpdateRate(MIN_TOPIC_UPDATE_INTERVAL); _px4io_status_pub.advertise(); @@ -518,9 +522,7 @@ void PX4IO::Run() perf_count(_interval_perf); /* if we have new control data from the ORB, handle it */ - if (_param_sys_hitl.get() <= 0) { - _mixing_output.update(); - } + _mixing_output.update(); if (hrt_elapsed_time(&_poll_last) >= 20_ms) { /* run at 50 */ @@ -533,13 +535,11 @@ void PX4IO::Run() io_publish_raw_rc(); } - if (_param_sys_hitl.get() <= 0) { - /* check updates on uORB topics and handle it */ - if (_t_actuator_armed.updated()) { - io_set_arming_state(); + /* check updates on uORB topics and handle it */ + if (_t_actuator_armed.updated()) { + io_set_arming_state(); - // TODO: throttle - } + // TODO: throttle } if (!_mixing_output.armed().armed) { @@ -807,14 +807,6 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } - // XXX this is for future support in the commander - // but can be removed if unneeded - // if (armed.termination_failsafe) { - // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } else { - // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; - // } - if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; diff --git a/src/drivers/rc/Kconfig b/src/drivers/rc/Kconfig index f4b771e2ba19..7a53a650f432 100644 --- a/src/drivers/rc/Kconfig +++ b/src/drivers/rc/Kconfig @@ -3,6 +3,9 @@ menu "RC" bool "Common RC" default n select DRIVERS_RC_CRSF_RC + select DRIVERS_RC_DSM_RC + select DRIVERS_RC_GHST_RC + select DRIVERS_RC_SBUS_RC ---help--- Enable default set of magnetometer drivers rsource "*/Kconfig" diff --git a/src/drivers/rc/crsf_rc/CMakeLists.txt b/src/drivers/rc/crsf_rc/CMakeLists.txt index ceb75310799a..a3ebb1a1ae87 100644 --- a/src/drivers/rc/crsf_rc/CMakeLists.txt +++ b/src/drivers/rc/crsf_rc/CMakeLists.txt @@ -46,6 +46,4 @@ px4_add_module( MODULE_CONFIG module.yaml - DEPENDS - rc ) diff --git a/src/drivers/rc/crsf_rc/CrsfParser.cpp b/src/drivers/rc/crsf_rc/CrsfParser.cpp index aa499db9f562..a81fe930f517 100644 --- a/src/drivers/rc/crsf_rc/CrsfParser.cpp +++ b/src/drivers/rc/crsf_rc/CrsfParser.cpp @@ -161,6 +161,8 @@ static float MapF(const float x, const float in_min, const float in_max, const f return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } +#define CONSTRAIN_CHAN(x) ConstrainF(x, CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX) + static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPacket_t *const new_packet) { uint32_t raw_channels[CRSF_CHANNEL_COUNT]; @@ -169,25 +171,24 @@ static bool ProcessChannelData(const uint8_t *data, const uint32_t size, CrsfPac new_packet->message_type = CRSF_MESSAGE_TYPE_RC_CHANNELS; // Decode channel data - raw_channels[0] = (data[0] | data[1] << 8) & 0x07FF; - raw_channels[1] = (data[1] >> 3 | data[2] << 5) & 0x07FF; - raw_channels[2] = (data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF; - raw_channels[3] = (data[4] >> 1 | data[5] << 7) & 0x07FF; - raw_channels[4] = (data[5] >> 4 | data[6] << 4) & 0x07FF; - raw_channels[5] = (data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF; - raw_channels[6] = (data[8] >> 2 | data[9] << 6) & 0x07FF; - raw_channels[7] = (data[9] >> 5 | data[10] << 3) & 0x07FF; - raw_channels[8] = (data[11] | data[12] << 8) & 0x07FF; - raw_channels[9] = (data[12] >> 3 | data[13] << 5) & 0x07FF; - raw_channels[10] = (data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF; - raw_channels[11] = (data[15] >> 1 | data[16] << 7) & 0x07FF; - raw_channels[12] = (data[16] >> 4 | data[17] << 4) & 0x07FF; - raw_channels[13] = (data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF; - raw_channels[14] = (data[19] >> 2 | data[20] << 6) & 0x07FF; - raw_channels[15] = (data[20] >> 5 | data[21] << 3) & 0x07FF; + raw_channels[0] = CONSTRAIN_CHAN((data[0] | data[1] << 8) & 0x07FF); + raw_channels[1] = CONSTRAIN_CHAN((data[1] >> 3 | data[2] << 5) & 0x07FF); + raw_channels[2] = CONSTRAIN_CHAN((data[2] >> 6 | data[3] << 2 | data[4] << 10) & 0x07FF); + raw_channels[3] = CONSTRAIN_CHAN((data[4] >> 1 | data[5] << 7) & 0x07FF); + raw_channels[4] = CONSTRAIN_CHAN((data[5] >> 4 | data[6] << 4) & 0x07FF); + raw_channels[5] = CONSTRAIN_CHAN((data[6] >> 7 | data[7] << 1 | data[8] << 9) & 0x07FF); + raw_channels[6] = CONSTRAIN_CHAN((data[8] >> 2 | data[9] << 6) & 0x07FF); + raw_channels[7] = CONSTRAIN_CHAN((data[9] >> 5 | data[10] << 3) & 0x07FF); + raw_channels[8] = CONSTRAIN_CHAN((data[11] | data[12] << 8) & 0x07FF); + raw_channels[9] = CONSTRAIN_CHAN((data[12] >> 3 | data[13] << 5) & 0x07FF); + raw_channels[10] = CONSTRAIN_CHAN((data[13] >> 6 | data[14] << 2 | data[15] << 10) & 0x07FF); + raw_channels[11] = CONSTRAIN_CHAN((data[15] >> 1 | data[16] << 7) & 0x07FF); + raw_channels[12] = CONSTRAIN_CHAN((data[16] >> 4 | data[17] << 4) & 0x07FF); + raw_channels[13] = CONSTRAIN_CHAN((data[17] >> 7 | data[18] << 1 | data[19] << 9) & 0x07FF); + raw_channels[14] = CONSTRAIN_CHAN((data[19] >> 2 | data[20] << 6) & 0x07FF); + raw_channels[15] = CONSTRAIN_CHAN((data[20] >> 5 | data[21] << 3) & 0x07FF); for (i = 0; i < CRSF_CHANNEL_COUNT; i++) { - raw_channels[i] = ConstrainF(raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX); new_packet->channel_data.channels[i] = MapF((float)raw_channels[i], CRSF_CHANNEL_VALUE_MIN, CRSF_CHANNEL_VALUE_MAX, 1000.0f, 2000.0f); } diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index dc30620e9831..3ea6c0ac273d 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -35,8 +35,6 @@ #include "CrsfParser.hpp" #include "Crc8.hpp" -#include -#include #include #include @@ -118,76 +116,74 @@ void CrsfRc::Run() { if (should_exit()) { ScheduleClear(); - ::close(_rc_fd); - _rc_fd = -1; + + if (_uart) { + (void) _uart->close(); + delete _uart; + _uart = nullptr; + } + exit_and_cleanup(); return; } - if (_rc_fd < 0) { - _rc_fd = ::open(_device, O_RDWR | O_NONBLOCK); + if (_uart == nullptr) { + // Create the UART port instance + _uart = new Serial(_device); - if (_rc_fd >= 0) { - struct termios t; - - tcgetattr(_rc_fd, &t); - cfsetspeed(&t, CRSF_BAUDRATE); - t.c_cflag &= ~(CSTOPB | PARENB | CRTSCTS); - t.c_lflag &= ~(ECHO | ECHONL | ICANON | IEXTEN | ISIG); - t.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON); - t.c_oflag = 0; - tcsetattr(_rc_fd, TCSANOW, &t); + if (_uart == nullptr) { + PX4_ERR("Error creating serial device %s", _device); + px4_sleep(1); + return; + } + } - if (board_rc_swap_rxtx(_device)) { -#if defined(TIOCSSWAP) - ioctl(_rc_fd, TIOCSSWAP, SER_SWAP_ENABLED); -#endif // TIOCSSWAP - } + if (! _uart->isOpen()) { + // Configure the desired baudrate if one was specified by the user. + // Otherwise the default baudrate will be used. + if (! _uart->setBaudrate(CRSF_BAUDRATE)) { + PX4_ERR("Error setting baudrate to %u on %s", CRSF_BAUDRATE, _device); + px4_sleep(1); + return; + } - if (board_rc_singlewire(_device)) { - _is_singlewire = true; -#if defined(TIOCSSINGLEWIRE) - ioctl(_rc_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); -#endif // TIOCSSINGLEWIRE - } + // Open the UART. If this is successful then the UART is ready to use. + if (! _uart->open()) { + PX4_ERR("Error opening serial device %s", _device); + px4_sleep(1); + return; + } - PX4_INFO("Crsf serial opened sucessfully"); + if (board_rc_swap_rxtx(_device)) { + _uart->setSwapRxTxMode(); + } - if (_is_singlewire) { - PX4_INFO("Crsf serial is single wire. Telemetry disabled"); - } + if (board_rc_singlewire(_device)) { + _is_singlewire = true; + _uart->setSingleWireMode(); + } - tcflush(_rc_fd, TCIOFLUSH); + PX4_INFO("Crsf serial opened sucessfully"); - Crc8Init(0xd5); + if (_is_singlewire) { + PX4_INFO("Crsf serial is single wire. Telemetry disabled"); } + _uart->flush(); + + Crc8Init(0xd5); + _input_rc.rssi_dbm = NAN; _input_rc.link_quality = -1; CrsfParser_Init(); - - - } - - // poll with 100mS timeout - pollfd fds[1]; - fds[0].fd = _rc_fd; - fds[0].events = POLLIN; - int ret = ::poll(fds, 1, 100); - - if (ret < 0) { - PX4_ERR("poll error"); - // try again with delay - ScheduleDelayed(100_ms); - return; } const hrt_abstime time_now_us = hrt_absolute_time(); perf_count_interval(_cycle_interval_perf, time_now_us); // Read all available data from the serial RC input UART - int new_bytes = ::read(_rc_fd, &_rcs_buf[0], RC_MAX_BUFFER_SIZE); + int new_bytes = _uart->readAtLeast(&_rcs_buf[0], RC_MAX_BUFFER_SIZE, 1, 100); if (new_bytes > 0) { _bytes_rx += new_bytes; @@ -228,8 +224,8 @@ void CrsfRc::Run() battery_status_s battery_status; if (_battery_status_sub.update(&battery_status)) { - uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t voltage = battery_status.voltage_v * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; this->SendTelemetryBattery(voltage, current, fuel, remaining); @@ -245,7 +241,7 @@ void CrsfRc::Run() int32_t longitude = static_cast(round(sensor_gps.longitude_deg * 1e7)); uint16_t groundspeed = sensor_gps.vel_d_m_s / 3.6f * 10.f; uint16_t gps_heading = math::degrees(sensor_gps.cog_rad) * 100.f; - uint16_t altitude = static_cast(sensor_gps.altitude_msl_m * 1e3) + 1000; + uint16_t altitude = static_cast(sensor_gps.altitude_msl_m) + 1000; uint8_t num_satellites = sensor_gps.satellites_used; this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites); } @@ -433,7 +429,8 @@ bool CrsfRc::SendTelemetryBattery(const uint16_t voltage, const uint16_t current write_uint24_t(buf, offset, fuel); write_uint8_t(buf, offset, remaining); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); + } bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, const uint16_t groundspeed, @@ -449,7 +446,7 @@ bool CrsfRc::SendTelemetryGps(const int32_t latitude, const int32_t longitude, c write_uint16_t(buf, offset, altitude); write_uint8_t(buf, offset, num_satellites); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, const int16_t yaw) @@ -461,7 +458,7 @@ bool CrsfRc::SendTelemetryAttitude(const int16_t pitch, const int16_t roll, cons write_uint16_t(buf, offset, roll); write_uint16_t(buf, offset, yaw); WriteFrameCrc(buf, offset, sizeof(buf)); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) @@ -480,7 +477,7 @@ bool CrsfRc::SendTelemetryFlightMode(const char *flight_mode) offset += length; buf[offset - 1] = 0; // ensure null-terminated string WriteFrameCrc(buf, offset, length + 4); - return write(_rc_fd, buf, offset) == offset; + return _uart->write((void *) buf, (size_t) offset); } int CrsfRc::print_status() diff --git a/src/drivers/rc/crsf_rc/CrsfRc.hpp b/src/drivers/rc/crsf_rc/CrsfRc.hpp index 6603e01ea3af..c3b0a4ec54d0 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.hpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.hpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,8 @@ #include #include +using namespace device; + class CrsfRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: @@ -87,7 +90,8 @@ class CrsfRc : public ModuleBase, public ModuleParams, public px4::Sched bool SendTelemetryFlightMode(const char *flight_mode); - int _rc_fd{-1}; + Serial *_uart = nullptr; ///< UART interface to RC + char _device[20] {}; ///< device / serial port path bool _is_singlewire{false}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt b/src/drivers/rc/dsm_rc/CMakeLists.txt similarity index 82% rename from src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt rename to src/drivers/rc/dsm_rc/CMakeLists.txt index c556db2b8f3f..addfa9afb59a 100644 --- a/src/modules/differential_drive/DifferentialDriveKinematics/CMakeLists.txt +++ b/src/drivers/rc/dsm_rc/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2024 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 @@ -30,11 +30,15 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - -px4_add_library(DifferentialDriveKinematics - DifferentialDriveKinematics.cpp -) - -target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -px4_add_functional_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics) +px4_add_module( + MODULE drivers__rc__dsm_rc + MAIN dsm_rc + COMPILE_FLAGS + SRCS + DsmRc.cpp + DsmRc.hpp + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/src/drivers/rc/dsm_rc/DsmRc.cpp b/src/drivers/rc/dsm_rc/DsmRc.cpp new file mode 100644 index 000000000000..f35bc4487ff0 --- /dev/null +++ b/src/drivers/rc/dsm_rc/DsmRc.cpp @@ -0,0 +1,417 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 "DsmRc.hpp" + +#include + +#include + +using namespace time_literals; + +DsmRc::DsmRc(const char *device) : + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), + _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +DsmRc::~DsmRc() +{ +#if defined(SPEKTRUM_POWER_PASSIVE) + // Disable power controls for Spektrum receiver + SPEKTRUM_POWER_PASSIVE(); +#endif // SPEKTRUM_POWER_PASSIVE + + perf_free(_cycle_perf); + perf_free(_publish_interval_perf); +} + +int DsmRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + bool silent = false; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + silent = false; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + DsmRc *instance = new DsmRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleOnInterval(_current_update_interval); + + return PX4_OK; + + } else if (silent) { + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); + + } else { + PX4_ERR("valid device required"); + } + } + + return PX4_ERROR; +} + +void DsmRc::Run() +{ + if (should_exit()) { + + close(_rcs_fd); + + dsm_deinit(); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + } + } + + const hrt_abstime cycle_timestamp = hrt_absolute_time(); + + + /* vehicle command */ + vehicle_command_s vcmd; + + if (_vehicle_cmd_sub.update(&vcmd)) { + // Check for a pairing command + if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) { + + uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; +#if defined(SPEKTRUM_POWER) + + if (!_rc_scan_locked && !_armed) { + if ((int)vcmd.param1 == 0) { + // DSM binding command + int dsm_bind_mode = (int)vcmd.param2; + + int dsm_bind_pulses = 0; + + if (dsm_bind_mode == 0) { + dsm_bind_pulses = DSM2_BIND_PULSES; + + } else if (dsm_bind_mode == 1) { + dsm_bind_pulses = DSMX_BIND_PULSES; + + } else { + dsm_bind_pulses = DSMX8_BIND_PULSES; + } + + bind_spektrum(dsm_bind_pulses); + + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } + + } else { + cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + +#endif // SPEKTRUM_POWER + + // publish acknowledgement + vehicle_command_ack_s command_ack{}; + command_ack.command = vcmd.command; + command_ack.result = cmd_ret; + command_ack.target_system = vcmd.source_system; + command_ack.target_component = vcmd.source_component; + command_ack.timestamp = hrt_absolute_time(); + uORB::Publication vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + vehicle_command_ack_pub.publish(command_ack); + } + } + + bool rc_updated = false; + + constexpr hrt_abstime rc_scan_max = 3_s; + + // read all available data from the serial RC input UART + uint8_t rcs_buf[DSM_BUFFER_SIZE] {}; + int newBytes = ::read(_rcs_fd, &rcs_buf[0], DSM_BUFFER_SIZE); + + if (newBytes > 0) { + _bytes_rx += newBytes; + } + + const bool rc_scan_locked = _rc_scan_locked; + + if (_rc_scan_begin == 0) { + _rc_scan_begin = cycle_timestamp; + + // Configure serial port + if (_rcs_fd < 0) { + _rcs_fd = open(_device, O_RDWR | O_NONBLOCK); + } + + dsm_config(_rcs_fd); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + + } else if (_rc_scan_locked + || cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t raw_rc_count = 0; + + int8_t dsm_rssi = 0; + bool dsm_11_bit = false; + unsigned frame_drops = 0; + + if (dsm_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, &dsm_11_bit, &frame_drops, + &dsm_rssi, input_rc_s::RC_INPUT_MAX_CHANNELS) + ) { + // we have a new DSM frame. Publish it. + input_rc_s input_rc{}; + input_rc.timestamp_last_signal = cycle_timestamp; + input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); + input_rc.rssi = dsm_rssi; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values[i]; + + if (raw_rc_values[i] != UINT16_MAX) { + valid_chans++; + } + } + + input_rc.channel_count = valid_chans; + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + input_rc.rc_lost = (valid_chans == 0); + input_rc.rc_lost_frame_count = frame_drops; + + input_rc.link_quality = -1; + input_rc.rssi_dbm = NAN; + + input_rc.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(input_rc); + perf_count(_publish_interval_perf); + + _timestamp_last_signal = cycle_timestamp; + rc_updated = true; + + if (valid_chans > 0) { + _rc_scan_locked = true; + } + } + } + + } else { + _rc_scan_begin = 0; + _rc_scan_locked = false; + + close(_rcs_fd); + _rcs_fd = -1; + } + + if (!rc_updated && !_armed && (hrt_elapsed_time(&_timestamp_last_signal) > 1_s)) { + _rc_scan_locked = false; + } + + if (!rc_scan_locked && _rc_scan_locked) { + PX4_INFO("RC input locked"); + } + + perf_end(_cycle_perf); +} + +#if defined(SPEKTRUM_POWER) +bool DsmRc::bind_spektrum(int arg) const +{ + int ret = PX4_ERROR; + + /* specify 11ms DSMX. RX will automatically fall back to 22ms or DSM2 if necessary */ + + /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ + PX4_INFO("DSM_BIND_START: DSM%s RX", (arg == 0) ? "2" : ((arg == 1) ? "-X" : "-X8")); + + if (arg == DSM2_BIND_PULSES || + arg == DSMX_BIND_PULSES || + arg == DSMX8_BIND_PULSES) { + + dsm_bind(DSM_CMD_BIND_POWER_DOWN, 0); + + dsm_bind(DSM_CMD_BIND_SET_RX_OUT, 0); + usleep(500000); + + dsm_bind(DSM_CMD_BIND_POWER_UP, 0); + usleep(72000); + + irqstate_t flags = px4_enter_critical_section(); + dsm_bind(DSM_CMD_BIND_SEND_PULSES, arg); + px4_leave_critical_section(flags); + + usleep(50000); + + dsm_bind(DSM_CMD_BIND_REINIT_UART, 0); + + ret = OK; + + } else { + PX4_ERR("DSM bind failed"); + ret = -EINVAL; + } + + return (ret == PX4_OK); +} +#endif /* SPEKTRUM_POWER */ + +int DsmRc::custom_command(int argc, char *argv[]) +{ +#if defined(SPEKTRUM_POWER) + const char *verb = argv[0]; + + if (!strcmp(verb, "bind")) { + uORB::Publication vehicle_command_pub{ORB_ID(vehicle_command)}; + vehicle_command_s vcmd{}; + vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR; + vcmd.timestamp = hrt_absolute_time(); + vehicle_command_pub.publish(vcmd); + return 0; + } + +#endif // SPEKTRUM_POWER + + if (!is_running()) { + int ret = DsmRc::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int DsmRc::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal"); + + perf_print_counter(_cycle_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int DsmRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does Spektrum DSM RC input parsing. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("dsm_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + +#if defined(SPEKTRUM_POWER) + PRINT_MODULE_USAGE_COMMAND_DESCR("bind", "Send a DSM bind command (module must be running)"); +#endif /* SPEKTRUM_POWER */ + + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int dsm_rc_main(int argc, char *argv[]) +{ + return DsmRc::main(argc, argv); +} diff --git a/src/drivers/rc/dsm_rc/DsmRc.hpp b/src/drivers/rc/dsm_rc/DsmRc.hpp new file mode 100644 index 000000000000..5ce935ec5de1 --- /dev/null +++ b/src/drivers/rc/dsm_rc/DsmRc.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class DsmRc : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + + DsmRc(const char *device); + virtual ~DsmRc(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + void Run() override; + +#if defined(SPEKTRUM_POWER) + bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const; +#endif // SPEKTRUM_POWER + + hrt_abstime _rc_scan_begin{0}; + hrt_abstime _timestamp_last_signal{0}; + + bool _rc_scan_locked{false}; + + static constexpr unsigned _current_update_interval{4000}; // 250 Hz + + uORB::Subscription _vehicle_cmd_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + bool _armed{false}; + + int _rcs_fd{-1}; + char _device[20] {}; ///< device / serial port path + + perf_counter_t _cycle_perf; + perf_counter_t _publish_interval_perf; + uint32_t _bytes_rx{0}; +}; diff --git a/src/drivers/rc/dsm_rc/Kconfig b/src/drivers/rc/dsm_rc/Kconfig new file mode 100644 index 000000000000..19095081606c --- /dev/null +++ b/src/drivers/rc/dsm_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_DSM_RC + bool "dsm_rc" + default n + ---help--- + Enable support for dsm_rc diff --git a/src/drivers/rc/dsm_rc/module.yaml b/src/drivers/rc/dsm_rc/module.yaml new file mode 100644 index 000000000000..12f36bc77a8e --- /dev/null +++ b/src/drivers/rc/dsm_rc/module.yaml @@ -0,0 +1,10 @@ +module_name: DSM RC Input Driver +serial_config: + - command: "dsm_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_DSM_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + DSM RC (Spektrum) driver. diff --git a/src/drivers/rc/ghst_rc/CMakeLists.txt b/src/drivers/rc/ghst_rc/CMakeLists.txt new file mode 100644 index 000000000000..2a0342584fd6 --- /dev/null +++ b/src/drivers/rc/ghst_rc/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015-2024 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_module( + MODULE drivers__rc__ghst_rc + MAIN ghst_rc + COMPILE_FLAGS + SRCS + GhstRc.cpp + GhstRc.hpp + ghst_telemetry.cpp + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/src/drivers/rc/ghst_rc/GhstRc.cpp b/src/drivers/rc/ghst_rc/GhstRc.cpp new file mode 100644 index 000000000000..1960c84ce6d4 --- /dev/null +++ b/src/drivers/rc/ghst_rc/GhstRc.cpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 "GhstRc.hpp" + +#include + +GhstRc::GhstRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), + _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +GhstRc::~GhstRc() +{ + delete _ghst_telemetry; + + perf_free(_cycle_perf); + perf_free(_publish_interval_perf); +} + +int GhstRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + bool silent = false; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + silent = false; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + GhstRc *instance = new GhstRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleOnInterval(_current_update_interval); + + return PX4_OK; + + } else if (silent) { + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); + + } else { + PX4_ERR("valid device required"); + } + } + + return PX4_ERROR; +} + +void GhstRc::Run() +{ + if (should_exit()) { + + close(_rcs_fd); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + const hrt_abstime cycle_timestamp = hrt_absolute_time(); + + bool rc_updated = false; + + constexpr hrt_abstime rc_scan_max = 3_s; + + // read all available data from the serial RC input UART + static constexpr size_t RC_MAX_BUFFER_SIZE{64}; + uint8_t rcs_buf[RC_MAX_BUFFER_SIZE] {}; + int newBytes = ::read(_rcs_fd, &rcs_buf[0], RC_MAX_BUFFER_SIZE); + + if (newBytes > 0) { + _bytes_rx += newBytes; + } + + const bool rc_scan_locked = _rc_scan_locked; + + if (_rc_scan_begin == 0) { + _rc_scan_begin = cycle_timestamp; + + // Configure serial port + if (_rcs_fd < 0) { + _rcs_fd = open(_device, O_RDWR | O_NONBLOCK); + } + + ghst_config(_rcs_fd); + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + + } else if (_rc_scan_locked + || cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t raw_rc_count = 0; + int8_t ghst_rssi = -1; + + if (ghst_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &ghst_rssi, + &raw_rc_count, input_rc_s::RC_INPUT_MAX_CHANNELS) + ) { + // we have a new GHST frame. Publish it. + input_rc_s input_rc{}; + input_rc.timestamp_last_signal = cycle_timestamp; + input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); + input_rc.rssi = ghst_rssi; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values[i]; + + if (raw_rc_values[i] != UINT16_MAX) { + valid_chans++; + } + } + + input_rc.channel_count = valid_chans; + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + input_rc.rc_lost = (valid_chans == 0); + + input_rc.link_quality = -1; + input_rc.rssi_dbm = NAN; + + input_rc.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(input_rc); + perf_count(_publish_interval_perf); + + _timestamp_last_signal = cycle_timestamp; + rc_updated = true; + + if (valid_chans > 0) { + _rc_scan_locked = true; + } + + if (!_rc_scan_locked && !_ghst_telemetry && _param_rc_ghst_tel_en.get()) { + _ghst_telemetry = new GHSTTelemetry(_rcs_fd); + } + + if (_ghst_telemetry) { + _ghst_telemetry->update(cycle_timestamp); + } + } + } + + } else { + _rc_scan_begin = 0; + _rc_scan_locked = false; + + close(_rcs_fd); + _rcs_fd = -1; + } + + if (!rc_updated && (hrt_elapsed_time(&_timestamp_last_signal) > 1_s)) { + _rc_scan_locked = false; + } + + if (!rc_scan_locked && _rc_scan_locked) { + PX4_INFO("RC input locked"); + } + + perf_end(_cycle_perf); +} + +int GhstRc::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + int ret = GhstRc::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int GhstRc::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal"); + + perf_print_counter(_cycle_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int GhstRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does Ghost (GHST) RC input parsing. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ghst_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int ghst_rc_main(int argc, char *argv[]) +{ + return GhstRc::main(argc, argv); +} diff --git a/src/drivers/rc/ghst_rc/GhstRc.hpp b/src/drivers/rc/ghst_rc/GhstRc.hpp new file mode 100644 index 000000000000..0090b216dc21 --- /dev/null +++ b/src/drivers/rc/ghst_rc/GhstRc.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ghst_telemetry.hpp" + +using namespace time_literals; + +class GhstRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + GhstRc(const char *device); + virtual ~GhstRc(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + void Run() override; + + hrt_abstime _rc_scan_begin{0}; + hrt_abstime _timestamp_last_signal{0}; + + bool _rc_scan_locked{false}; + + static constexpr unsigned _current_update_interval{4000}; // 250 Hz + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + int _rcs_fd{-1}; + char _device[20] {}; ///< device / serial port path + + GHSTTelemetry *_ghst_telemetry{nullptr}; + + perf_counter_t _cycle_perf; + perf_counter_t _publish_interval_perf; + uint32_t _bytes_rx{0}; + + DEFINE_PARAMETERS( + (ParamBool) _param_rc_ghst_tel_en + ) +}; diff --git a/src/drivers/rc/ghst_rc/Kconfig b/src/drivers/rc/ghst_rc/Kconfig new file mode 100644 index 000000000000..fbff9b26d4d3 --- /dev/null +++ b/src/drivers/rc/ghst_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_GHST_RC + bool "ghst_rc" + default n + ---help--- + Enable support for ghst_rc diff --git a/src/drivers/rc/ghst_rc/ghst_telemetry.cpp b/src/drivers/rc/ghst_rc/ghst_telemetry.cpp new file mode 100644 index 000000000000..9a910063931a --- /dev/null +++ b/src/drivers/rc/ghst_rc/ghst_telemetry.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * 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 ghst_telemetry.cpp + * + * IRC Ghost (Immersion RC Ghost) telemetry. + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#include "ghst_telemetry.hpp" +#include + +using time_literals::operator ""_s; + +GHSTTelemetry::GHSTTelemetry(int uart_fd) : + _uart_fd(uart_fd) +{ +} + +bool GHSTTelemetry::update(const hrt_abstime &now) +{ + bool success = false; + + if ((now - _last_update) > (1_s / (UPDATE_RATE_HZ * NUM_DATA_TYPES))) { + + switch (_next_type) { + case 0U: + success = send_battery_status(); + break; + + case 1U: + success = send_gps1_status(); + break; + + case 2U: + success = send_gps2_status(); + break; + + default: + success = false; + break; + } + + _last_update = now; + _next_type = (_next_type + 1U) % NUM_DATA_TYPES; + } + + return success; +} + +bool GHSTTelemetry::send_battery_status() +{ + bool success = false; + float voltage_in_10mV; + float current_in_10mA; + float fuel_in_10mAh; + battery_status_s battery_status; + + if (_battery_status_sub.update(&battery_status)) { + voltage_in_10mV = battery_status.voltage_v * FACTOR_VOLTS_TO_10MV; + current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA; + fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH; + success = ghst_send_telemetry_battery_status(_uart_fd, + static_cast(voltage_in_10mV), + static_cast(current_in_10mA), + static_cast(fuel_in_10mAh)); + } + + return success; +} + +bool GHSTTelemetry::send_gps1_status() +{ + sensor_gps_s vehicle_gps_position; + + if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) { + return false; + } + + int32_t latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); // 1e-7 degrees + int32_t longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); // 1e-7 degrees + uint16_t altitude = static_cast(round(vehicle_gps_position.altitude_msl_m)); // meters + + return ghst_send_telemetry_gps1_status(_uart_fd, latitude, longitude, altitude); +} + +bool GHSTTelemetry::send_gps2_status() +{ + sensor_gps_s vehicle_gps_position; + + if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) { + return false; + } + + uint16_t ground_speed = (uint16_t)(vehicle_gps_position.vel_d_m_s / 3.6f * 10.f); + uint16_t ground_course = (uint16_t)(math::degrees(vehicle_gps_position.cog_rad) * 100.f); + uint8_t num_sats = vehicle_gps_position.satellites_used; + + // TBD: Can these be computed in a RC telemetry driver? + uint16_t home_dist = 0; + uint16_t home_dir = 0; + uint8_t flags = 0; + + return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags); +} diff --git a/src/drivers/rc/ghst_rc/ghst_telemetry.hpp b/src/drivers/rc/ghst_rc/ghst_telemetry.hpp new file mode 100644 index 000000000000..faa61f69b28e --- /dev/null +++ b/src/drivers/rc/ghst_rc/ghst_telemetry.hpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 ghst_telemetry.cpp + * + * IRC Ghost (Immersion RC Ghost) telemetry. + * + * @author Igor Misic + * @author Juraj Ciberlin + */ + +#pragma once + +#include +#include +#include +#include + +/** + * High-level class that handles sending of GHST telemetry data + */ +class GHSTTelemetry +{ +public: + /** + * @param uart_fd file descriptor for the UART to use. It is expected to be configured + * already. + */ + explicit GHSTTelemetry(int uart_fd); + + ~GHSTTelemetry() = default; + + /** + * Send telemetry data. Call this regularly (i.e. at 100Hz), it will automatically + * limit the sending rate. + * @return true if new data sent + */ + bool update(const hrt_abstime &now); + +private: + bool send_battery_status(); + bool send_gps1_status(); + bool send_gps2_status(); + + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + + int _uart_fd; + hrt_abstime _last_update {0U}; + uint32_t _next_type {0U}; + + static constexpr uint32_t NUM_DATA_TYPES {3U}; // number of different telemetry data types + static constexpr uint32_t UPDATE_RATE_HZ {10U}; // update rate [Hz] + + // Factors that should be applied to get correct values + static constexpr float FACTOR_VOLTS_TO_10MV {100.0F}; + static constexpr float FACTOR_AMPS_TO_10MA {100.0F}; + static constexpr float FACTOR_MAH_TO_10MAH {0.1F}; + +}; diff --git a/src/drivers/rc/ghst_rc/module.yaml b/src/drivers/rc/ghst_rc/module.yaml new file mode 100644 index 000000000000..2e15a42d97a3 --- /dev/null +++ b/src/drivers/rc/ghst_rc/module.yaml @@ -0,0 +1,22 @@ +module_name: GHST RC Input Driver +serial_config: + - command: "ghst_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_GHST_PRT_CFG + group: Serial + #default: RC + #depends_on_port: RC + description_extended: | + Ghost (GHST) RC driver. + +parameters: + - group: RC + definitions: + RC_GHST_TEL_EN: + description: + short: Ghost RC telemetry enable + long: | + Ghost telemetry enable + + type: boolean + default: [0] diff --git a/src/drivers/rc/sbus_rc/CMakeLists.txt b/src/drivers/rc/sbus_rc/CMakeLists.txt new file mode 100644 index 000000000000..fd5e56263330 --- /dev/null +++ b/src/drivers/rc/sbus_rc/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2015-2024 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_module( + MODULE drivers__rc__sbus_rc + MAIN sbus_rc + COMPILE_FLAGS + SRCS + SbusRc.cpp + SbusRc.hpp + MODULE_CONFIG + module.yaml + DEPENDS + rc + ) diff --git a/src/drivers/rc/sbus_rc/Kconfig b/src/drivers/rc/sbus_rc/Kconfig new file mode 100644 index 000000000000..d79e5745b59c --- /dev/null +++ b/src/drivers/rc/sbus_rc/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_RC_SBUS_RC + bool "sbus_rc" + default n + ---help--- + Enable support for sbus_rc diff --git a/src/drivers/rc/sbus_rc/SbusRc.cpp b/src/drivers/rc/sbus_rc/SbusRc.cpp new file mode 100644 index 000000000000..12d57da141e1 --- /dev/null +++ b/src/drivers/rc/sbus_rc/SbusRc.cpp @@ -0,0 +1,315 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 "SbusRc.hpp" + +#include + +SbusRc::SbusRc(const char *device) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device)), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), + _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) +{ + if (device) { + strncpy(_device, device, sizeof(_device) - 1); + _device[sizeof(_device) - 1] = '\0'; + } +} + +SbusRc::~SbusRc() +{ + perf_free(_cycle_perf); + perf_free(_publish_interval_perf); +} + +int SbusRc::task_spawn(int argc, char *argv[]) +{ + bool error_flag = false; + + int myoptind = 1; + int ch; + const char *myoptarg = nullptr; + const char *device_name = nullptr; + bool silent = false; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + device_name = myoptarg; + silent = false; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + SbusRc *instance = new SbusRc(device_name); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleOnInterval(_current_update_interval); + + return PX4_OK; + + } else if (silent) { + return PX4_OK; + + } else { + if (device_name) { + PX4_ERR("invalid device (-d) %s", device_name); + + } else { + PX4_ERR("valid device required"); + } + } + + return PX4_ERROR; +} + +void SbusRc::Run() +{ + if (should_exit()) { + + close(_rcs_fd); + + exit_and_cleanup(); + return; + } + + perf_begin(_cycle_perf); + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + } + + const hrt_abstime cycle_timestamp = hrt_absolute_time(); + + bool rc_updated = false; + + constexpr hrt_abstime rc_scan_max = 3_s; + + // read all available data from the serial RC input UART + uint8_t rcs_buf[SBUS_BUFFER_SIZE] {}; + int newBytes = ::read(_rcs_fd, &rcs_buf[0], SBUS_BUFFER_SIZE); + + if (newBytes > 0) { + _bytes_rx += newBytes; + } + + const bool rc_scan_locked = _rc_scan_locked; + + if (_rc_scan_begin == 0) { + _rc_scan_begin = cycle_timestamp; + + // Configure serial port + if (_rcs_fd < 0) { + _rcs_fd = open(_device, O_RDWR | O_NONBLOCK); + } + + sbus_config(_rcs_fd, board_rc_singlewire(_device)); + + // First check if the board provides a board-specific inversion method (e.g. via GPIO), + // and if not use an IOCTL + if (!board_rc_invert_input(_device, true)) { +#if defined(TIOCSINVERT) + ioctl(_rcs_fd, TIOCSINVERT, SER_INVERT_ENABLED_RX | SER_INVERT_ENABLED_TX); +#endif // TIOCSINVERT + } + + // flush serial buffer and any existing buffered data + tcflush(_rcs_fd, TCIOFLUSH); + + } else if (_rc_scan_locked + || cycle_timestamp - _rc_scan_begin < rc_scan_max) { + + if (newBytes > 0) { + uint16_t raw_rc_values[input_rc_s::RC_INPUT_MAX_CHANNELS] {}; + uint16_t raw_rc_count = 0; + bool sbus_failsafe = false; + bool sbus_frame_drop = false; + unsigned sbus_frame_drops = 0; + + if (sbus_parse(cycle_timestamp, &rcs_buf[0], newBytes, &raw_rc_values[0], &raw_rc_count, + &sbus_failsafe, &sbus_frame_drop, &sbus_frame_drops, input_rc_s::RC_INPUT_MAX_CHANNELS) + ) { + // we have a new SBUS frame. Publish it. + input_rc_s input_rc{}; + input_rc.timestamp_last_signal = cycle_timestamp; + input_rc.channel_count = math::constrain(raw_rc_count, (uint16_t)0, (uint16_t)input_rc_s::RC_INPUT_MAX_CHANNELS); + input_rc.rssi = -1; + input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; + + unsigned valid_chans = 0; + + for (unsigned i = 0; i < input_rc.channel_count; i++) { + input_rc.values[i] = raw_rc_values[i]; + + if (raw_rc_values[i] != UINT16_MAX) { + valid_chans++; + } + } + + input_rc.channel_count = valid_chans; + + if ((_param_rc_rssi_pwm_chan.get() > 0) && (_param_rc_rssi_pwm_chan.get() < input_rc.channel_count)) { + const int32_t rssi_pwm_chan = _param_rc_rssi_pwm_chan.get(); + const int32_t rssi_pwm_min = _param_rc_rssi_pwm_min.get(); + const int32_t rssi_pwm_max = _param_rc_rssi_pwm_max.get(); + + // get RSSI from input channel + int rc_rssi = ((input_rc.values[rssi_pwm_chan - 1] - rssi_pwm_min) * 100) / (rssi_pwm_max - rssi_pwm_min); + input_rc.rssi = math::constrain(rc_rssi, 0, 100); + } + + if (valid_chans == 0) { + input_rc.rssi = 0; + } + + input_rc.rc_failsafe = sbus_failsafe; + input_rc.rc_lost = (valid_chans == 0); + input_rc.rc_lost_frame_count = sbus_frame_drops; + + input_rc.link_quality = -1; + input_rc.rssi_dbm = NAN; + + input_rc.timestamp = hrt_absolute_time(); + _input_rc_pub.publish(input_rc); + perf_count(_publish_interval_perf); + + _timestamp_last_signal = cycle_timestamp; + rc_updated = true; + + if (valid_chans > 0) { + _rc_scan_locked = true; + } + } + } + + } else { + _rc_scan_begin = 0; + _rc_scan_locked = false; + + close(_rcs_fd); + _rcs_fd = -1; + } + + if (!rc_updated && (hrt_elapsed_time(&_timestamp_last_signal) > 1_s)) { + _rc_scan_locked = false; + } + + if (!rc_scan_locked && _rc_scan_locked) { + PX4_INFO("RC input locked"); + } + + perf_end(_cycle_perf); +} + +int SbusRc::custom_command(int argc, char *argv[]) +{ + if (!is_running()) { + int ret = SbusRc::task_spawn(argc, argv); + + if (ret) { + return ret; + } + } + + return print_usage("unknown command"); +} + +int SbusRc::print_status() +{ + PX4_INFO("Max update rate: %u Hz", 1000000 / _current_update_interval); + + if (_device[0] != '\0') { + PX4_INFO("UART device: %s", _device); + PX4_INFO("UART RX bytes: %" PRIu32, _bytes_rx); + } + + PX4_INFO("RC state: %s", _rc_scan_locked ? "found" : "searching for signal"); + + perf_print_counter(_cycle_perf); + perf_print_counter(_publish_interval_perf); + + return 0; +} + +int SbusRc::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +This module does SBUS RC input parsing. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("sbus_rc", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS3", "", "RC device", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int sbus_rc_main(int argc, char *argv[]) +{ + return SbusRc::main(argc, argv); +} diff --git a/src/drivers/rc/sbus_rc/SbusRc.hpp b/src/drivers/rc/sbus_rc/SbusRc.hpp new file mode 100644 index 000000000000..c02d7e3f50a2 --- /dev/null +++ b/src/drivers/rc/sbus_rc/SbusRc.hpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2024 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 +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SbusRc : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + SbusRc(const char *device); + virtual ~SbusRc(); + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase::print_status() */ + int print_status() override; + +private: + + void Run() override; + + hrt_abstime _rc_scan_begin{0}; + hrt_abstime _timestamp_last_signal{0}; + + bool _rc_scan_locked{false}; + + static constexpr unsigned _current_update_interval{4000}; // 250 Hz + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + + uORB::PublicationMulti _input_rc_pub{ORB_ID(input_rc)}; + + int _rcs_fd{-1}; + char _device[20] {}; ///< device / serial port path + + perf_counter_t _cycle_perf; + perf_counter_t _publish_interval_perf; + uint32_t _bytes_rx{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_rc_rssi_pwm_chan, + (ParamInt) _param_rc_rssi_pwm_min, + (ParamInt) _param_rc_rssi_pwm_max + ) +}; diff --git a/src/drivers/rc/sbus_rc/module.yaml b/src/drivers/rc/sbus_rc/module.yaml new file mode 100644 index 000000000000..bc8a0ee14078 --- /dev/null +++ b/src/drivers/rc/sbus_rc/module.yaml @@ -0,0 +1,10 @@ +module_name: SBUS RC Input Driver +serial_config: + - command: "sbus_rc start -d ${SERIAL_DEV}" + port_config_param: + name: RC_SBUS_PRT_CFG + group: Serial + default: RC + #depends_on_port: RC + description_extended: | + SBUS RC driver. diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index bea3af194c16..12cd1ab0bf40 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -191,7 +191,7 @@ RCInput::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -void +int32_t RCInput::fill_rc_in(uint16_t raw_rc_count_local, uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], hrt_abstime now, bool frame_drop, bool failsafe, @@ -262,6 +262,8 @@ RCInput::fill_rc_in(uint16_t raw_rc_count_local, _input_rc.rc_lost = (valid_chans == 0); _input_rc.rc_lost_frame_count = frame_drops; _input_rc.rc_total_frame_count = 0; + + return valid_chans; } void RCInput::set_rc_scan_state(RC_SCAN newState) @@ -494,9 +496,12 @@ void RCInput::Run() if (rc_updated) { // we have a new SBUS frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SBUS; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, - sbus_frame_drop, sbus_failsafe, frame_drops); - _rc_scan_locked = true; + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, + sbus_frame_drop, sbus_failsafe, frame_drops); + + if (valid_chans > 0) { + _rc_scan_locked = true; + } } } @@ -533,9 +538,12 @@ void RCInput::Run() if (rc_updated) { // we have a new DSM frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_DSM; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, - false, false, frame_drops, dsm_rssi); - _rc_scan_locked = true; + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, + false, false, frame_drops, dsm_rssi); + + if (valid_chans > 0) { + _rc_scan_locked = true; + } } } @@ -580,9 +588,12 @@ void RCInput::Run() if (lost_count == 0) { // we have a new ST24 frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_ST24; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, - false, false, frame_drops, st24_rssi); - _rc_scan_locked = true; + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, + false, false, frame_drops, st24_rssi); + + if (valid_chans > 0) { + _rc_scan_locked = true; + } } else { // if the lost count > 0 means that there is an RC loss @@ -629,9 +640,12 @@ void RCInput::Run() if (rc_updated) { // we have a new SUMD frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_SUMD; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, - false, sumd_failsafe, frame_drops, sumd_rssi); - _rc_scan_locked = true; + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, + false, sumd_failsafe, frame_drops, sumd_rssi); + + if (valid_chans > 0) { + _rc_scan_locked = true; + } } } @@ -657,8 +671,12 @@ void RCInput::Run() // we have a new PPM frame. Publish it. rc_updated = true; _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; - fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0); - _rc_scan_locked = true; + int32_t valid_chans = fill_rc_in(ppm_decoded_channels, ppm_buffer, cycle_timestamp, false, false, 0); + + if (valid_chans > 0) { + _rc_scan_locked = true; + } + _input_rc.rc_ppm_frame_length = ppm_frame_length; _input_rc.timestamp_last_signal = ppm_last_valid_decode; } @@ -699,7 +717,7 @@ void RCInput::Run() if (rc_updated) { // we have a new CRSF frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_CRSF; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0); + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0); // on Pixhawk (-related) boards we cannot write to the RC UART // another option is to use a different UART port @@ -711,7 +729,9 @@ void RCInput::Run() #endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ - _rc_scan_locked = true; + if (valid_chans > 0) { + _rc_scan_locked = true; + } if (_crsf_telemetry) { _crsf_telemetry->update(cycle_timestamp); @@ -749,7 +769,7 @@ void RCInput::Run() if (rc_updated) { // we have a new GHST frame. Publish it. _input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_GHST; - fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); + int32_t valid_chans = fill_rc_in(_raw_rc_count, _raw_rc_values, cycle_timestamp, false, false, 0, ghst_rssi); // ghst telemetry works on fmu-v5 // on other Pixhawk (-related) boards we cannot write to the RC UART @@ -762,7 +782,9 @@ void RCInput::Run() #endif /* BOARD_SUPPORTS_RC_SERIAL_PORT_OUTPUT */ - _rc_scan_locked = true; + if (valid_chans > 0) { + _rc_scan_locked = true; + } if (_ghst_telemetry) { _ghst_telemetry->update(cycle_timestamp); diff --git a/src/drivers/rc_input/RCInput.hpp b/src/drivers/rc_input/RCInput.hpp index be781471c840..626d84d88cc4 100644 --- a/src/drivers/rc_input/RCInput.hpp +++ b/src/drivers/rc_input/RCInput.hpp @@ -118,10 +118,10 @@ class RCInput : public ModuleBase, public ModuleParams, public px4::Sch bool bind_spektrum(int arg = DSMX8_BIND_PULSES) const; #endif // SPEKTRUM_POWER - void fill_rc_in(uint16_t raw_rc_count_local, - uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], - hrt_abstime now, bool frame_drop, bool failsafe, - unsigned frame_drops, int rssi); + int32_t fill_rc_in(uint16_t raw_rc_count_local, + uint16_t raw_rc_values_local[input_rc_s::RC_INPUT_MAX_CHANNELS], + hrt_abstime now, bool frame_drop, bool failsafe, + unsigned frame_drops, int rssi); void set_rc_scan_state(RC_SCAN _rc_scan_state); diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 720437c82301..60286e0f40aa 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -81,8 +81,8 @@ bool CRSFTelemetry::send_battery() return false; } - uint16_t voltage = battery_status.voltage_filtered_v * 10; - uint16_t current = battery_status.current_filtered_a * 10; + uint16_t voltage = battery_status.voltage_v * 10; + uint16_t current = battery_status.current_a * 10; int fuel = battery_status.discharged_mah; uint8_t remaining = battery_status.remaining * 100; return crsf_send_telemetry_battery(_uart_fd, voltage, current, fuel, remaining); diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp index 6edd0de598e6..9a910063931a 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -90,8 +90,8 @@ bool GHSTTelemetry::send_battery_status() battery_status_s battery_status; if (_battery_status_sub.update(&battery_status)) { - voltage_in_10mV = battery_status.voltage_filtered_v * FACTOR_VOLTS_TO_10MV; - current_in_10mA = battery_status.current_filtered_a * FACTOR_AMPS_TO_10MA; + voltage_in_10mV = battery_status.voltage_v * FACTOR_VOLTS_TO_10MV; + current_in_10mA = battery_status.current_a * FACTOR_AMPS_TO_10MA; fuel_in_10mAh = battery_status.discharged_mah * FACTOR_MAH_TO_10MAH; success = ghst_send_telemetry_battery_status(_uart_fd, static_cast(voltage_in_10mV), @@ -136,4 +136,3 @@ bool GHSTTelemetry::send_gps2_status() return ghst_send_telemetry_gps2_status(_uart_fd, ground_speed, ground_course, num_sats, home_dist, home_dir, flags); } - diff --git a/src/drivers/rc_input/module.yaml b/src/drivers/rc_input/module.yaml index d0f9040a6ff8..bf15df092074 100644 --- a/src/drivers/rc_input/module.yaml +++ b/src/drivers/rc_input/module.yaml @@ -32,4 +32,3 @@ serial_config: description_extended: | Setting this to 'Disabled' will use a board-specific default port for RC input. - diff --git a/src/drivers/rpi_rc_in/CMakeLists.txt b/src/drivers/rpi_rc_in/CMakeLists.txt index 69ff3eb7dcba..06a106b13866 100644 --- a/src/drivers/rpi_rc_in/CMakeLists.txt +++ b/src/drivers/rpi_rc_in/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( rpi_rc_in.cpp DEPENDS ) - diff --git a/src/drivers/rpi_rc_in/rpi_rc_in.cpp b/src/drivers/rpi_rc_in/rpi_rc_in.cpp index 6e52a274c805..619e5c00f17f 100644 --- a/src/drivers/rpi_rc_in/rpi_rc_in.cpp +++ b/src/drivers/rpi_rc_in/rpi_rc_in.cpp @@ -211,4 +211,3 @@ int rpi_rc_in_main(int argc, char **argv) return 1; } - diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp index bb6b61da0b36..9061a6d3e4aa 100644 --- a/src/drivers/smart_battery/batmon/batmon.cpp +++ b/src/drivers/smart_battery/batmon/batmon.cpp @@ -146,13 +146,11 @@ void Batmon::RunImpl() // Convert millivolts to volts. new_report.voltage_v = ((float)result) / 1000.0f; - new_report.voltage_filtered_v = new_report.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); new_report.current_a = (-1.0f * ((float)(*(int16_t *)&result)) / 1000.0f); - new_report.current_filtered_a = new_report.current_a; // Read average current. ret |= _interface->read_word(BATT_SMBUS_AVERAGE_CURRENT, result); @@ -240,7 +238,7 @@ int Batmon::get_batmon_startup_info() _cell_count = math::min((uint8_t)num_cells, (uint8_t)MAX_CELL_COUNT); int32_t _num_cells = num_cells; - param_set(param_find("BAT_N_CELLS"), &_num_cells); + param_set(param_find("BAT1_N_CELLS"), &_num_cells); return ret; } diff --git a/src/drivers/tap_esc/tap_esc_uploader.cpp b/src/drivers/tap_esc/tap_esc_uploader.cpp index 03b3b84c5056..350e366b7347 100644 --- a/src/drivers/tap_esc/tap_esc_uploader.cpp +++ b/src/drivers/tap_esc/tap_esc_uploader.cpp @@ -163,7 +163,7 @@ int TAP_ESC_UPLOADER::upload_id(uint8_t esc_id, int32_t fw_size) /****************************************** * second: get device bootloader revision ******************************************/ - uint32_t bl_rev; + uint32_t bl_rev = 0; ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_BL_REV, bl_rev); if (ret == OK) { @@ -390,7 +390,7 @@ int TAP_ESC_UPLOADER::checkcrc(const char *filenames[]) return -EIO; } - uint32_t temp_revision; + uint32_t temp_revision = 0; /* get device bootloader revision */ ret = get_device_info(esc_id, PROTO_GET_DEVICE, PROTO_DEVICE_BL_REV, temp_revision); @@ -1098,7 +1098,7 @@ int TAP_ESC_UPLOADER::verify_crc(uint8_t esc_id, size_t fw_size_local) uint32_t sum = 0; uint32_t bytes_read = 0; uint32_t crc = 0; - uint32_t fw_size_remote; + uint32_t fw_size_remote = 0; uint8_t fill_blank = 0xff; file_buf = new uint8_t[PROG_MULTI_MAX]; diff --git a/src/drivers/tap_esc/tap_esc_uploader.h b/src/drivers/tap_esc/tap_esc_uploader.h index 1f1a0964bf36..abfd5ee65224 100644 --- a/src/drivers/tap_esc/tap_esc_uploader.h +++ b/src/drivers/tap_esc/tap_esc_uploader.h @@ -44,7 +44,7 @@ #include #include -#define TAP_ESC_FW_SEARCH_PATHS {"/etc/extras/tap_esc.bin", "/fs/microsd/tap_esc.bin", nullptr } +#define TAP_ESC_FW_SEARCH_PATHS {"/etc/extras/tap_esc.bin", CONFIG_BOARD_ROOT_PATH "/tap_esc.bin", nullptr } #define PROTO_SUPPORT_BL_REV 5 /**< supported bootloader protocol revision */ #define SYNC_RETRY_TIMES 5 /**< (uint8) esc sync failed allow retry times*/ #define UPLOADER_RETRY_TIMES 2 /**< esc uploader failed allow retry times*/ diff --git a/src/drivers/tattu_can/TattuCan.cpp b/src/drivers/tattu_can/TattuCan.cpp index 746a24137225..1f414c090796 100644 --- a/src/drivers/tattu_can/TattuCan.cpp +++ b/src/drivers/tattu_can/TattuCan.cpp @@ -115,9 +115,7 @@ void TattuCan::Run() battery_status.state_of_health = static_cast(tattu_message.health_status); battery_status.voltage_v = static_cast(tattu_message.voltage) / 1000.0f; - battery_status.voltage_filtered_v = static_cast(tattu_message.voltage) / 1000.0f; battery_status.current_a = static_cast(tattu_message.current) / 1000.0f; - battery_status.current_filtered_a = static_cast(tattu_message.current) / 1000.0f; battery_status.remaining = static_cast(tattu_message.remaining_percent) / 100.0f; battery_status.temperature = static_cast(tattu_message.temperature); battery_status.capacity = tattu_message.standard_capacity; diff --git a/src/drivers/telemetry/CMakeLists.txt b/src/drivers/telemetry/CMakeLists.txt index 0980f2d9f3e8..97e0ed7806cd 100644 --- a/src/drivers/telemetry/CMakeLists.txt +++ b/src/drivers/telemetry/CMakeLists.txt @@ -34,4 +34,4 @@ add_subdirectory(bst) add_subdirectory(frsky_telemetry) add_subdirectory(hott) -#add_subdirectory(iridiumsbd) +add_subdirectory(iridiumsbd) diff --git a/src/drivers/telemetry/bst/CMakeLists.txt b/src/drivers/telemetry/bst/CMakeLists.txt index c9c9e7ff798e..1bbeec9b8dd3 100644 --- a/src/drivers/telemetry/bst/CMakeLists.txt +++ b/src/drivers/telemetry/bst/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( bst.cpp DEPENDS ) - diff --git a/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt b/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt index 719227703b75..49c86518a3da 100644 --- a/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/telemetry/frsky_telemetry/CMakeLists.txt @@ -41,4 +41,3 @@ px4_add_module( MODULE_CONFIG module.yaml ) - diff --git a/src/drivers/telemetry/frsky_telemetry/module.yaml b/src/drivers/telemetry/frsky_telemetry/module.yaml index 796342de9c66..62f32aabec89 100644 --- a/src/drivers/telemetry/frsky_telemetry/module.yaml +++ b/src/drivers/telemetry/frsky_telemetry/module.yaml @@ -4,4 +4,3 @@ serial_config: port_config_param: name: TEL_FRSKY_CONFIG group: Telemetry - diff --git a/src/drivers/telemetry/hott/hott_sensors/CMakeLists.txt b/src/drivers/telemetry/hott/hott_sensors/CMakeLists.txt index e2f9ee6e7919..41e762a9824f 100644 --- a/src/drivers/telemetry/hott/hott_sensors/CMakeLists.txt +++ b/src/drivers/telemetry/hott/hott_sensors/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( hott_sensors.cpp DEPENDS ) - diff --git a/src/drivers/telemetry/hott/hott_telemetry/module.yaml b/src/drivers/telemetry/hott/hott_telemetry/module.yaml index ff50a8e3d4ee..1467a377ddb0 100644 --- a/src/drivers/telemetry/hott/hott_telemetry/module.yaml +++ b/src/drivers/telemetry/hott/hott_telemetry/module.yaml @@ -4,4 +4,3 @@ serial_config: port_config_param: name: TEL_HOTT_CONFIG group: Telemetry - diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp index 5fddd4ac46fe..1ee9d2c6276a 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.cpp @@ -235,7 +235,7 @@ int IridiumSBD::print_status() PX4_INFO("RX session pending: %d", _rx_session_pending); PX4_INFO("RX read pending: %d", _rx_read_pending); PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check); - PX4_INFO("Last heartbeat: %" PRId64, _last_heartbeat); + PX4_INFO("Last heartbeat: %" PRId64, _last_at_ok_timestamp); return 0; } @@ -333,6 +333,11 @@ void IridiumSBD::standby_loop(void) } } + if (!is_modem_responsive()) { + VERBOSE_INFO("MODEM IS NOT RENSPONSIVE"); + return; + } + // check for incoming SBDRING, handled inside read_at_command() read_at_command(); @@ -477,7 +482,6 @@ void IridiumSBD::sbdsession_loop(void) _ring_pending = false; _tx_session_pending = false; _last_read_time = hrt_absolute_time(); - _last_heartbeat = _last_read_time; ++_successful_sbd_sessions; if (mt_queued > 0) { @@ -498,8 +502,6 @@ void IridiumSBD::sbdsession_loop(void) case 1: VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL"); - _last_heartbeat = hrt_absolute_time(); - // after a successful session reset the tx buffer _tx_buf_write_idx = 0; ++_successful_sbd_sessions; @@ -552,11 +554,6 @@ void IridiumSBD::start_csq(void) _last_signal_check = hrt_absolute_time(); - if (!is_modem_ready()) { - VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!"); - return; - } - write_at("AT+CSQ"); _new_state = SATCOM_STATE_CSQ; } @@ -571,11 +568,6 @@ void IridiumSBD::start_sbd_session(void) VERBOSE_INFO("STARTING SBD SESSION"); } - if (!is_modem_ready()) { - VERBOSE_INFO("SBD SESSION: MODEM NOT READY!"); - return; - } - if (_ring_pending) { write_at("AT+SBDIXA"); @@ -610,8 +602,8 @@ void IridiumSBD::start_test(void) printf("\n"); } - if (!is_modem_ready()) { - PX4_WARN("MODEM NOT READY!"); + if (!is_modem_responsive()) { + PX4_WARN("MODEM NOT RENSPONSIVE!"); return; } @@ -718,11 +710,6 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen) void IridiumSBD::write_tx_buf() { - if (!is_modem_ready()) { - VERBOSE_INFO("WRITE SBD: MODEM NOT READY!"); - return; - } - pthread_mutex_lock(&_tx_buf_mutex); char command[13]; @@ -779,11 +766,6 @@ void IridiumSBD::write_tx_buf() void IridiumSBD::read_rx_buf(void) { - if (!is_modem_ready()) { - VERBOSE_INFO("READ SBD: MODEM NOT READY!"); - return; - } - pthread_mutex_lock(&_rx_buf_mutex); @@ -949,11 +931,12 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name) return SATCOM_UART_OK; } -bool IridiumSBD::is_modem_ready(void) +bool IridiumSBD::is_modem_responsive(void) { write_at("AT"); if (read_at_command() == SATCOM_RESULT_OK) { + _last_at_ok_timestamp = hrt_absolute_time(); return true; } else { @@ -980,9 +963,9 @@ void IridiumSBD::publish_iridium_status() { bool need_to_publish = false; - if (_status.last_heartbeat != _last_heartbeat) { + if (_status.last_at_ok_timestamp != _last_at_ok_timestamp) { need_to_publish = true; - _status.last_heartbeat = _last_heartbeat; + _status.last_at_ok_timestamp = _last_at_ok_timestamp; } if (_status.tx_buf_write_index != _tx_buf_write_idx) { diff --git a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h index 3d628440e6ac..897201cf715c 100644 --- a/src/drivers/telemetry/iridiumsbd/IridiumSBD.h +++ b/src/drivers/telemetry/iridiumsbd/IridiumSBD.h @@ -245,7 +245,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase /* * Checks if the modem responds to the "AT" command */ - bool is_modem_ready(void); + bool is_modem_responsive(void); /* * Get the poll state @@ -321,7 +321,7 @@ class IridiumSBD : public cdev::CDev, public ModuleBase hrt_abstime _last_write_time = 0; hrt_abstime _last_read_time = 0; - hrt_abstime _last_heartbeat = 0; + hrt_abstime _last_at_ok_timestamp = 0; hrt_abstime _session_start_time = 0; satcom_state _state = SATCOM_STATE_STANDBY; diff --git a/src/drivers/telemetry/iridiumsbd/module.yaml b/src/drivers/telemetry/iridiumsbd/module.yaml index 5fa8d138c4c4..7ced65126767 100644 --- a/src/drivers/telemetry/iridiumsbd/module.yaml +++ b/src/drivers/telemetry/iridiumsbd/module.yaml @@ -12,4 +12,3 @@ serial_config: port_config_param: name: ISBD_CONFIG group: Iridium SBD - diff --git a/src/drivers/test_ppm/CMakeLists.txt b/src/drivers/test_ppm/CMakeLists.txt index dbe3b7b64828..2c9c91c5c455 100644 --- a/src/drivers/test_ppm/CMakeLists.txt +++ b/src/drivers/test_ppm/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( test_ppm.cpp DEPENDS ) - diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index 379f651be9f5..b11f13f237ba 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -52,8 +52,6 @@ SagetechMXS::SagetechMXS(const char *port) : SagetechMXS::~SagetechMXS() { - free((char *)_port); - if (!(_fd < 0)) { close(_fd); } diff --git a/src/drivers/uavcan/CMakeLists.txt b/src/drivers/uavcan/CMakeLists.txt index 4146edab05b8..c70d029ea7e3 100644 --- a/src/drivers/uavcan/CMakeLists.txt +++ b/src/drivers/uavcan/CMakeLists.txt @@ -75,6 +75,7 @@ add_definitions( -DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1 -DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER} + -DUAVCAN_NUM_IFACES=${config_uavcan_num_ifaces} -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER} -DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1 @@ -167,6 +168,7 @@ px4_add_module( sensors/battery.cpp sensors/airspeed.cpp sensors/flow.cpp + sensors/fuel_tank_status.cpp sensors/gnss_relative.cpp sensors/gnss.cpp sensors/mag.cpp diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index 1d91acb6221e..0076d36568f5 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -1,6 +1,7 @@ menuconfig DRIVERS_UAVCAN bool "uavcan" default n + depends on PLATFORM_NUTTX ---help--- Enable support for uavcan @@ -53,6 +54,10 @@ if DRIVERS_UAVCAN bool "Subscribe to Flow: com::hex::equipment::flow::Measurement" default y + config UAVCAN_SENSOR_FUEL_TANK_STATUS + bool "Subscribe to Fuel Tank Status: uavcan::equipment::ice::FuelTankStatus" + default y + config UAVCAN_SENSOR_GNSS bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" default y diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp index ab09c327407c..77698cff16d1 100644 --- a/src/drivers/uavcan/sensors/battery.cpp +++ b/src/drivers/uavcan/sensors/battery.cpp @@ -104,9 +104,7 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure + */ + +#include "fuel_tank_status.hpp" + +#include + +const char *const UavcanFuelTankStatusBridge::NAME = "fuel_tank_status"; + +UavcanFuelTankStatusBridge::UavcanFuelTankStatusBridge(uavcan::INode &node) : + UavcanSensorBridgeBase("uavcan_fuel_tank_status", ORB_ID(fuel_tank_status)), + _sub_fuel_tank_status_data(node) +{ } + +int UavcanFuelTankStatusBridge::init() +{ + int res = _sub_fuel_tank_status_data.start(FuelTankStatusCbBinder(this, + &UavcanFuelTankStatusBridge::fuel_tank_status_sub_cb)); + + if (res < 0) { + DEVICE_LOG("failed to start uavcan sub: %d", res); + return res; + } + + // Fetch maximum fuel capacity (in liters) + param_get(param_find("UAVCAN_ECU_MAXF"), &_max_fuel_capacity); + + // Fetching fuel type + param_get(param_find("UAVCAN_ECU_FUELT"), &_fuel_type); + + return 0; +} + +void UavcanFuelTankStatusBridge::fuel_tank_status_sub_cb(const + uavcan::ReceivedDataStructure &msg) +{ + auto report = ::fuel_tank_status_s(); + report.timestamp = hrt_absolute_time(); + report.maximum_fuel_capacity = _max_fuel_capacity * 1000.0f; // convert to ml + report.fuel_type = static_cast(_fuel_type); + report.consumed_fuel = NAN; // only the remaining fuel is measured + report.fuel_consumption_rate = msg.fuel_consumption_rate_cm3pm / 60.0f; // convert to ml/s + report.percent_remaining = msg.available_fuel_volume_percent; + report.remaining_fuel = msg.available_fuel_volume_cm3; + report.fuel_tank_id = msg.fuel_tank_id; + + // Optional temperature field, in Kelvin, set to NaN if not provided. + report.temperature = !PX4_ISFINITE(msg.fuel_temperature) ? NAN : msg.fuel_temperature; + + publish(msg.getSrcNodeID().get(), &report); +} + +int UavcanFuelTankStatusBridge::init_driver(uavcan_bridge::Channel *channel) +{ + return PX4_OK; +} diff --git a/src/drivers/uavcan/sensors/fuel_tank_status.hpp b/src/drivers/uavcan/sensors/fuel_tank_status.hpp new file mode 100644 index 000000000000..ca58666c3cc5 --- /dev/null +++ b/src/drivers/uavcan/sensors/fuel_tank_status.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 fuel_tank_status.hpp + * @author Nuno Marques + * @brief UAVCAN bridge for Fuel Tank Status messages. + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include + +#include + +class UavcanFuelTankStatusBridge : public UavcanSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanFuelTankStatusBridge(uavcan::INode &node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + + void fuel_tank_status_sub_cb(const uavcan::ReceivedDataStructure &msg); + + int init_driver(uavcan_bridge::Channel *channel) override; + + typedef uavcan::MethodBinder &)> + FuelTankStatusCbBinder; + + uavcan::Subscriber _sub_fuel_tank_status_data; + + float _max_fuel_capacity{0.0f}; + int32_t _fuel_type{fuel_tank_status_s::MAV_FUEL_TYPE_UNKNOWN}; +}; diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 49c033e43beb..db88941a564b 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -436,7 +436,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure } // If we haven't already done so, set the system clock using GPS data - if (valid_pos_cov && !_system_clock_set) { + if ((fix_type >= sensor_gps_s::FIX_TYPE_2D) && !_system_clock_set) { timespec ts{}; // get the whole microseconds diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index c219d90f3f04..1fa67050e1f2 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -57,6 +57,9 @@ #if defined(CONFIG_UAVCAN_SENSOR_FLOW) #include "flow.hpp" #endif +#if defined(CONFIG_UAVCAN_SENSOR_FUEL_TANK_STATUS) +#include "fuel_tank_status.hpp" +#endif #if defined(CONFIG_UAVCAN_SENSOR_GNSS) #include "gnss.hpp" #endif @@ -137,6 +140,17 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, Listcan_id; + + if (recv_frame->len > CANFD_MAX_DLEN) { + return -EFAULT; + } + out_frame.dlc = recv_frame->len; memcpy(out_frame.data, &recv_frame->data, recv_frame->len); } else { struct can_frame *recv_frame = (struct can_frame *)&_recv_frame; out_frame.id = recv_frame->can_id; + + if (recv_frame->can_dlc > CAN_MAX_DLEN) { + return -EFAULT; + } + out_frame.dlc = recv_frame->can_dlc; memcpy(out_frame.data, &recv_frame->data, recv_frame->can_dlc); } diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 277b2526ddd4..fbdc534a9c30 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -744,6 +744,41 @@ UavcanNode::Run() _node.spinOnce(); // expected to be non-blocking + // Publish status + constexpr hrt_abstime status_pub_interval = 100_ms; + + if (hrt_absolute_time() - _last_can_status_pub >= status_pub_interval) { + _last_can_status_pub = hrt_absolute_time(); + + for (int i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) { + if (i > UAVCAN_NUM_IFACES) { + break; + } + + auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i); + + if (!iface) { + continue; + } + + auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i); + can_interface_status_s status{ + .timestamp = hrt_absolute_time(), + .io_errors = iface_perf_cnt.errors, + .frames_tx = iface_perf_cnt.frames_tx, + .frames_rx = iface_perf_cnt.frames_rx, + .interface = static_cast(i), + }; + + if (_can_status_pub_handles[i] == nullptr) { + int instance{0}; + _can_status_pub_handles[i] = orb_advertise_multi(ORB_ID(can_interface_status), nullptr, &instance); + } + + (void)orb_publish(ORB_ID(can_interface_status), _can_status_pub_handles[i], &status); + } + } + // check for parameter updates if (_parameter_update_sub.updated()) { // clear update diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 54119259ef16..5afa04ef672e 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -96,6 +96,7 @@ #include #include #include +#include #include #include #include @@ -309,6 +310,10 @@ class UavcanNode : public px4::ScheduledWorkItem, public ModuleParams uORB::Publication _param_response_pub{ORB_ID(uavcan_parameter_value)}; uORB::Publication _command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::PublicationMulti _can_status_pub{ORB_ID(can_interface_status)}; + + hrt_abstime _last_can_status_pub{0}; + orb_advert_t _can_status_pub_handles[UAVCAN_NUM_IFACES] = {nullptr}; /* * The MAVLink parameter bridge needs to know the maximum parameter index diff --git a/src/drivers/uavcan/uavcan_module.hpp b/src/drivers/uavcan/uavcan_module.hpp index 08d6bd5e987a..bd055cda9be7 100644 --- a/src/drivers/uavcan/uavcan_module.hpp +++ b/src/drivers/uavcan/uavcan_module.hpp @@ -49,7 +49,7 @@ // firmware paths #define UAVCAN_MAX_PATH_LENGTH (128 + 40) -#define UAVCAN_SD_ROOT_PATH "/fs/microsd/" +#define UAVCAN_SD_ROOT_PATH CONFIG_BOARD_ROOT_PATH "/" #define UAVCAN_FIRMWARE_PATH UAVCAN_SD_ROOT_PATH"ufw" #define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw" #define UAVCAN_ROMFS_FW_PREFIX "_" diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c index 7a9b5ea51b3b..edd2c41ea4c3 100644 --- a/src/drivers/uavcan/uavcan_params.c +++ b/src/drivers/uavcan/uavcan_params.c @@ -97,6 +97,40 @@ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MIN, 0.3f); */ PARAM_DEFINE_FLOAT(UAVCAN_RNG_MAX, 200.0f); +/** + * UAVCAN fuel tank maximum capacity + * + * This parameter defines the maximum fuel capacity of the vehicle's fuel tank. + * + * @min 0.0 + * @max 100000.0 + * @unit liters + * @decimal 1 + * @increment 0.1 + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_FLOAT(UAVCAN_ECU_MAXF, 15.0f); + +/** + * UAVCAN fuel tank fuel type + * + * This parameter defines the type of fuel used in the vehicle's fuel tank. + * + * 0: Unknown + * 1: Liquid (e.g., gasoline, diesel) + * 2: Gas (e.g., hydrogen, methane, propane) + * + * @min 0 + * @max 2 + * @value 0 Unknown + * @value 1 Liquid + * @value 2 Gas + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ECU_FUELT, 1); + /** * UAVCAN ANTI_COLLISION light operating mode * @@ -292,6 +326,17 @@ PARAM_DEFINE_INT32(UAVCAN_SUB_DPRES, 0); */ PARAM_DEFINE_INT32(UAVCAN_SUB_FLOW, 0); +/** + * subscription fuel tank + * + * Enable UAVCAN fuel tank status subscription. + * + * @boolean + * @reboot_required true + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_SUB_FUEL, 0); + /** * subscription GPS * diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index fa4b2aab10de..3d35ea523ed5 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -180,7 +180,7 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p while ((dev_dirent = readdir(sd_root_dir)) != nullptr) { - uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor; + uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor{0}; // Looking for all uavcan.bin files. diff --git a/src/drivers/uavcannode/Kconfig b/src/drivers/uavcannode/Kconfig index d20c2baf0624..9aa7073a5b0f 100644 --- a/src/drivers/uavcannode/Kconfig +++ b/src/drivers/uavcannode/Kconfig @@ -1,6 +1,7 @@ menuconfig DRIVERS_UAVCANNODE bool "uavcannode" default n + depends on BOARD_ROMFSROOT != "px4fmu_common" ---help--- Enable support for uavcannode diff --git a/src/drivers/voxl2_io/Kconfig b/src/drivers/voxl2_io/Kconfig index 15964deeeb90..622cad470d06 100644 --- a/src/drivers/voxl2_io/Kconfig +++ b/src/drivers/voxl2_io/Kconfig @@ -2,4 +2,4 @@ menuconfig DRIVERS_VOXL2_IO bool "voxl2_io" default n ---help--- - Enable support for voxl2_io \ No newline at end of file + Enable support for voxl2_io diff --git a/src/drivers/voxl2_io/module.yaml b/src/drivers/voxl2_io/module.yaml index 02aa2122e3ea..021305e955fa 100644 --- a/src/drivers/voxl2_io/module.yaml +++ b/src/drivers/voxl2_io/module.yaml @@ -10,4 +10,3 @@ actuator_output: group_label: 'PWMs' channel_label: 'PWM Channel' num_channels: 4 - diff --git a/src/drivers/voxl2_io/voxl2_io_params.c b/src/drivers/voxl2_io/voxl2_io_params.c index c718a4f62caf..704cca989e09 100644 --- a/src/drivers/voxl2_io/voxl2_io_params.c +++ b/src/drivers/voxl2_io/voxl2_io_params.c @@ -63,4 +63,3 @@ PARAM_DEFINE_INT32(VOXL2_IO_MIN, 1000); * @unit us */ PARAM_DEFINE_INT32(VOXL2_IO_MAX, 2000); - diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 425c09f3a790..56878ec9855c 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -66,15 +66,12 @@ void FakeMagnetometer::Run() if (_vehicle_gps_position_sub.copy(&gps)) { if (gps.eph < 1000) { - const double lat = gps.latitude_deg; - const double lon = gps.longitude_deg; - // magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = get_mag_declination_radians(lat, lon); - const float mag_inclination_gps = get_mag_inclination_radians(lat, lon); - const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + const float declination_rad = math::radians(get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(gps.latitude_deg, gps.longitude_deg)); + const float field_strength_gauss = get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg); - _mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0); + _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); _mag_earth_available = true; } diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 0ef2ab28af70..b7ccbd014335 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -41,6 +41,7 @@ */ #include +#include #include #include #include @@ -48,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -60,6 +60,9 @@ #include #include +#include +#include + __EXPORT int matlab_csv_serial_main(int argc, char *argv[]); static bool thread_should_exit = false; /**< Daemon exit flag */ static bool thread_running = false; /**< Daemon status flag */ @@ -221,9 +224,9 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates - dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, - (int)accel0.z_raw, - (int)accel1.x_raw, (int)accel1.y_raw, (int)accel1.z_raw); + dprintf(serial_fd, "%"PRId64",%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x, (int)accel0.y, + (int)accel0.z, + (int)accel1.x, (int)accel1.y, (int)accel1.z); } } diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 4e20ce106ba8..7a7b5973fd4a 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -64,6 +64,7 @@ add_subdirectory(perf EXCLUDE_FROM_ALL) add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL) add_subdirectory(pid EXCLUDE_FROM_ALL) add_subdirectory(pid_design EXCLUDE_FROM_ALL) +add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL) add_subdirectory(rate_control EXCLUDE_FROM_ALL) add_subdirectory(rc EXCLUDE_FROM_ALL) add_subdirectory(ringbuffer EXCLUDE_FROM_ALL) diff --git a/src/lib/adsb/AdsbConflict.cpp b/src/lib/adsb/AdsbConflict.cpp index cb40f27a7df6..8fa11a8f2a72 100644 --- a/src/lib/adsb/AdsbConflict.cpp +++ b/src/lib/adsb/AdsbConflict.cpp @@ -108,12 +108,14 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index) { _traffic_buffer.icao_address.remove(traffic_index); _traffic_buffer.timestamp.remove(traffic_index); + PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address) { _traffic_buffer.timestamp.push_back(hrt_absolute_time()); _traffic_buffer.icao_address.push_back(icao_address); + PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size()); } void AdsbConflict::get_traffic_state() @@ -129,7 +131,6 @@ void AdsbConflict::get_traffic_state() if (old_conflict && _conflict_detected) { old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT); - } if (new_traffic && _conflict_detected && !_traffic_buffer_full) { @@ -154,65 +155,56 @@ void AdsbConflict::get_traffic_state() } +void AdsbConflict::remove_expired_conflicts() +{ + for (uint8_t traffic_index = 0; traffic_index < _traffic_buffer.timestamp.size();) { + if (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > TRAFFIC_CONFLICT_LIFETIME) { + events::send(events::ID("navigator_traffic_expired"), events::Log::Notice, + "Traffic Conflict {1} Expired and removed from buffer", + _traffic_buffer.icao_address[traffic_index]); + remove_icao_address_from_conflict_list(traffic_index); + + } else { + traffic_index++; + } + } +} bool AdsbConflict::handle_traffic_conflict() { get_traffic_state(); - char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages - - //convert UAS_id byte array to char array for User Warning - for (int i = 0; i < 5; i++) { - snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]); - } - - uint64_t uas_id_int = 0; - - for (int i = 0; i < 8; i++) { - uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8); - } - - bool take_action = false; switch (_traffic_state) { case TRAFFIC_STATE::ADD_CONFLICT: case TRAFFIC_STATE::REMIND_CONFLICT: { - - take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180, - (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id, + take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f), + (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, _transponder_report.callsign, - uas_id_int); - + _transponder_report.icao_address); } break; case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: { - - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!", - _transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? - _transponder_report.callsign : uas_id); - - events::send(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved", - uas_id_int); - + events::send(events::ID("navigator_traffic_resolved"), events::Log::Notice, + "Traffic Conflict Resolved {1}!", + _transponder_report.icao_address); + _last_traffic_warning_time = hrt_absolute_time(); } break; case TRAFFIC_STATE::BUFFER_FULL: { - if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) { - PX4_WARN("Too much traffic! Showing all messages from now on"); + if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) + && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) { + events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on"); + _last_buffer_full_warning_time = hrt_absolute_time(); } - //stop buffering incoming conflicts - take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180, - (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id, - _transponder_report.callsign, - uas_id_int); - + //disable conflict warnings when buffer is full } break; @@ -242,49 +234,67 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int) + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address) { switch (_conflict_detection_params.traffic_avoidance_mode) { case 0: { - PX4_WARN("TRAFFIC %s! dst %d, hdg %d", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); + + if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { + + PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d", + tr_callsign, + traffic_seperation, + traffic_direction, (int)icao_address); + + } + + + _last_traffic_warning_time = hrt_absolute_time(); + break; } case 1: { - mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); + + + if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) { + + PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d", + tr_callsign, + traffic_seperation, + traffic_direction, (int)icao_address); + + } + /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); + break; } case 2: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_rtl"), events::Log::Critical, - "Traffic alert, returning home", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_rtl"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); return true; @@ -292,19 +302,17 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } case 3: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_land"), events::Log::Critical, - "Traffic alert, landing", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_land"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); return true; @@ -313,19 +321,18 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper } case 4: { - mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t", - tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id, - traffic_seperation, - traffic_direction); /* EVENT * @description - * - ID: {1} - * - Distance: {2m} - * - Direction: {3} degrees + * - ICAO Address: {1} + * - Traffic Separation Distance: {2m} + * - Heading: {3} degrees */ - events::send(events::ID("navigator_traffic_hold"), events::Log::Critical, - "Traffic alert, holding position", - uas_id_int, traffic_seperation, traffic_direction); + events::send(events::ID("navigator_traffic_hold"), events::Log::Notice, + "Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position", + icao_address, traffic_seperation, traffic_direction); + + _last_traffic_warning_time = hrt_absolute_time(); + return true; @@ -391,8 +398,7 @@ void AdsbConflict::fake_traffic(const char *callsign, float distance, float dire void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav) { - - /* + //Test with buffer size of 10 //first conflict fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, @@ -402,19 +408,20 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); - fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, + fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); - fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, + fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav, alt_uav); //stop spamming - + //new conflicts fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav, alt_uav); + fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, alt_uav); @@ -445,6 +452,9 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, alt_uav); //buffer full + + //buffer full conflicts + fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav, alt_uav); @@ -458,7 +468,7 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, alt_uav); - //end conflict + //end conflicts fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav, alt_uav); @@ -474,30 +484,29 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav, alt_uav); - //new conflicts with space in buffer fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, alt_uav); - + //spam fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav, alt_uav); - + //new conflict fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, alt_uav); - + //spam fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav, alt_uav); - + //new conflict fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f, transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav, alt_uav); + for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) { PX4_INFO("%u ", static_cast(_traffic_buffer.icao_address[i])); } - */ } diff --git a/src/lib/adsb/AdsbConflict.h b/src/lib/adsb/AdsbConflict.h index b00d04f4dbd3..9942a6b921b6 100644 --- a/src/lib/adsb/AdsbConflict.h +++ b/src/lib/adsb/AdsbConflict.h @@ -46,7 +46,6 @@ #include #include -#include #include #include @@ -57,14 +56,15 @@ using namespace time_literals; static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10}; -static constexpr uint8_t UTM_GUID_MSG_LENGTH{11}; - static constexpr uint8_t UTM_CALLSIGN_LENGTH{9}; static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s}; static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f}; +static constexpr uint64_t TRAFFIC_WARNING_TIMESTEP{60_s}; //limits the max warning rate when traffic conflict buffer is full + +static constexpr uint64_t TRAFFIC_CONFLICT_LIFETIME{120_s}; //limits the time a conflict can be in the buffer without being seen (as a conflict) struct traffic_data_s { double lat_traffic; @@ -118,7 +118,7 @@ class AdsbConflict bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags, - char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int); + char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address); transponder_report_s _transponder_report{}; @@ -132,6 +132,8 @@ class AdsbConflict void run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav); + void remove_expired_conflicts(); + bool _conflict_detected{false}; TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT}; @@ -144,15 +146,15 @@ class AdsbConflict private: - orb_advert_t _mavlink_log_pub{nullptr}; - crosstrack_error_s _crosstrack_error{}; - transponder_report_s tr{}; orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr); TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT}; + hrt_abstime _last_traffic_warning_time{0}; + + hrt_abstime _last_buffer_full_warning_time{0}; }; diff --git a/src/lib/adsb/AdsbConflictTest.cpp b/src/lib/adsb/AdsbConflictTest.cpp index d83ad8241463..5d6b6ccbe630 100644 --- a/src/lib/adsb/AdsbConflictTest.cpp +++ b/src/lib/adsb/AdsbConflictTest.cpp @@ -50,7 +50,7 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) for (uint32_t i = 0; i < traffic_dataset_size; i++) { - printf("---------------%d--------------\n", i); + //printf("---------------%d--------------\n", i); struct traffic_data_s traffic = traffic_dataset[i]; @@ -64,10 +64,10 @@ TEST_F(AdsbConflictTest, detectTrafficConflict) adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now); - printf("conflict_detected %d \n", adsb_conflict._conflict_detected); + //printf("conflict_detected %d \n", adsb_conflict._conflict_detected); - printf("------------------------------\n"); - printf("------------------------------\n \n"); + //printf("------------------------------\n"); + //printf("------------------------------\n \n"); EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict); } @@ -191,7 +191,6 @@ TEST_F(AdsbConflictTest, trafficAlerts) TEST_F(AdsbConflictTest, trafficReminder) { - struct traffic_buffer_s used_buffer; used_buffer.icao_address.push_back(2345); used_buffer.icao_address.push_back(1234); @@ -201,11 +200,11 @@ TEST_F(AdsbConflictTest, trafficReminder) used_buffer.icao_address.push_back(5000); used_buffer.timestamp.push_back(3_s); - used_buffer.timestamp.push_back(800_s); + used_buffer.timestamp.push_back(80_s); + used_buffer.timestamp.push_back(10_s); + used_buffer.timestamp.push_back(1000_s); used_buffer.timestamp.push_back(100_s); - used_buffer.timestamp.push_back(20000_s); - used_buffer.timestamp.push_back(6000_s); - used_buffer.timestamp.push_back(6587_s); + used_buffer.timestamp.push_back(187_s); struct traffic_buffer_s full_buffer; full_buffer.icao_address.push_back(2345); @@ -220,15 +219,15 @@ TEST_F(AdsbConflictTest, trafficReminder) full_buffer.icao_address.push_back(99999); full_buffer.timestamp.push_back(1_s); - full_buffer.timestamp.push_back(800_s); + full_buffer.timestamp.push_back(80_s); + full_buffer.timestamp.push_back(10_s); + full_buffer.timestamp.push_back(1000_s); full_buffer.timestamp.push_back(100_s); - full_buffer.timestamp.push_back(20000_s); - full_buffer.timestamp.push_back(6000_s); - full_buffer.timestamp.push_back(19000_s); - full_buffer.timestamp.push_back(5000_s); + full_buffer.timestamp.push_back(900_s); + full_buffer.timestamp.push_back(500_s); full_buffer.timestamp.push_back(2_s); - full_buffer.timestamp.push_back(1000_s); - full_buffer.timestamp.push_back(58943_s); + full_buffer.timestamp.push_back(100_s); + full_buffer.timestamp.push_back(5843_s); TestAdsbConflict adsb_conflict; diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp index 8c833af1e024..5b41dde3b846 100644 --- a/src/lib/battery/battery.cpp +++ b/src/lib/battery/battery.cpp @@ -46,6 +46,7 @@ #include using namespace time_literals; +using namespace matrix; Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, const uint8_t source) : ModuleParams(parent), @@ -53,10 +54,9 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, _source(source) { const float expected_filter_dt = static_cast(sample_interval_us) / 1_s; - _voltage_filter_v.setParameters(expected_filter_dt, 1.f); - _current_filter_a.setParameters(expected_filter_dt, .5f); _current_average_filter_a.setParameters(expected_filter_dt, 50.f); - _throttle_filter.setParameters(expected_filter_dt, 1.f); + _ocv_filter_v.setParameters(expected_filter_dt, 1.f); + _cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f); if (index > 9 || index < 1) { PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index); @@ -81,9 +81,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, snprintf(param_name, sizeof(param_name), "BAT%d_CAPACITY", _index); _param_handles.capacity = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_V_LOAD_DROP", _index); - _param_handles.v_load_drop = param_find(param_name); - snprintf(param_name, sizeof(param_name), "BAT%d_R_INTERNAL", _index); _param_handles.r_internal = param_find(param_name); @@ -102,24 +99,17 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us, void Battery::updateVoltage(const float voltage_v) { _voltage_v = voltage_v; - _voltage_filter_v.update(voltage_v); } void Battery::updateCurrent(const float current_a) { _current_a = current_a; - _current_filter_a.update(current_a); } void Battery::updateBatteryStatus(const hrt_abstime ×tamp) { - if (!_battery_initialized) { - _voltage_filter_v.reset(_voltage_v); - _current_filter_a.reset(_current_a); - } - // Require minimum voltage otherwise override connected status - if (_voltage_filter_v.getState() < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { + if (_voltage_v < LITHIUM_BATTERY_RECOGNITION_VOLTAGE) { _connected = false; } @@ -127,12 +117,16 @@ void Battery::updateBatteryStatus(const hrt_abstime ×tamp) _last_unconnected_timestamp = timestamp; } - // wait with initializing filters to avoid relying on a voltage sample from the rising edge + // Wait with initializing filters to avoid relying on a voltage sample from the rising edge _battery_initialized = _connected && (timestamp > _last_unconnected_timestamp + 2_s); + if (_connected && !_battery_initialized && _internal_resistance_initialized && _params.n_cells > 0) { + resetInternalResistanceEstimation(_voltage_v, _current_a); + } + sumDischarged(timestamp, _current_a); _state_of_charge_volt_based = - calculateStateOfChargeVoltageBased(_voltage_filter_v.getState(), _current_filter_a.getState()); + calculateStateOfChargeVoltageBased(_voltage_v, _current_a); if (!_external_state_of_charge) { estimateStateOfCharge(); @@ -149,9 +143,7 @@ battery_status_s Battery::getBatteryStatus() { battery_status_s battery_status{}; battery_status.voltage_v = _voltage_v; - battery_status.voltage_filtered_v = _voltage_filter_v.getState(); battery_status.current_a = _current_a; - battery_status.current_filtered_a = _current_filter_a.getState(); battery_status.current_average_a = _current_average_filter_a.getState(); battery_status.discharged_mah = _discharged_mah; battery_status.remaining = _state_of_charge; @@ -167,6 +159,15 @@ battery_status_s Battery::getBatteryStatus() battery_status.warning = _warning; battery_status.timestamp = hrt_absolute_time(); battery_status.faults = determineFaults(); + battery_status.internal_resistance_estimate = _internal_resistance_estimate; + battery_status.ocv_estimate = _voltage_v + _internal_resistance_estimate * _params.n_cells * _current_a; + battery_status.ocv_estimate_filtered = _ocv_filter_v.getState(); + battery_status.volt_based_soc_estimate = _params.n_cells > 0 ? math::interpolate(_ocv_filter_v.getState() / + _params.n_cells, + _params.v_empty, _params.v_charged, 0.f, 1.f) : -1.f; + battery_status.voltage_prediction = _voltage_prediction; + battery_status.prediction_error = _prediction_error; + battery_status.estimation_covariance_norm = _estimation_covariance_norm; return battery_status; } @@ -213,36 +214,78 @@ float Battery::calculateStateOfChargeVoltageBased(const float voltage_v, const f // remaining battery capacity based on voltage float cell_voltage = voltage_v / _params.n_cells; - // correct battery voltage locally for load drop to avoid estimation fluctuations - if (_params.r_internal >= 0.f && current_a > FLT_EPSILON) { - cell_voltage += _params.r_internal * current_a; - - } else { - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint); - const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz); - const float throttle = thrust_setpoint.length(); + // correct battery voltage locally for load drop according to internal resistance and current + if (current_a > FLT_EPSILON) { + updateInternalResistanceEstimation(voltage_v, current_a); - _throttle_filter.update(throttle); + if (_params.r_internal >= 0.f) { // Use user specified internal resistance value + cell_voltage += _params.r_internal * current_a; - if (!_battery_initialized) { - _throttle_filter.reset(throttle); + } else { // Use estimated internal resistance value + cell_voltage += _internal_resistance_estimate * current_a; } - // assume linear relation between throttle and voltage drop - cell_voltage += throttle * _params.v_load_drop; } - return math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f); + _cell_voltage_filter_v.update(cell_voltage); + return math::interpolate(_cell_voltage_filter_v.getState(), _params.v_empty, _params.v_charged, 0.f, 1.f); +} + +void Battery::updateInternalResistanceEstimation(const float voltage_v, const float current_a) +{ + Vector2f x{1, -current_a}; + _voltage_prediction = (x.transpose() * _RLS_est)(0, 0); + _prediction_error = voltage_v - _voltage_prediction; + const Vector2f gamma = _estimation_covariance * x / (LAMBDA + (x.transpose() * _estimation_covariance * x)(0, 0)); + const Vector2f RSL_est_temp = _RLS_est + gamma * _prediction_error; + const Matrix2f estimation_covariance_temp = (_estimation_covariance + - Matrix(gamma) * (x.transpose() * _estimation_covariance)) / LAMBDA; + const float estimation_covariance_temp_norm = + sqrtf(powf(estimation_covariance_temp(0, 0), 2.f) + + 2.f * powf(estimation_covariance_temp(1, 0), 2.f) + + powf(estimation_covariance_temp(1, 1), 2.f)); + + if (estimation_covariance_temp_norm < _estimation_covariance_norm) { // Only update if estimation improves + _RLS_est = RSL_est_temp; + _estimation_covariance = estimation_covariance_temp; + _estimation_covariance_norm = estimation_covariance_temp_norm; + _internal_resistance_estimate = + math::max(_RLS_est(1) / _params.n_cells, 0.f); // Only use positive values + + } else { // Update OCV estimate with IR estimate + _RLS_est(0) = voltage_v + _RLS_est(1) * current_a; + } + + _ocv_filter_v.update(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a); +} + +void Battery::resetInternalResistanceEstimation(const float voltage_v, const float current_a) +{ + _RLS_est(0) = voltage_v; + _RLS_est(1) = R_DEFAULT * _params.n_cells; + _estimation_covariance.setZero(); + _estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells; + _estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells; + _estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0), + 2.f) + powf(_estimation_covariance(1, 1), 2.f)); + _internal_resistance_estimate = R_DEFAULT; + _ocv_filter_v.reset(voltage_v + _internal_resistance_estimate * _params.n_cells * current_a); + + if (_params.r_internal >= 0.f) { // Use user specified internal resistance value + _cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _params.r_internal * current_a); + + } else { // Use estimated internal resistance value + _cell_voltage_filter_v.reset(voltage_v / _params.n_cells + _internal_resistance_estimate * current_a); + } } void Battery::estimateStateOfCharge() { // choose which quantity we're using for final reporting - if (_params.capacity > 0.f && _battery_initialized) { + if ((_params.capacity > 0.f) && _battery_initialized) { // if battery capacity is known, fuse voltage measurement with used capacity // The lower the voltage the more adjust the estimate with it to avoid deep discharge - const float weight_v = 3e-4f * (1 - _state_of_charge_volt_based); + const float weight_v = 3e-2f * (1 - _state_of_charge_volt_based); _state_of_charge = (1 - weight_v) * _state_of_charge + weight_v * _state_of_charge_volt_based; // directly apply current capacity slope calculated using current _state_of_charge -= _discharged_mah_loop / _params.capacity; @@ -287,17 +330,12 @@ uint16_t Battery::determineFaults() void Battery::computeScale() { - const float voltage_range = (_params.v_charged - _params.v_empty); + _scale = _params.v_charged / _cell_voltage_filter_v.getState(); - // reusing capacity calculation to get single cell voltage before drop - const float bat_v = _params.v_empty + (voltage_range * _state_of_charge_volt_based); + if (PX4_ISFINITE(_scale)) { + _scale = math::constrain(_scale, 1.f, 1.3f); // Allow at most 30% compensation - _scale = _params.v_charged / bat_v; - - if (_scale > 1.3f) { // Allow at most 30% compensation - _scale = 1.3f; - - } else if (!PX4_ISFINITE(_scale) || _scale < 1.f) { // Shouldn't ever be more than the power at full battery + } else { _scale = 1.f; } } @@ -305,19 +343,27 @@ void Battery::computeScale() float Battery::computeRemainingTime(float current_a) { float time_remaining_s = NAN; + bool reset_current_avg_filter = false; if (_vehicle_status_sub.updated()) { vehicle_status_s vehicle_status; if (_vehicle_status_sub.copy(&vehicle_status)) { _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + + if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) { + reset_current_avg_filter = true; + } + _vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); } } _flight_phase_estimation_sub.update(); - if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON) { + // reset filter if not feasible, negative or we did a VTOL transition to FW mode + if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON + || reset_current_avg_filter) { _current_average_filter_a.reset(_params.bat_avrg_current); } @@ -342,11 +388,11 @@ float Battery::computeRemainingTime(float current_a) void Battery::updateParams() { + const int n_cells = _first_parameter_update ? 0 : _params.n_cells; param_get(_param_handles.v_empty, &_params.v_empty); param_get(_param_handles.v_charged, &_params.v_charged); param_get(_param_handles.n_cells, &_params.n_cells); param_get(_param_handles.capacity, &_params.capacity); - param_get(_param_handles.v_load_drop, &_params.v_load_drop); param_get(_param_handles.r_internal, &_params.r_internal); param_get(_param_handles.source, &_params.source); param_get(_param_handles.low_thr, &_params.low_thr); @@ -354,6 +400,22 @@ void Battery::updateParams() param_get(_param_handles.emergen_thr, &_params.emergen_thr); param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current); + if (n_cells != _params.n_cells) { + _internal_resistance_initialized = false; + } + + if (!_internal_resistance_initialized && _params.n_cells > 0) { + _RLS_est(0) = OCV_DEFAULT * _params.n_cells; + _RLS_est(1) = R_DEFAULT * _params.n_cells; + _estimation_covariance(0, 0) = OCV_COVARIANCE * _params.n_cells; + _estimation_covariance(0, 1) = 0.f; + _estimation_covariance(1, 0) = 0.f; + _estimation_covariance(1, 1) = R_COVARIANCE * _params.n_cells; + _estimation_covariance_norm = sqrtf(powf(_estimation_covariance(0, 0), 2.f) + 2.f * powf(_estimation_covariance(1, 0), + 2.f) + powf(_estimation_covariance(1, 1), 2.f)); + _internal_resistance_initialized = true; + } + ModuleParams::updateParams(); _first_parameter_update = false; diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h index 91edbe058053..04d018960256 100644 --- a/src/lib/battery/battery.h +++ b/src/lib/battery/battery.h @@ -58,7 +58,6 @@ #include #include #include -#include /** * BatteryBase is a base class for any type of battery. @@ -118,7 +117,6 @@ class Battery : public ModuleParams param_t v_charged; param_t n_cells; param_t capacity; - param_t v_load_drop; param_t r_internal; param_t low_thr; param_t crit_thr; @@ -132,7 +130,6 @@ class Battery : public ModuleParams float v_charged; int32_t n_cells; float capacity; - float v_load_drop; float r_internal; float low_thr; float crit_thr; @@ -155,7 +152,6 @@ class Battery : public ModuleParams void computeScale(); float computeRemainingTime(float current_a); - uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::SubscriptionData _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)}; uORB::PublicationMulti _battery_status_pub{ORB_ID(battery_status)}; @@ -167,12 +163,11 @@ class Battery : public ModuleParams uint8_t _priority{0}; bool _battery_initialized{false}; float _voltage_v{0.f}; - AlphaFilter _voltage_filter_v; + AlphaFilter _ocv_filter_v; + AlphaFilter _cell_voltage_filter_v; float _current_a{-1}; - AlphaFilter _current_filter_a; AlphaFilter _current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight. - AlphaFilter _throttle_filter; float _discharged_mah{0.f}; float _discharged_mah_loop{0.f}; float _state_of_charge_volt_based{-1.f}; // [0,1] @@ -183,4 +178,20 @@ class Battery : public ModuleParams bool _armed{false}; bool _vehicle_status_is_fw{false}; hrt_abstime _last_unconnected_timestamp{0}; + + // Internal Resistance estimation + void updateInternalResistanceEstimation(const float voltage_v, const float current_a); + void resetInternalResistanceEstimation(const float voltage_v, const float current_a); + matrix::Vector2f _RLS_est; // [Open circuit voltage estimate [V], Total internal resistance estimate [Ohm]]^T + matrix::Matrix2f _estimation_covariance; + bool _internal_resistance_initialized{false}; + float _estimation_covariance_norm{0.f}; + float _internal_resistance_estimate{0.005f}; // [Ohm] Per cell estimate of the internal resistance + float _voltage_prediction{0.f}; // [V] Predicted voltage of the estimator + float _prediction_error{0.f}; // [V] Error between the predicted and measured voltage + static constexpr float LAMBDA = 0.95f; // [0, 1] Forgetting factor (Tuning parameter for the RLS algorithm) + static constexpr float R_DEFAULT = 0.005f; // [Ohm] Initial per cell estimate of the internal resistance + static constexpr float OCV_DEFAULT = 4.2f; // [V] Initial per cell estimate of the open circuit voltage + static constexpr float R_COVARIANCE = 0.1f; // Initial per cell covariance of the internal resistance + static constexpr float OCV_COVARIANCE = 1.5f; // Initial per cell covariance of the open circuit voltage }; diff --git a/src/lib/battery/battery_params_common.c b/src/lib/battery/battery_params_common.c deleted file mode 100644 index b98c4cff2e9b..000000000000 --- a/src/lib/battery/battery_params_common.c +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 battery_params.c - * - * Parameters defined by the battery lib, shared between all batteries. - * - * @author Julian Oes - */ - -/** - * Low threshold - * - * Sets the threshold when the battery will be reported as low. - * This has to be higher than the critical threshold. - * - * @group Battery Calibration - * @unit norm - * @min 0.12 - * @max 0.5 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(BAT_LOW_THR, 0.15f); - -/** - * Critical threshold - * - * Sets the threshold when the battery will be reported as critically low. - * This has to be lower than the low threshold. This threshold commonly - * will trigger RTL. - * - * @group Battery Calibration - * @unit norm - * @min 0.05 - * @max 0.25 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(BAT_CRIT_THR, 0.07f); - -/** - * Emergency threshold - * - * Sets the threshold when the battery will be reported as dangerously low. - * This has to be lower than the critical threshold. This threshold commonly - * will trigger landing. - * - * @group Battery Calibration - * @unit norm - * @min 0.03 - * @max 0.1 - * @decimal 2 - * @increment 0.01 - */ -PARAM_DEFINE_FLOAT(BAT_EMERGEN_THR, 0.05f); - -/** - * Expected battery current in flight. - * - * This value is used to initialize the in-flight average current estimation, - * which in turn is used for estimating remaining flight time and RTL triggering. - * - * @group Battery Calibration - * @unit A - * @min 0 - * @max 500 - * @increment 0.1 - */ -PARAM_DEFINE_FLOAT(BAT_AVRG_CURRENT, 15.0f); diff --git a/src/lib/battery/int_res_est_replay.py b/src/lib/battery/int_res_est_replay.py new file mode 100644 index 000000000000..336dba474478 --- /dev/null +++ b/src/lib/battery/int_res_est_replay.py @@ -0,0 +1,208 @@ +# Test internal resistance estimator on flight logs +# run with: +# python3 int_res_est_replay.py -f -c <#batteryCells> +# -u <(optional)fullCellVoltage> -e <(optional)emptyCellVoltage> -l <(optional)forgettingFactor> -d <(optional)filterMeasurements> +# Note: Can lead to slightly different results than the online estimation due to the fact that +# the log frequency of the voltage and current are not the same as the online frequency. + +from pyulog import ULog +import matplotlib.pyplot as plt +import numpy as np +import argparse + +def getData(log, topic_name, variable_name, instance=0): + for elem in log.data_list: + if elem.name == topic_name and instance == elem.multi_id: + return elem.data[variable_name] + return np.array([]) + +def us2s(time_us): + return time_us * 1e-6 + +def getParam(log, param_name): + if param_name in log.initial_parameters: + return log.initial_parameters[param_name] + else: + print(f"Parameter {param_name} not found in log.") + return None + +def rls_update(theta, P, x, V, I, lam): + gamma = P @ x / (lam + x.T @ P @ x) + error = V - x.T @ theta + data_cov = x.T @ P @ x + theta_temp = theta + gamma * error + P_temp = (P - gamma @ x.T @ P) / lam + if (abs(np.linalg.norm(P)) < abs(np.linalg.norm(P_temp))): + theta_corr = np.array([V + theta[1] * I, theta[1]]) # Correct OCV estimation + P_corr = P + return theta_corr, P_corr, error, data_cov, 0, 0 + return theta_temp, P_temp, error, data_cov, gamma[0], gamma[1] + +def main(log_name, n_cells, full_cell, empty_cell, lam): + log = ULog(log_name) + + log_n_cells = getParam(log, 'BAT1_N_CELLS') + log_full_cell = getParam(log, 'BAT1_V_CHARGED') + log_empty_cell = getParam(log, 'BAT1_V_EMPTY') + + # Debug information + print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}") + + # Use log parameters unless overridden + if n_cells is None: + n_cells = log_n_cells + else: + print(f"Using override for n_cells: {n_cells}") + if full_cell is None: + full_cell = log_full_cell + else: + print(f"Using override for full_cell: {full_cell}") + if empty_cell is None: + empty_cell = log_empty_cell + else: + print(f"Using override for empty_cell: {empty_cell}") + + # Debug information for final parameter values + print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}") + + timestamps = us2s(getData(log, 'battery_status', 'timestamp')) + I = getData(log, 'battery_status', 'current_a') + V = getData(log, 'battery_status', 'voltage_v') + remaining = getData(log, 'battery_status', 'remaining') + + if not timestamps.size or not I.size or not V.size or not remaining.size: + print("Error: Incomplete data.") + return + + # Initializations + theta = np.array([[V[0] + 0.005 * n_cells * I[0]], [0.005 * n_cells]]) # Initial VOC and R + P = np.diag([1.2 * n_cells, 0.1 * n_cells]) # Initial covariance + error = 0 + + # For plotting + VOC_est = np.zeros_like(I) + R_est = np.zeros_like(I) + error_hist = np.zeros_like(I) + v_est = np.zeros_like(I) + internal_resistance_stable = np.zeros_like(I) + internal_resistance_stable[-1] = 0.005 + cov_norm = np.zeros_like(I) + r_cov = np.zeros_like(I) + ocv_cov = np.zeros_like(I) + mixed_cov = np.zeros_like(I) + data_cov_hist = np.zeros_like(I) + gamma_voc_hist = np.zeros_like(I) + gamma_r_hist = np.zeros_like(I) + + for index in range(len(I)): + # RLS algorithm + x = np.array([[1.0], [-I[index]]]) # Input vector + theta, P, error, data_cov, gamma_voc_hist[index], gamma_r_hist[index] = rls_update(theta, P, x, V[index], I[index], lam) # Run RLS + + # For plotting + VOC_est[index] = theta[0][0] + R_est[index] = theta[1][0] + error_hist[index] = error + v_est[index] = x.T @ theta + cov_norm[index] = abs(np.linalg.norm(P)) + ocv_cov[index] = P[0][0] + r_cov[index] = P[1][1] + mixed_cov[index] = P[0][1] + data_cov_hist[index] = data_cov + internal_resistance_stable[index] = max(R_est[index]/n_cells, 0.001) + + # Plot data + print("Internal Resistance mean (per cell): ", np.mean(R_est) / n_cells) + + # Summary plot + sumFig = plt.figure("Battery Estimation with RLS") + + volt = plt.subplot(2, 3, 1) + volt.plot(timestamps, V, label='Measured voltage') + volt.plot(timestamps, v_est, label='Estimated voltage') + volt.plot(timestamps, np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, label='OCV estimate') + ocv_smoothed = np.convolve(np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, np.ones(30)/30, mode='full')[0:np.size(timestamps)] + ocv_smoothed[0:30] = ocv_smoothed[31] + volt.plot(timestamps, ocv_smoothed, label='OCV estimate smoothed') + volt.plot(timestamps, np.full_like(I, full_cell * n_cells), label='100% SOC') + volt.set_title("Measured Voltage vs Estimated voltage vs Estimated Open circuit voltage [V]") + volt.legend() + + intR = plt.subplot(2, 3, 2) + intR.plot(timestamps, np.array(R_est) * 1000 / n_cells, label='Internal resistance estimate') + intR.set_title("Internal resistance estimate (per cell) [mOhm]") + intR.legend() + + soc = plt.subplot(2, 3, 3) + soc.plot(timestamps, remaining, label='SoC logged') + soc.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator') + soc.plot(timestamps, np.interp(ocv_smoothed / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator smoothed') + soc.set_title("State of charge") + soc.legend() + + curr = plt.subplot(2, 3, 4) + curr.plot(timestamps, I, label='Measured current') + curr.set_title("Measured Current [A]") + curr.legend() + + err = plt.subplot(2, 3, 5) + err.plot(timestamps, error_hist, label='$Error$') + err.set_title("Voltage estimation error [V]") + err.legend() + + cov = plt.subplot(2, 3, 6) + cov.plot(timestamps, cov_norm, label = 'Covariance norm') + cov.set_title("Covariance norm") + cov.legend() + + # # SoC estimation plots + # socFig = plt.figure("SoC estimation") + # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.005 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with default $R_{int}$') + # plt.plot(timestamps, remaining, label='SoC logged') + # plt.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator') + # # plt.plot(timestamps, np.convolve(np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), np.ones(500)/500, mode='full')[0:np.size(timestamps)], label='SoC with estimator smoothed') + # # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.0009 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with $R_{int}$ measured beforehand') + # # plt.plot(timestamps, np.interp(VOC_est/n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with VOC estimate') + # plt.legend() + + # # Covariance plots + # covFig = plt.figure("Covariance plots") + # covR = plt.subplot(2, 2, 1) + # covR.plot(timestamps, r_cov, label = 'r_cov') + # covR.set_title("Internal resistance covariance") + # covR.legend() + # covVOC = plt.subplot(2, 2, 2) + # covVOC.plot(timestamps, ocv_cov, label = 'ocv_cov') + # covVOC.set_title("Open circuit covariance") + # covVOC.legend() + # covM = plt.subplot(2, 2, 3) + # covM.plot(timestamps, mixed_cov, label = 'mixed_cov') + # covM.set_title("Mixed covariance") + # covM.legend() + # covM = plt.subplot(2, 2, 4) + # covM.plot(timestamps, cov_norm, label = 'cov_norm') + # covM.set_title("Covariance norm") + # covM.legend() + + # # Gain plots + # gainFig = plt.figure("Gain plots") + # gainVoc = plt.subplot(1, 2, 1) + # gainVoc.plot(timestamps, gamma_voc_hist, label = 'gain_voc') + # gainVoc.set_title("Gain VOC") + # gainVoc.legend() + # gainR = plt.subplot(1, 2, 2) + # gainR.plot(timestamps, gamma_r_hist, label = 'gain_r') + # gainR.set_title("Gain R") + # gainR.legend() + + plt.show() + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.') + parser.add_argument('-f', type = str, required = True, help = 'Full path to ulog file') + parser.add_argument('-c', type = float, required = False, help = 'Number of cells in battery') + parser.add_argument('-u', type = float, required = False, default = None, help = 'Full cell voltage') + parser.add_argument('-e', type = float, required = False, default = None, help = 'Empty cell voltage') + parser.add_argument('-l', type = float, required = False, default = 0.99, help = 'Forgetting factor') + args = parser.parse_args() + main(args.f, args.c, args.u, args.e, args.l) diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index 44f379002870..b20900fa08d4 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -7,12 +7,12 @@ parameters: definitions: BAT${i}_V_EMPTY: description: - short: Empty cell voltage (5C load) + short: Empty cell voltage long: | - Defines the voltage where a single cell of battery 1 is considered empty. - The voltage should be chosen before the steep dropoff to 2.8V. A typical - lithium battery can only be discharged down to 10% before it drops off - to a voltage level damaging the cells. + Defines the voltage where a single cell of the battery is considered empty. + The voltage should be chosen above the steep dropoff at 3.5V. A typical + lithium battery can only be discharged under high load down to 10% before + it drops off to a voltage level damaging the cells. type: float unit: V @@ -25,10 +25,10 @@ parameters: BAT${i}_V_CHARGED: description: - short: Full cell voltage (5C load) + short: Full cell voltage long: | - Defines the voltage where a single cell of battery 1 is considered full - under a mild load. This will never be the nominal voltage of 4.2V + Defines the voltage where a single cell of the battery is considered full. + For a more accurate estimate set this below the nominal voltage of e.g. 4.2V type: float unit: V @@ -39,33 +39,11 @@ parameters: instance_start: 1 default: [4.05, 4.05, 4.05] - BAT${i}_V_LOAD_DROP: - description: - short: Voltage drop per cell on full throttle - long: | - This implicitly defines the internal resistance - to maximum current ratio for battery 1 and assumes linearity. - A good value to use is the difference between the - 5C and 20-25C load. Not used if BAT${i}_R_INTERNAL is - set. - - type: float - unit: V - min: 0.07 - max: 0.5 - decimal: 2 - increment: 0.01 - reboot_required: true - num_instances: *max_num_config_instances - instance_start: 1 - default: [0.1, 0.1, 0.1] - BAT${i}_R_INTERNAL: description: short: Explicitly defines the per cell internal resistance for battery ${i} long: | - If non-negative, then this will be used in place of - BAT${i}_V_LOAD_DROP for all calculations. + If non-negative, then this will be used instead of the online estimated internal resistance. type: float unit: Ohm @@ -76,7 +54,7 @@ parameters: reboot_required: true num_instances: *max_num_config_instances instance_start: 1 - default: [0.005, 0.005, 0.005] + default: [-1.0, -1.0, -1.0] BAT${i}_N_CELLS: description: @@ -103,7 +81,6 @@ parameters: 14: 14S Battery 15: 15S Battery 16: 16S Battery - reboot_required: true num_instances: *max_num_config_instances instance_start: 1 default: [0, 0, 0] @@ -143,3 +120,61 @@ parameters: num_instances: *max_num_config_instances instance_start: 1 default: [0, -1, -1] + + BAT_LOW_THR: + description: + short: Low threshold. + long: | + Sets the threshold when the battery will be reported as low. + This has to be higher than the critical threshold. + type: float + unit: norm + min: 0.12 + max: 0.5 + decimal: 2 + increment: 0.01 + default: 0.15 + + BAT_CRIT_THR: + description: + short: Critical threshold. + long: | + Sets the threshold when the battery will be reported as critically low. + This has to be lower than the low threshold. This threshold commonly + will trigger RTL. + type: float + unit: norm + min: 0.05 + max: 0.5 + decimal: 2 + increment: 0.01 + default: 0.07 + + BAT_EMERGEN_THR: + description: + short: Emergency threshold. + long: | + Sets the threshold when the battery will be reported as dangerously low. + This has to be lower than the critical threshold. This threshold commonly + will trigger landing. + type: float + unit: norm + min: 0.03 + max: 0.5 + decimal: 2 + increment: 0.01 + default: 0.05 + + BAT_AVRG_CURRENT: + description: + short: Expected battery current in flight. + long: | + This value is used to initialize the in-flight average current estimation, + which in turn is used for estimating remaining flight time and RTL triggering. + type: float + unit: A + min: 0 + max: 500 + decimal: 2 + increment: 0.1 + default: 15 diff --git a/src/lib/button/ButtonPublisher.cpp b/src/lib/button/ButtonPublisher.cpp index b3b35a7b3af1..826e5ed49f9a 100644 --- a/src/lib/button/ButtonPublisher.cpp +++ b/src/lib/button/ButtonPublisher.cpp @@ -70,9 +70,9 @@ void ButtonPublisher::pairingButtonTriggerEvent() led_control_s led_control{}; led_control.led_mask = 0xff; led_control.mode = led_control_s::MODE_BLINK_FAST; - led_control.color = led_control_s::COLOR_GREEN; - led_control.num_blinks = 1; - led_control.priority = 0; + led_control.color = led_control_s::COLOR_WHITE; + led_control.num_blinks = 20; + led_control.priority = 2; led_control.timestamp = hrt_absolute_time(); _led_control_pub.publish(led_control); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 9994745a0216..f09120a14578 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -179,5 +179,5 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample) void PX4Accelerometer::UpdateClipLimit() { // 99.9% of potential max - _clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX); + _clip_limit = fabsf(_range / _scale * 0.999f); } diff --git a/src/lib/drivers/device/qurt/uart.c b/src/lib/drivers/device/qurt/uart.c index 798d20534443..ca1d0f66c740 100644 --- a/src/lib/drivers/device/qurt/uart.c +++ b/src/lib/drivers/device/qurt/uart.c @@ -25,19 +25,32 @@ void configure_uart_callbacks(open_uart_func_t open_func, } } -int qurt_uart_open(const char *dev, speed_t speed) +int qurt_uart_get_port(const char *dev) { - if (_callbacks_configured) { + if (dev != NULL) { // Convert device string into a uart port number char *endptr = NULL; - uint8_t port_number = (uint8_t) strtol(dev, &endptr, 10); + int port_number = strtol(dev, &endptr, 10); if ((port_number == 0) && (endptr == dev)) { PX4_ERR("Could not convert %s into a valid uart port number", dev); return -1; + + } else { + return port_number; } + } + + return -1; +} + +int qurt_uart_open(const char *dev, speed_t speed) +{ + int port_number = qurt_uart_get_port(dev); + + if (_callbacks_configured && (port_number >= 0)) { - return _open_uart(port_number, speed); + return _open_uart((uint8_t) port_number, speed); } else { PX4_ERR("Cannot open uart until callbacks have been configured"); diff --git a/src/lib/drivers/device/qurt/uart.h b/src/lib/drivers/device/qurt/uart.h index 03055961898f..d1632fe6a2c8 100644 --- a/src/lib/drivers/device/qurt/uart.h +++ b/src/lib/drivers/device/qurt/uart.h @@ -7,6 +7,7 @@ extern "C" { #include #include +int qurt_uart_get_port(const char *dev); int qurt_uart_open(const char *dev, speed_t speed); int qurt_uart_write(int fd, const char *buf, size_t len); int qurt_uart_read(int fd, char *buf, size_t len, uint32_t timeout_us); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index fdce2004a185..687747d4aa92 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -178,5 +178,5 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample) void PX4Gyroscope::UpdateClipLimit() { // 99.9% of potential max - _clip_limit = math::constrain((_range / _scale) * 0.999f, 0.f, (float)INT16_MAX); + _clip_limit = fabsf(_range / _scale * 0.999f); } diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp index fcb8c614ceb8..dc421b1ca7fe 100644 --- a/src/lib/drivers/smbus_sbs/SBS.hpp +++ b/src/lib/drivers/smbus_sbs/SBS.hpp @@ -261,13 +261,11 @@ int SMBUS_SBS_BaseClass::populate_smbus_data(battery_status_s &data) // Convert millivolts to volts. data.voltage_v = ((float)result) * 0.001f; - data.voltage_filtered_v = data.voltage_v; // Read current. ret |= _interface->read_word(BATT_SMBUS_CURRENT, result); data.current_a = (-1.0f * ((float)(*(int16_t *)&result)) * 0.001f); - data.current_filtered_a = data.current_a; // Read remaining capacity. ret |= _interface->read_word(BATT_SMBUS_RELATIVE_SOC, result); diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index 1b23bcfab4c5..1a0817c8adbb 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -364,7 +364,7 @@ }, "1": { "name": "manual_control_loss", - "description": "manual control loss" + "description": "Manual control loss" }, "2": { "name": "gcs_connection_loss", @@ -372,19 +372,19 @@ }, "3": { "name": "low_battery_level", - "description": "low battery level" + "description": "Low battery level" }, "4": { "name": "critical_battery_level", - "description": "critical battery level" + "description": "Critical battery level" }, "5": { "name": "emergency_battery_level", - "description": "emergency battery level" + "description": "Emergency battery level" }, "6": { "name": "low_remaining_flight_time", - "description": "low remaining flight time" + "description": "Remaining flight time low" } } }, @@ -402,19 +402,19 @@ }, "2": { "name": "fallback_posctrl", - "description": "fallback to position control" + "description": "Position mode" }, "3": { "name": "fallback_altctrl", - "description": "fallback to altitude control" + "description": "Altitude mode" }, "4": { "name": "fallback_stabilized", - "description": "fallback to stabilized" + "description": "Stabilized mode" }, "5": { "name": "hold", - "description": "hold" + "description": "Hold" }, "6": { "name": "rtl", @@ -422,11 +422,11 @@ }, "7": { "name": "land", - "description": "land" + "description": "Land" }, "8": { "name": "descend", - "description": "descend" + "description": "Descend" }, "9": { "name": "disarm", @@ -631,7 +631,7 @@ "description": "Battery reported over-current" }, "4": { - "name": "fault_temperature", + "name": "over_temperature", "description": "Battery has reached a critical temperature which can result in a critical failure" }, "5": { @@ -655,26 +655,8 @@ "description": "Battery reported an hardware problem" }, "10": { - "name": "over_temperature", - "description": "Battery is over-heating" - } - } - }, - "battery_mode_t": { - "type": "uint8_t", - "description": "Smart battery modes of operation", - "entries": { - "0": { - "name": "unknown", - "description": "unknown" - }, - "1": { - "name": "autodischarging", - "description": "auto discharging towards storage level" - }, - "2": { - "name": "hotswap", - "description": "hot-swap" + "name": "failed_to_arm", + "description": "Battery failed to arm" } } }, diff --git a/src/lib/events/events.cpp b/src/lib/events/events.cpp index 9bb215d3561f..2cac17c3f1e5 100644 --- a/src/lib/events/events.cpp +++ b/src/lib/events/events.cpp @@ -45,7 +45,7 @@ static uint16_t event_sequence{events::initial_event_sequence}; namespace events { -void send(EventType &event) +void send(event_s &event) { event.timestamp = hrt_absolute_time(); diff --git a/src/lib/events/events_ioctl.h b/src/lib/events/events_ioctl.h index 7b97c66aa41a..e3b6b0e07461 100644 --- a/src/lib/events/events_ioctl.h +++ b/src/lib/events/events_ioctl.h @@ -47,5 +47,5 @@ #define EVENTSIOCSEND _EVENTSIOC(1) typedef struct eventiocsend { - events::EventType &event; + event_s &event; } eventiocsend_t; diff --git a/src/lib/events/libevents b/src/lib/events/libevents index 59f7f5c0ec2e..9474657606d1 160000 --- a/src/lib/events/libevents +++ b/src/lib/events/libevents @@ -1 +1 @@ -Subproject commit 59f7f5c0ec2e76fadbc1dc40cc0705d614472edc +Subproject commit 9474657606d13301d426e044450c4f84de2221be diff --git a/src/lib/events/usr_events.cpp b/src/lib/events/usr_events.cpp index a2f076d06555..e989437a7734 100644 --- a/src/lib/events/usr_events.cpp +++ b/src/lib/events/usr_events.cpp @@ -44,7 +44,7 @@ namespace events { -void send(EventType &event) +void send(event_s &event) { eventiocsend_t data = {event}; boardctl(EVENTSIOCSEND, reinterpret_cast(&data)); diff --git a/src/lib/fw_performance_model/PerformanceModel.hpp b/src/lib/fw_performance_model/PerformanceModel.hpp index c283fd4d01ab..fbdd8693c21d 100644 --- a/src/lib/fw_performance_model/PerformanceModel.hpp +++ b/src/lib/fw_performance_model/PerformanceModel.hpp @@ -131,7 +131,6 @@ class PerformanceModel : public ModuleParams (ParamFloat) _param_weight_gross, (ParamFloat) _param_service_ceiling, (ParamFloat) _param_fw_thr_trim, - (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_max, (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_aspd_min, diff --git a/src/lib/heatshrink/CMakeLists.txt b/src/lib/heatshrink/CMakeLists.txt index d1ed75f06188..bd07dc825409 100644 --- a/src/lib/heatshrink/CMakeLists.txt +++ b/src/lib/heatshrink/CMakeLists.txt @@ -40,4 +40,3 @@ px4_add_library(heatshrink target_compile_options(heatshrink PRIVATE ${MAX_CUSTOM_OPT_LEVEL} -DHEATSHRINK_DYNAMIC_ALLOC=0) - diff --git a/src/lib/heatshrink/heatshrink_encode.py b/src/lib/heatshrink/heatshrink_encode.py index dc2cec88940e..01beb02c7a58 100644 --- a/src/lib/heatshrink/heatshrink_encode.py +++ b/src/lib/heatshrink/heatshrink_encode.py @@ -419,4 +419,3 @@ def encode(data, window_size, lookahead_size): if sunk == in_size: heatshrink_encoder_finish(hse) return ret - diff --git a/src/lib/mathlib/CMakeLists.txt b/src/lib/mathlib/CMakeLists.txt index aad1d3446c51..7f3a3d534e0a 100644 --- a/src/lib/mathlib/CMakeLists.txt +++ b/src/lib/mathlib/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_library(mathlib math/test/test.cpp + math/filter/FilteredDerivative.hpp math/filter/LowPassFilter2p.hpp math/filter/MedianFilter.hpp math/filter/NotchFilter.hpp diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp index 7fb54442d9b4..77cdb33c3475 100644 --- a/src/lib/mathlib/math/Functions.hpp +++ b/src/lib/mathlib/math/Functions.hpp @@ -290,6 +290,11 @@ inline bool isFinite(const float &value) return PX4_ISFINITE(value); } +inline bool isFinite(const matrix::Vector2f &value) +{ + return value.isAllFinite(); +} + inline bool isFinite(const matrix::Vector3f &value) { return value.isAllFinite(); diff --git a/src/lib/mathlib/math/Utilities.hpp b/src/lib/mathlib/math/Utilities.hpp index 9b8f7047c422..890346b9733c 100644 --- a/src/lib/mathlib/math/Utilities.hpp +++ b/src/lib/mathlib/math/Utilities.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 @@ -51,17 +51,18 @@ static constexpr float sq(float var) { return var * var; } // rot312(1) - Second rotation is a RH rotation about the X axis (rad) // rot312(2) - Third rotation is a RH rotation about the Y axis (rad) // See http://www.atacolorado.com/eulersequences.doc -inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) +template +inline matrix::Dcm taitBryan312ToRotMat(const matrix::Vector3 &rot312) { // Calculate the frame2 to frame 1 rotation matrix from a 312 Tait-Bryan rotation sequence - const float c2 = cosf(rot312(2)); // third rotation is pitch - const float s2 = sinf(rot312(2)); - const float s1 = sinf(rot312(1)); // second rotation is roll - const float c1 = cosf(rot312(1)); - const float s0 = sinf(rot312(0)); // first rotation is yaw - const float c0 = cosf(rot312(0)); - - matrix::Dcmf R; + const T c2 = cosf(rot312(2)); // third rotation is pitch + const T s2 = sinf(rot312(2)); + const T s1 = sinf(rot312(1)); // second rotation is roll + const T c1 = cosf(rot312(1)); + const T s0 = sinf(rot312(0)); // first rotation is yaw + const T c0 = cosf(rot312(0)); + + matrix::Dcm R; R(0, 0) = c0 * c2 - s0 * s1 * s2; R(1, 1) = c0 * c1; R(2, 2) = c2 * c1; @@ -75,20 +76,21 @@ inline matrix::Dcmf taitBryan312ToRotMat(const matrix::Vector3f &rot312) return R; } -inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) +template +inline matrix::Dcm quatToInverseRotMat(const matrix::Quaternion &quat) { - const float q00 = quat(0) * quat(0); - const float q11 = quat(1) * quat(1); - const float q22 = quat(2) * quat(2); - const float q33 = quat(3) * quat(3); - const float q01 = quat(0) * quat(1); - const float q02 = quat(0) * quat(2); - const float q03 = quat(0) * quat(3); - const float q12 = quat(1) * quat(2); - const float q13 = quat(1) * quat(3); - const float q23 = quat(2) * quat(3); - - matrix::Dcmf dcm; + const T q00 = quat(0) * quat(0); + const T q11 = quat(1) * quat(1); + const T q22 = quat(2) * quat(2); + const T q33 = quat(3) * quat(3); + const T q01 = quat(0) * quat(1); + const T q02 = quat(0) * quat(2); + const T q03 = quat(0) * quat(3); + const T q12 = quat(1) * quat(2); + const T q13 = quat(1) * quat(3); + const T q23 = quat(2) * quat(3); + + matrix::Dcm dcm; dcm(0, 0) = q00 + q11 - q22 - q33; dcm(1, 1) = q00 - q11 + q22 - q33; dcm(2, 2) = q00 - q11 - q22 + q33; @@ -105,40 +107,46 @@ inline matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat) // We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence // when there is more roll than pitch tilt and a 3-1-2 rotation sequence // when there is more pitch than roll tilt to avoid gimbal lock. -inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) +template +inline bool shouldUse321RotationSequence(const matrix::Dcm &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); } -inline float getEuler321Yaw(const matrix::Dcmf &R) +template +inline float getEuler321Yaw(const matrix::Dcm &R) { return atan2f(R(1, 0), R(0, 0)); } -inline float getEuler312Yaw(const matrix::Dcmf &R) +template +inline float getEuler312Yaw(const matrix::Dcm &R) { return atan2f(-R(0, 1), R(1, 1)); } -inline float getEuler321Yaw(const matrix::Quatf &q) +template +inline T getEuler321Yaw(const matrix::Quaternion &q) { // Values from yaw_input_321.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw321.m - const float a = 2.f * (q(0) * q(3) + q(1) * q(2)); - const float b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) + q(1) * q(2)); + const T b = sq(q(0)) + sq(q(1)) - sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEuler312Yaw(const matrix::Quatf &q) +template +inline T getEuler312Yaw(const matrix::Quaternion &q) { // Values from yaw_input_312.c file produced by // https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/quat2yaw312.m - const float a = 2.f * (q(0) * q(3) - q(1) * q(2)); - const float b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); + const T a = static_cast(2.) * (q(0) * q(3) - q(1) * q(2)); + const T b = sq(q(0)) - sq(q(1)) + sq(q(2)) - sq(q(3)); return atan2f(a, b); } -inline float getEulerYaw(const matrix::Dcmf &R) +template +inline T getEulerYaw(const matrix::Dcm &R) { if (shouldUse321RotationSequence(R)) { return getEuler321Yaw(R); @@ -148,23 +156,32 @@ inline float getEulerYaw(const matrix::Dcmf &R) } } -inline matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline T getEulerYaw(const matrix::Quaternion &q) { - matrix::Eulerf euler321(rot_in); + return getEulerYaw(matrix::Dcm(q)); +} + +template +inline matrix::Dcm updateEuler321YawInRotMat(T yaw, const matrix::Dcm &rot_in) +{ + matrix::Euler euler321(rot_in); euler321(2) = yaw; - return matrix::Dcmf(euler321); + return matrix::Dcm(euler321); } -inline matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateEuler312YawInRotMat(T yaw, const matrix::Dcm &rot_in) { - const matrix::Vector3f rotVec312(yaw, // yaw - asinf(rot_in(2, 1)), // roll - atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch + const matrix::Vector3 rotVec312(yaw, // yaw + asinf(rot_in(2, 1)), // roll + atan2f(-rot_in(2, 0), rot_in(2, 2))); // pitch return taitBryan312ToRotMat(rotVec312); } // Checks which euler rotation sequence to use and update yaw in rotation matrix -inline matrix::Dcmf updateYawInRotMat(float yaw, const matrix::Dcmf &rot_in) +template +inline matrix::Dcm updateYawInRotMat(T yaw, const matrix::Dcm &rot_in) { if (shouldUse321RotationSequence(rot_in)) { return updateEuler321YawInRotMat(yaw, rot_in); diff --git a/src/lib/mathlib/math/filter/FilteredDerivative.hpp b/src/lib/mathlib/math/filter/FilteredDerivative.hpp new file mode 100644 index 000000000000..f0099334d2e7 --- /dev/null +++ b/src/lib/mathlib/math/filter/FilteredDerivative.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 FilteredDerivative.hpp + * + * @brief Derivative function passed through a first order "alpha" IIR digital filter + * + * @author Silvan Fuhrer + */ + +#pragma once + +// #include +// #include +#include + +using namespace math; + +template +class FilteredDerivative +{ +public: + FilteredDerivative() = default; + ~FilteredDerivative() = default; + + /** + * Set filter parameters for time abstraction + * + * Both parameters have to be provided in the same units. + * + * @param sample_interval interval between two samples + * @param time_constant filter time constant determining convergence + */ + void setParameters(float sample_interval, float time_constant) + { + _alpha_filter.setParameters(sample_interval, time_constant); + _sample_interval = sample_interval; + } + + /** + * Set filter state to an initial value + * + * @param sample new initial value + */ + void reset(const T &sample) + { + _alpha_filter.reset(sample); + _initialized = false; + } + + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ + const T &update(const T &sample) + { + if (_initialized) { + if (_sample_interval > FLT_EPSILON) { + _alpha_filter.update((sample - _previous_sample) / _sample_interval); + + } else { + _initialized = false; + } + + } else { + // don't update in the first iteration + _initialized = true; + } + + _previous_sample = sample; + return _alpha_filter.getState(); + } + + const T &getState() const { return _alpha_filter.getState(); } + + +private: + AlphaFilter _alpha_filter; + float _sample_interval{0.f}; + T _previous_sample{0.f}; + bool _initialized{false}; +}; diff --git a/src/lib/matrix/matrix/Dual.hpp b/src/lib/matrix/matrix/Dual.hpp index e7eafa033129..43f1d2abd3e0 100644 --- a/src/lib/matrix/matrix/Dual.hpp +++ b/src/lib/matrix/matrix/Dual.hpp @@ -375,4 +375,3 @@ OStream &operator<<(OStream &os, const matrix::Dual &dual) } } - diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index 14400683f0ef..9e60b9ebef38 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -402,6 +402,7 @@ class Matrix } const Matrix &self = *this; + bool is_prev_symmetric = true; // assume symmetric until one element is not for (unsigned i = 0; i < M; i++) { printf("%2u|", i); // print row numbering @@ -410,7 +411,7 @@ class Matrix double d = static_cast(self(i, j)); // if symmetric don't print upper triangular elements - if ((M == N) && (j > i) && (i < N) && (j < M) + if (is_prev_symmetric && (M == N) && (j > i) && (i < N) && (j < M) && (fabs(d - static_cast(self(j, i))) < (double)eps) ) { // print empty space @@ -428,6 +429,8 @@ class Matrix } else { printf("% 6.5f ", d); } + + is_prev_symmetric = false; // not symmetric if once inside here } } diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 76cb9a3ebc47..0cd20bc5bc4e 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -618,6 +618,7 @@ SquareMatrix choleskyInv(const SquareMatrix &A) return L_inv.T() * L_inv; } +using Matrix2f = SquareMatrix; using Matrix3f = SquareMatrix; using Matrix3d = SquareMatrix; diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp index 3b5bbadca8a7..3376445fdaec 100644 --- a/src/lib/matrix/matrix/Vector2.hpp +++ b/src/lib/matrix/matrix/Vector2.hpp @@ -56,6 +56,45 @@ class Vector2 : public Vector return a(0) * b(1, 0) - a(1) * b(0, 0); } + /** + * Override matrix ops so Vector2 type is returned + */ + + Vector2 operator+(Vector2 other) const + { + return Matrix21::operator+(other); + } + + Vector2 operator+(Type scalar) const + { + return Matrix21::operator+(scalar); + } + + Vector2 operator-(Vector2 other) const + { + return Matrix21::operator-(other); + } + + Vector2 operator-(Type scalar) const + { + return Matrix21::operator-(scalar); + } + + Vector2 operator-() const + { + return Matrix21::operator-(); + } + + Vector2 operator*(Type scalar) const + { + return Matrix21::operator*(scalar); + } + + Type operator*(Vector2 b) const + { + return Vector::operator*(b); + } + Type operator%(const Matrix21 &b) const { return (*this).cross(b); diff --git a/src/lib/matrix/matrix/Vector4.hpp b/src/lib/matrix/matrix/Vector4.hpp index 4eba1de6394f..5115d420e35f 100644 --- a/src/lib/matrix/matrix/Vector4.hpp +++ b/src/lib/matrix/matrix/Vector4.hpp @@ -82,6 +82,46 @@ class Vector4 : public Vector Vector4(const Slice &slice_in) : Vector(slice_in) { } + + /** + * Override matrix ops so Vector4 type is returned + */ + + Vector4 operator+(Vector4 other) const + { + return Matrix41::operator+(other); + } + + Vector4 operator+(Type scalar) const + { + return Matrix41::operator+(scalar); + } + + Vector4 operator-(Vector4 other) const + { + return Matrix41::operator-(other); + } + + Vector4 operator-(Type scalar) const + { + return Matrix41::operator-(scalar); + } + + Vector4 operator-() const + { + return Matrix41::operator-(); + } + + Vector4 operator*(Type scalar) const + { + return Matrix41::operator*(scalar); + } + + Type operator*(Vector4 b) const + { + return Vector::operator*(b); + } + }; using Vector4f = Vector4; diff --git a/src/lib/metadata/CMakeLists.txt b/src/lib/metadata/CMakeLists.txt index a333722507cd..28f50c214af3 100644 --- a/src/lib/metadata/CMakeLists.txt +++ b/src/lib/metadata/CMakeLists.txt @@ -58,4 +58,3 @@ add_custom_command(OUTPUT ${generated_actuators_metadata_file} COMMENT "Generating actuators.json" ) add_custom_target(actuators_json DEPENDS ${generated_actuators_metadata_file}) - diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp index 38f7d096a94e..171c28a81750 100644 --- a/src/lib/mixer_module/functions/FunctionManualRC.hpp +++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp @@ -45,9 +45,7 @@ class FunctionManualRC : public FunctionProviderBase public: FunctionManualRC() { - for (int i = 0; i < num_data_points; ++i) { - _data[i] = NAN; - } + resetAllToDisarmedValue(); } static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); } @@ -57,17 +55,22 @@ class FunctionManualRC : public FunctionProviderBase manual_control_setpoint_s manual_control_setpoint; if (_topic.update(&manual_control_setpoint)) { - _data[0] = manual_control_setpoint.roll; - _data[1] = manual_control_setpoint.pitch; - _data[2] = manual_control_setpoint.throttle; - _data[3] = manual_control_setpoint.yaw; - _data[4] = manual_control_setpoint.flaps; - _data[5] = manual_control_setpoint.aux1; - _data[6] = manual_control_setpoint.aux2; - _data[7] = manual_control_setpoint.aux3; - _data[8] = manual_control_setpoint.aux4; - _data[9] = manual_control_setpoint.aux5; - _data[10] = manual_control_setpoint.aux6; + if (manual_control_setpoint.valid) { + _data[0] = manual_control_setpoint.roll; + _data[1] = manual_control_setpoint.pitch; + _data[2] = manual_control_setpoint.throttle; + _data[3] = manual_control_setpoint.yaw; + _data[4] = manual_control_setpoint.flaps; + _data[5] = manual_control_setpoint.aux1; + _data[6] = manual_control_setpoint.aux2; + _data[7] = manual_control_setpoint.aux3; + _data[8] = manual_control_setpoint.aux4; + _data[9] = manual_control_setpoint.aux5; + _data[10] = manual_control_setpoint.aux6; + + } else { + resetAllToDisarmedValue(); + } } } @@ -76,6 +79,13 @@ class FunctionManualRC : public FunctionProviderBase private: static constexpr int num_data_points = 11; + void resetAllToDisarmedValue() + { + for (int i = 0; i < num_data_points; ++i) { + _data[i] = NAN; + } + } + static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1, "number of functions mismatch"); diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index dc5f791af22e..ee75bcf9fa0e 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -455,7 +455,8 @@ bool MixingOutput::update() } } - if (!all_disabled) { + // Send output if any function mapped or one last disabling sample + if (!all_disabled || !_was_all_disabled) { if (!_armed.armed && !_armed.manual_lockdown) { _actuator_test.overrideValues(outputs, _max_num_outputs); } @@ -463,6 +464,8 @@ bool MixingOutput::update() limitAndUpdateOutputs(outputs, has_updates); } + _was_all_disabled = all_disabled; + return true; } diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index c791feb10333..3deaa54aa42f 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -288,6 +288,7 @@ class MixingOutput : public ModuleParams hrt_abstime _lowrate_schedule_interval{300_ms}; ActuatorTest _actuator_test{_function_assignment}; uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only) + bool _was_all_disabled{false}; uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback diff --git a/src/lib/mixer_module/mixer_module_tests.cpp b/src/lib/mixer_module/mixer_module_tests.cpp index ef48f3e3f217..3901a1c85de9 100644 --- a/src/lib/mixer_module/mixer_module_tests.cpp +++ b/src/lib/mixer_module/mixer_module_tests.cpp @@ -191,11 +191,15 @@ TEST_F(MixerModuleTest, basic) mixing_output.setAllMaxValues(MAX_VALUE); EXPECT_EQ(test_module.num_updates, 0); - // all functions disabled: not expected to get an update + // all functions disabled: expect to get one single update to process disabling the output signal mixing_output.update(); mixing_output.updateSubscriptions(false); mixing_output.update(); - EXPECT_EQ(test_module.num_updates, 0); + EXPECT_EQ(test_module.num_updates, 1); + mixing_output.update(); + mixing_output.updateSubscriptions(false); + mixing_output.update(); + EXPECT_EQ(test_module.num_updates, 1); test_module.reset(); // configure motor, ensure all still disarmed diff --git a/src/lib/parameters/autosave.cpp b/src/lib/parameters/autosave.cpp index a539ee484ba6..f06ff8df45aa 100644 --- a/src/lib/parameters/autosave.cpp +++ b/src/lib/parameters/autosave.cpp @@ -128,4 +128,3 @@ void ParamAutosave::Run() _retry_count = 0; } } - diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 93cbddcdec42..490ad02f22c3 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -73,5 +73,74 @@ param_modify_on_import_ret param_modify_on_import(bson_node_t node) } } + // 2024-04-15 SYS_MC_EST_GROUP removed + if ((node->type == bson_type_t::BSON_INT32) && (strcmp("SYS_MC_EST_GROUP", node->name) == 0)) { + + int32_t value = node->i32; + + // value 1 local_position_estimator, attitude_estimator_q (unsupported) + if (value == 1) { + // enable local_position_estimator + int32_t lpe_en_val = 1; + int lpe_en_ret = param_set(param_find("LPE_EN"), &lpe_en_val); + + // enable attitude_estimator_q + int32_t att_en_val = 1; + int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val); + + // disable ekf2 (only if enabling lpe and att_w was successful) + if (lpe_en_ret == PX4_OK && att_en_ret == PX4_OK) { + int32_t ekf2_en_val = 0; + param_set(param_find("EKF2_EN"), &ekf2_en_val); + + } else { + int32_t ekf2_en_val = 1; + param_set(param_find("EKF2_EN"), &ekf2_en_val); + } + + return param_modify_on_import_ret::PARAM_MODIFIED; + } + + // value 2 ekf2 (recommended) + if (value == 2) { + // disable local_position_estimator + int32_t lpe_en_val = 0; + param_set(param_find("LPE_EN"), &lpe_en_val); + + // disable attitude_estimator_q + int32_t att_en_val = 0; + param_set(param_find("ATT_EN"), &att_en_val); + + // enable ekf2 + int32_t ekf2_en_val = 1; + param_set(param_find("EKF2_EN"), &ekf2_en_val); + + return param_modify_on_import_ret::PARAM_MODIFIED; + } + + // value 3 Q attitude estimator (no position) + if (value == 3) { + // disable local_position_estimator + int32_t lpe_en_val = 0; + param_set(param_find("LPE_EN"), &lpe_en_val); + + // enable attitude_estimator_q + int32_t att_en_val = 1; + int att_en_ret = param_set(param_find("ATT_EN"), &att_en_val); + + // disable ekf2 (only if enabling att_w was successful) + if (att_en_ret == PX4_OK) { + int32_t ekf2_en_val = 0; + param_set(param_find("EKF2_EN"), &ekf2_en_val); + + } else { + int32_t ekf2_en_val = 1; + param_set(param_find("EKF2_EN"), &ekf2_en_val); + } + + return param_modify_on_import_ret::PARAM_MODIFIED; + } + } + return param_modify_on_import_ret::PARAM_NOT_MODIFIED; } diff --git a/src/lib/parameters/parameters_primary.cpp b/src/lib/parameters/parameters_primary.cpp index 02de11e608d0..3d61fe167a15 100644 --- a/src/lib/parameters/parameters_primary.cpp +++ b/src/lib/parameters/parameters_primary.cpp @@ -311,4 +311,4 @@ void param_primary_reset_all() void param_primary_get_counters(struct param_primary_counters *cnt) { *cnt = param_primary_counters; -} \ No newline at end of file +} diff --git a/src/lib/parameters/parameters_primary.h b/src/lib/parameters/parameters_primary.h index 7994822d4c05..2d534cc24c4f 100644 --- a/src/lib/parameters/parameters_primary.h +++ b/src/lib/parameters/parameters_primary.h @@ -49,4 +49,3 @@ void param_primary_set_value(param_t param, const void *val); void param_primary_reset(param_t param); void param_primary_reset_all(); void param_primary_get_counters(struct param_primary_counters *cnt); - diff --git a/src/lib/parameters/px4params/jsonout.py b/src/lib/parameters/px4params/jsonout.py index 45f6cf291df4..e4a8b021bd19 100644 --- a/src/lib/parameters/px4params/jsonout.py +++ b/src/lib/parameters/px4params/jsonout.py @@ -146,4 +146,3 @@ def get_typed_value(value: str, type_name: str): def Save(self, filename): with codecs.open(filename, 'w', 'utf-8') as f: f.write(self.output) - diff --git a/src/lib/parameters/px4params/markdownout.py b/src/lib/parameters/px4params/markdownout.py index cd4f5e921cc3..4c698fb5115a 100644 --- a/src/lib/parameters/px4params/markdownout.py +++ b/src/lib/parameters/px4params/markdownout.py @@ -7,48 +7,37 @@ def __init__(self, groups): result = ( """# Parameter Reference -:::note +::: info This documentation was auto-generated from the source code for this PX4 version (using `make parameters_metadata`). ::: -:::tip +::: tip If a listed parameter is missing from the Firmware see: [Finding/Updating Parameters](../advanced_config/parameters.md#parameter-not-in-firmware). ::: - - """ ) for group in groups: - result += '## %s\n\n' % group.GetName() - result += ( -""" - - - - - -""" - ) + result += f'## {group.GetName()}\n\n' + for param in group.GetParams(): - code = param.GetName() - name = param.GetFieldValue("short_desc") or '' - name = html.escape(name) + name = param.GetName() + short_desc = param.GetFieldValue("short_desc") or '' + + # Add fullstop to short_desc if not present + if short_desc: + if not short_desc.strip().endswith('.'): + short_desc += "." + long_desc = param.GetFieldValue("long_desc") or '' - long_desc = html.escape(long_desc) + + #Strip out short text from start of long text, if it ends in fullstop + if long_desc.startswith(short_desc): + long_desc = long_desc[len(short_desc):].lstrip() + min_val = param.GetFieldValue("min") or '' max_val = param.GetFieldValue("max") or '' increment = param.GetFieldValue("increment") or '' @@ -62,60 +51,43 @@ def __init__(self, groups): #field_codes = param.GetFieldCodes() ## Disabled as not needed for display. #boolean = param.GetFieldValue("boolean") # or '' # Disabled - does not appear useful. - # Format values for display. - # Display min/max/increment value based on what values are defined. - max_min_combined = '' - if min_val or max_val: - if not min_val: - min_val='?' - if not max_val: - max_val='?' - max_min_combined+=f"[{min_val}, {max_val}] " - if increment: - max_min_combined+='(%s)' % increment - - if long_desc != '': - long_desc = f"

Comment: {long_desc}

" - - if name == code: - name = "" - code=f"{code}" - - if reboot_required: - reboot_required='

Reboot required: %s

\n' % reboot_required - enum_codes=param.GetEnumCodes() or '' # Gets numerical values for parameter. enum_output='' # Format codes and their descriptions for display. if enum_codes: - enum_output+='Values:
    ' + enum_output+='**Values:**\n\n' enum_codes=sorted(enum_codes,key=float) for item in enum_codes: - enum_output+=f"\n
  • {item}: {html.escape(param.GetEnumValue(item))}
    • " - enum_output+='\n
' - + enum_output+=f"- `{item}`: {param.GetEnumValue(item)}\n" + enum_output+='\n\n' bitmask_list=param.GetBitmaskList() #Gets bitmask values for parameter bitmask_output='' #Format bitmask values if bitmask_list: - bitmask_output+='Bitmask:
    ' + bitmask_output+='**Bitmask:**\n\n' for bit in bitmask_list: bit_text = param.GetBitmaskBit(bit) - bit_text = html.escape(bit_text) - bitmask_output+=f"
  • {bit}: {bit_text}
    • \n" - bitmask_output+='
\n' + bitmask_output+=f"- `{bit}`: {bit_text}\n" + bitmask_output+='\n\n' if is_boolean and def_val=='1': def_val='Enabled (1)' if is_boolean and def_val=='0': def_val='Disabled (0)' - result += f"\n \n \n \n \n \n\n" - - #Close the table. - result += '
NameDescription[Min, Max] (Incr.)DefaultUnits
{code} ({type}){name} {long_desc} {enum_output} {bitmask_output} {reboot_required}{max_min_combined}{def_val}{unit}
\n\n' + result += f'### {name} (`{type}`)' + ' {#' + name + '}\n\n' + if short_desc: + result += f'{short_desc}\n\n' + if long_desc: + result += f'{long_desc}\n\n' + if enum_codes: + result += enum_output + if bitmask_list: + result += bitmask_output + # Format the ranges as a table. + result += f"Reboot | minValue | maxValue | increment | default | unit\n--- | --- | --- | --- | --- | ---\n{'✓' if reboot_required else ' ' } | {min_val} | {max_val} | {increment} | {def_val} | {unit} \n\n" self.output = result diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index a2fdf3705dca..8bdb10f645b1 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/src/lib/parameters/px4params/srcparser.py @@ -356,7 +356,7 @@ def Validate(self): 'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m', 'rad s/m', 'bit/s', 'B/s', 'deg', 'deg*1e7', 'deg/s', 'deg/s^2', - 'celcius', 'gauss', 'gauss/s', 'gauss^2', + 'celcius', 'gauss', 'gauss/s', 'gauss^2', 'liters', 'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3', 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad', 'g0', 'Ohm', 'V', 'A', diff --git a/src/modules/ekf2/Utility/CMakeLists.txt b/src/lib/pure_pursuit/CMakeLists.txt similarity index 83% rename from src/modules/ekf2/Utility/CMakeLists.txt rename to src/lib/pure_pursuit/CMakeLists.txt index 7a635acebe7c..19154fb5a53f 100644 --- a/src/modules/ekf2/Utility/CMakeLists.txt +++ b/src/lib/pure_pursuit/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2024 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 @@ -29,17 +29,12 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. # -############################################################################# - -px4_add_library(EKF2Utility - PreFlightChecker.cpp -) +############################################################################ -target_include_directories(EKF2Utility - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} +px4_add_library(pure_pursuit + PurePursuit.cpp + PurePursuit.hpp ) -target_link_libraries(EKF2Utility PRIVATE mathlib) - -px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility) +px4_add_functional_gtest(SRC PurePursuitTest.cpp LINKLIBS pure_pursuit) +set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/module.yaml) diff --git a/src/lib/pure_pursuit/PurePursuit.cpp b/src/lib/pure_pursuit/PurePursuit.cpp new file mode 100644 index 000000000000..324a504959ba --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuit.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "PurePursuit.hpp" +#include + + +PurePursuit::PurePursuit(ModuleParams *parent) : ModuleParams(parent) +{ + _param_handles.lookahead_gain = param_find("PP_LOOKAHD_GAIN"); + _param_handles.lookahead_max = param_find("PP_LOOKAHD_MAX"); + _param_handles.lookahead_min = param_find("PP_LOOKAHD_MIN"); + updateParams(); +} + +void PurePursuit::updateParams() +{ + param_get(_param_handles.lookahead_gain, &_params.lookahead_gain); + param_get(_param_handles.lookahead_max, &_params.lookahead_max); + param_get(_param_handles.lookahead_min, &_params.lookahead_min); + + ModuleParams::updateParams(); + +} + +float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, + const float vehicle_speed) +{ + // Check input validity + if (!curr_wp_ned.isAllFinite() || !curr_pos_ned.isAllFinite() || vehicle_speed < -FLT_EPSILON + || !PX4_ISFINITE(vehicle_speed) || !prev_wp_ned.isAllFinite()) { + return NAN; + } + + _lookahead_distance = math::constrain(_params.lookahead_gain * vehicle_speed, + _params.lookahead_min, _params.lookahead_max); + + // Pure pursuit + const Vector2f curr_pos_to_curr_wp = curr_wp_ned - curr_pos_ned; + const Vector2f prev_wp_to_curr_wp = curr_wp_ned - prev_wp_ned; + + if (curr_pos_to_curr_wp.norm() < _lookahead_distance + || prev_wp_to_curr_wp.norm() < + FLT_EPSILON) { // Target current waypoint if closer to it than lookahead or waypoints overlap + return atan2f(curr_pos_to_curr_wp(1), curr_pos_to_curr_wp(0)); + } + + const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned; + const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero(); + const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * + prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp + const Vector2f curr_pos_to_path = distance_on_line_segment - + prev_wp_to_curr_pos; // Shortest vector from the current position to the path + + if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point + return atan2f(curr_pos_to_path(1), curr_pos_to_path(0)); + } + + const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(), + 2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point + const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension * + prev_wp_to_curr_wp_u; + const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos; + return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0)); + +} diff --git a/src/lib/pure_pursuit/PurePursuit.hpp b/src/lib/pure_pursuit/PurePursuit.hpp new file mode 100644 index 000000000000..9de498c2e1b1 --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuit.hpp @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +using namespace matrix; + +/** + * @file PurePursuit.hpp + * + * Implementation of pure pursuit guidance logic. + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the idea from [1] (not including any code). + * + * [1] Coulter, R. C. (1992). Implementation of the Pure Pursuit Path Tracking Algorithm + * (Techreport CMU-RI-TR-92-01). + * + * Pure pursuit is a path following algorithm that uses the intersection between the path and + * a circle (the radius of which is referred to as lookahead distance) around the vehicle as + * the target point for the vehicle. + * The lookahead distance is defined as v * k. + * v: Vehicle ground speed [m/s] + * k: Tuning parameter + * The lookahead distance is further constrained between an upper and lower threshhold. + * C + * / + * __/__ + * / / \ + * / / \ + * | / V | + * \/ / + * /\ _____ / + * N (0 rad) / + * ^ P + * | + * | D + * (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad) + * | + * | + * ⌄ + * (+- 3.14159 rad) + * + * Input: Current/prev waypoint and the vehicle position in NED frame as well as the vehicle speed. + * Output: Calculates the intersection points as described above and returns the heading towards the point that is closer to the current waypoint. + */ +class PurePursuit : public ModuleParams +{ +public: + PurePursuit(ModuleParams *parent); + ~PurePursuit() = default; + + /** + * @brief Return heading towards the intersection point between a circle with a radius of + * vehicle_speed * PP_LOOKAHD_GAIN around the vehicle and an extended line segment from the previous to the current waypoint. + * Exceptions: + * Will return heading towards the current waypoint if it is closer to the vehicle than the lookahead or if the waypoints overlap. + * Will return heading towards the closest point on the path if there are no intersection points (crosstrack error bigger than lookahead). + * Will return NAN if input is invalid. + * @param curr_wp_ned North/East coordinates of current waypoint in NED frame [m]. + * @param prev_wp_ned North/East coordinates of previous waypoint in NED frame [m]. + * @param curr_pos_ned North/East coordinates of current position of the vehicle in NED frame [m]. + * @param vehicle_speed Vehicle speed [m/s]. + * @param PP_LOOKAHD_GAIN Tuning parameter [-] + * @param PP_LOOKAHD_MAX Maximum lookahead distance [m] + * @param PP_LOOKAHD_MIN Minimum lookahead distance [m] + */ + float calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, + float vehicle_speed); + + float getLookaheadDistance() {return _lookahead_distance;}; + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + + struct { + param_t lookahead_gain; + param_t lookahead_max; + param_t lookahead_min; + } _param_handles{}; + + struct { + float lookahead_gain{1.f}; + float lookahead_max{10.f}; + float lookahead_min{1.f}; + } _params{}; +private: + float _lookahead_distance{0.f}; +}; diff --git a/src/lib/pure_pursuit/PurePursuitTest.cpp b/src/lib/pure_pursuit/PurePursuitTest.cpp new file mode 100644 index 000000000000..c5b6897fbdaf --- /dev/null +++ b/src/lib/pure_pursuit/PurePursuitTest.cpp @@ -0,0 +1,201 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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. + * + ****************************************************************************/ + +/****************************************************************** + * Test code for the Pure Pursuit algorithm + * Run this test only using "make tests TESTFILTER=PurePursuit" + * + * Graphic interpretation: + * Legend: + * C: Current waypoint + * P: Previous waypoint + * V: Vehicle + * |: Line segment + * Orientation: + * C + * / + * __/__ + * / / \ + * / / \ + * | / V | + * \/ / + * /\ _____ / + * N (0 rad) / + * ^ P + * | + * | D + * (-1.5708 rad) <----- ⨂ -----> E (1.5708 rad) + * | + * | + * ⌄ + * (+- 3.14159 rad) + * + * NOTE: + * The tuning parameters for the pure pursuit algorithm are set to the following for all tests: + * PP_LOOKAHD_GAIN = 1.f + * PP_LOOKAHD_MAX = 10.f + * PP_LOOKAHD_MIN = 1.f + * This way passing the vehicle_speed in calcDesiredHeading function is equivalent to passing + * the lookahead distance. + * +******************************************************************/ + +#include +#include + +using namespace matrix; + +class PurePursuitTest : public ::testing::Test +{ +public: + PurePursuit pure_pursuit{nullptr}; +}; + +TEST_F(PurePursuitTest, InvalidSpeed) +{ + // V C + // / + // / + // / + // P + const Vector2f curr_wp_ned(10.f, 10.f); + const Vector2f prev_wp_ned(0.f, 0.f); + const Vector2f curr_pos_ned(10.f, 0.f); + // Negative speed + const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, -1.f); + // NaN speed + const float desired_heading2 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, NAN); + EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); + EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); +} + +TEST_F(PurePursuitTest, InvalidWaypoints) +{ + // V C + // / + // / + // / + // P + const Vector2f curr_wp_ned(10.f, 10.f); + const Vector2f prev_wp_ned(0.f, 0.f); + const Vector2f curr_pos_ned(10.f, 0.f); + const float lookahead_distance{5.f}; + // Prev WP is NAN + const float desired_heading1 = pure_pursuit.calcDesiredHeading(curr_wp_ned, Vector2f(NAN, NAN), curr_pos_ned, + lookahead_distance); + // Curr WP is NAN + const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(NAN, NAN), prev_wp_ned, curr_pos_ned, + lookahead_distance); + + // Curr Pos is NAN + const float desired_heading3 = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, Vector2f(NAN, NAN), + lookahead_distance); + EXPECT_FALSE(PX4_ISFINITE(desired_heading1)); + EXPECT_FALSE(PX4_ISFINITE(desired_heading2)); + EXPECT_FALSE(PX4_ISFINITE(desired_heading3)); +} + +TEST_F(PurePursuitTest, OutOfLookahead) +{ + const float lookahead_distance{5.f}; + // V C + // / + // / + // / + // P + const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 0.f), + lookahead_distance); + // V + // + // P ----- C + const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), + lookahead_distance); + EXPECT_NEAR(desired_heading1, M_PI_2_F + M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading2, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path +} + +TEST_F(PurePursuitTest, WaypointOverlap) +{ + const float lookahead_distance{5.f}; + // C/P + // + // + // + // V + const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(10.f, 10.f), Vector2f(10.f, 10.f), Vector2f(0.f, + 0.f), + lookahead_distance); + // V + // + // + // + // C/P + const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), + lookahead_distance); + EXPECT_NEAR(desired_heading1, M_PI_4_F, FLT_EPSILON); // Fallback: Bearing to closest point on path + EXPECT_NEAR(desired_heading2, -(M_PI_4_F + M_PI_2_F), FLT_EPSILON); // Fallback: Bearing to closest point on path +} + +TEST_F(PurePursuitTest, CurrAndPrevSameNorthCoordinate) +{ + const float lookahead_distance{5.f}; + // P -- V -- C + const float desired_heading1 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(0.f, + 10.f), + lookahead_distance); + + // V + // P ------ C + const float desired_heading2 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), + Vector2f(5.f / sqrtf(2.f), 10.f), + lookahead_distance); + // V + // C ------ P + const float desired_heading3 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 0.f), Vector2f(0.f, 20.f), + Vector2f(5.f / sqrtf(2.f), 10.f), + lookahead_distance); + // V + // + // P ------ C + const float desired_heading4 = pure_pursuit.calcDesiredHeading(Vector2f(0.f, 20.f), Vector2f(0.f, 0.f), Vector2f(10.f, + 10.f), + lookahead_distance); + + EXPECT_NEAR(desired_heading1, M_PI_2_F, FLT_EPSILON); + EXPECT_NEAR(desired_heading2, M_PI_2_F + M_PI_4_F, FLT_EPSILON); + EXPECT_NEAR(desired_heading3, -(M_PI_2_F + M_PI_4_F), FLT_EPSILON); + EXPECT_NEAR(desired_heading4, M_PI_F, FLT_EPSILON); // Fallback: Bearing to closest point on path +} diff --git a/src/lib/pure_pursuit/module.yaml b/src/lib/pure_pursuit/module.yaml new file mode 100644 index 000000000000..5e7cc109d464 --- /dev/null +++ b/src/lib/pure_pursuit/module.yaml @@ -0,0 +1,37 @@ +module_name: Pure Pursuit + +parameters: + - group: Pure Pursuit + definitions: + PP_LOOKAHD_GAIN: + description: + short: Tuning parameter for the pure pursuit controller + long: Lower value -> More aggressive controller (beware overshoot/oscillations) + type: float + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + PP_LOOKAHD_MIN: + description: + short: Minimum lookahead distance for the pure pursuit controller + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + PP_LOOKAHD_MAX: + description: + short: Maximum lookahead distance for the pure pursuit controller + type: float + unit: m + min: 0.1 + max: 100 + increment: 0.01 + decimal: 2 + default: 10 diff --git a/src/lib/rc/dsm.cpp b/src/lib/rc/dsm.cpp index d1b78a99bf69..257307a62078 100644 --- a/src/lib/rc/dsm.cpp +++ b/src/lib/rc/dsm.cpp @@ -481,6 +481,9 @@ void dsm_proto_init() channel_buffer[i].last_seen = 0; channel_buffer[i].value = 0; } + + /* reset the format detector */ + dsm_guess_format(true); } /** diff --git a/src/lib/rc/ghst.cpp b/src/lib/rc/ghst.cpp index d3c13d7ad25c..1af68e8da01c 100644 --- a/src/lib/rc/ghst.cpp +++ b/src/lib/rc/ghst.cpp @@ -428,4 +428,3 @@ bool ghst_send_telemetry_gps2_status(int uart_fd, uint16_t ground_speed, uint16_ return write(uart_fd, buf, offset) == offset; } - diff --git a/src/lib/rc/rc_tests/RCTest.cpp b/src/lib/rc/rc_tests/RCTest.cpp index 5894bf7549db..1997c19e85a3 100644 --- a/src/lib/rc/rc_tests/RCTest.cpp +++ b/src/lib/rc/rc_tests/RCTest.cpp @@ -19,7 +19,7 @@ #if defined(CONFIG_ARCH_BOARD_PX4_SITL) #define TEST_DATA_PATH "./test_data/" #else -#define TEST_DATA_PATH "/fs/microsd" +#define TEST_DATA_PATH CONFIG_BOARD_ROOT_PATH #endif extern "C" __EXPORT int rc_tests_main(int argc, char *argv[]); @@ -520,7 +520,4 @@ bool RCTest::sumdTest() return true; } - - ut_declare_test_c(rc_tests_main, RCTest) - diff --git a/src/lib/systemlib/hardfault_log.h b/src/lib/systemlib/hardfault_log.h index 48340e295db9..678a0398f3ed 100644 --- a/src/lib/systemlib/hardfault_log.h +++ b/src/lib/systemlib/hardfault_log.h @@ -213,7 +213,7 @@ typedef struct ssarc_s dump_s; * Specifier to the xxxx_NUM definei.e %Y is YYYY so add 2 and %s is -2 * Also xxxxTIME_FMT need to match in size. See CCASERT in hardfault_log.c */ -#define LOG_PATH_BASE "/fs/microsd/" +#define LOG_PATH_BASE CONFIG_BOARD_ROOT_PATH "/" #define LOG_PATH_BASE_LEN ((arraySize(LOG_PATH_BASE))-1) #define LOG_NAME_FMT "fault_%s.log" diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index 1417cd37b46e..1ca273be8473 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -84,18 +84,17 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); PARAM_DEFINE_INT32(SYS_HITL, 0); /** - * Set multicopter estimator group + * Parameter version * - * Set the group of estimators used for multicopters and VTOLs + * This is used internally only: an airframe configuration might set an expected + * parameter version value via PARAM_DEFAULTS_VER. This is checked on bootup + * against SYS_PARAM_VER, and if they do not match, parameters are reset and + * reloaded from the airframe configuration. * - * @value 1 local_position_estimator, attitude_estimator_q (unsupported) - * @value 2 ekf2 (recommended) - * @value 3 Q attitude estimator (no position) - * - * @reboot_required true + * @min 0 * @group System */ -PARAM_DEFINE_INT32(SYS_MC_EST_GROUP, 2); +PARAM_DEFINE_INT32(SYS_PARAM_VER, 1); /** * Enable auto start of rate gyro thermal calibration at the next power up. diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b0d9c43b2058..4a6d3593e894 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -231,6 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param AltitudePitchControl control_setpoint; + control_setpoint.tas_setpoint = setpoint.tas_setpoint; + control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param); @@ -274,6 +276,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input & AltitudePitchControl control_setpoint; + control_setpoint.tas_setpoint = setpoint.tas_setpoint; control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag); if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) { @@ -320,9 +323,11 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In // if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints if (flag.airspeed_enabled) { // Calculate limits for the demanded rate of change of speed based on physical performance limits - // with a 50% margin to allow the total energy controller to correct for errors. - const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON); - const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON); + // with a 50% margin to allow the total energy controller to correct for errors. Increase it in case of fast descend + const float max_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_max / math::max(input.tas, + FLT_EPSILON); + const float min_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_min / math::max(input.tas, + FLT_EPSILON); airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp, max_tas_rate_sp); } @@ -348,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt // Calculate specific energy rate demands in units of (m**2/sec**3) specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change - specific_energy_rates.ske_rate.setpoint = input.tas * + specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change // Calculate specific energy rates in units of (m**2/sec**3) @@ -394,19 +399,20 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co } else if (!flag.airspeed_enabled) { pitch_speed_weight = 0.0f; + } else if (param.fast_descend > FLT_EPSILON) { + // pitch loop controls the airspeed to max + pitch_speed_weight = 1.f + param.fast_descend; + } - // don't allow any weight to be larger than one, as it has the same effect as reducing the control - // loop time constant and therefore can lead to a destabilization of that control loop - weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f); - weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f); + weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 2.f); + weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 2.f); return weight; } void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates, - const Param ¶m, - const Flag &flag) + const Param ¶m, const Flag &flag) { const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; @@ -514,10 +520,22 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate; _ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const); _ste_rate_estimate_filter.update(STE_rate_estimate_raw); - ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)}; - _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); - float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)}; + float throttle_setpoint{param.throttle_min}; + + if (1.f - param.fast_descend < FLT_EPSILON) { + // During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending + if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing + throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet + + } else { + throttle_setpoint = param.throttle_min; + } + + } else { + _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); + throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag); + } // Rate limit the throttle demand if (fabsf(param.throttle_slewrate) > FLT_EPSILON) { @@ -576,7 +594,7 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit // Calculate a throttle demand from the integrated total energy rate error // This will be added to the total throttle demand to compensate for steady state errors - _throttle_integ_state = _throttle_integ_state + throttle_integ_input; + _throttle_integ_state = PX4_ISFINITE(throttle_integ_input) ? _throttle_integ_state + throttle_integ_input : 0.f; } else { _throttle_integ_state = 0.0f; @@ -651,6 +669,7 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa _reference_param.target_sinkrate = target_sinkrate; // Control _control_param.tas_min = eas_to_tas * _equivalent_airspeed_min; + _control_param.tas_max = eas_to_tas * _equivalent_airspeed_max; _control_param.pitch_max = pitch_limit_max; _control_param.pitch_min = pitch_limit_min; _control_param.throttle_trim = throttle_trim; @@ -705,6 +724,11 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas); } else { + /* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control + loop to control the speed to the maximum airspeed. */ + _setFastDescend(hgt_setpoint, altitude); + _control_param.fast_descend = _fast_descend; + // Update airspeedfilter submodule const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed, .equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas}; @@ -712,15 +736,25 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled); // Update Reference model submodule - const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, - .alt_rate = hgt_rate_sp}; + if (1.f - _fast_descend < FLT_EPSILON) { + // Reset the altitude reference model, while we are in fast descend. + const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ + .alt = altitude, + .alt_rate = hgt_rate}; + _altitude_reference_model.initialize(init_state); - _altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param); + } else { + const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, + .alt_rate = hgt_rate_sp}; + + _altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param); + } TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint; + control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas * + EAS_setpoint; const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, @@ -740,3 +774,17 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _update_timestamp = now; } +void TECS::_setFastDescend(const float alt_setpoint, const float alt) +{ + if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) + && ((alt_setpoint + _fast_descend_alt_err) < alt)) { + _fast_descend = 1.f; + + } else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { + // Were in fast descend, scale it down. up until 5m above target altitude + _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); + + } else { + _fast_descend = 0.f; + } +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 8749ac0a21af..f033bf72dabf 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -202,6 +202,7 @@ class TECSControl float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²]. float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s]. float tas_min; ///< True airspeed demand lower limit [m/s]. + float tas_max; ///< True airspeed demand upper limit [m/s]. float pitch_max; ///< Maximum pitch angle allowed in [rad]. float pitch_min; ///< Minimal pitch angle allowed in [rad]. float throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1] @@ -233,6 +234,8 @@ class TECSControl float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³]. float load_factor; ///< Additional normal load factor. + + float fast_descend; }; /** @@ -363,6 +366,7 @@ class TECSControl struct AltitudePitchControl { float altitude_rate_setpoint; ///< Controlled altitude rate setpoint [m/s]. float tas_rate_setpoint; ///< Controlled true airspeed rate setpoint [m/s²]. + float tas_setpoint; ///< Controller true airspeed setpoint [m/s] }; /** @@ -393,7 +397,7 @@ class TECSControl * @brief calculate airspeed control proportional output. * * @param setpoint is the control setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. * @return controlled airspeed rate setpoint in [m/s²]. @@ -404,7 +408,7 @@ class TECSControl * @brief calculate altitude control proportional output. * * @param setpoint is the control setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @return controlled altitude rate setpoint in [m/s]. */ @@ -413,14 +417,14 @@ class TECSControl * @brief Calculate specific energy rates. * * @param control_setpoint is the controlles altitude and airspeed rate setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @return Specific energy rates in [m²/s³]. */ SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const; /** * @brief Detect underspeed. * - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. */ @@ -602,9 +606,11 @@ class TECS void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; }; void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; }; - void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; }; + void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); }; + void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; }; void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } + void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; } void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; } @@ -665,7 +671,10 @@ class TECS hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call. - float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) + float _equivalent_airspeed_min{10.0f}; ///< equivalent airspeed demand lower limit (m/sec) + float _equivalent_airspeed_max{20.0f}; ///< equivalent airspeed demand upper limit (m/sec) + float _fast_descend_alt_err{-1.f}; ///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m]. + float _fast_descend{0.f}; ///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude. static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) @@ -697,6 +706,7 @@ class TECS .vert_accel_limit = 0.0f, .equivalent_airspeed_trim = 15.0f, .tas_min = 10.0f, + .tas_max = 20.0f, .pitch_max = 0.5f, .pitch_min = -0.5f, .throttle_trim = 0.0f, @@ -716,11 +726,19 @@ class TECS .throttle_slewrate = 0.0f, .load_factor_correction = 0.0f, .load_factor = 1.0f, + .fast_descend = 0.f }; TECSControl::Flag _control_flag{ .airspeed_enabled = false, .detect_underspeed_enabled = false, }; -}; + /** + * @brief Set fast descend value + * + * @param alt_setpoint is the altitude setpoint + * @param alt is the current altitude + */ + void _setFastDescend(float alt_setpoint, float alt); +}; diff --git a/src/lib/version/get_git_tag_or_branch_version.sh b/src/lib/version/get_git_tag_or_branch_version.sh index a7d9c13b3346..4c8cfab62c81 100755 --- a/src/lib/version/get_git_tag_or_branch_version.sh +++ b/src/lib/version/get_git_tag_or_branch_version.sh @@ -18,4 +18,3 @@ if [ ! -f "$version_file" ]; then fi sed -n 's/.*PX4_GIT_TAG_OR_BRANCH_NAME\s*"\(.*\)".*/version=\1/p' "$version_file" - diff --git a/src/lib/version/version.c b/src/lib/version/version.c index ebf1749985d5..593c799e3331 100644 --- a/src/lib/version/version.c +++ b/src/lib/version/version.c @@ -366,4 +366,3 @@ const char *px4_firmware_oem_version_string(void) { return PX4_GIT_OEM_VERSION_STR; } - diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 451393f835ed..cf6ad0ed83d4 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -206,4 +206,3 @@ __EXPORT uint64_t px4_os_version_binary(void); __EXPORT const char *px4_firmware_oem_version_string(void); __END_DECLS - diff --git a/src/lib/wind_estimator/python/wind_estimator_replay.py b/src/lib/wind_estimator/python/wind_estimator_replay.py index efa19eaf0e71..dcc292286df6 100644 --- a/src/lib/wind_estimator/python/wind_estimator_replay.py +++ b/src/lib/wind_estimator/python/wind_estimator_replay.py @@ -54,7 +54,7 @@ def getData(log, topic_name, variable_name, instance=0): def us2s(time_ms): return time_ms * 1e-6 -def run(logfile, use_gnss): +def run(logfile, use_gnss, scale_init): log = ULog(logfile) if use_gnss: @@ -75,7 +75,12 @@ def run(logfile, use_gnss): dist_bottom = getData(log, 'vehicle_local_position', 'dist_bottom') t_dist_bottom = us2s(getData(log, 'vehicle_local_position', 'timestamp')) - state = np.array([0.0, 0.0, 1.0]) + if scale_init is None: + scale_init = 1.0 + + # The estimator estimates the inverse scale factor to have a simpler measurement jacobian + inverse_scale_init = 1 / scale_init + state = np.array([0.0, 0.0, inverse_scale_init]) P = np.diag([1.0, 1.0, 1e-4]) wind_nsd = 1e-2 scale_nsd = 1e-4 @@ -103,19 +108,19 @@ def run(logfile, use_gnss): P += Q * dt - if t_true_airspeed[i_airspeed] < t_now: + if i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now: while i_airspeed < len(t_true_airspeed) and t_true_airspeed[i_airspeed] < t_now: i_airspeed += 1 i_airspeed -= 1 - (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P.flatten(), true_airspeed[i_airspeed], R, epsilon) + (H, K, innov_var, innov) = fuse_airspeed(np.asarray(v_local[:,i]), state, P, true_airspeed[i_airspeed], R, epsilon) state += np.array(K) * innov P -= K * H * P i_airspeed += 1 wind_est_n[i] = state[0] wind_est_e[i] = state[1] - scale_est[i] = state[2] + scale_est[i] = 1 / state[2] plt.figure(1) ax1 = plt.subplot(2, 1, 1) @@ -145,8 +150,9 @@ def run(logfile, use_gnss): parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) parser.add_argument('--gnss', help='Use GNSS velocity instead of local velocity estimate', action='store_true') + parser.add_argument('--scale_init', help='Initial airsped scale factor (1.0 if not specified)', type=float) args = parser.parse_args() logfile = os.path.abspath(args.logfile) # Convert to absolute path - run(logfile, args.gnss) + run(logfile, args.gnss, args.scale_init) diff --git a/src/lib/world_magnetic_model/fetch_noaa_table.py b/src/lib/world_magnetic_model/fetch_noaa_table.py index 8914506f91c9..83ff3dda1e06 100755 --- a/src/lib/world_magnetic_model/fetch_noaa_table.py +++ b/src/lib/world_magnetic_model/fetch_noaa_table.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 ############################################################################ # -# Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2024 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,6 +33,7 @@ ############################################################################ import math +import numpy import json import statistics import sys @@ -49,7 +50,7 @@ def constrain(n, nmin, nmax): header = """/**************************************************************************** * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 @@ -80,6 +81,9 @@ def constrain(n, nmin, nmax): * ****************************************************************************/ """ + +key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml) + print(header) print('#include \n') @@ -97,188 +101,106 @@ def constrain(n, nmin, nmax): print('static constexpr int LON_DIM = {}'.format(LON_DIM) + ';') print('\n') -print('// *INDENT-OFF*') - -# Declination -params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'd', 'resultFormat': 'json'}) -key=sys.argv[1] # NOAA key (https://www.ngdc.noaa.gov/geomag/CalcSurvey.shtml) -f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) -data = json.loads(f.read()) -print("// Magnetic declination data in radians * 10^-4") -print('// Model: {},'.format(data['model'])) -print('// Version: {},'.format(data['version'])) -print('// Date: {},'.format(data['result'][0]['date'])) -print('static constexpr const int16_t declination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {") -print('\t// LONGITUDE: ', end='') -for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): - print('{0:6d},'.format(l), end='') -print('') - -declination_min = float('inf') -declination_max = float('-inf') -declination_min_lat_lon = () -declination_max_lat_lon = () - -for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): - params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'd', 'resultFormat': 'json'}) - f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) - data = json.loads(f.read()) - - latitude_blackout_zone = False - - print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') - for p in data['result']: - declination_rad = math.radians(p['declination']) - - warning = False +print('// *INDENT-OFF*\n\n\n') - try: - if p['warning']: - warning = True - latitude_blackout_zone = True - #print("WARNING black out zone!", p) - except: - pass - # declination in radians * 10^-4 - declination_int = constrain(int(round(declination_rad * 10000)), 32767, -32768) - print('{0:6d},'.format(declination_int), end='') +# build the world magnetic model dictionary +world_magnitude_model = {} # lat/lon dictionary with grid result - if (declination_rad > declination_max): - declination_max = declination_rad - declination_max_lat_lon = (p['latitude'], p['longitude']) - - if (declination_rad < declination_min): - declination_min = declination_rad - declination_min_lat_lon = (p['latitude'], p['longitude']) - - if latitude_blackout_zone: - print(' }, // WARNING! black out zone') - else: - print(' },') - -print("};") - -print('static constexpr float WMM_DECLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_min, declination_min_lat_lon[0], declination_min_lat_lon[1])) -print('static constexpr float WMM_DECLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(declination_max, declination_max_lat_lon[0], declination_max_lat_lon[1])) -print("\n") - - -# Inclination -params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'}) -f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) +params = urllib.parse.urlencode({'lat1': 0, 'lon1': 0, 'resultFormat': 'json'}) +f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params)) data = json.loads(f.read()) -print("// Magnetic inclination data in radians * 10^-4") -print('// Model: {},'.format(data['model'])) -print('// Version: {},'.format(data['version'])) -print('// Date: {},'.format(data['result'][0]['date'])) -print('static constexpr const int16_t inclination_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {") -print('\t// LONGITUDE: ', end='') -for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): - print('{0:6d},'.format(l), end='') -print('') -inclination_min = float('inf') -inclination_max = float('-inf') -inclination_min_lat_lon = () -inclination_max_lat_lon = () +world_magnitude_model_units = data['units'] for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): - params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'i', 'resultFormat': 'json'}) - f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) - data = json.loads(f.read()) + world_magnitude_model[latitude] = {} - latitude_blackout_zone = False + for longitude in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): + params = urllib.parse.urlencode({'lat1': latitude, 'lon1': longitude, 'lon2': SAMPLING_MAX_LON, 'resultFormat': 'json'}) + f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfwmm?key=%s&%s" % (key, params)) + data = json.loads(f.read()) + #print(json.dumps(data, indent = 4)) # debugging - print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') - for p in data['result']: - inclination_rad = math.radians(p['inclination']) + world_magnitude_model[latitude][longitude] = data['result'][0] + #print(world_magnitude_model[latitude][longitude]) - warning = False - try: - if p['warning']: - warning = True - latitude_blackout_zone = True - #print("WARNING black out zone!", p) +def print_wmm_table(key_name): - except: - pass + value_min = float('inf') + value_min_lat_lon = () - # inclination in radians * 10^-4 - inclination_int = constrain(int(round(inclination_rad * 10000)), 32767, -32768) - print('{0:6d},'.format(inclination_int), end='') + value_max = float('-inf') + value_max_lat_lon = () - if (inclination_rad > inclination_max): - inclination_max = inclination_rad - inclination_max_lat_lon = (p['latitude'], p['longitude']) + for latitude, lat_row in world_magnitude_model.items(): + #print(latitude, lat_row) + for longitude, result in lat_row.items(): + #print(result) - if (inclination_rad < inclination_min): - inclination_min = inclination_rad - inclination_min_lat_lon = (p['latitude'], p['longitude']) + value = float(result[key_name]) - if latitude_blackout_zone: - print(' }, // WARNING! black out zone') - else: - print(' },') + if (value > value_max): + value_max = value + value_max_lat_lon = (latitude, longitude) -print("};") + if (value < value_min): + value_min = value + value_min_lat_lon = (latitude, longitude) -print('static constexpr float WMM_INCLINATION_MIN_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_min, inclination_min_lat_lon[0], inclination_min_lat_lon[1])) -print('static constexpr float WMM_INCLINATION_MAX_RAD = {:.3f}; // latitude: {:.0f}, longitude: {:.0f}'.format(inclination_max, inclination_max_lat_lon[0], inclination_max_lat_lon[1])) -print("\n") + # scale the values to fit into int16_t + value_scale_max = abs(numpy.iinfo(numpy.int16).max) / abs(value_max) + value_scale_min = abs(numpy.iinfo(numpy.int16).min) / abs(value_min) + value_scale = min(value_scale_max, value_scale_min) -# total intensity -params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'}) -f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) -data = json.loads(f.read()) -print("// Magnetic strength data in milli-Gauss * 10") -print('// Model: {},'.format(data['model'])) -print('// Version: {},'.format(data['version'])) -print('// Date: {},'.format(data['result'][0]['date'])) -print('static constexpr const int16_t strength_table[{}][{}]'.format(LAT_DIM, LON_DIM) + " {") -print('\t// LONGITUDE: ', end='') -for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): - print('{0:5d},'.format(l), end='') -print('') + units_str = world_magnitude_model_units[key_name].split(' ')[0] -strength_min = float('inf') -strength_max = 0 -strength_min_lat_lon = () -strength_max_lat_lon = () -strength_sum = 0 -strength_sum_count = 0 + # print the table + print('// Magnetic {} data in {:.4g} {}'.format(key_name, 1.0 / value_scale, units_str)) + print('// Model: {},'.format(data['model'])) + print('// Version: {},'.format(data['version'])) + print('// Date: {},'.format(data['result'][0]['date'])) + print('static constexpr const int16_t {}_table[{}][{}]'.format(key_name, LAT_DIM, LON_DIM) + " {") + print('\t// LONGITUDE: ', end='') + for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): + print('{0:6d},'.format(l), end='') + print('') -strength_list = [] + for latitude, lat_row in world_magnitude_model.items(): + print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') + latitude_blackout_zone = False -for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): - params = urllib.parse.urlencode({'lat1': latitude, 'lat2': latitude, 'lon1': SAMPLING_MIN_LON, 'lon2': SAMPLING_MAX_LON, 'latStepSize': 1, 'lonStepSize': SAMPLING_RES, 'magneticComponent': 'f', 'resultFormat': 'json'}) - f = urllib.request.urlopen("https://www.ngdc.noaa.gov/geomag-web/calculators/calculateIgrfgrid?key=%s&%s" % (key, params)) - data = json.loads(f.read()) + for longitude, result in lat_row.items(): + + value = float(result[key_name]) + + # value scaled to fit into int16_t + value_int = int(round(value * value_scale)) + print('{0:6d},'.format(value_int), end='') - print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') - for p in data['result']: + # blackout warning at this latitude + try: + if result['warning']: + latitude_blackout_zone = True - strength_gauss = p['totalintensity'] * 1e-3 - strength_list.append(strength_gauss) + except: + pass - totalintensity_int = int(round(p['totalintensity']/10)) - print('{0:5d},'.format(totalintensity_int), end='') + if latitude_blackout_zone: + print(' }, // WARNING! black out zone') + else: + print(' },') - if (strength_gauss > strength_max): - strength_max = strength_gauss - strength_max_lat_lon = (p['latitude'], p['longitude']) + print("};") - if (strength_gauss < strength_min): - strength_min = strength_gauss - strength_min_lat_lon = (p['latitude'], p['longitude']) + print('static constexpr float WMM_{}_SCALE_TO_{} = {:.9g}f;'.format(key_name.upper(), units_str.upper(), 1.0 / value_scale)) + print('static constexpr float WMM_{}_MIN_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_min, value_min_lat_lon[0], value_min_lat_lon[1])) + print('static constexpr float WMM_{}_MAX_{} = {:.1f}f; // latitude: {:.0f}, longitude: {:.0f}'.format(key_name.upper(), units_str.upper(), value_max, value_max_lat_lon[0], value_max_lat_lon[1])) + print("\n") - print(' },') -print("};") -print('static constexpr float WMM_STRENGTH_MIN_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_min, strength_min_lat_lon[0], strength_min_lat_lon[1])) -print('static constexpr float WMM_STRENGTH_MAX_GS = {:.1f}; // latitude: {:.0f}, longitude: {:.0f}'.format(strength_max, strength_max_lat_lon[0], strength_max_lat_lon[1])) -print('static constexpr float WMM_STRENGTH_MEAN_GS = {:.1f};'.format(statistics.mean(strength_list))) -print('static constexpr float WMM_STRENGTH_MEDIAN_GS = {:.1f};'.format(statistics.median(strength_list))) -print("\n") +print_wmm_table('declination') +print_wmm_table('inclination') +print_wmm_table('totalintensity') diff --git a/src/lib/world_magnetic_model/generate_gtest.py b/src/lib/world_magnetic_model/generate_gtest.py index bcf204a49c26..966e56a4bd02 100755 --- a/src/lib/world_magnetic_model/generate_gtest.py +++ b/src/lib/world_magnetic_model/generate_gtest.py @@ -44,7 +44,7 @@ header = """/**************************************************************************** * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 diff --git a/src/lib/world_magnetic_model/geo_mag_declination.cpp b/src/lib/world_magnetic_model/geo_mag_declination.cpp index f40a77935004..64738426f501 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.cpp +++ b/src/lib/world_magnetic_model/geo_mag_declination.cpp @@ -66,21 +66,21 @@ static constexpr unsigned get_lookup_table_index(float *val, float min, float ma return static_cast((-(min) + *val) / SAMPLING_RES); } -static constexpr float get_table_data(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM]) +static constexpr float get_table_data(float latitude_deg, float longitude_deg, const int16_t table[LAT_DIM][LON_DIM]) { - lat = math::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); + latitude_deg = math::constrain(latitude_deg, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); - if (lon > SAMPLING_MAX_LON) { - lon -= 360; + if (longitude_deg > SAMPLING_MAX_LON) { + longitude_deg -= 360.f; } - if (lon < SAMPLING_MIN_LON) { - lon += 360; + if (longitude_deg < SAMPLING_MIN_LON) { + longitude_deg += 360.f; } /* round down to nearest sampling resolution */ - float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES; - float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES; + float min_lat = floorf(latitude_deg / SAMPLING_RES) * SAMPLING_RES; + float min_lon = floorf(longitude_deg / SAMPLING_RES) * SAMPLING_RES; /* find index of nearest low sampling point */ unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); @@ -92,8 +92,8 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[ const float data_nw = table[min_lat_index + 1][min_lon_index]; /* perform bilinear interpolation on the four grid corners */ - const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f); - const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f); + const float lat_scale = constrain((latitude_deg - min_lat) / SAMPLING_RES, 0.f, 1.f); + const float lon_scale = constrain((longitude_deg - min_lon) / SAMPLING_RES, 0.f, 1.f); const float data_min = lon_scale * (data_se - data_sw) + data_sw; const float data_max = lon_scale * (data_ne - data_nw) + data_nw; @@ -101,32 +101,27 @@ static constexpr float get_table_data(float lat, float lon, const int16_t table[ return lat_scale * (data_max - data_min) + data_min; } -float get_mag_declination_radians(float lat, float lon) +float get_mag_declination_degrees(float latitude_deg, float longitude_deg) { - return get_table_data(lat, lon, declination_table) * 1e-4f; // declination table stored as 10^-4 radians + // table stored as scaled degrees + return get_table_data(latitude_deg, longitude_deg, declination_table) * WMM_DECLINATION_SCALE_TO_DEGREES; } -float get_mag_declination_degrees(float lat, float lon) +float get_mag_inclination_degrees(float latitude_deg, float longitude_deg) { - return math::degrees(get_mag_declination_radians(lat, lon)); + // table stored as scaled degrees + return get_table_data(latitude_deg, longitude_deg, inclination_table) * WMM_INCLINATION_SCALE_TO_DEGREES; } -float get_mag_inclination_radians(float lat, float lon) +float get_mag_strength_gauss(float latitude_deg, float longitude_deg) { - return get_table_data(lat, lon, inclination_table) * 1e-4f; // inclination table stored as 10^-4 radians + // 1 Gauss = 1e4 Tesla + return get_mag_strength_tesla(latitude_deg, longitude_deg) * 1e4f; } -float get_mag_inclination_degrees(float lat, float lon) +float get_mag_strength_tesla(float latitude_deg, float longitude_deg) { - return math::degrees(get_mag_inclination_radians(lat, lon)); -} - -float get_mag_strength_gauss(float lat, float lon) -{ - return get_table_data(lat, lon, strength_table) * 1e-4f; // strength table table stored as milli-Gauss * 10 -} - -float get_mag_strength_tesla(float lat, float lon) -{ - return get_mag_strength_gauss(lat, lon) * 1e-4f; // 1 Gauss == 0.0001 Tesla + // table stored as scaled nanotesla + return get_table_data(latitude_deg, longitude_deg, totalintensity_table) + * WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA * 1e-9f; } diff --git a/src/lib/world_magnetic_model/geo_mag_declination.h b/src/lib/world_magnetic_model/geo_mag_declination.h index 790f3b353a07..7e6f34670663 100644 --- a/src/lib/world_magnetic_model/geo_mag_declination.h +++ b/src/lib/world_magnetic_model/geo_mag_declination.h @@ -41,14 +41,12 @@ #pragma once -// Return magnetic declination in degrees or radians -float get_mag_declination_degrees(float lat, float lon); -float get_mag_declination_radians(float lat, float lon); +// Return magnetic declination in degrees +float get_mag_declination_degrees(float latitude_deg, float longitude_deg); -// Return magnetic field inclination in degrees or radians -float get_mag_inclination_degrees(float lat, float lon); -float get_mag_inclination_radians(float lat, float lon); +// Return magnetic field inclination in degrees +float get_mag_inclination_degrees(float latitude_deg, float longitude_deg); // return magnetic field strength in Gauss or Tesla -float get_mag_strength_gauss(float lat, float lon); -float get_mag_strength_tesla(float lat, float lon); +float get_mag_strength_gauss(float latitude_deg, float longitude_deg); +float get_mag_strength_tesla(float latitude_deg, float longitude_deg); diff --git a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp index 3a1268ae400e..855a971406b8 100644 --- a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp +++ b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 @@ -44,95 +44,97 @@ static constexpr int LON_DIM = 37; // *INDENT-OFF* -// Magnetic declination data in radians * 10^-4 + + + +// Magnetic declination data in 0.005451 degrees // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2024.0684, +// Date: 2024.41257, static constexpr const int16_t declination_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, - /* LAT: -90 */ { 25949, 24204, 22458, 20713, 18968, 17222, 15477, 13732, 11986, 10241, 8496, 6750, 5005, 3260, 1514, -231, -1976, -3722, -5467, -7212, -8958,-10703,-12448,-14194,-15939,-17684,-19430,-21175,-22920,-24666,-26411,-28156,-29902, 31185, 29440, 27694, 25949, }, - /* LAT: -80 */ { 22511, 20385, 18448, 16677, 15039, 13502, 12039, 10628, 9254, 7905, 6573, 5255, 3947, 2642, 1334, 13, -1329, -2704, -4116, -5571, -7068, -8608,-10188,-11810,-13476,-15197,-16985,-18860,-20845,-22966,-25242,-27677,-30245, 29950, 27336, 24836, 22511, }, - /* LAT: -70 */ { 14988, 13587, 12457, 11491, 10618, 9783, 8939, 8049, 7095, 6075, 5005, 3913, 2830, 1777, 757, -252, -1291, -2401, -3608, -4915, -6302, -7737, -9189,-10634,-12063,-13481,-14912,-16402,-18033,-19960,-22502,-26331, 30582, 24088, 19617, 16858, 14988, }, // WARNING! black out zone - /* LAT: -60 */ { 8460, 8207, 7918, 7636, 7376, 7117, 6803, 6366, 5747, 4924, 3920, 2809, 1692, 671, -201, -952, -1680, -2508, -3524, -4735, -6077, -7455, -8781, -9992,-11055,-11951,-12664,-13158,-13330,-12882,-10766, -3382, 5057, 7748, 8493, 8605, 8460, }, // WARNING! black out zone - /* LAT: -50 */ { 5516, 5549, 5488, 5392, 5313, 5271, 5232, 5100, 4751, 4082, 3066, 1790, 453, -715, -1568, -2116, -2503, -2947, -3651, -4691, -5957, -7246, -8389, -9286, -9875,-10102, -9893, -9123, -7602, -5231, -2320, 430, 2541, 3966, 4841, 5314, 5516, }, - /* LAT: -40 */ { 3977, 4068, 4072, 4021, 3956, 3918, 3919, 3905, 3727, 3185, 2151, 702, -859, -2158, -2999, -3428, -3594, -3645, -3829, -4438, -5452, -6544, -7426, -7951, -8039, -7639, -6731, -5338, -3632, -1939, -487, 732, 1779, 2650, 3311, 3745, 3977, }, - /* LAT: -30 */ { 3003, 3088, 3113, 3092, 3028, 2945, 2881, 2845, 2716, 2229, 1178, -353, -1958, -3193, -3909, -4234, -4292, -4059, -3621, -3439, -3855, -4626, -5316, -5645, -5497, -4889, -3917, -2714, -1520, -590, 79, 677, 1305, 1916, 2433, 2801, 3003, }, - /* LAT: -20 */ { 2360, 2404, 2416, 2410, 2361, 2262, 2149, 2071, 1924, 1420, 347, -1155, -2629, -3669, -4175, -4268, -4043, -3462, -2592, -1820, -1589, -1975, -2627, -3080, -3097, -2722, -2080, -1269, -508, -33, 221, 519, 966, 1455, 1887, 2202, 2360, }, - /* LAT: -10 */ { 1966, 1959, 1931, 1919, 1885, 1794, 1677, 1582, 1396, 839, -238, -1623, -2888, -3693, -3930, -3676, -3084, -2295, -1450, -709, -266, -325, -809, -1310, -1509, -1409, -1091, -597, -111, 118, 156, 311, 689, 1139, 1545, 1841, 1966, }, - /* LAT: 0 */ { 1752, 1717, 1655, 1639, 1621, 1545, 1432, 1311, 1055, 430, -618, -1847, -2888, -3450, -3420, -2898, -2123, -1344, -699, -169, 235, 331, 37, -380, -627, -679, -580, -323, -41, 38, -35, 49, 397, 854, 1287, 1617, 1752, }, - /* LAT: 10 */ { 1612, 1619, 1571, 1581, 1601, 1545, 1414, 1220, 842, 124, -899, -1971, -2783, -3100, -2872, -2252, -1473, -767, -260, 116, 438, 577, 403, 80, -149, -257, -285, -210, -113, -163, -313, -290, 19, 487, 979, 1396, 1612, }, - /* LAT: 20 */ { 1419, 1567, 1624, 1712, 1794, 1768, 1605, 1296, 751, -104, -1144, -2086, -2666, -2757, -2413, -1803, -1092, -447, 5, 310, 566, 706, 607, 362, 166, 46, -53, -127, -214, -411, -655, -715, -474, -16, 534, 1059, 1419, }, - /* LAT: 30 */ { 1105, 1471, 1730, 1952, 2110, 2117, 1924, 1498, 769, -255, -1364, -2227, -2625, -2550, -2143, -1560, -904, -289, 167, 468, 696, 837, 810, 660, 512, 385, 219, -5, -297, -677, -1049, -1207, -1042, -608, -27, 584, 1105, }, - /* LAT: 40 */ { 732, 1317, 1812, 2206, 2456, 2499, 2283, 1748, 839, -380, -1603, -2452, -2759, -2599, -2149, -1556, -902, -275, 231, 591, 857, 1050, 1137, 1123, 1047, 895, 615, 189, -362, -974, -1493, -1732, -1610, -1189, -591, 77, 732, }, - /* LAT: 50 */ { 425, 1168, 1851, 2413, 2785, 2895, 2669, 2019, 887, -593, -1994, -2892, -3177, -2982, -2491, -1845, -1135, -440, 172, 674, 1088, 1437, 1711, 1876, 1891, 1695, 1235, 507, -401, -1306, -1977, -2253, -2123, -1682, -1054, -331, 425, }, - /* LAT: 60 */ { 203, 1050, 1857, 2558, 3072, 3295, 3093, 2299, 813, -1110, -2799, -3769, -4020, -3763, -3195, -2453, -1630, -793, 11, 759, 1447, 2073, 2610, 2999, 3148, 2946, 2284, 1141, -289, -1606, -2460, -2757, -2588, -2101, -1421, -635, 203, }, - /* LAT: 70 */ { -80, 847, 1743, 2546, 3170, 3479, 3255, 2178, 39, -2583, -4513, -5355, -5387, -4922, -4167, -3243, -2226, -1167, -98, 958, 1979, 2941, 3805, 4505, 4941, 4955, 4314, 2796, 566, -1545, -2828, -3261, -3103, -2582, -1848, -994, -80, }, // WARNING! black out zone - /* LAT: 80 */ { -944, -26, 822, 1513, 1909, 1772, 716, -1590, -4521, -6612, -7444, -7396, -6828, -5954, -4893, -3716, -2466, -1173, 144, 1465, 2775, 4055, 5282, 6422, 7420, 8182, 8523, 8052, 6011, 1989, -1732, -3421, -3747, -3398, -2713, -1863, -944, }, // WARNING! black out zone - /* LAT: 90 */ { -29255,-27510,-25765,-24019,-22274,-20529,-18783,-17038,-15293,-13547,-11802,-10057, -8311, -6566, -4821, -3075, -1330, 415, 2161, 3906, 5651, 7397, 9142, 10887, 12633, 14378, 16123, 17869, 19614, 21359, 23105, 24850, 26595, 28341, 30086,-31001,-29255, }, // WARNING! black out zone + /* LAT: -90 */ { 27264, 25429, 23595, 21761, 19926, 18092, 16258, 14423, 12589, 10754, 8920, 7086, 5251, 3417, 1583, -252, -2086, -3921, -5755, -7589, -9424,-11258,-13092,-14927,-16761,-18596,-20430,-22264,-24099,-25933,-27768,-29602,-31436, 32767, 30933, 29098, 27264, }, + /* LAT: -80 */ { 23650, 21416, 19382, 17521, 15799, 14184, 12647, 11165, 9721, 8303, 6904, 5519, 4143, 2772, 1397, 9, -1403, -2848, -4333, -5863, -7437, -9056,-10718,-12423,-14175,-15984,-17864,-19835,-21922,-24152,-26545,-29105,-31803, 31464, 28718, 26092, 23650, }, + /* LAT: -70 */ { 15755, 14282, 13094, 12077, 11159, 10281, 9392, 8457, 7454, 6381, 5257, 4109, 2971, 1865, 794, -267, -1359, -2527, -3797, -5172, -6632, -8142, -9669,-11188,-12690,-14181,-15686,-17253,-18969,-20997,-23675,-27707, 32110, 25303, 20617, 17720, 15755, }, // WARNING! black out zone + /* LAT: -60 */ { 8903, 8634, 8328, 8030, 7755, 7481, 7150, 6689, 6037, 5171, 4116, 2948, 1775, 704, -211, -998, -1763, -2635, -3706, -4982, -6396, -7846, -9240,-10513,-11630,-12572,-13320,-13839,-14019,-13549,-11322, -3523, 5353, 8167, 8943, 9057, 8903, }, // WARNING! black out zone + /* LAT: -50 */ { 5806, 5839, 5774, 5672, 5587, 5541, 5499, 5359, 4991, 4286, 3216, 1875, 470, -754, -1647, -2218, -2623, -3091, -3835, -4934, -6270, -7627, -8830, -9771,-10388,-10623,-10400, -9587, -7986, -5492, -2431, 460, 2679, 4178, 5097, 5594, 5806, }, + /* LAT: -40 */ { 4186, 4282, 4284, 4229, 4159, 4118, 4119, 4103, 3914, 3342, 2253, 728, -912, -2273, -3151, -3597, -3767, -3819, -4016, -4665, -5740, -6891, -7817, -8365, -8453, -8028, -7069, -5603, -3811, -2034, -509, 772, 1872, 2790, 3486, 3942, 4186, }, + /* LAT: -30 */ { 3161, 3250, 3275, 3251, 3182, 3094, 3026, 2988, 2851, 2337, 1229, -384, -2070, -3364, -4111, -4446, -4503, -4252, -3790, -3609, -4059, -4874, -5597, -5938, -5777, -5133, -4109, -2845, -1594, -619, 83, 710, 1372, 2016, 2561, 2948, 3161, }, + /* LAT: -20 */ { 2485, 2532, 2542, 2534, 2481, 2375, 2255, 2172, 2018, 1486, 353, -1228, -2777, -3865, -4390, -4481, -4240, -3623, -2707, -1902, -1669, -2082, -2768, -3241, -3254, -2856, -2179, -1328, -531, -35, 229, 542, 1013, 1529, 1985, 2318, 2485, }, + /* LAT: -10 */ { 2072, 2064, 2032, 2018, 1980, 1883, 1758, 1657, 1461, 874, -261, -1719, -3046, -3887, -4129, -3855, -3229, -2397, -1510, -734, -274, -342, -854, -1379, -1586, -1478, -1143, -624, -116, 121, 159, 321, 720, 1197, 1625, 1939, 2072, }, + /* LAT: 0 */ { 1847, 1810, 1742, 1723, 1703, 1621, 1500, 1371, 1102, 444, -660, -1952, -3043, -3628, -3589, -3035, -2219, -1401, -724, -169, 254, 350, 38, -401, -658, -711, -608, -338, -43, 36, -42, 45, 413, 896, 1354, 1703, 1847, }, + /* LAT: 10 */ { 1699, 1705, 1653, 1662, 1681, 1620, 1480, 1275, 878, 122, -954, -2080, -2929, -3256, -3012, -2356, -1536, -796, -265, 129, 466, 610, 425, 85, -155, -268, -299, -222, -121, -175, -335, -312, 14, 509, 1030, 1470, 1699, }, + /* LAT: 20 */ { 1494, 1649, 1708, 1799, 1884, 1855, 1681, 1355, 781, -117, -1209, -2197, -2803, -2893, -2528, -1885, -1137, -461, 13, 332, 601, 746, 640, 383, 178, 51, -55, -135, -228, -437, -694, -758, -503, -19, 561, 1114, 1494, }, + /* LAT: 30 */ { 1160, 1544, 1816, 2050, 2215, 2221, 2017, 1567, 802, -275, -1438, -2341, -2756, -2673, -2242, -1629, -939, -294, 184, 499, 738, 885, 855, 696, 542, 408, 230, -8, -317, -718, -1109, -1275, -1099, -642, -30, 613, 1160, }, + /* LAT: 40 */ { 765, 1379, 1900, 2314, 2577, 2622, 2393, 1831, 876, -403, -1685, -2574, -2892, -2722, -2248, -1623, -936, -278, 252, 629, 908, 1109, 1199, 1184, 1104, 944, 647, 195, -387, -1030, -1576, -1826, -1696, -1253, -625, 77, 765, }, + /* LAT: 50 */ { 437, 1218, 1936, 2528, 2919, 3036, 2799, 2116, 928, -623, -2091, -3030, -3327, -3121, -2604, -1925, -1180, -449, 193, 719, 1153, 1519, 1805, 1977, 1991, 1784, 1297, 527, -430, -1381, -2085, -2373, -2234, -1771, -1112, -355, 437, }, + /* LAT: 60 */ { 198, 1087, 1936, 2674, 3216, 3452, 3243, 2411, 855, -1158, -2926, -3941, -4206, -3936, -3339, -2560, -1696, -817, 27, 812, 1534, 2191, 2754, 3160, 3315, 3098, 2397, 1190, -317, -1699, -2593, -2902, -2725, -2214, -1502, -680, 198, }, + /* LAT: 70 */ { -111, 863, 1803, 2648, 3305, 3634, 3405, 2285, 58, -2678, -4700, -5589, -5627, -5143, -4352, -3383, -2317, -1205, -83, 1026, 2098, 3108, 4014, 4748, 5204, 5214, 4533, 2927, 575, -1644, -2988, -3441, -3276, -2730, -1962, -1068, -111, }, // WARNING! black out zone + /* LAT: 80 */ { -1054, -90, 801, 1527, 1946, 1812, 731, -1632, -4654, -6842, -7732, -7698, -7114, -6204, -5096, -3864, -2554, -1197, 184, 1572, 2947, 4292, 5581, 6779, 7828, 8630, 8988, 8486, 6313, 2026, -1910, -3677, -4009, -3636, -2914, -2019, -1054, }, // WARNING! black out zone + /* LAT: 90 */ { -30607,-28773,-26938,-25104,-23269,-21435,-19601,-17766,-15932,-14097,-12263,-10429, -8594, -6760, -4926, -3091, -1257, 578, 2412, 4246, 6081, 7915, 9749, 11584, 13418, 15253, 17087, 18921, 20756, 22590, 24424, 26259, 28093, 29928, 31762,-32441,-30607, }, // WARNING! black out zone }; -static constexpr float WMM_DECLINATION_MIN_RAD = -3.100; // latitude: 90, longitude: 170 -static constexpr float WMM_DECLINATION_MAX_RAD = 3.118; // latitude: -90, longitude: 150 +static constexpr float WMM_DECLINATION_SCALE_TO_DEGREES = 0.00545143529f; +static constexpr float WMM_DECLINATION_MIN_DEGREES = -176.9f; // latitude: 90, longitude: 170 +static constexpr float WMM_DECLINATION_MAX_DEGREES = 178.6f; // latitude: -90, longitude: 150 -// Magnetic inclination data in radians * 10^-4 +// Magnetic inclination data in 0.002699 degrees // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2024.0684, +// Date: 2024.41257, static constexpr const int16_t inclination_table[19][37] { // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, - /* LAT: -90 */ { -12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562,-12562, }, - /* LAT: -80 */ { -13644,-13510,-13349,-13169,-12975,-12774,-12571,-12371,-12181,-12005,-11849,-11715,-11605,-11519,-11455,-11414,-11394,-11396,-11423,-11477,-11561,-11676,-11821,-11995,-12194,-12412,-12641,-12874,-13102,-13314,-13500,-13649,-13752,-13802,-13798,-13743,-13644, }, - /* LAT: -70 */ { -14091,-13772,-13453,-13130,-12799,-12455,-12102,-11746,-11404,-11097,-10846,-10664,-10553,-10501,-10487,-10488,-10492,-10499,-10521,-10576,-10685,-10861,-11110,-11427,-11802,-12221,-12670,-13135,-13601,-14053,-14469,-14811,-14995,-14937,-14705,-14407,-14091, }, // WARNING! black out zone - /* LAT: -60 */ { -13509,-13155,-12816,-12483,-12140,-11768,-11353,-10900,-10434,-10006, -9680, -9510, -9511, -9650, -9853,-10040,-10158,-10193,-10174,-10157,-10205,-10369,-10664,-11078,-11581,-12140,-12727,-13324,-13913,-14474,-14971,-15259,-15076,-14687,-14279,-13884,-13509, }, // WARNING! black out zone - /* LAT: -50 */ { -12492,-12148,-11816,-11493,-11170,-10823,-10425, -9954, -9426, -8909, -8526, -8410, -8620, -9086, -9648,-10152,-10499,-10648,-10604,-10443,-10301,-10315,-10547,-10974,-11522,-12117,-12703,-13240,-13683,-13979,-14086,-14012,-13807,-13523,-13193,-12844,-12492, }, - /* LAT: -40 */ { -11239,-10888,-10538,-10191, -9852, -9514, -9155, -8732, -8213, -7652, -7235, -7210, -7686, -8518, -9438,-10259,-10906,-11324,-11443,-11256,-10906,-10641,-10650,-10946,-11416,-11922,-12363,-12679,-12833,-12837,-12748,-12613,-12435,-12204,-11917,-11588,-11239, }, - /* LAT: -30 */ { -9603, -9218, -8834, -8442, -8050, -7677, -7323, -6936, -6425, -5817, -5378, -5486, -6282, -7498, -8746, -9831,-10733,-11426,-11801,-11765,-11376,-10868,-10548,-10564,-10824,-11146,-11397,-11501,-11434,-11258,-11088,-10959,-10818,-10614,-10333, -9985, -9603, }, - /* LAT: -20 */ { -7374, -6924, -6499, -6068, -5624, -5196, -4810, -4404, -3842, -3160, -2729, -3025, -4173, -5803, -7426, -8778, -9823,-10578,-10993,-11002,-10618, -9994, -9442, -9212, -9270, -9434, -9570, -9580, -9403, -9133, -8945, -8869, -8775, -8573, -8256, -7840, -7374, }, - /* LAT: -10 */ { -4420, -3869, -3405, -2963, -2506, -2057, -1649, -1205, -588, 107, 447, -20, -1391, -3330, -5297, -6877, -7942, -8550, -8793, -8710, -8280, -7581, -6919, -6585, -6553, -6646, -6760, -6778, -6587, -6293, -6148, -6175, -6151, -5943, -5559, -5026, -4420, }, - /* LAT: 0 */ { -913, -272, 205, 613, 1033, 1448, 1832, 2262, 2833, 3397, 3578, 3047, 1712, -222, -2259, -3876, -4847, -5247, -5286, -5101, -4641, -3909, -3204, -2843, -2786, -2852, -2974, -3044, -2913, -2681, -2639, -2805, -2890, -2713, -2288, -1650, -913, }, - /* LAT: 10 */ { 2555, 3197, 3642, 3988, 4344, 4710, 5056, 5430, 5867, 6224, 6243, 5728, 4617, 3031, 1345, -3, -774, -994, -886, -639, -213, 438, 1072, 1402, 1466, 1428, 1331, 1240, 1281, 1380, 1291, 1002, 789, 851, 1205, 1815, 2555, }, - /* LAT: 20 */ { 5412, 5951, 6338, 6637, 6951, 7293, 7631, 7968, 8290, 8479, 8378, 7898, 7045, 5939, 4818, 3927, 3420, 3323, 3486, 3737, 4077, 4551, 5012, 5264, 5324, 5314, 5270, 5211, 5196, 5172, 4987, 4639, 4326, 4229, 4402, 4831, 5412, }, - /* LAT: 30 */ { 7567, 7945, 8266, 8551, 8860, 9206, 9559, 9893, 10162, 10266, 10112, 9680, 9044, 8331, 7675, 7174, 6895, 6867, 7019, 7236, 7488, 7793, 8083, 8256, 8315, 8334, 8340, 8330, 8304, 8215, 7986, 7624, 7260, 7036, 7020, 7218, 7567, }, - /* LAT: 40 */ { 9266, 9488, 9744, 10030, 10356, 10715, 11080, 11414, 11660, 11735, 11580, 11216, 10741, 10270, 9878, 9601, 9460, 9464, 9581, 9747, 9926, 10114, 10287, 10412, 10492, 10556, 10614, 10648, 10628, 10512, 10265, 9909, 9538, 9251, 9108, 9122, 9266, }, - /* LAT: 50 */ { 10802, 10922, 11123, 11391, 11713, 12065, 12416, 12729, 12947, 13002, 12861, 12563, 12200, 11857, 11585, 11403, 11315, 11317, 11388, 11494, 11611, 11729, 11847, 11964, 12083, 12208, 12324, 12396, 12383, 12253, 12003, 11672, 11331, 11046, 10857, 10777, 10802, }, - /* LAT: 60 */ { 12320, 12390, 12538, 12754, 13021, 13319, 13619, 13883, 14057, 14084, 13950, 13702, 13416, 13147, 12929, 12775, 12687, 12659, 12678, 12728, 12798, 12885, 12993, 13127, 13288, 13465, 13630, 13737, 13739, 13614, 13387, 13107, 12829, 12593, 12423, 12331, 12320, }, - /* LAT: 70 */ { 13757, 13796, 13888, 14027, 14204, 14404, 14610, 14790, 14895, 14879, 14747, 14549, 14334, 14132, 13960, 13826, 13734, 13680, 13663, 13677, 13720, 13794, 13899, 14037, 14205, 14393, 14577, 14716, 14759, 14683, 14520, 14321, 14127, 13963, 13842, 13772, 13757, }, // WARNING! black out zone - /* LAT: 80 */ { 14991, 15001, 15036, 15092, 15166, 15249, 15325, 15371, 15359, 15287, 15180, 15059, 14938, 14826, 14727, 14646, 14584, 14544, 14526, 14531, 14558, 14608, 14680, 14774, 14886, 15014, 15150, 15283, 15390, 15432, 15389, 15299, 15201, 15115, 15049, 15007, 14991, }, // WARNING! black out zone - /* LAT: 90 */ { 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, 15400, }, // WARNING! black out zone + /* LAT: -90 */ { -26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663,-26663, }, + /* LAT: -80 */ { -28959,-28675,-28333,-27951,-27540,-27113,-26682,-26258,-25854,-25482,-25150,-24867,-24633,-24450,-24315,-24226,-24183,-24188,-24245,-24361,-24539,-24783,-25092,-25462,-25884,-26346,-26833,-27328,-27812,-28262,-28656,-28973,-29191,-29297,-29287,-29170,-28959, }, + /* LAT: -70 */ { -29906,-29230,-28552,-27867,-27164,-26436,-25685,-24930,-24205,-23555,-23023,-22638,-22403,-22293,-22261,-22264,-22272,-22285,-22330,-22447,-22678,-23053,-23582,-24256,-25053,-25944,-26898,-27885,-28874,-29833,-30717,-31441,-31829,-31704,-31211,-30578,-29906, }, // WARNING! black out zone + /* LAT: -60 */ { -28675,-27922,-27202,-26495,-25766,-24977,-24097,-23135,-22147,-21240,-20551,-20191,-20196,-20492,-20922,-21317,-21564,-21635,-21592,-21554,-21657,-22006,-22635,-23517,-24587,-25775,-27023,-28290,-29540,-30732,-31788,-32398,-32004,-31178,-30310,-29471,-28675, }, // WARNING! black out zone + /* LAT: -50 */ { -26518,-25787,-25081,-24395,-23709,-22973,-22127,-21129,-20009,-18913,-18103,-17862,-18313,-19305,-20497,-21563,-22295,-22602,-22503,-22157,-21856,-21888,-22387,-23298,-24466,-25730,-26974,-28112,-29052,-29680,-29904,-29747,-29313,-28709,-28008,-27265,-26518, }, + /* LAT: -40 */ { -23859,-23111,-22367,-21631,-20911,-20195,-19433,-18535,-17434,-16245,-15366,-15320,-16340,-18110,-20062,-21801,-23168,-24048,-24290,-23883,-23136,-22578,-22605,-23240,-24242,-25316,-26251,-26918,-27243,-27250,-27062,-26776,-26401,-25910,-25300,-24601,-23859, }, + /* LAT: -30 */ { -20386,-19567,-18750,-17917,-17085,-16293,-15544,-14724,-13640,-12351,-11424,-11666,-13370,-15956,-18605,-20904,-22812,-24277,-25061,-24972,-24136,-23057,-22385,-22426,-22983,-23667,-24196,-24414,-24269,-23896,-23536,-23265,-22968,-22536,-21939,-21200,-20386, }, + /* LAT: -20 */ { -15655,-14695,-13789,-12872,-11930,-11023,-10206, -9346, -8156, -6711, -5803, -6449, -8904,-12373,-15815,-18676,-20887,-22480,-23348,-23356,-22530,-21203,-20034,-19551,-19678,-20026,-20313,-20331,-19954,-19380,-18986,-18828,-18633,-18206,-17535,-16650,-15655, }, + /* LAT: -10 */ { -9385, -8209, -7218, -6278, -5306, -4355, -3491, -2554, -1249, 222, 935, -75, -3005, -7132,-11304,-14645,-16891,-18169,-18676,-18490,-17568,-16077,-14672,-13969,-13905,-14103,-14342,-14377,-13970,-13348,-13045,-13109,-13066,-12627,-11813,-10677, -9385, }, + /* LAT: 0 */ { -1941, -572, 447, 1316, 2208, 3089, 3901, 4808, 6012, 7204, 7577, 6434, 3584, -534, -4855, -8273,-10317,-11152,-11228,-10827, -9842, -8280, -6784, -6023, -5903, -6042, -6299, -6446, -6167, -5676, -5592, -5955, -6145, -5775, -4872, -3514, -1941, }, + /* LAT: 10 */ { 5421, 6791, 7742, 8478, 9236, 10013, 10743, 11532, 12453, 13205, 13235, 12131, 9759, 6384, 2805, -45, -1666, -2121, -1883, -1354, -444, 946, 2291, 2989, 3125, 3048, 2844, 2651, 2737, 2945, 2751, 2128, 1664, 1792, 2543, 3843, 5421, }, + /* LAT: 20 */ { 11488, 12637, 13462, 14098, 14766, 15492, 16206, 16917, 17594, 17991, 17771, 16744, 14925, 12573, 10194, 8309, 7245, 7046, 7400, 7937, 8662, 9672, 10651, 11186, 11314, 11296, 11203, 11079, 11045, 10991, 10595, 9848, 9176, 8967, 9335, 10249, 11488, }, + /* LAT: 30 */ { 16064, 16869, 17552, 18157, 18813, 19547, 20295, 21001, 21567, 21785, 21452, 20532, 19178, 17663, 16272, 15213, 14627, 14573, 14900, 15364, 15901, 16550, 17167, 17534, 17662, 17704, 17719, 17697, 17639, 17449, 16959, 16185, 15409, 14931, 14899, 15320, 16064, }, + /* LAT: 40 */ { 19670, 20142, 20688, 21294, 21986, 22747, 23519, 24226, 24746, 24903, 24570, 23795, 22787, 21787, 20958, 20373, 20075, 20088, 20340, 20695, 21077, 21477, 21845, 22111, 22280, 22419, 22544, 22615, 22571, 22323, 21795, 21038, 20247, 19638, 19335, 19364, 19670, }, + /* LAT: 50 */ { 22933, 23187, 23612, 24182, 24864, 25609, 26354, 27016, 27477, 27593, 27291, 26658, 25888, 25162, 24586, 24201, 24016, 24023, 24176, 24404, 24654, 24906, 25156, 25404, 25658, 25925, 26171, 26324, 26295, 26017, 25483, 24780, 24056, 23451, 23049, 22878, 22933, }, + /* LAT: 60 */ { 26155, 26303, 26615, 27072, 27639, 28270, 28905, 29465, 29833, 29890, 29605, 29081, 28474, 27904, 27442, 27117, 26932, 26874, 26915, 27023, 27173, 27359, 27588, 27873, 28215, 28592, 28943, 29169, 29170, 28904, 28420, 27827, 27237, 26736, 26376, 26179, 26155, }, + /* LAT: 70 */ { 29204, 29286, 29480, 29773, 30146, 30572, 31008, 31388, 31612, 31580, 31301, 30883, 30427, 29998, 29634, 29351, 29155, 29043, 29006, 29037, 29130, 29287, 29511, 29805, 30163, 30562, 30953, 31246, 31335, 31173, 30826, 30404, 29992, 29643, 29386, 29237, 29204, }, // WARNING! black out zone + /* LAT: 80 */ { 31820, 31841, 31914, 32033, 32188, 32362, 32524, 32622, 32598, 32449, 32224, 31969, 31713, 31474, 31265, 31093, 30963, 30878, 30840, 30850, 30908, 31015, 31169, 31368, 31607, 31878, 32167, 32450, 32679, 32767, 32672, 32479, 32271, 32087, 31945, 31855, 31820, }, // WARNING! black out zone + /* LAT: 90 */ { 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, 32694, }, // WARNING! black out zone }; -static constexpr float WMM_INCLINATION_MIN_RAD = -1.526; // latitude: -60, longitude: 130 -static constexpr float WMM_INCLINATION_MAX_RAD = 1.543; // latitude: 80, longitude: 110 +static constexpr float WMM_INCLINATION_SCALE_TO_DEGREES = 0.00269892697f; +static constexpr float WMM_INCLINATION_MIN_DEGREES = -87.4f; // latitude: -60, longitude: 130 +static constexpr float WMM_INCLINATION_MAX_DEGREES = 88.4f; // latitude: 80, longitude: 110 -// Magnetic strength data in milli-Gauss * 10 +// Magnetic totalintensity data in 2.042 nanoTesla // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2024.0684, -static constexpr const int16_t strength_table[19][37] { - // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, - /* LAT: -90 */ { 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, 5442, }, - /* LAT: -80 */ { 6049, 5985, 5905, 5812, 5709, 5596, 5478, 5356, 5234, 5114, 5000, 4895, 4802, 4722, 4660, 4615, 4591, 4590, 4612, 4659, 4730, 4825, 4940, 5072, 5216, 5367, 5517, 5660, 5791, 5905, 5997, 6065, 6109, 6128, 6123, 6096, 6049, }, - /* LAT: -70 */ { 6294, 6159, 6007, 5841, 5661, 5468, 5263, 5048, 4829, 4615, 4413, 4231, 4074, 3943, 3840, 3764, 3718, 3706, 3735, 3813, 3942, 4124, 4354, 4623, 4919, 5227, 5531, 5815, 6064, 6268, 6418, 6512, 6553, 6544, 6493, 6407, 6294, }, - /* LAT: -60 */ { 6179, 5985, 5782, 5572, 5352, 5116, 4860, 4581, 4289, 4000, 3734, 3508, 3330, 3196, 3098, 3025, 2974, 2954, 2981, 3074, 3245, 3499, 3829, 4217, 4640, 5075, 5496, 5880, 6204, 6451, 6613, 6690, 6691, 6626, 6511, 6358, 6179, }, - /* LAT: -50 */ { 5838, 5606, 5373, 5140, 4907, 4661, 4390, 4087, 3758, 3427, 3126, 2888, 2728, 2638, 2590, 2555, 2520, 2492, 2498, 2572, 2750, 3045, 3445, 3920, 4428, 4934, 5408, 5826, 6163, 6403, 6540, 6579, 6535, 6423, 6260, 6060, 5838, }, - /* LAT: -40 */ { 5389, 5142, 4895, 4654, 4419, 4181, 3925, 3641, 3327, 3001, 2704, 2483, 2369, 2344, 2363, 2383, 2385, 2371, 2357, 2387, 2523, 2806, 3230, 3748, 4296, 4819, 5283, 5669, 5960, 6146, 6233, 6234, 6164, 6031, 5849, 5629, 5389, }, - /* LAT: -30 */ { 4876, 4633, 4393, 4157, 3930, 3711, 3491, 3260, 3004, 2728, 2471, 2291, 2224, 2250, 2317, 2387, 2451, 2498, 2517, 2528, 2600, 2807, 3178, 3672, 4204, 4700, 5114, 5427, 5628, 5727, 5752, 5725, 5645, 5511, 5331, 5113, 4876, }, - /* LAT: -20 */ { 4320, 4105, 3895, 3689, 3492, 3310, 3141, 2977, 2798, 2599, 2408, 2277, 2239, 2286, 2376, 2487, 2614, 2739, 2823, 2856, 2882, 2985, 3234, 3624, 4077, 4503, 4847, 5077, 5180, 5186, 5156, 5109, 5027, 4899, 4732, 4534, 4320, }, - /* LAT: -10 */ { 3789, 3627, 3472, 3325, 3190, 3069, 2965, 2872, 2773, 2657, 2535, 2437, 2396, 2424, 2514, 2644, 2799, 2955, 3075, 3134, 3147, 3177, 3307, 3565, 3894, 4216, 4478, 4637, 4669, 4616, 4548, 4485, 4398, 4273, 4123, 3959, 3789, }, - /* LAT: 0 */ { 3412, 3317, 3232, 3159, 3103, 3064, 3037, 3017, 2990, 2941, 2861, 2768, 2690, 2664, 2710, 2815, 2947, 3082, 3194, 3267, 3297, 3320, 3399, 3560, 3772, 3986, 4165, 4268, 4272, 4203, 4115, 4023, 3912, 3781, 3646, 3521, 3412, }, - /* LAT: 10 */ { 3282, 3250, 3229, 3224, 3248, 3294, 3348, 3399, 3432, 3421, 3352, 3238, 3113, 3022, 3000, 3044, 3126, 3225, 3325, 3409, 3473, 3536, 3626, 3746, 3884, 4024, 4143, 4212, 4212, 4149, 4038, 3895, 3733, 3573, 3437, 3339, 3282, }, - /* LAT: 20 */ { 3399, 3400, 3425, 3477, 3568, 3688, 3815, 3930, 4009, 4020, 3946, 3805, 3640, 3504, 3432, 3423, 3460, 3535, 3632, 3730, 3821, 3921, 4033, 4145, 4256, 4369, 4472, 4537, 4546, 4485, 4347, 4143, 3915, 3705, 3541, 3439, 3399, }, - /* LAT: 30 */ { 3722, 3726, 3778, 3876, 4017, 4186, 4361, 4515, 4622, 4648, 4575, 4421, 4234, 4074, 3972, 3929, 3935, 3988, 4077, 4176, 4275, 4382, 4499, 4617, 4737, 4866, 4988, 5074, 5098, 5038, 4880, 4639, 4364, 4109, 3909, 3779, 3722, }, - /* LAT: 40 */ { 4222, 4216, 4278, 4399, 4564, 4750, 4932, 5088, 5192, 5218, 5151, 5004, 4820, 4649, 4524, 4451, 4427, 4452, 4515, 4595, 4683, 4781, 4897, 5030, 5182, 5345, 5497, 5606, 5643, 5586, 5428, 5190, 4916, 4657, 4446, 4299, 4222, }, - /* LAT: 50 */ { 4832, 4820, 4872, 4979, 5123, 5281, 5431, 5552, 5627, 5637, 5575, 5450, 5290, 5128, 4993, 4898, 4845, 4836, 4862, 4914, 4984, 5076, 5196, 5346, 5522, 5707, 5873, 5989, 6030, 5984, 5851, 5656, 5432, 5218, 5038, 4906, 4832, }, - /* LAT: 60 */ { 5393, 5376, 5400, 5459, 5543, 5637, 5724, 5792, 5827, 5820, 5768, 5675, 5556, 5430, 5312, 5218, 5153, 5120, 5119, 5148, 5205, 5293, 5412, 5559, 5726, 5894, 6040, 6142, 6183, 6160, 6077, 5951, 5807, 5665, 5543, 5450, 5393, }, - /* LAT: 70 */ { 5726, 5703, 5697, 5707, 5727, 5753, 5778, 5793, 5795, 5777, 5740, 5684, 5616, 5541, 5468, 5405, 5358, 5331, 5327, 5348, 5393, 5463, 5555, 5664, 5781, 5895, 5993, 6065, 6103, 6106, 6077, 6024, 5957, 5887, 5821, 5766, 5726, }, - /* LAT: 80 */ { 5790, 5772, 5756, 5743, 5733, 5723, 5713, 5702, 5688, 5670, 5649, 5625, 5599, 5573, 5549, 5529, 5516, 5511, 5517, 5532, 5558, 5594, 5636, 5684, 5734, 5782, 5824, 5858, 5882, 5895, 5897, 5889, 5875, 5855, 5833, 5811, 5790, }, - /* LAT: 90 */ { 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, 5685, }, +// Date: 2024.41257, +static constexpr const int16_t totalintensity_table[19][37] { + // LONGITUDE: -180, -170, -160, -150, -140, -130, -120, -110, -100, -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 60, 70, 80, 90, 100, 110, 120, 130, 140, 150, 160, 170, 180, + /* LAT: -90 */ { 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, 26644, }, + /* LAT: -80 */ { 29615, 29299, 28908, 28453, 27945, 27395, 26815, 26218, 25619, 25032, 24474, 23959, 23502, 23115, 22808, 22592, 22476, 22469, 22578, 22807, 23157, 23621, 24187, 24836, 25543, 26279, 27014, 27716, 28358, 28914, 29365, 29700, 29913, 30004, 29979, 29846, 29615, }, + /* LAT: -70 */ { 30814, 30152, 29407, 28592, 27711, 26765, 25759, 24706, 23635, 22584, 21596, 20706, 19938, 19300, 18794, 18422, 18195, 18139, 18285, 18666, 19302, 20195, 21322, 22642, 24092, 25601, 27090, 28480, 29701, 30696, 31432, 31894, 32090, 32046, 31794, 31371, 30814, }, // WARNING! black out zone + /* LAT: -60 */ { 30256, 29303, 28305, 27273, 26195, 25041, 23783, 22419, 20989, 19575, 18274, 17168, 16296, 15644, 15162, 14803, 14555, 14459, 14593, 15046, 15888, 17136, 18753, 20656, 22732, 24862, 26925, 28803, 30388, 31598, 32391, 32767, 32766, 32449, 31882, 31131, 30256, }, // WARNING! black out zone + /* LAT: -50 */ { 28585, 27448, 26301, 25162, 24017, 22813, 21484, 19998, 18389, 16768, 15296, 14132, 13352, 12914, 12677, 12503, 12330, 12195, 12223, 12590, 13464, 14914, 16881, 19207, 21698, 24175, 26497, 28540, 30193, 31368, 32034, 32224, 32006, 31456, 30654, 29673, 28585, }, + /* LAT: -40 */ { 26388, 25175, 23965, 22783, 21629, 20463, 19212, 17819, 16281, 14687, 13231, 12153, 11593, 11476, 11568, 11662, 11671, 11598, 11530, 11680, 12349, 13743, 15828, 18371, 21056, 23615, 25886, 27774, 29196, 30107, 30531, 30536, 30188, 29538, 28642, 27566, 26388, }, + /* LAT: -30 */ { 23875, 22687, 21506, 20350, 19238, 18163, 17088, 15953, 14699, 13348, 12090, 11211, 10886, 11019, 11346, 11688, 11997, 12225, 12312, 12367, 12723, 13749, 15575, 17999, 20611, 23034, 25059, 26587, 27567, 28051, 28176, 28039, 27646, 26992, 26106, 25040, 23875, }, + /* LAT: -20 */ { 21154, 20100, 19069, 18060, 17097, 16201, 15373, 14569, 13693, 12716, 11782, 11141, 10963, 11194, 11640, 12182, 12805, 13413, 13818, 13974, 14106, 14617, 15845, 17765, 19985, 22068, 23750, 24872, 25370, 25402, 25252, 25023, 24621, 23994, 23174, 22204, 21154, }, + /* LAT: -10 */ { 18558, 17760, 17002, 16280, 15615, 15025, 14516, 14058, 13572, 13001, 12403, 11927, 11727, 11873, 12314, 12952, 13712, 14475, 15055, 15339, 15405, 15556, 16199, 17471, 19086, 20662, 21941, 22713, 22869, 22606, 22275, 21968, 21540, 20930, 20196, 19388, 18558, }, + /* LAT: 0 */ { 16709, 16246, 15828, 15466, 15193, 15003, 14869, 14765, 14634, 14389, 14000, 13543, 13167, 13043, 13274, 13791, 14440, 15095, 15645, 15999, 16143, 16257, 16647, 17443, 18484, 19534, 20407, 20910, 20925, 20587, 20154, 19705, 19162, 18518, 17858, 17244, 16709, }, + /* LAT: 10 */ { 16075, 15916, 15810, 15786, 15900, 16127, 16390, 16638, 16796, 16740, 16399, 15842, 15234, 14793, 14691, 14911, 15314, 15798, 16286, 16698, 17008, 17318, 17762, 18355, 19031, 19718, 20303, 20638, 20637, 20325, 19783, 19078, 18284, 17502, 16834, 16355, 16075, }, + /* LAT: 20 */ { 16646, 16650, 16769, 17024, 17466, 18051, 18672, 19233, 19618, 19672, 19310, 18619, 17813, 17152, 16804, 16764, 16949, 17315, 17793, 18272, 18719, 19208, 19759, 20311, 20853, 21411, 21916, 22235, 22275, 21976, 21295, 20297, 19178, 18148, 17345, 16845, 16646, }, + /* LAT: 30 */ { 18228, 18245, 18498, 18973, 19662, 20491, 21341, 22097, 22620, 22746, 22388, 21634, 20724, 19941, 19447, 19240, 19272, 19535, 19972, 20460, 20944, 21470, 22044, 22622, 23213, 23845, 24444, 24864, 24980, 24684, 23906, 22724, 21376, 20127, 19146, 18510, 18228, }, + /* LAT: 40 */ { 20676, 20646, 20945, 21535, 22339, 23247, 24138, 24900, 25411, 25538, 25208, 24491, 23592, 22759, 22150, 21796, 21685, 21808, 22117, 22515, 22946, 23429, 23995, 24649, 25392, 26192, 26939, 27472, 27650, 27367, 26593, 25423, 24081, 22812, 21778, 21055, 20676, }, + /* LAT: 50 */ { 23666, 23603, 23852, 24371, 25074, 25848, 26580, 27175, 27541, 27593, 27291, 26681, 25898, 25111, 24452, 23986, 23733, 23688, 23819, 24073, 24420, 24871, 25459, 26197, 27059, 27964, 28776, 29343, 29542, 29310, 28661, 27703, 26610, 25561, 24680, 24034, 23666, }, + /* LAT: 60 */ { 26412, 26324, 26436, 26723, 27132, 27589, 28019, 28351, 28524, 28493, 28240, 27789, 27209, 26590, 26018, 25556, 25239, 25081, 25078, 25221, 25503, 25932, 26515, 27239, 28055, 28877, 29590, 30087, 30289, 30171, 29764, 29152, 28446, 27753, 27153, 26695, 26412, }, + /* LAT: 70 */ { 28043, 27927, 27896, 27940, 28040, 28166, 28285, 28364, 28372, 28290, 28109, 27838, 27503, 27138, 26784, 26477, 26247, 26116, 26097, 26198, 26422, 26766, 27217, 27750, 28323, 28879, 29358, 29708, 29894, 29908, 29765, 29507, 29181, 28836, 28513, 28242, 28043, }, // WARNING! black out zone + /* LAT: 80 */ { 28358, 28266, 28189, 28125, 28072, 28025, 27977, 27922, 27854, 27769, 27667, 27549, 27422, 27295, 27178, 27083, 27020, 26999, 27025, 27102, 27229, 27402, 27613, 27847, 28090, 28324, 28531, 28697, 28813, 28875, 28885, 28848, 28776, 28680, 28572, 28462, 28358, }, // WARNING! black out zone + /* LAT: 90 */ { 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, 27849, }, // WARNING! black out zone }; -static constexpr float WMM_STRENGTH_MIN_GS = 22.2; // latitude: -30, longitude: -60 -static constexpr float WMM_STRENGTH_MAX_GS = 66.9; // latitude: -60, longitude: 140 -static constexpr float WMM_STRENGTH_MEAN_GS = 46.4; -static constexpr float WMM_STRENGTH_MEDIAN_GS = 48.8; - - +static constexpr float WMM_TOTALINTENSITY_SCALE_TO_NANOTESLA = 2.04183477f; +static constexpr float WMM_TOTALINTENSITY_MIN_NANOTESLA = 22226.9f; // latitude: -30, longitude: -60 +static constexpr float WMM_TOTALINTENSITY_MAX_NANOTESLA = 66904.8f; // latitude: -60, longitude: 130 diff --git a/src/lib/world_magnetic_model/test_geo_lookup.cpp b/src/lib/world_magnetic_model/test_geo_lookup.cpp index f8dfe3e45bb3..674d4252f14c 100644 --- a/src/lib/world_magnetic_model/test_geo_lookup.cpp +++ b/src/lib/world_magnetic_model/test_geo_lookup.cpp @@ -40,15 +40,15 @@ TEST(GeoLookupTest, declination) { - EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.6, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.7, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -175), 31.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -170), 31.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -165), 31.7, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -160), 31.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -160), 31.5, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -155), 31.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -150), 30.9, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -145), 30.6, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -140), 30.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -145), 30.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -140), 30.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -135), 30.3, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -130), 30.2, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -125), 30.1, 0.36 + 1.0); @@ -57,70 +57,70 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-50, -110), 29.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -105), 28.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -100), 27.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -95), 25.6, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -95), 25.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -90), 23.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -85), 20.7, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -80), 17.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -80), 17.5, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -75), 14.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -70), 10.3, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -70), 10.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -65), 6.4, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -60), 2.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -55), -0.9, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -55), -1.0, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -50), -4.1, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -45), -6.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -40), -9.0, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -35), -10.7, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -30), -12.1, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -25), -13.3, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -25), -13.2, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, -20), -14.3, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -15), -15.5, 0.53 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -10), -16.9, 0.53 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, -5), -18.7, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -15), -15.4, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -10), -16.8, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -5), -18.6, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 0), -20.9, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 5), -23.7, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 10), -26.9, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 15), -30.4, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 20), -34.1, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 20), -34.2, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 25), -37.9, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.5, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -44.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.6, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -45.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -48.1, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.8, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 50), -53.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 55), -55.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 50), -53.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 55), -55.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 60), -56.6, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 65), -57.5, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 70), -57.9, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 75), -57.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 75), -57.7, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 80), -56.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 85), -54.9, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 90), -52.3, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 95), -48.5, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 100), -43.6, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 100), -43.5, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 105), -37.3, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 110), -30.0, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 115), -21.8, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 110), -29.9, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 115), -21.7, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 120), -13.3, 0.59 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 125), -5.1, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 125), -5.0, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 130), 2.5, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 135), 9.0, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 135), 9.1, 0.55 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 140), 14.6, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 145), 19.1, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 150), 22.7, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 150), 22.8, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 155), 25.6, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 160), 27.7, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 165), 29.3, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 170), 30.4, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 160), 27.8, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 165), 29.4, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 170), 30.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-50, 175), 31.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-50, 180), 31.6, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -180), 26.6, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -175), 26.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 180), 31.7, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -180), 26.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -175), 27.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -170), 27.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -165), 27.0, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -160), 26.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -165), 27.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -160), 27.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -155), 26.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -150), 26.5, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -145), 26.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -150), 26.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -145), 26.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -140), 26.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -135), 26.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -130), 26.0, 0.36 + 1.0); @@ -128,72 +128,72 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-45, -120), 26.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -115), 26.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -110), 25.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -105), 25.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -105), 25.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -100), 24.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -95), 22.9, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -90), 20.9, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -85), 18.3, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -85), 18.2, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -80), 15.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -75), 11.3, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.2, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -75), 11.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.1, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -65), 2.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -60), -1.2, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -55), -5.0, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -55), -5.1, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -50), -8.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -45), -11.1, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -40), -13.3, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -35), -14.9, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, -30), -16.0, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.8, 0.53 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -20), -17.5, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -15), -18.1, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -10), -18.8, 0.58 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, -5), -19.9, 0.59 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 0), -21.5, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.8, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -20), -17.4, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -15), -18.0, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -10), -18.7, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -5), -19.8, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 0), -21.4, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 5), -23.7, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 10), -26.5, 0.56 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 15), -29.7, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 20), -33.2, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 25), -36.7, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 30), -40.1, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 25), -36.8, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 30), -40.2, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.3, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -46.0, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 45), -48.2, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -50.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 45), -48.3, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -50.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 55), -51.3, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 60), -51.9, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 65), -51.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 60), -52.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 65), -52.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 70), -51.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 75), -49.9, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 80), -47.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 85), -44.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 90), -40.8, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 95), -36.1, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 100), -30.6, 0.47 + 1.8); - EXPECT_NEAR(get_mag_declination_degrees(-45, 105), -24.6, 0.48 + 1.8); - EXPECT_NEAR(get_mag_declination_degrees(-45, 110), -18.4, 0.48 + 1.8); + EXPECT_NEAR(get_mag_declination_degrees(-45, 95), -36.0, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 100), -30.5, 0.47 + 1.8); + EXPECT_NEAR(get_mag_declination_degrees(-45, 105), -24.5, 0.48 + 1.8); + EXPECT_NEAR(get_mag_declination_degrees(-45, 110), -18.3, 0.48 + 1.8); EXPECT_NEAR(get_mag_declination_degrees(-45, 115), -12.2, 0.48 + 1.8); EXPECT_NEAR(get_mag_declination_degrees(-45, 120), -6.4, 0.47 + 1.8); - EXPECT_NEAR(get_mag_declination_degrees(-45, 125), -1.1, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 125), -1.0, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 130), 3.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 135), 8.2, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 140), 12.0, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 145), 15.4, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 140), 12.1, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 145), 15.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 150), 18.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 155), 20.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 160), 22.7, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 165), 24.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 170), 25.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-45, 175), 26.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-45, 180), 26.6, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 180), 26.7, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -180), 22.8, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -175), 23.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -175), 23.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -170), 23.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -165), 23.4, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -160), 23.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -160), 23.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -155), 23.2, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -150), 23.0, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -145), 22.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -150), 23.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -145), 22.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -140), 22.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -135), 22.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -130), 22.4, 0.35 + 1.0); @@ -201,68 +201,68 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-40, -120), 22.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -115), 22.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -110), 22.4, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -105), 22.1, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -100), 21.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -105), 22.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -100), 21.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -95), 20.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -90), 18.2, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -85), 15.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -80), 12.3, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -75), 8.4, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -75), 8.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -70), 4.0, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -65), -0.5, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -60), -4.9, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -55), -8.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -65), -0.6, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -60), -5.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -55), -9.0, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -50), -12.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -45), -15.1, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, -40), -17.2, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -35), -18.7, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -30), -19.6, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -25), -20.3, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -20), -20.6, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.8, 0.60 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -10), -20.9, 0.62 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.2, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -35), -18.6, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -30), -19.6, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -25), -20.2, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -20), -20.5, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.7, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -10), -20.8, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.1, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -21.9, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 5), -23.3, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 10), -25.4, 0.61 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 15), -28.1, 0.58 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 20), -31.2, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 25), -34.4, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 30), -37.5, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.2, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.5, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 45), -44.3, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 15), -28.2, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 20), -31.3, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 25), -34.5, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 30), -37.6, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.3, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.6, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 45), -44.4, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 50), -45.6, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 55), -46.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 55), -46.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 60), -46.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 65), -45.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 70), -43.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 75), -41.5, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 80), -38.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 80), -38.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 85), -34.9, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 90), -30.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 90), -30.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 95), -25.8, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 100), -20.8, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 105), -15.8, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 110), -11.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 115), -6.8, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 115), -6.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 120), -2.8, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 125), 0.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 125), 0.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 130), 4.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 135), 7.3, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 140), 10.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 140), 10.2, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 145), 12.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 150), 15.2, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 155), 17.2, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 155), 17.3, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 160), 19.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 165), 20.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 170), 21.5, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-40, 175), 22.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 175), 22.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-40, 180), 22.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -180), 19.7, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -175), 20.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -170), 20.2, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -165), 20.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -175), 20.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -170), 20.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -165), 20.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -160), 20.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -155), 20.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -150), 20.2, 0.34 + 1.0); @@ -275,34 +275,34 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, -115), 19.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -110), 19.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -105), 18.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -100), 18.4, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -95), 17.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -90), 15.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -100), 18.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -95), 17.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -90), 15.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -85), 12.9, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -80), 9.5, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -75), 5.4, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 0.9, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.8, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.3, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 0.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.4, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -55), -12.4, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -50), -15.7, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -50), -15.8, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -45), -18.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -40), -20.3, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -35), -21.6, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -30), -22.5, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -25), -23.0, 0.54 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -20), -23.2, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -15), -23.0, 0.60 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.6, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -25), -22.9, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -20), -23.1, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -15), -22.9, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.5, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.1, 0.65 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -21.9, 0.66 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -21.8, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 5), -22.2, 0.66 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 10), -23.3, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 15), -25.2, 0.61 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 20), -27.6, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 20), -27.7, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 25), -30.4, 0.54 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -33.0, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 35), -35.4, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -33.1, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 35), -35.5, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 40), -37.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 45), -38.8, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 50), -39.6, 0.42 + 1.0); @@ -324,19 +324,19 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-35, 130), 4.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 135), 6.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 140), 8.7, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 145), 10.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 145), 10.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 150), 12.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 155), 14.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 160), 16.2, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 165), 17.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-35, 170), 18.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 165), 17.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 170), 18.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 175), 19.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-35, 180), 19.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -180), 17.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -175), 17.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -170), 17.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -165), 17.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -160), 17.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -160), 17.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -155), 17.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -150), 17.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -145), 17.6, 0.33 + 1.0); @@ -348,67 +348,67 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-30, -115), 16.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -110), 16.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -105), 16.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -100), 15.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -100), 15.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -95), 14.5, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -90), 12.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -85), 10.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -80), 6.8, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -75), 2.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -70), -2.0, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -65), -6.7, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -60), -11.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -55), -15.1, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -90), 12.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -85), 10.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -80), 6.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -75), 2.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -70), -2.1, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -65), -6.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -60), -11.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -55), -15.2, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -50), -18.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -45), -20.7, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -40), -22.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -35), -23.5, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -30), -24.3, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -30), -24.2, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, -25), -24.6, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.6, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.1, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.3, 0.60 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.0, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.5, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.1, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.2, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -21.9, 0.63 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 0), -20.7, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 5), -19.8, 0.65 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 10), -19.7, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 10), -19.7, 0.64 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 15), -20.5, 0.61 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 20), -22.1, 0.58 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 25), -24.2, 0.54 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 30), -26.5, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 25), -24.3, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 30), -26.6, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 35), -28.7, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 40), -30.5, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 45), -31.7, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 50), -32.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 45), -31.8, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 50), -32.4, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 55), -32.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 60), -31.5, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 65), -30.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 65), -30.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 70), -28.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 75), -25.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 80), -22.4, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 85), -19.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 85), -19.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 90), -15.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 95), -12.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 100), -8.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 105), -5.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 110), -3.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 115), -1.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 120), 0.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 125), 2.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 120), 0.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 125), 2.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 130), 3.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 135), 5.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 140), 7.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 145), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 150), 11.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 155), 12.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 160), 13.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 160), 14.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 165), 15.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 170), 16.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-30, 175), 16.7, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 170), 16.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 175), 16.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-30, 180), 17.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -180), 15.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -175), 15.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -170), 15.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -165), 15.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -165), 15.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -160), 15.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -155), 15.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -150), 15.6, 0.32 + 1.0); @@ -416,47 +416,47 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, -140), 15.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -135), 15.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -130), 14.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -125), 14.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -125), 14.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -120), 14.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -115), 14.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -110), 13.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -110), 13.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -105), 13.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -100), 13.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -95), 12.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -95), 12.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -90), 10.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -85), 7.7, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -80), 4.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -75), 0.0, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -85), 7.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -80), 4.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -75), -0.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.5, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -55), -17.1, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -50), -20.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -55), -17.2, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -50), -20.1, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -45), -22.2, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -40), -23.6, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -35), -24.5, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -30), -24.9, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -25), -25.0, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -25), -24.9, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -20), -24.6, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, -15), -23.7, 0.53 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -10), -22.3, 0.55 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, -5), -20.4, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -10), -22.2, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -5), -20.3, 0.57 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 0), -18.2, 0.59 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 5), -16.3, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 5), -16.2, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 10), -15.0, 0.58 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 15), -14.7, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 20), -15.3, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 20), -15.4, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 25), -16.8, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 30), -18.7, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.7, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 40), -22.6, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 45), -23.9, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 45), -24.0, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 50), -24.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 55), -24.8, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 60), -24.2, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 60), -24.1, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 65), -22.9, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 70), -21.2, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 75), -19.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 70), -21.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 75), -18.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 80), -16.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 85), -13.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 90), -10.7, 0.34 + 1.0); @@ -465,7 +465,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, 105), -3.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 110), -1.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 115), -0.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 120), 1.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 120), 1.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 125), 2.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 130), 3.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 135), 4.9, 0.32 + 1.0); @@ -475,47 +475,47 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-25, 155), 10.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 160), 12.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 165), 13.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-25, 170), 14.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 170), 14.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 175), 14.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-25, 180), 15.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -180), 13.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -175), 13.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -170), 13.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -165), 13.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -160), 13.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -155), 13.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -160), 13.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -155), 13.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -150), 13.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -145), 13.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -140), 13.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -135), 13.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -130), 13.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -130), 12.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -125), 12.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -120), 12.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -115), 12.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -110), 11.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -115), 12.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -110), 11.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -105), 11.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -100), 11.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -95), 9.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -90), 8.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -85), 5.5, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -80), 2.0, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -2.1, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -70), -6.6, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -11.0, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -85), 5.4, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -80), 1.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -2.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -70), -6.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -11.1, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -15.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.4, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -50), -21.0, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -45), -22.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.5, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -50), -21.1, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -45), -22.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -40), -23.9, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -35), -24.4, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -30), -24.5, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -30), -24.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -25), -24.0, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -20), -23.2, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -15), -21.8, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.8, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -20), -23.1, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -15), -21.7, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.7, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.4, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -14.9, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.4, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -14.8, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.3, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 10), -10.4, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 15), -9.3, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 20), -9.1, 0.48 + 1.0); @@ -523,22 +523,22 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-20, 30), -11.3, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 35), -13.2, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 40), -15.1, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 45), -16.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 50), -17.6, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 55), -18.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 45), -16.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 50), -17.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 55), -18.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 60), -17.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 65), -16.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 70), -15.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 75), -13.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 80), -11.9, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 85), -9.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 90), -7.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 85), -9.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 90), -7.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 95), -4.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 100), -2.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 105), -1.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 110), -0.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 115), 0.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-20, 120), 1.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 120), 1.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 125), 2.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 130), 3.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 135), 4.2, 0.31 + 1.0); @@ -551,9 +551,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-20, 170), 12.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 175), 13.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-20, 180), 13.5, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -180), 12.2, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -175), 12.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -170), 12.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -180), 12.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -175), 12.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -170), 12.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -165), 12.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -160), 12.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -155), 12.3, 0.31 + 1.0); @@ -566,37 +566,37 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, -120), 10.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -115), 10.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -110), 10.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -105), 10.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -105), 9.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -100), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -95), 8.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -90), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -85), 3.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -80), 0.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -75), -3.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -70), -8.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -65), -12.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -75), -4.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -70), -8.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -65), -12.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -60), -16.1, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -55), -19.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -55), -19.2, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -50), -21.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -45), -22.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -40), -23.5, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -35), -23.6, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -30), -23.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -25), -22.1, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.7, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -15), -18.8, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -35), -23.5, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -30), -23.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -25), -22.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -15), -18.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.5, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -14.0, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 0), -11.4, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -13.9, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 0), -11.3, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 5), -8.8, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 10), -6.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 15), -5.2, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 20), -4.5, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 25), -4.7, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 30), -5.6, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 30), -5.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 35), -7.2, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 40), -9.0, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 45), -10.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 40), -9.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 45), -10.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 50), -11.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 55), -12.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 60), -12.6, 0.33 + 1.0); @@ -604,30 +604,30 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-15, 70), -11.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 75), -10.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 80), -8.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 85), -6.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 85), -6.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 90), -4.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 95), -3.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 100), -1.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 100), -1.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 105), -0.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 110), 0.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 110), 0.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 115), 0.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 120), 1.2, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 125), 1.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 125), 1.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 130), 2.4, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 135), 3.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 135), 3.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 140), 4.7, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 145), 6.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 145), 6.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 150), 7.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 155), 8.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 160), 9.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 165), 10.7, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 170), 11.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 170), 11.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-15, 175), 12.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-15, 180), 12.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 180), 12.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -180), 11.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -175), 11.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -170), 11.2, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -165), 11.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -170), 11.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -165), 11.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -160), 11.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -155), 11.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -150), 11.0, 0.31 + 1.0); @@ -638,37 +638,37 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, -125), 9.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -120), 9.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -115), 9.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -110), 9.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -110), 9.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -105), 8.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -100), 8.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -95), 6.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -95), 6.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -90), 4.8, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -85), 2.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -85), 2.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -80), -1.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -75), -5.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -70), -9.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -70), -9.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -65), -13.2, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -60), -16.5, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -60), -16.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -55), -19.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -50), -21.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -45), -22.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -40), -22.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -35), -22.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -30), -21.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -25), -19.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -20), -17.7, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -15), -15.5, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -30), -21.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -25), -19.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -20), -17.6, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -15), -15.4, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.1, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.7, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.3, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 10), -4.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 15), -2.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.6, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 10), -4.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 15), -2.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 20), -1.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 25), -1.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 30), -1.9, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 35), -3.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 40), -4.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 40), -4.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 45), -6.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 50), -7.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 55), -8.3, 0.32 + 1.0); @@ -676,7 +676,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, 65), -8.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 70), -8.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 75), -7.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 80), -6.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 80), -6.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 85), -4.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 90), -3.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 95), -1.9, 0.30 + 1.0); @@ -685,22 +685,22 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-10, 110), 0.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 115), 0.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 120), 0.9, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 125), 1.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 125), 1.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 130), 1.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 135), 2.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 140), 3.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 145), 5.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 150), 6.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 155), 7.7, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 160), 8.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 160), 8.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 165), 9.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 170), 10.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-10, 175), 11.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 175), 11.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-10, 180), 11.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -180), 10.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -175), 10.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -175), 10.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -170), 10.4, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -165), 10.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -165), 10.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -160), 10.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -155), 10.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -150), 10.0, 0.31 + 1.0); @@ -708,35 +708,35 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-5, -140), 9.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -135), 9.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -130), 9.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -125), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -125), 9.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -120), 8.7, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -115), 8.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -110), 8.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -115), 8.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -110), 8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -105), 7.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -100), 6.9, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -95), 5.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -95), 5.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -90), 3.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -85), 0.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -85), 0.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -80), -2.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -75), -6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -70), -10.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -65), -13.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -65), -13.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -60), -16.7, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -55), -19.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -55), -19.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -50), -20.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -45), -21.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -40), -21.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -35), -20.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -35), -20.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -30), -18.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, -25), -16.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -25), -16.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -20), -14.7, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -15), -12.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -10), -10.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, -5), -7.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -5.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -4.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -5.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -3.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 10), -2.2, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 15), -0.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 15), -0.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 20), 0.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 25), 0.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 30), 0.5, 0.33 + 1.0); @@ -749,68 +749,68 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(-5, 65), -5.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 70), -5.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 75), -5.2, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 80), -4.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 80), -4.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 85), -3.6, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 90), -2.5, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 90), -2.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 95), -1.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 100), -0.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 105), 0.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 110), 0.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 115), 0.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 120), 0.4, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 125), 0.6, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 130), 1.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 135), 2.0, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 140), 3.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 125), 0.5, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 130), 1.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 135), 1.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 140), 3.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 145), 4.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 150), 5.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 155), 7.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 160), 8.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 165), 9.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 170), 9.9, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(-5, 175), 10.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 175), 10.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(-5, 180), 10.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -180), 10.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -180), 10.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -175), 10.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -170), 9.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -165), 9.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -170), 9.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -165), 9.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -160), 9.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -155), 9.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -150), 9.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -145), 9.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -140), 9.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -135), 9.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -130), 8.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -130), 8.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -125), 8.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -120), 8.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -115), 7.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -115), 7.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -110), 7.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -105), 7.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -105), 6.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -100), 6.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -95), 4.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -90), 2.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -95), 4.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -90), 2.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -85), -0.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -80), -3.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -80), -3.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -75), -7.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -70), -10.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -65), -13.8, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -60), -16.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -65), -13.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -60), -16.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -55), -18.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -50), -19.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -50), -19.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -45), -20.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -40), -19.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -35), -18.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -30), -16.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -25), -14.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -20), -12.2, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -15), -9.9, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, -10), -7.7, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -35), -18.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -30), -16.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -25), -14.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -20), -12.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -15), -9.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -10), -7.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, -5), -5.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 0), -4.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 0), -3.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 5), -2.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 10), -1.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 15), 0.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 20), 1.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 10), -0.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 15), 0.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 20), 1.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 25), 1.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 30), 1.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 35), 1.3, 0.32 + 1.0); @@ -819,7 +819,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(0, 50), -2.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 55), -3.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 60), -3.6, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 65), -3.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 65), -3.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 70), -3.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 75), -3.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 80), -3.3, 0.30 + 1.0); @@ -829,12 +829,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(0, 100), -0.2, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 105), 0.2, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 110), 0.2, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 115), 0.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 115), -0.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 120), -0.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 125), -0.2, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 130), 0.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 130), 0.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 135), 1.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 140), 2.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 140), 2.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 145), 3.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 150), 4.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 155), 6.2, 0.30 + 1.0); @@ -842,9 +842,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(0, 165), 8.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 170), 9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(0, 175), 9.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(0, 180), 10.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -180), 9.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -175), 9.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 180), 10.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -180), 9.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -175), 9.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -170), 9.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -165), 9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -160), 9.1, 0.32 + 1.0); @@ -859,12 +859,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, -115), 7.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -110), 7.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -105), 6.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -100), 5.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -100), 5.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -95), 3.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -90), 1.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -85), -1.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -85), -1.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -80), -4.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -75), -7.7, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -75), -7.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -70), -11.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -65), -13.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -60), -16.3, 0.33 + 1.0); @@ -874,17 +874,17 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, -40), -18.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -35), -16.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -30), -14.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, -10), -5.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 5), -1.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 5), -1.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 10), -0.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 15), 1.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 20), 2.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 25), 2.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 20), 2.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 25), 2.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 30), 2.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 35), 2.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 40), 1.5, 0.31 + 1.0); @@ -894,7 +894,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, 60), -2.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 65), -2.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 70), -2.5, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 75), -2.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 75), -2.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 80), -2.4, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 85), -2.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 90), -1.5, 0.29 + 1.0); @@ -902,12 +902,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, 100), -0.4, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 105), -0.2, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 110), -0.3, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 115), -0.6, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 120), -0.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 115), -0.7, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 120), -1.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 125), -1.0, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 130), -0.6, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 135), 0.2, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 140), 1.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 130), -0.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 135), 0.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 140), 1.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 145), 2.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 150), 3.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 155), 5.3, 0.30 + 1.0); @@ -915,8 +915,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(5, 165), 7.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 170), 8.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(5, 175), 9.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(5, 180), 9.6, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -180), 9.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 180), 9.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -180), 9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -175), 9.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -170), 9.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -165), 9.1, 0.32 + 1.0); @@ -926,11 +926,11 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, -145), 9.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -140), 9.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -135), 9.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -130), 8.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -130), 8.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -125), 8.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -120), 8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -115), 7.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -110), 7.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -110), 6.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -105), 6.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -100), 4.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -95), 3.0, 0.33 + 1.0); @@ -940,22 +940,22 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, -75), -8.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -70), -11.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -65), -13.9, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -60), -15.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -60), -16.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -55), -17.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -50), -17.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -45), -17.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -40), -16.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -35), -14.9, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -30), -12.9, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -25), -10.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -50), -17.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -45), -17.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -40), -16.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -35), -14.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -30), -12.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -25), -10.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -20), -8.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -15), -6.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, -10), -4.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -15), -6.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -10), -4.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, -5), -2.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 0), -1.5, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 5), -0.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 0), -1.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 5), -0.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 10), 0.7, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 15), 1.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 15), 1.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 20), 2.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 25), 3.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 30), 3.3, 0.30 + 1.0); @@ -964,7 +964,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, 45), 1.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 50), 0.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 55), -0.3, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 60), -0.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 60), -0.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 65), -1.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 70), -1.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 75), -1.6, 0.29 + 1.0); @@ -972,12 +972,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, 85), -1.5, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 90), -1.2, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 95), -0.9, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 100), -0.6, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 100), -0.7, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 105), -0.7, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 110), -0.9, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 110), -1.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 115), -1.4, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 120), -1.8, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 125), -1.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 125), -2.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 130), -1.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 135), -1.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 140), 0.1, 0.30 + 1.0); @@ -988,27 +988,27 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(10, 165), 6.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 170), 8.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(10, 175), 8.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(10, 180), 9.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 180), 9.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -180), 8.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -175), 9.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -170), 9.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -165), 9.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -160), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -160), 9.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -155), 9.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -150), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -145), 9.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -140), 9.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -135), 9.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -130), 9.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -130), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -125), 9.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -120), 8.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -115), 7.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -110), 7.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -105), 6.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -100), 4.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -95), 2.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -105), 5.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -100), 4.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -95), 2.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -90), -0.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -85), -2.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -85), -2.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -80), -5.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -75), -8.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -70), -11.6, 0.33 + 1.0); @@ -1019,9 +1019,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, -45), -16.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -40), -15.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -35), -13.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -30), -11.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -30), -11.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -25), -9.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, -20), -7.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -20), -7.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -15), -5.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -10), -3.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, -5), -1.8, 0.31 + 1.0); @@ -1033,7 +1033,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, 25), 3.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 30), 3.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 35), 3.5, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 40), 2.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 40), 3.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 45), 2.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 50), 1.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 55), 0.6, 0.30 + 1.0); @@ -1045,27 +1045,27 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(15, 85), -1.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 90), -1.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 95), -0.9, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 100), -0.9, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 100), -1.0, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 105), -1.2, 0.29 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 110), -1.6, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 110), -1.7, 0.29 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 115), -2.2, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 120), -2.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 120), -2.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 125), -3.0, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 130), -2.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 130), -2.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 135), -2.2, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 140), -1.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 140), -1.3, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 145), 0.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 150), 1.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 155), 2.9, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 160), 4.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 165), 5.9, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(15, 170), 7.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 170), 7.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 175), 8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(15, 180), 8.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -180), 8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -175), 8.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -170), 9.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -165), 9.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -165), 9.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -160), 9.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -155), 9.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -150), 9.8, 0.33 + 1.0); @@ -1073,43 +1073,43 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(20, -140), 10.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -135), 10.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -130), 10.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -125), 9.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -125), 9.7, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -120), 9.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -115), 8.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -110), 7.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -105), 6.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -105), 6.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -100), 4.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -95), 2.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -95), 2.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -90), -0.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -85), -3.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -85), -3.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -80), -6.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -75), -9.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -75), -9.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -70), -12.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -65), -13.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -65), -14.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -60), -15.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -55), -15.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -50), -15.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -45), -15.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -45), -15.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -40), -13.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -35), -12.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -25), -8.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -20), -6.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -15), -4.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, -10), -2.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -25), -8.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -20), -6.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -15), -4.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -10), -2.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, -5), -1.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 0), 0.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 0), 0.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 5), 1.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 10), 1.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 15), 2.5, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 20), 3.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 15), 2.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 20), 3.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 25), 3.8, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 30), 4.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 35), 3.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 30), 4.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 35), 4.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 40), 3.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 45), 2.8, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 50), 2.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 55), 1.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 55), 1.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 60), 1.0, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 65), 0.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 70), 0.3, 0.30 + 1.0); @@ -1123,13 +1123,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(20, 110), -2.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 115), -3.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 120), -3.8, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 125), -4.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 125), -4.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 130), -4.1, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 135), -3.6, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 140), -2.7, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 145), -1.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 150), -0.1, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(20, 155), 1.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 155), 1.4, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 160), 3.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 165), 4.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(20, 170), 6.1, 0.32 + 1.0); @@ -1141,39 +1141,39 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(25, -165), 9.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -160), 9.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -155), 10.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -150), 10.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -145), 10.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -150), 10.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -145), 10.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -140), 11.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -135), 11.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -130), 11.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -125), 10.7, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -120), 10.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -115), 9.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -120), 10.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -115), 9.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -110), 7.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -105), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -100), 4.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -95), 1.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -90), -1.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -85), -4.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -85), -4.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -80), -7.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -75), -10.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -70), -12.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -65), -14.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -60), -15.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -55), -15.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -50), -15.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -45), -14.2, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -40), -12.9, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -35), -11.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -30), -9.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -25), -7.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -20), -5.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -50), -15.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -45), -14.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -40), -12.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -35), -11.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -30), -9.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -25), -7.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -20), -5.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -15), -3.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, -10), -2.0, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.5, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 0), 0.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 5), 1.5, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, 10), 2.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 10), 2.3, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 15), 3.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 20), 3.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 25), 4.1, 0.31 + 1.0); @@ -1194,8 +1194,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(25, 100), -1.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 105), -2.2, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 110), -3.1, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, 115), -4.0, 0.30 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(25, 120), -4.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 115), -4.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 120), -4.9, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 125), -5.4, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 130), -5.5, 0.30 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(25, 135), -5.1, 0.31 + 1.0); @@ -1222,7 +1222,7 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, -125), 11.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -120), 11.0, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -115), 10.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -110), 8.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -110), 8.5, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -105), 6.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -100), 4.4, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -95), 1.6, 0.35 + 1.0); @@ -1236,13 +1236,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, -55), -15.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -50), -14.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -45), -13.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -35), -10.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -35), -10.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -30), -8.9, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -20), -5.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -20), -5.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -15), -3.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, -10), -1.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -10), -1.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, -5), -0.2, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 0), 1.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 5), 1.9, 0.32 + 1.0); @@ -1251,23 +1251,23 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, 20), 4.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 25), 4.5, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 30), 4.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 35), 4.8, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 40), 4.6, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 45), 4.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 35), 4.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 40), 4.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 45), 4.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 50), 3.8, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 55), 3.3, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 60), 2.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 60), 3.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 65), 2.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 70), 2.2, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 75), 1.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 80), 1.3, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 85), 0.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 85), 0.6, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 90), -0.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 95), -0.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 100), -1.7, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 105), -2.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 105), -2.8, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 110), -3.9, 0.31 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(30, 115), -5.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 115), -5.1, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 120), -6.0, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 125), -6.7, 0.31 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 130), -6.9, 0.31 + 1.0); @@ -1281,11 +1281,11 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(30, 170), 3.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 175), 4.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(30, 180), 6.3, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -180), 5.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -180), 5.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -175), 6.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -170), 8.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -165), 9.2, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -160), 10.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -165), 9.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -160), 10.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -155), 11.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -150), 11.9, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -145), 12.6, 0.35 + 1.0); @@ -1295,39 +1295,39 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, -125), 12.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -120), 12.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -115), 10.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -110), 9.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -110), 9.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -105), 7.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -100), 4.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -95), 1.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -95), 1.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -90), -1.8, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -85), -5.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -85), -5.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -80), -8.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -75), -11.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -70), -13.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -65), -14.7, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -60), -15.3, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -55), -15.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -65), -14.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -60), -15.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -55), -15.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -50), -14.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -45), -13.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -35), -10.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -45), -13.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -35), -10.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.7, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -25), -6.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -25), -6.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, -20), -5.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -10), -1.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, -5), -0.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 5), 2.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -10), -1.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -5), 0.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 5), 2.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 10), 3.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 15), 3.8, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 20), 4.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 25), 4.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 20), 4.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 25), 5.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 30), 5.3, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 35), 5.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 40), 5.4, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 45), 5.2, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 50), 4.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 40), 5.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 45), 5.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 50), 5.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 55), 4.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 60), 4.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 65), 3.9, 0.32 + 1.0); @@ -1335,13 +1335,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, 75), 3.0, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 80), 2.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 85), 1.4, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 90), 0.5, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 95), -0.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 90), 0.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 95), -0.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 100), -1.9, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 105), -3.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 110), -4.7, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 115), -6.1, 0.32 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 120), -7.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 120), -7.3, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 125), -8.1, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 130), -8.4, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 135), -8.3, 0.32 + 1.0); @@ -1351,9 +1351,9 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(35, 155), -3.6, 0.32 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 160), -1.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 165), 0.0, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 170), 1.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 170), 1.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(35, 175), 3.6, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(35, 180), 5.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 180), 5.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -180), 4.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -175), 5.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -170), 7.5, 0.35 + 1.0); @@ -1362,48 +1362,48 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, -155), 11.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -150), 12.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -145), 13.5, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -140), 14.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -135), 14.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -140), 14.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -135), 14.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -130), 14.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -125), 13.9, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -120), 13.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -120), 13.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -115), 11.8, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -110), 10.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -105), 7.7, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -100), 4.8, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -95), 1.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -95), 1.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -90), -2.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -85), -5.8, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -80), -9.2, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -75), -12.0, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -70), -14.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -70), -14.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -65), -15.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -60), -15.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -55), -15.6, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -50), -14.9, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -50), -14.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -45), -13.7, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -40), -12.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -35), -10.7, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -30), -8.9, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -25), -7.1, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -20), -5.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -35), -10.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -30), -8.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -25), -7.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -20), -5.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, -15), -3.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -10), -1.6, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, -5), -0.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 0), 1.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 5), 2.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -10), -1.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -5), 0.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 0), 1.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 5), 2.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 10), 3.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 15), 4.2, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 20), 4.9, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 25), 5.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 20), 5.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 25), 5.6, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 30), 6.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 35), 6.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 35), 6.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 40), 6.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 45), 6.5, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 50), 6.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 45), 6.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 50), 6.5, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 55), 6.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 60), 6.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 65), 5.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 65), 5.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 70), 5.1, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 75), 4.4, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 80), 3.5, 0.33 + 1.0); @@ -1411,12 +1411,12 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, 90), 1.1, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 95), -0.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 100), -2.1, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 105), -3.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 105), -3.9, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 110), -5.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 115), -7.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 120), -8.6, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 125), -9.5, 0.33 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 130), -9.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 130), -10.0, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 135), -9.8, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 140), -9.2, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 145), -8.2, 0.33 + 1.0); @@ -1425,24 +1425,24 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(40, 160), -3.4, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 165), -1.5, 0.33 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 170), 0.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(40, 175), 2.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 175), 2.3, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(40, 180), 4.2, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -180), 3.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -175), 5.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -170), 7.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -165), 8.9, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -165), 8.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -160), 10.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -155), 12.0, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -150), 13.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -150), 13.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -145), 14.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -140), 15.0, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -135), 15.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -130), 15.4, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -125), 15.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -120), 14.2, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -115), 12.8, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -110), 10.8, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -105), 8.2, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -120), 14.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -115), 12.7, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -110), 10.7, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -105), 8.1, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -100), 5.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -95), 1.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -90), -2.7, 0.41 + 1.0); @@ -1451,164 +1451,164 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(45, -75), -13.0, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -70), -15.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -65), -16.3, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -60), -16.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -60), -16.7, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -55), -16.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -50), -15.7, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -45), -14.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -40), -13.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -35), -11.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -30), -9.5, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -25), -7.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -30), -9.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -25), -7.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -20), -5.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -15), -3.7, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, -10), -1.9, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -10), -1.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, -5), -0.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 0), 1.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 5), 2.5, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 10), 3.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 15), 4.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 0), 1.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 5), 2.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 10), 3.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 15), 4.7, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 20), 5.5, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 25), 6.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 30), 7.0, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 35), 7.5, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 40), 7.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 35), 7.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 40), 8.0, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 45), 8.2, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 50), 8.3, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 55), 8.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 50), 8.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 55), 8.4, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 60), 8.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 65), 7.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 70), 7.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 75), 6.3, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 80), 5.1, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 85), 3.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 90), 1.9, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 90), 1.8, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 95), -0.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 100), -2.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 100), -2.3, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 105), -4.4, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.5, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.6, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 115), -8.4, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 120), -9.9, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 120), -10.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 125), -11.0, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 130), -11.4, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 135), -11.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 140), -10.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 130), -11.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 135), -11.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 140), -10.8, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 145), -9.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 150), -8.3, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 155), -6.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 155), -6.7, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 160), -4.8, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 165), -2.8, 0.34 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(45, 170), -0.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 165), -2.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 170), -0.9, 0.34 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 175), 1.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(45, 180), 3.2, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -180), 2.4, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -175), 4.6, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -170), 6.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -175), 4.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -170), 6.6, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -165), 8.7, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -160), 10.6, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -155), 12.3, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -150), 13.8, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -145), 15.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -140), 16.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -135), 16.5, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -130), 16.6, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -145), 15.0, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -140), 15.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -135), 16.4, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -130), 16.5, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -125), 16.2, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -120), 15.3, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -115), 13.8, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -110), 11.6, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -105), 8.7, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -115), 13.7, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -110), 11.5, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -105), 8.6, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -100), 5.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -95), 1.0, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -90), -3.4, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -85), -7.7, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -85), -7.6, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -80), -11.4, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -75), -14.4, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -70), -16.6, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -65), -17.8, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -60), -18.2, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -55), -17.9, 0.40 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -50), -17.1, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -70), -16.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -65), -17.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -60), -18.1, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -55), -17.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -50), -17.0, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, -45), -15.8, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -40), -14.3, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -35), -12.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -30), -10.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -25), -8.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -20), -6.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -15), -4.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -10), -2.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, -5), -0.7, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 0), 1.0, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 5), 2.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -40), -14.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -35), -12.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -30), -10.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -25), -8.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -20), -6.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -15), -4.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -10), -2.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -5), -0.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 0), 1.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 5), 2.6, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 10), 3.9, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 15), 5.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 20), 6.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 15), 5.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 20), 6.3, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 25), 7.3, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 30), 8.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 30), 8.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 35), 9.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 40), 9.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 45), 10.4, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 50), 10.7, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 50), 10.8, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 55), 10.9, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 60), 10.8, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 65), 10.4, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 60), 10.9, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 65), 10.5, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 70), 9.7, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 75), 8.6, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 80), 7.1, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 85), 5.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 85), 5.1, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 90), 2.9, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 95), 0.4, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 95), 0.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 100), -2.3, 0.39 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 105), -5.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 110), -7.5, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 115), -9.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 120), -11.3, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 125), -12.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 115), -9.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 120), -11.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 125), -12.5, 0.37 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 130), -12.9, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 135), -12.8, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 140), -12.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 145), -11.1, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 150), -9.6, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 155), -7.9, 0.35 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 160), -6.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 150), -9.7, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 155), -8.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 160), -6.1, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 165), -4.0, 0.35 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 170), -1.9, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(50, 175), 0.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 175), 0.2, 0.36 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(50, 180), 2.4, 0.36 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.8, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.0, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.3, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.6, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -160), 10.7, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -155), 12.6, 0.39 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -150), 14.3, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -160), 10.6, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -155), 12.5, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -150), 14.2, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -145), 15.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -140), 16.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -135), 17.5, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -130), 17.8, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -130), 17.7, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -125), 17.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -120), 16.5, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -115), 14.9, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -115), 14.8, 0.48 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -110), 12.4, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -105), 9.1, 0.51 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -100), 5.0, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -95), 0.4, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -90), -4.5, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -85), -9.2, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -80), -13.3, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -75), -16.5, 0.49 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -70), -18.7, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -65), -19.9, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -60), -20.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -55), -19.9, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -50), -19.0, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.7, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -80), -13.2, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -75), -16.4, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -70), -18.6, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -65), -19.8, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -60), -20.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -55), -19.8, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -50), -18.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -40), -15.9, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.0, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, -25), -9.9, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -20), -7.8, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -15), -5.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.4, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, -5), -1.4, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -20), -7.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -15), -5.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -5), -1.3, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 0), 0.6, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 5), 2.4, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 10), 4.1, 0.41 + 1.0); @@ -1617,8 +1617,8 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, 25), 8.6, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 30), 9.9, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 35), 11.1, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 40), 12.1, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 45), 13.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 40), 12.2, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 45), 13.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 50), 13.7, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 55), 14.1, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 60), 14.1, 0.43 + 1.0); @@ -1627,13 +1627,13 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, 75), 11.5, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 80), 9.6, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 85), 7.2, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 90), 4.4, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 95), 1.2, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 90), 4.3, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 95), 1.1, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 100), -2.2, 0.44 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 105), -5.5, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 110), -8.4, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 110), -8.5, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 115), -10.9, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 120), -12.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 120), -12.8, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 125), -13.9, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 130), -14.4, 0.40 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 135), -14.2, 0.39 + 1.0); @@ -1641,60 +1641,60 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(55, 145), -12.4, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 150), -10.9, 0.38 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(55, 155), -9.1, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 160), -7.1, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 165), -5.0, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 170), -2.8, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 175), -0.5, 0.37 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(55, 180), 1.8, 0.38 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -180), 1.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -175), 3.6, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -170), 6.0, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -165), 8.4, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 160), -7.2, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 165), -5.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 170), -2.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 175), -0.6, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 180), 1.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -180), 1.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -175), 3.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -170), 5.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -165), 8.3, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -160), 10.6, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -155), 12.8, 0.43 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -150), 14.7, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -145), 16.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -140), 17.6, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -135), 18.5, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -130), 18.9, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -125), 18.7, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -155), 12.7, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -150), 14.6, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -145), 16.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -140), 17.5, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -135), 18.4, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -130), 18.8, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -125), 18.6, 0.52 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -120), 17.7, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -115), 15.9, 0.57 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.2, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.1, 0.59 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -105), 9.4, 0.62 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -100), 4.7, 0.64 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -95), -0.8, 0.65 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -90), -6.4, 0.65 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -85), -11.6, 0.64 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.0, 0.62 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -75), -19.4, 0.59 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -70), -21.6, 0.56 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -65), -22.8, 0.54 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.0, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.6, 0.50 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.6, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.1, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.3, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.3, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.1, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.7, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -15), -6.9, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -95), -0.7, 0.65 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -90), -6.3, 0.65 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -85), -11.5, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.0, 0.61 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -75), -19.3, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -70), -21.5, 0.56 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -65), -22.7, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -60), -22.9, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.5, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.5, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.0, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.2, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.2, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.0, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.6, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -15), -6.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.5, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, -5), -2.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -5), -2.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 0), 0.1, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 5), 2.3, 0.45 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 10), 4.3, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 10), 4.4, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.4, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 20), 8.3, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 25), 10.1, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 20), 8.4, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 25), 10.2, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 30), 11.9, 0.46 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 35), 13.5, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 35), 13.6, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 40), 15.0, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 45), 16.2, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 45), 16.3, 0.47 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 50), 17.2, 0.47 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 55), 17.8, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 60), 18.0, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 55), 17.9, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 60), 18.1, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 65), 17.8, 0.49 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 70), 16.9, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 75), 15.3, 0.51 + 1.0); @@ -1704,21 +1704,21 @@ TEST(GeoLookupTest, declination) EXPECT_NEAR(get_mag_declination_degrees(60, 95), 2.5, 0.54 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 100), -1.7, 0.53 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 105), -5.7, 0.52 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 110), -9.2, 0.51 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 115), -12.0, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 110), -9.3, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 115), -12.1, 0.50 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 120), -14.1, 0.48 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 125), -15.3, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 125), -15.4, 0.46 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 130), -15.8, 0.45 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 135), -15.6, 0.44 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 140), -14.8, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 140), -14.9, 0.43 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 145), -13.6, 0.42 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 150), -12.0, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 150), -12.1, 0.42 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 155), -10.2, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 160), -8.1, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 165), -5.9, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 170), -3.6, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 160), -8.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 165), -6.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 170), -3.7, 0.41 + 1.0); EXPECT_NEAR(get_mag_declination_degrees(60, 175), -1.3, 0.41 + 1.0); - EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.1, 0.41 + 1.0); } TEST(GeoLookupTest, inclination) @@ -1729,7 +1729,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-50, -165), -68.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -160), -67.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -155), -66.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -150), -65.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -150), -65.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -145), -64.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -140), -64.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -135), -63.0, 0.21 + 1.2); @@ -1759,10 +1759,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-50, -15), -60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -10), -61.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, -5), -61.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 0), -60.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 0), -60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, 5), -60.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, 10), -59.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 15), -59.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 15), -59.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, 20), -59.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, 25), -58.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-50, 30), -59.1, 0.21 + 1.2); @@ -1820,27 +1820,27 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, -75), -44.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -70), -45.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -65), -45.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -60), -46.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -55), -48.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -50), -50.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -60), -47.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -55), -48.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -50), -50.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -45), -52.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -40), -54.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -35), -56.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -30), -58.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -25), -60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -40), -55.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -35), -57.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -30), -58.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -25), -60.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -20), -61.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -15), -62.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -10), -63.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, -5), -63.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 0), -63.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 0), -63.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 5), -62.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 10), -62.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 15), -61.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 20), -60.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 25), -60.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 30), -59.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 35), -60.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 40), -60.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 35), -60.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 40), -60.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 45), -61.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 50), -62.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 55), -64.3, 0.21 + 1.2); @@ -1848,9 +1848,9 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-45, 65), -67.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 70), -69.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 75), -70.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 80), -72.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 80), -72.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 85), -73.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 90), -74.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 90), -74.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 95), -75.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 100), -76.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-45, 105), -76.9, 0.21 + 1.2); @@ -1881,7 +1881,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-40, -135), -55.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -130), -54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -125), -53.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -120), -52.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -120), -52.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -115), -51.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -110), -50.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -105), -48.6, 0.21 + 1.2); @@ -1890,15 +1890,15 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-40, -90), -43.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -85), -42.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -80), -41.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -75), -41.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -75), -41.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -70), -41.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -65), -42.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -60), -44.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -65), -42.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -60), -44.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -55), -46.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -50), -48.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -50), -48.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -45), -51.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -40), -54.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -35), -56.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -35), -56.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -30), -58.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -25), -60.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, -20), -62.5, 0.21 + 1.2); @@ -1909,10 +1909,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-40, 5), -65.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 10), -64.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 15), -63.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 20), -62.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 25), -61.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 30), -61.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 35), -60.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 20), -62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 25), -61.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 30), -60.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 35), -60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 40), -61.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 45), -61.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 50), -62.7, 0.21 + 1.2); @@ -1920,9 +1920,9 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-40, 60), -65.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 65), -66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 70), -68.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 75), -69.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 75), -69.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 80), -70.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 85), -71.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 85), -71.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 90), -72.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 95), -73.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 100), -73.5, 0.21 + 1.2); @@ -1933,7 +1933,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-40, 125), -72.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 130), -72.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 135), -71.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 140), -71.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 140), -71.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 145), -70.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 150), -69.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-40, 155), -69.1, 0.21 + 1.2); @@ -1966,24 +1966,24 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-35, -75), -36.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -70), -36.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -65), -38.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -55), -43.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -55), -43.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -58.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -58.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -20), -62.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -15), -64.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -15), -64.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -10), -65.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, -5), -66.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 0), -67.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 0), -67.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 5), -67.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 10), -66.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 15), -65.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 15), -65.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 20), -64.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 25), -63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 25), -63.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 30), -62.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 35), -61.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-35, 40), -61.1, 0.21 + 1.2); @@ -2036,28 +2036,28 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-30, -90), -33.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -85), -31.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -80), -30.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -75), -30.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -70), -31.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -75), -30.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -70), -31.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -65), -33.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -60), -36.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -55), -39.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -43.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -45), -46.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -40), -50.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -30), -56.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -25), -59.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -20), -61.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -60), -36.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -55), -39.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -43.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -45), -46.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -40), -50.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -30), -56.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -25), -59.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -20), -61.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -15), -63.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, -10), -65.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -5), -66.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -5), -66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 0), -67.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 5), -67.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 10), -67.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 15), -66.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 20), -65.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 20), -65.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 25), -63.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 30), -62.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 30), -62.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 35), -61.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 40), -60.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 45), -60.2, 0.21 + 1.2); @@ -2084,7 +2084,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-30, 150), -60.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 155), -60.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 160), -59.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 165), -58.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 165), -58.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 170), -57.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 175), -56.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-30, 180), -55.0, 0.21 + 1.2); @@ -2092,11 +2092,11 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-25, -175), -47.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -170), -46.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -165), -45.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -160), -44.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -160), -44.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -155), -43.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -150), -42.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -145), -40.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -140), -39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -140), -39.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -135), -38.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -130), -37.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -125), -36.3, 0.21 + 1.2); @@ -2110,28 +2110,28 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-25, -85), -24.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -80), -23.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, -75), -23.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -24.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -42.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -25.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -43.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 0), -66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 5), -66.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 10), -66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 15), -65.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 20), -64.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 20), -64.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 25), -62.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 30), -60.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 35), -59.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 30), -60.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 35), -59.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 40), -58.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 45), -57.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 50), -57.5, 0.21 + 1.2); @@ -2154,26 +2154,26 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-25, 135), -56.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 140), -56.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 145), -56.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 150), -55.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 150), -55.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 155), -54.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 160), -53.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 165), -52.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 165), -52.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 170), -51.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 175), -50.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-25, 180), -49.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -180), -42.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -180), -42.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -175), -40.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -170), -39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -165), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -160), -37.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -155), -36.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -150), -34.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -150), -34.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -145), -33.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -140), -32.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -135), -31.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -135), -30.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -130), -29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -125), -28.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -120), -27.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -120), -27.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -115), -26.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -110), -25.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -105), -23.8, 0.21 + 1.2); @@ -2181,21 +2181,21 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, -95), -20.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -90), -18.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -85), -16.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -80), -15.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -75), -15.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -70), -17.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -20.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -38.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -25), -53.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -20), -56.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -80), -15.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -75), -15.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -70), -17.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -20.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -24.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -38.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -25), -53.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -20), -56.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -15), -58.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -10), -60.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -10), -60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, -5), -62.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 0), -63.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 5), -63.3, 0.21 + 1.2); @@ -2203,7 +2203,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, 15), -62.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 20), -60.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 25), -59.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 30), -57.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 30), -57.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 35), -55.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 40), -54.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 45), -53.2, 0.21 + 1.2); @@ -2211,7 +2211,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, 55), -52.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 60), -53.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 65), -53.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 70), -54.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 70), -54.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 75), -54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 80), -54.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 85), -55.0, 0.21 + 1.2); @@ -2221,7 +2221,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, 105), -53.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 110), -52.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 115), -51.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 120), -51.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 120), -51.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 125), -51.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 130), -50.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 135), -50.6, 0.21 + 1.2); @@ -2233,17 +2233,17 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-20, 165), -46.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 170), -44.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-20, 175), -43.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 180), -42.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 180), -42.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -180), -34.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -175), -32.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -170), -31.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -165), -30.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -170), -31.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -165), -30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -160), -28.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -155), -27.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -150), -26.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -145), -25.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -140), -23.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -135), -22.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -150), -26.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -145), -25.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -140), -23.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -135), -22.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -130), -21.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -125), -20.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -120), -18.9, 0.21 + 1.2); @@ -2253,22 +2253,22 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-15, -100), -13.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -95), -11.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -90), -9.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -85), -7.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -80), -6.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -85), -7.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -80), -6.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -75), -7.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -9.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -12.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -32.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -37.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -48.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -12.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -32.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -37.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -49.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -15), -54.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -10), -55.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -10), -55.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, -5), -57.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 0), -57.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 5), -57.9, 0.21 + 1.2); @@ -2276,35 +2276,35 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-15, 15), -56.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 20), -55.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 25), -53.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 30), -51.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 35), -49.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 40), -47.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 45), -46.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 50), -46.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 30), -51.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 35), -49.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 40), -47.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 45), -46.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 50), -46.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 55), -46.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 60), -46.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 65), -46.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 70), -46.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 75), -47.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 80), -47.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 85), -47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 85), -47.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 90), -47.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 95), -47.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 100), -46.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 105), -45.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 110), -44.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 105), -45.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 110), -44.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 115), -44.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 120), -43.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 120), -43.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 125), -43.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 130), -43.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 135), -43.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 140), -43.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 145), -42.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 150), -42.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 155), -41.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 160), -40.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 145), -42.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 150), -42.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 155), -41.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 160), -40.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 165), -38.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 170), -37.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 170), -37.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 175), -35.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-15, 180), -34.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -180), -25.3, 0.21 + 1.2); @@ -2312,37 +2312,37 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, -170), -22.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -165), -20.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -160), -19.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -155), -18.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -150), -17.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -145), -15.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -140), -14.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -155), -18.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -150), -16.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -145), -15.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -140), -14.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -135), -13.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -130), -11.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -125), -10.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -120), -9.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -115), -8.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -115), -8.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -110), -6.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -105), -5.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -100), -3.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -95), -1.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -90), 0.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -85), 2.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -80), 2.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -75), 1.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -70), -0.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -8.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -19.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -35.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -25), -42.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -20), -45.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -15), -47.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -80), 2.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -75), 1.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -70), -0.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -8.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -19.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -25.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -35.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -25), -42.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -20), -45.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -15), -47.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, -10), -49.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -5), -49.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -5), -50.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 0), -50.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 5), -50.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 10), -49.9, 0.21 + 1.2); @@ -2350,7 +2350,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, 20), -47.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 25), -45.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 30), -43.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 35), -41.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 35), -41.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 40), -39.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 45), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 50), -37.7, 0.21 + 1.2); @@ -2364,14 +2364,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, 90), -38.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 95), -38.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 100), -37.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 105), -36.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 110), -36.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 105), -36.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 110), -36.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 115), -35.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 120), -35.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 125), -35.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 130), -35.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 135), -35.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 140), -35.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 140), -35.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 145), -34.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 150), -34.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-10, 155), -33.1, 0.21 + 1.2); @@ -2382,11 +2382,11 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-10, 180), -25.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -180), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -175), -13.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -170), -12.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -170), -12.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -165), -10.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -160), -9.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -155), -8.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -150), -6.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -150), -6.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -145), -5.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -140), -4.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -135), -3.0, 0.21 + 1.2); @@ -2394,25 +2394,25 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, -125), -0.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -120), 0.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -115), 1.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -110), 3.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -110), 3.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -105), 4.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -100), 6.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -95), 8.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -90), 10.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -85), 11.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -80), 11.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -80), 11.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -75), 10.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 8.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 0.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -22.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -30), -31.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -35.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -20), -37.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 8.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 0.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -22.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -30), -31.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -35.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -20), -37.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -15), -39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -10), -40.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, -5), -41.0, 0.21 + 1.2); @@ -2421,70 +2421,70 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(-5, 10), -40.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 15), -39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 20), -37.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 25), -35.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 30), -33.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 25), -35.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 30), -33.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 35), -31.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 40), -29.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 40), -29.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 45), -28.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 50), -27.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 55), -27.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 50), -27.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 55), -27.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 60), -27.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 65), -27.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 65), -27.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 70), -27.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 75), -28.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 75), -28.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 80), -28.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 85), -28.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 90), -28.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 90), -28.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 95), -28.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 100), -27.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 105), -27.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 100), -27.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 105), -26.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 110), -26.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 115), -25.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 120), -25.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 125), -25.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 125), -25.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 130), -26.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 135), -26.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 140), -26.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 145), -25.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 140), -26.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 145), -26.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 150), -25.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 155), -24.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 155), -24.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 160), -22.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 165), -21.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 165), -21.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 170), -19.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 175), -17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(-5, 180), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -180), -5.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -175), -3.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -170), -1.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -170), -1.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -165), -0.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -160), 1.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -155), 2.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -150), 3.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -150), 3.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -145), 4.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -140), 5.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -135), 7.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -140), 6.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -135), 7.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -130), 8.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -125), 9.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -120), 10.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -115), 11.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -115), 11.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -110), 13.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -105), 14.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -100), 16.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -95), 18.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -90), 19.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -90), 19.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -85), 20.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -80), 20.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -70), 17.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 9.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -55), 4.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -1.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -45), -7.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -40), -12.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -18.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -30), -22.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -25), -25.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -70), 17.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 9.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -55), 4.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -1.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -45), -7.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -40), -13.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -18.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -30), -22.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -25), -25.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -20), -27.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -15), -29.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, -10), -30.1, 0.21 + 1.2); @@ -2495,48 +2495,48 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(0, 15), -28.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 20), -26.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 25), -24.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 30), -22.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 30), -22.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 35), -20.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 40), -18.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 40), -18.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 45), -17.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 50), -16.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 55), -16.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 60), -16.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 60), -15.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 65), -16.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 70), -16.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 75), -16.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 75), -16.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 80), -17.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 85), -17.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 90), -17.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 95), -17.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -16.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 110), -15.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -15.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 110), -15.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 115), -15.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 120), -15.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 125), -15.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 130), -16.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 135), -16.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 140), -16.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 145), -16.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 150), -15.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 145), -16.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 150), -15.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 155), -14.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 160), -13.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 165), -11.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 170), -9.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 175), -7.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 175), -7.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(0, 180), -5.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -180), 5.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -175), 7.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -170), 8.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -170), 8.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -165), 10.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -160), 11.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -155), 12.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -150), 13.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -145), 14.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -140), 15.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -135), 16.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -130), 18.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -135), 17.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -130), 18.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -125), 19.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -120), 20.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -115), 21.2, 0.21 + 1.2); @@ -2548,52 +2548,52 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(5, -85), 28.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -80), 28.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -75), 27.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -70), 25.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -65), 22.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -55), 13.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -50), 8.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -45), 2.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -30), -11.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -25), -14.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -70), 25.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -65), 22.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -55), 13.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -50), 8.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -45), 2.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -30), -11.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -25), -14.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -20), -16.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -15), -17.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, -10), -18.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -5), -18.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 0), -18.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -5), -18.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 0), -18.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 5), -17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 10), -16.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 15), -15.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 20), -14.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 25), -12.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 25), -12.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 30), -10.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 35), -8.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 40), -6.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 35), -7.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 40), -6.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 45), -4.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 50), -4.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 55), -3.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 50), -4.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 55), -3.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 60), -3.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 65), -3.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 70), -4.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 75), -4.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 80), -4.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 80), -4.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 85), -5.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 90), -5.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 95), -5.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 100), -4.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 105), -4.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 90), -5.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 95), -5.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 100), -4.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 105), -4.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 110), -3.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 115), -3.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 115), -3.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 120), -3.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 125), -4.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 125), -4.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 130), -5.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 135), -5.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 140), -6.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 140), -6.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 145), -6.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 150), -5.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 155), -4.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 155), -4.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 160), -3.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 165), -1.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(5, 170), 0.6, 0.21 + 1.2); @@ -2605,10 +2605,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(10, -165), 19.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -160), 20.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -155), 21.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -150), 22.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -145), 23.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -150), 22.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -145), 23.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -140), 24.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -135), 25.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -135), 26.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -130), 27.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -125), 28.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -120), 29.0, 0.21 + 1.2); @@ -2617,21 +2617,21 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(10, -105), 32.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -100), 33.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -95), 34.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -90), 35.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -90), 35.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -85), 36.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -80), 35.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -80), 35.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -75), 34.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -70), 32.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -65), 30.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -60), 26.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -30), -0.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -70), 32.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -65), 30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -60), 26.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -30), -0.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -15), -5.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -10), -5.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, -5), -5.5, 0.21 + 1.2); @@ -2640,32 +2640,32 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(10, 10), -3.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 15), -2.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 20), -1.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 25), 0.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 30), 2.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 25), 0.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 30), 2.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 35), 4.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 40), 6.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 45), 7.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 50), 8.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 55), 8.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 40), 6.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 45), 7.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 50), 8.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 55), 8.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 60), 8.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 65), 8.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 65), 8.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 70), 8.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 75), 7.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 80), 7.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 85), 7.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 90), 7.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 95), 7.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 100), 7.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 75), 8.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 80), 7.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 85), 7.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 90), 7.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 95), 7.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 100), 7.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 105), 7.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 110), 7.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 115), 7.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 115), 7.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 120), 7.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 125), 6.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 130), 5.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 135), 5.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 140), 4.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 145), 4.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 150), 4.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 145), 4.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 150), 4.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 155), 5.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 160), 6.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(10, 165), 8.5, 0.21 + 1.2); @@ -2677,34 +2677,34 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, -170), 26.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -165), 28.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -160), 29.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -155), 30.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -150), 31.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -145), 31.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -140), 32.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -135), 33.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -130), 34.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -155), 30.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -150), 31.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -145), 32.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -140), 33.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -135), 34.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -130), 35.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -125), 35.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -120), 36.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -115), 37.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -115), 37.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -110), 38.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -105), 39.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -100), 41.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -95), 41.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -90), 42.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -85), 42.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -85), 42.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -80), 42.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -75), 41.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -70), 39.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -65), 36.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -60), 33.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 26.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 21.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -70), 39.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -65), 36.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -60), 33.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 25.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 21.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 17.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -25), 9.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -20), 7.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -20), 7.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -15), 7.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -10), 7.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, -5), 7.3, 0.21 + 1.2); @@ -2716,10 +2716,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, 25), 13.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 30), 14.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 35), 16.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 40), 17.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 40), 18.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 45), 19.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 50), 19.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 55), 19.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 50), 19.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 55), 20.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 60), 20.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 65), 20.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 70), 19.9, 0.21 + 1.2); @@ -2727,8 +2727,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, 80), 19.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 85), 19.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 90), 19.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 95), 18.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 100), 19.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 95), 19.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 100), 19.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 105), 19.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 110), 19.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 115), 19.0, 0.21 + 1.2); @@ -2736,9 +2736,9 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(15, 125), 17.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 130), 16.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 135), 15.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 140), 15.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 140), 14.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 145), 14.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 150), 14.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 150), 14.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 155), 15.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 160), 16.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(15, 165), 17.8, 0.21 + 1.2); @@ -2753,7 +2753,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(20, -155), 37.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -150), 38.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -145), 38.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -140), 39.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -140), 39.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -135), 40.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -130), 41.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -125), 42.8, 0.21 + 1.2); @@ -2767,18 +2767,18 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(20, -85), 48.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -80), 48.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -75), 46.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -70), 45.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -65), 43.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -60), 40.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -55), 37.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 34.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -40), 27.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -35), 24.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -30), 22.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -70), 45.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -65), 43.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -60), 40.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -55), 37.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 33.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -40), 27.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -35), 24.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -30), 22.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -25), 20.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -20), 19.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -15), 19.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -15), 19.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -10), 19.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, -5), 19.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 0), 20.0, 0.21 + 1.2); @@ -2786,24 +2786,24 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(20, 10), 21.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 15), 22.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 20), 23.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 25), 24.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 25), 24.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 30), 26.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 35), 27.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 40), 28.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 45), 29.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 50), 30.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 55), 30.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 55), 30.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 60), 30.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 65), 30.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 70), 30.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 75), 30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 70), 30.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 75), 30.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 80), 30.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 30.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 90), 29.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 95), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 100), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 110), 29.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 110), 29.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 115), 29.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 120), 28.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(20, 125), 27.6, 0.21 + 1.2); @@ -2823,8 +2823,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, -170), 40.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -165), 41.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -160), 42.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -155), 43.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -150), 43.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -155), 43.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -150), 44.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -145), 44.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -140), 45.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -135), 46.7, 0.21 + 1.2); @@ -2840,16 +2840,16 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, -85), 53.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -80), 53.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -75), 52.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -55), 43.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -30), 32.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -20), 30.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -15), 29.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, -10), 29.9, 0.21 + 1.2); @@ -2863,20 +2863,20 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, 30), 36.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 35), 37.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 40), 38.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 45), 38.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 45), 39.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 50), 39.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 55), 39.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 60), 39.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 65), 39.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 70), 39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 55), 39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 60), 39.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 65), 39.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 70), 39.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 75), 39.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 80), 39.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 85), 39.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 90), 39.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 80), 39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 85), 39.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 90), 39.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 95), 39.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 100), 39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 105), 39.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 110), 38.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 110), 39.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 115), 38.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 120), 37.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 125), 36.8, 0.21 + 1.2); @@ -2885,8 +2885,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(25, 140), 33.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 145), 33.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 150), 32.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 155), 32.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 160), 33.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 155), 32.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 160), 33.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 165), 33.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 170), 34.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(25, 175), 36.2, 0.21 + 1.2); @@ -2898,10 +2898,10 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(30, -160), 47.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -155), 48.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -150), 49.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -145), 49.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -145), 49.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -140), 50.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -135), 51.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -130), 52.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -130), 52.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -125), 53.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -120), 54.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -115), 55.8, 0.21 + 1.2); @@ -2912,45 +2912,45 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(30, -90), 58.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -85), 58.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -80), 57.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -75), 56.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -70), 55.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -75), 56.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -70), 55.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -65), 53.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -60), 51.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -55), 49.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -55), 49.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -50), 47.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -45), 45.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -40), 44.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -35), 42.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -45), 45.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -40), 43.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -35), 42.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -30), 41.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -25), 40.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -20), 39.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -15), 39.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -15), 39.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -10), 39.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, -5), 39.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 0), 40.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 5), 40.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 10), 41.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 15), 42.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 15), 42.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 20), 42.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 25), 43.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 30), 44.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 35), 45.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 25), 43.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 30), 44.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 35), 45.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 40), 46.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 45), 46.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 50), 47.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 55), 47.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 60), 47.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 55), 47.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 60), 47.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 65), 47.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 70), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 75), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 80), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 85), 47.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 90), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 90), 47.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 95), 47.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 100), 47.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 105), 47.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 110), 47.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 115), 46.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 115), 46.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 120), 45.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 125), 44.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 130), 43.7, 0.21 + 1.2); @@ -2961,7 +2961,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(30, 155), 40.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 160), 40.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 165), 40.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 170), 41.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 170), 41.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 175), 42.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(30, 180), 43.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -180), 48.4, 0.21 + 1.2); @@ -2988,15 +2988,15 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(35, -75), 61.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -70), 60.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -65), 58.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -60), 56.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -55), 55.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -50), 53.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -45), 52.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -60), 56.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -55), 55.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -50), 53.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -45), 52.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -40), 50.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -35), 49.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -30), 48.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -25), 47.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -20), 47.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -20), 47.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -15), 47.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -10), 47.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, -5), 47.7, 0.21 + 1.2); @@ -3011,20 +3011,20 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(35, 40), 53.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 45), 53.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 50), 54.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 55), 54.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 55), 54.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 60), 54.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 65), 54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 70), 54.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 75), 54.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 80), 54.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 85), 54.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 85), 54.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 90), 54.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 95), 54.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 95), 54.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 100), 54.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 105), 54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 110), 54.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 115), 53.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 120), 52.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 115), 53.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 120), 52.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 125), 51.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 130), 50.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(35, 135), 49.6, 0.21 + 1.2); @@ -3056,13 +3056,13 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(40, -100), 66.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -95), 67.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -90), 67.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -85), 67.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -85), 66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -80), 66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -75), 65.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -70), 64.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -70), 64.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -65), 62.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -60), 61.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -55), 60.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -55), 60.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -50), 58.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -45), 57.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, -40), 56.6, 0.21 + 1.2); @@ -3075,14 +3075,14 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(40, -5), 54.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 0), 54.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 5), 55.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 10), 55.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 10), 55.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 15), 56.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 20), 56.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 25), 57.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 30), 57.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 30), 58.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 35), 58.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 40), 58.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 45), 59.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 40), 59.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 45), 59.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 50), 59.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 55), 59.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 60), 60.1, 0.21 + 1.2); @@ -3090,15 +3090,15 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(40, 70), 60.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 75), 60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 80), 60.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 85), 60.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 85), 61.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 90), 61.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 95), 61.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 100), 60.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 105), 60.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 105), 60.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 110), 60.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 115), 59.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 120), 58.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 125), 57.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 125), 57.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 130), 56.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 135), 55.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(40, 140), 54.6, 0.21 + 1.2); @@ -3132,16 +3132,16 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(45, -85), 70.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -80), 70.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -75), 69.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -70), 68.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -70), 68.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -65), 67.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -60), 65.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -60), 65.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -55), 64.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -50), 63.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -45), 62.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -40), 61.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -35), 61.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -30), 60.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -25), 60.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -25), 60.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -20), 59.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -15), 59.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, -10), 60.0, 0.21 + 1.2); @@ -3150,25 +3150,25 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(45, 5), 60.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 10), 61.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 15), 61.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 20), 62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 20), 62.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 25), 62.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 30), 63.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 35), 63.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 40), 63.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 45), 64.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 50), 64.4, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 55), 64.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 45), 64.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 50), 64.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 55), 64.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 60), 65.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 65), 65.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 70), 65.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 70), 65.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 75), 65.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 80), 66.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 85), 66.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 90), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 80), 66.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 85), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 90), 66.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 95), 66.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 100), 66.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 105), 66.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 110), 65.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 110), 65.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 115), 64.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 120), 64.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(45, 125), 63.2, 0.21 + 1.2); @@ -3205,8 +3205,8 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(50, -85), 74.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, -80), 73.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, -75), 72.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -70), 72.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -65), 71.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -70), 71.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -65), 70.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, -60), 69.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, -55), 68.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, -50), 67.9, 0.21 + 1.2); @@ -3226,20 +3226,20 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(50, 20), 66.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 25), 66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 30), 67.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 35), 67.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 35), 67.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 40), 67.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 45), 68.2, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 50), 68.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 50), 68.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 55), 68.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 60), 69.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 65), 69.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 70), 69.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 70), 70.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 75), 70.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 80), 70.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 85), 70.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 90), 71.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 95), 71.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 100), 70.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 100), 71.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 105), 70.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 110), 70.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 115), 69.6, 0.21 + 1.2); @@ -3248,7 +3248,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(50, 130), 66.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 135), 65.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 140), 64.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 145), 64.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 145), 64.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 150), 63.3, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 155), 62.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(50, 160), 62.2, 0.21 + 1.2); @@ -3273,18 +3273,18 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(55, -110), 76.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -105), 77.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -100), 77.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -95), 77.8, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -90), 77.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -95), 77.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -90), 77.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -85), 77.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -80), 77.0, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -75), 76.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -80), 76.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -75), 76.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -70), 75.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -65), 74.5, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -60), 73.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -55), 72.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -60), 73.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -55), 72.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -50), 71.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -45), 71.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -40), 70.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -40), 70.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -35), 69.9, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -30), 69.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, -25), 69.2, 0.21 + 1.2); @@ -3305,7 +3305,7 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(55, 50), 72.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 55), 72.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 60), 72.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 65), 73.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 65), 73.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 70), 73.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 75), 74.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(55, 80), 74.6, 0.21 + 1.2); @@ -3350,13 +3350,13 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(60, -90), 80.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -85), 80.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -80), 79.9, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -75), 79.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -75), 79.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -70), 78.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -65), 77.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -60), 76.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -60), 76.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -55), 76.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -50), 75.3, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -45), 74.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -45), 74.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -40), 74.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -35), 73.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, -30), 73.2, 0.21 + 1.2); @@ -3373,20 +3373,20 @@ TEST(GeoLookupTest, inclination) EXPECT_NEAR(get_mag_inclination_degrees(60, 25), 73.6, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 30), 73.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 35), 74.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 40), 74.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 40), 74.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 45), 74.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 50), 75.2, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 55), 75.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 60), 76.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 65), 76.6, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 70), 77.1, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 75), 77.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 60), 76.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 65), 76.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 70), 77.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 75), 77.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 80), 78.1, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 85), 78.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 90), 78.7, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 95), 78.8, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 100), 78.7, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 105), 78.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 105), 78.5, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 110), 78.0, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 115), 77.4, 0.21 + 1.2); EXPECT_NEAR(get_mag_inclination_degrees(60, 120), 76.7, 0.21 + 1.2); @@ -3406,1683 +3406,1683 @@ TEST(GeoLookupTest, inclination) TEST(GeoLookupTest, strength) { - EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58381, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57229, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56063, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54893, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53726, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52562, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51404, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50243, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49068, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47864, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46612, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45295, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43900, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42422, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40865, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39243, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37579, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35908, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34268, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32705, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31261, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 29976, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28878, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 27982, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27283, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26762, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26384, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26111, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25901, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25721, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25548, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25373, 145 + 254); - EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25198, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25040, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24925, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24890, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 24979, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25242, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25725, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26469, 145 + 265); - EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27500, 145 + 275); - EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28830, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30449, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32336, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34454, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36757, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39196, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41719, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44280, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46832, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49338, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51765, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54080, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56254, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58256, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60058, 145 + 601); - EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61633, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62963, 145 + 630); - EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64035, 145 + 640); - EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64845, 145 + 648); - EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65399, 145 + 654); - EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65709, 145 + 657); - EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65791, 145 + 658); - EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65664, 145 + 657); - EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65349, 145 + 653); + EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58366, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57212, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56045, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54873, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53703, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52538, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51377, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50215, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49039, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47834, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46581, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45263, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43868, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42390, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40832, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39210, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37547, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35876, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34237, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32675, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31233, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 29950, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28854, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 27960, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27263, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26744, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26368, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26095, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25885, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25704, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25530, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25353, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25176, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25017, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24901, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24866, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 24956, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25221, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25707, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26455, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27492, 145 + 275); + EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28827, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30453, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32345, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34467, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36775, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39216, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41742, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44303, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46856, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49362, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51788, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54102, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56274, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58275, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60075, 145 + 601); + EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61649, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62978, 145 + 630); + EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64047, 145 + 640); + EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64856, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65408, 145 + 654); + EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65716, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65796, 145 + 658); + EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65667, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65350, 145 + 654); EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64864, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64231, 145 + 642); - EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63468, 145 + 635); - EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62596, 145 + 626); - EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61633, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60598, 145 + 606); - EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59508, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58381, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56229, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55022, 145 + 935); - EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53807, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52592, 145 + 894); - EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51384, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50188, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49004, 145 + 833); - EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47828, 145 + 813); - EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46650, 145 + 793); - EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45453, 145 + 773); - EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44217, 145 + 752); - EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42923, 145 + 730); - EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41554, 145 + 706); - EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40100, 145 + 682); - EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38562, 145 + 656); - EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36949, 145 + 628); - EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35284, 145 + 600); - EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33598, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31937, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30351, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28896, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27622, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26568, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25754, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25177, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24809, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24603, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24505, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24462, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24429, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24379, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24298, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24185, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24053, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23926, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23840, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23846, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24003, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24375, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25024, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 25994, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27307, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28961, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30928, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33162, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35604, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38189, 145 + 649); - EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40851, 145 + 694); - EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43529, 145 + 740); - EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46171, 145 + 785); - EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48733, 145 + 828); - EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51183, 145 + 870); - EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53492, 145 + 909); - EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55634, 145 + 946); - EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57582, 145 + 979); - EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59309, 145 + 1008); - EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60794, 145 + 1033); - EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62019, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62979, 145 + 1071); - EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63675, 145 + 1082); - EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64118, 145 + 1090); - EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64325, 145 + 1094); - EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64313, 145 + 1093); - EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64102, 145 + 1090); - EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63711, 145 + 1083); + EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64229, 145 + 642); + EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63464, 145 + 635); + EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62590, 145 + 626); + EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61624, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60587, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59496, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58366, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56216, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55008, 145 + 935); + EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53791, 145 + 914); + EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52574, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51364, 145 + 873); + EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50166, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 48980, 145 + 833); + EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47803, 145 + 813); + EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46624, 145 + 793); + EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45425, 145 + 772); + EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44189, 145 + 751); + EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42894, 145 + 729); + EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41524, 145 + 706); + EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40070, 145 + 681); + EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38532, 145 + 655); + EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36919, 145 + 628); + EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35253, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33568, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31908, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30323, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28870, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27598, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26546, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25735, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25160, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24794, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24590, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24492, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24448, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24414, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24362, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24278, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24163, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24029, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23900, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23814, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23820, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 23978, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24354, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25008, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 25984, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27304, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28965, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30939, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33179, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35626, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38215, 145 + 650); + EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40879, 145 + 695); + EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43558, 145 + 740); + EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46199, 145 + 785); + EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48760, 145 + 829); + EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51209, 145 + 871); + EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53516, 145 + 910); + EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55655, 145 + 946); + EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57601, 145 + 979); + EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59327, 145 + 1009); + EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60810, 145 + 1034); + EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62034, 145 + 1055); + EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62992, 145 + 1071); + EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63686, 145 + 1083); + EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64127, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64332, 145 + 1094); + EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64319, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64106, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63714, 145 + 1083); EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63158, 145 + 1074); - EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62458, 145 + 1062); - EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61631, 145 + 1048); - EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60693, 145 + 1032); - EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59665, 145 + 1014); - EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58566, 145 + 996); - EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57415, 145 + 976); - EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56229, 145 + 956); - EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53891, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52657, 145 + 895); - EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51417, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50180, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48951, 145 + 832); - EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47737, 145 + 812); - EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46540, 145 + 791); - EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45359, 145 + 771); - EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44187, 145 + 751); - EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43009, 145 + 731); - EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41807, 145 + 711); - EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40562, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39255, 145 + 667); - EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37873, 145 + 644); - EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36411, 145 + 619); - EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34872, 145 + 593); - EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33271, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31637, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30015, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28461, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27040, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25814, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24834, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24126, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23686, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23477, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23442, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23514, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23631, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23745, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23827, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23864, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23851, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23794, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23708, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23618, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23571, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23630, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23874, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24383, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25228, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26449, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28055, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30022, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32298, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34811, 145 + 592); - EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37480, 145 + 637); - EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40222, 145 + 684); - EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42960, 145 + 730); - EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45631, 145 + 776); - EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48187, 145 + 819); - EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50596, 145 + 860); - EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52831, 145 + 898); - EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54870, 145 + 933); - EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56691, 145 + 964); - EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58272, 145 + 991); - EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59598, 145 + 1013); - EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60660, 145 + 1031); - EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61461, 145 + 1045); - EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 62012, 145 + 1054); - EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62330, 145 + 1060); - EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62435, 145 + 1061); - EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62344, 145 + 1060); - EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62073, 145 + 1055); - EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61636, 145 + 1048); - EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61045, 145 + 1038); - EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60313, 145 + 1025); - EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59455, 145 + 1011); - EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58486, 145 + 994); - EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57425, 145 + 976); - EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56292, 145 + 957); - EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55108, 145 + 937); - EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53891, 145 + 916); - EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51395, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50159, 145 + 853); - EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48920, 145 + 832); - EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47684, 145 + 811); - EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46457, 145 + 790); - EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45244, 145 + 769); - EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44050, 145 + 749); - EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42876, 145 + 729); - EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41719, 145 + 709); - EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40571, 145 + 690); - EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39417, 145 + 670); - EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38241, 145 + 650); - EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37025, 145 + 629); - EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35754, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34415, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33004, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31525, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30000, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28473, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27001, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25658, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24516, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23633, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23041, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22734, 145 + 386); - EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22669, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22780, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 22992, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23243, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23488, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23703, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23879, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24009, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24089, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24115, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24097, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24065, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24076, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24213, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24572, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25246, 145 + 429); - EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26301, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27767, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29634, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31853, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34345, 145 + 584); - EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 37016, 145 + 629); - EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39765, 145 + 676); - EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42499, 145 + 722); - EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45143, 145 + 767); - EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47642, 145 + 810); - EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49959, 145 + 849); - EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52070, 145 + 885); - EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53955, 145 + 917); - EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55595, 145 + 945); - EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56975, 145 + 969); - EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58088, 145 + 987); - EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58937, 145 + 1002); - EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59541, 145 + 1012); - EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59924, 145 + 1019); - EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60110, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60120, 145 + 1022); - EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59966, 145 + 1019); - EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59656, 145 + 1014); - EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59195, 145 + 1006); - EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58591, 145 + 996); - EXPECT_NEAR(get_mag_strength_tesla(-35, 150) * 1e9, 57852, 145 + 983); - EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56990, 145 + 969); - EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56016, 145 + 952); - EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54949, 145 + 934); - EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53808, 145 + 915); - EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52617, 145 + 894); - EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51395, 145 + 874); - EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48756, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47544, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46333, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45126, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43927, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42740, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41570, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40422, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39299, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38196, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37106, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36016, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34912, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33779, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32597, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31351, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30036, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28666, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27279, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25935, 145 + 259); - EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24708, 145 + 247); - EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23678, 145 + 237); - EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22907, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22429, 145 + 224); - EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22237, 145 + 222); - EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22285, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22503, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22819, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23172, 145 + 232); - EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23528, 145 + 235); - EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23872, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24202, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24508, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24776, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 24980, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25108, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25167, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25198, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25280, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25510, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 25997, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26831, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28071, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29731, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31780, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34142, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36716, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39386, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 42045, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44603, 145 + 446); - EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46997, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49186, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51141, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52842, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54270, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55414, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56277, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56882, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57266, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57469, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57524, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57449, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57246, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56913, 145 + 569); - EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56445, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55843, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55113, 145 + 551); - EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54264, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53306, 145 + 533); - EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52256, 145 + 523); - EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51132, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49958, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48756, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46003, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44843, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43688, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42540, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41400, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40270, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39156, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38065, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37002, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35968, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34961, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 33976, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33002, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32022, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31015, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29956, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28831, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27647, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26436, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25254, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24176, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23277, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22621, 145 + 226); - EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22242, 145 + 222); - EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22133, 145 + 221); - EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22255, 145 + 223); - EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22541, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22924, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23354, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23805, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24272, 145 + 243); - EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24757, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25249, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25722, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26133, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26446, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26647, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26756, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26835, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 26977, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27290, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27881, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28837, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30207, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31984, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34111, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36486, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38986, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41492, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43903, 145 + 439); - EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46147, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48178, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49960, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51465, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52671, 145 + 527); - EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53568, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54171, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54522, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54681, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54707, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54640, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54492, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54255, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53910, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53442, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52848, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52135, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51310, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50386, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49373, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48289, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47158, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46003, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43196, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42117, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41050, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 39994, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38947, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37911, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36890, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35892, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34924, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 33992, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33096, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32237, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31407, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30591, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29765, 145 + 298); - EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28901, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 27980, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27000, 145 + 270); - EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 25988, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 24993, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24080, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23319, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22766, 145 + 228); - EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22457, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22392, 145 + 224); - EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22542, 145 + 225); - EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22856, 145 + 229); - EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23277, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23763, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24295, 145 + 243); - EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24870, 145 + 249); - EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25491, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26144, 145 + 261); - EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26796, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27393, 145 + 274); - EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27882, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28231, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28442, 145 + 284); - EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28557, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28651, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28824, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29189, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29852, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30892, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32337, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34151, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36243, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38491, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40770, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42974, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 45026, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46871, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48468, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49779, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50772, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51436, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51796, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51911, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51864, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51730, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51558, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51352, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51088, 145 + 511); - EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50735, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50269, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49684, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48989, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48197, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47316, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(-20, 165) * 1e9, 46358, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-20, 170) * 1e9, 45338, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44275, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43196, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40441, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39475, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38527, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37595, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36675, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35769, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34880, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34015, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33183, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32388, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31635, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30924, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30253, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29607, 145 + 296); - EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28964, 145 + 290); - EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28295, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27580, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26812, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26009, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25208, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24461, 145 + 245); - EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23824, 145 + 238); - EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23349, 145 + 233); - EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23070, 145 + 231); - EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23000, 145 + 230); - EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23130, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62457, 145 + 1062); + EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61628, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60688, 145 + 1032); + EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59658, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58557, 145 + 995); + EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57404, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56216, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53880, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52645, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51403, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50164, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48933, 145 + 832); + EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47717, 145 + 811); + EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46519, 145 + 791); + EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45336, 145 + 771); + EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44163, 145 + 751); + EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 42984, 145 + 731); + EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41781, 145 + 710); + EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40535, 145 + 689); + EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39228, 145 + 667); + EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37846, 145 + 643); + EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36383, 145 + 619); + EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34844, 145 + 592); + EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33243, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31609, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 29988, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28435, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27015, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25792, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24815, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24110, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23672, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23466, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23432, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23504, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23620, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23732, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23812, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23846, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23831, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23771, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23682, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23591, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23542, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23602, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23849, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24364, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25215, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26445, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28060, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30036, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32319, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34838, 145 + 592); + EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37511, 145 + 638); + EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40255, 145 + 684); + EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42993, 145 + 731); + EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45663, 145 + 776); + EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48218, 145 + 820); + EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50623, 145 + 861); + EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52855, 145 + 899); + EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54891, 145 + 933); + EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56710, 145 + 964); + EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58289, 145 + 991); + EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59613, 145 + 1013); + EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60674, 145 + 1031); + EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61473, 145 + 1045); + EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 62022, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62339, 145 + 1060); + EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62442, 145 + 1062); + EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62350, 145 + 1060); + EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62078, 145 + 1055); + EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61638, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61046, 145 + 1038); + EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60312, 145 + 1025); + EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59452, 145 + 1011); + EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58482, 145 + 994); + EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57419, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56285, 145 + 957); + EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55099, 145 + 937); + EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53880, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51386, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50148, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48908, 145 + 831); + EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47670, 145 + 810); + EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46441, 145 + 790); + EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45226, 145 + 769); + EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44030, 145 + 749); + EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42855, 145 + 729); + EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41698, 145 + 709); + EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40549, 145 + 689); + EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39394, 145 + 670); + EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38217, 145 + 650); + EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37000, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35729, 145 + 607); + EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34390, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 32978, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31499, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 29975, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28447, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 26977, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25635, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24495, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23615, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23026, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22722, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22660, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22772, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 22985, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23235, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23478, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23691, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23864, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 23992, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24068, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24092, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24071, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24036, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24046, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24185, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24550, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25231, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26295, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27772, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29648, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31876, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34375, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 37050, 145 + 630); + EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39801, 145 + 677); + EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42536, 145 + 723); + EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45178, 145 + 768); + EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47674, 145 + 810); + EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49987, 145 + 850); + EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52094, 145 + 886); + EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53976, 145 + 918); + EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55613, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56990, 145 + 969); + EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58101, 145 + 988); + EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58949, 145 + 1002); + EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59552, 145 + 1012); + EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59934, 145 + 1019); + EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60119, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60127, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59972, 145 + 1020); + EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59660, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59198, 145 + 1006); + EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58593, 145 + 996); + EXPECT_NEAR(get_mag_strength_tesla(-35, 150) * 1e9, 57853, 145 + 983); + EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56988, 145 + 969); + EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56013, 145 + 952); + EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54945, 145 + 934); + EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53803, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52610, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51386, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48750, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47536, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46322, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45113, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43912, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42723, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41552, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40404, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39280, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38176, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37085, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 35995, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34891, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33756, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32574, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31328, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30012, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28642, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27255, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25911, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24686, 145 + 247); + EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23658, 145 + 237); + EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22890, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22416, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22227, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22278, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22498, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22814, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23167, 145 + 232); + EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23521, 145 + 235); + EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23864, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24192, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24496, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24760, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 24962, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25085, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25140, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25169, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25250, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25485, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 25978, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26822, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28072, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29744, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31802, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34173, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36752, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39425, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 42084, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44640, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 47031, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49216, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51166, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52862, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54286, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55427, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56288, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56892, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57275, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57477, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57531, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57454, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57252, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56917, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56448, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55845, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55114, 145 + 551); + EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54263, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53305, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52253, 145 + 523); + EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51128, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49953, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48750, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 45998, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44836, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43679, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42529, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41387, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40255, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39140, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38048, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 36984, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35950, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34943, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 33957, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 32983, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32003, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 30994, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29935, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28809, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27624, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26412, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25230, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24153, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23257, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22604, 145 + 226); + EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22229, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22125, 145 + 221); + EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22250, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22538, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22923, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23353, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23803, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24270, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24753, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25243, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25713, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26120, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26428, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26624, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26729, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26807, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 26950, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27268, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27868, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28836, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30216, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 32004, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34140, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36520, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 39024, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41531, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43941, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46182, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48208, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49985, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51484, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52685, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53579, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54179, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54529, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54687, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54713, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54645, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54497, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54260, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53914, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53446, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52851, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52137, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51311, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50385, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49372, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48287, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47155, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 45998, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43192, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42112, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41042, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 39983, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38935, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37897, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36875, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35877, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34909, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 33976, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33080, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32220, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31390, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30574, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29747, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28882, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 27959, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 26978, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 25964, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 24968, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24057, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23298, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22748, 145 + 227); + EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22443, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22384, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22538, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22855, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23279, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23766, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24298, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24873, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25493, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26145, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26794, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27386, 145 + 274); + EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27870, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28213, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28420, 145 + 284); + EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28532, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28626, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28803, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29174, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29846, 145 + 298); + EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30897, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32352, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34175, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36274, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38526, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40807, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 43010, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 45060, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46901, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48493, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49798, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50785, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51445, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51802, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51916, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51868, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51734, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51561, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51355, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51092, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50739, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50273, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49688, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48992, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48199, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47318, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(-20, 165) * 1e9, 46359, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-20, 170) * 1e9, 45337, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44274, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43192, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40439, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39471, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38520, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37585, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36664, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35756, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34866, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34002, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33169, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32374, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31620, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30910, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30237, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29590, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28946, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28276, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27559, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26790, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 25985, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25184, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24437, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23802, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23330, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23055, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 22991, 145 + 230); + EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23126, 145 + 231); EXPECT_NEAR(get_mag_strength_tesla(-15, -50) * 1e9, 23427, 145 + 234); - EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23852, 145 + 239); - EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24370, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24960, 145 + 250); - EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25616, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26332, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27093, 145 + 271); - EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27860, 145 + 279); - EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28579, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29192, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29657, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29963, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30129, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30210, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30290, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30476, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30883, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23856, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24376, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24967, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25623, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26339, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27098, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27863, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28578, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29186, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29645, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29945, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30108, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30188, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30270, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30460, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30874, 145 + 309); EXPECT_NEAR(get_mag_strength_tesla(-15, 35) * 1e9, 31612, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32718, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34189, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35952, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37891, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39886, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41832, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43652, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45289, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46694, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47817, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48619, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49084, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49242, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49164, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48951, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48690, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48432, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48176, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47888, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47523, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47053, 145 + 471); - EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46474, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45797, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45039, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44210, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43321, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42384, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32728, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34208, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35978, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37922, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39919, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41865, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43683, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45317, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46717, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47835, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48631, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49092, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49247, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49168, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48954, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48693, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48434, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48179, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47892, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47528, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47058, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46479, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45802, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45042, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44214, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43324, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42385, 145 + 424); EXPECT_NEAR(get_mag_strength_tesla(-15, 175) * 1e9, 41416, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40441, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37894, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37070, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36269, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35488, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34724, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33977, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33252, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32556, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31895, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31273, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30692, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30152, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29653, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29182, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28721, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28245, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27732, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27172, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26571, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25951, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25349, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24809, 145 + 248); - EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24373, 145 + 244); - EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24080, 145 + 241); - EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23956, 145 + 240); - EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24012, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40439, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37892, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37066, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36263, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35480, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34714, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33967, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33241, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32545, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31883, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31261, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30679, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30139, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29639, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29166, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28704, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28226, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27711, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27149, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26546, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25926, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25324, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24785, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24352, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24063, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23944, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24006, 145 + 240); EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24242, 145 + 242); - EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24626, 145 + 246); - EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25136, 145 + 251); - EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25746, 145 + 257); - EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26437, 145 + 264); - EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27192, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27988, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28791, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29552, 145 + 296); - EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30217, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30746, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31118, 145 + 311); - EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31336, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31432, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31471, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31546, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31772, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32256, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33071, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34222, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35652, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37262, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38943, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40600, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42161, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43572, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44779, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45726, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46366, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46678, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46690, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46482, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46156, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45806, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45480, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45175, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44852, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44462, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43976, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43393, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42729, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 42004, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41232, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40423, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39586, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24630, 145 + 246); + EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25144, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25756, 145 + 258); + EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26447, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27201, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27997, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28798, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29555, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30216, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30740, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31106, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31320, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31415, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31454, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31531, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31762, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32254, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33077, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34236, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35673, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37288, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38971, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40628, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42189, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43597, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44800, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45743, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46377, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46685, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46695, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46485, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46158, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45807, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45482, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45178, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44855, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44466, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43982, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43399, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42735, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 42009, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41237, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40426, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39588, 145 + 396); EXPECT_NEAR(get_mag_strength_tesla(-10, 175) * 1e9, 38737, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37894, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35733, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35073, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34440, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33829, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33238, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32669, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32129, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31625, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31161, 145 + 312); - EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30737, 145 + 307); - EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30352, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30002, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29686, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29395, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29115, 145 + 291); - EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28825, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28503, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28133, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27708, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27238, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26745, 145 + 267); - EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26262, 145 + 263); - EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25827, 145 + 258); - EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25480, 145 + 255); - EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25259, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25194, 145 + 252); - EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25305, 145 + 253); - EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25593, 145 + 256); - EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26038, 145 + 260); - EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26610, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27277, 145 + 273); - EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 28006, 145 + 280); - EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28771, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29538, 145 + 295); - EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30268, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30922, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31464, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31871, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32135, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32270, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32320, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32362, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32500, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32837, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33445, 145 + 334); - EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34337, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35472, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36769, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38138, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39501, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40797, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41977, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42987, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43770, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44275, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44481, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44410, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44136, 145 + 441); - EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43753, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43346, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42962, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42598, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42216, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41777, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41255, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40654, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39994, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39297, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38581, 145 + 386); - EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37857, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37133, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37892, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35732, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35070, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34435, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33822, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33230, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32660, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32119, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31615, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31150, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30726, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30340, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 29990, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29672, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29380, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29098, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28806, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28482, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28109, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27682, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27211, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26718, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26236, 145 + 262); + EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25804, 145 + 258); + EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25461, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25244, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25185, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25302, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25596, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26045, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26620, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27287, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 28017, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28781, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29546, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30274, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30924, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31462, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31865, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32125, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32257, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32306, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32350, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32492, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32834, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33448, 145 + 334); + EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34348, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35489, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36790, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38162, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39526, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40822, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 42000, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 43007, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43786, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44287, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44489, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44416, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44139, 145 + 441); + EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43755, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43349, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42965, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42600, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42220, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41782, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41261, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40661, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 40000, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39303, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38586, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37860, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37135, 145 + 371); EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36422, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35733, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34117, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33632, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33175, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32739, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32324, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31937, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31588, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31286, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31031, 145 + 310); - EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30820, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30645, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30498, 145 + 305); - EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30374, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30267, 145 + 303); - EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30166, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30053, 145 + 301); - EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29903, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29693, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29407, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29042, 145 + 290); - EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28614, 145 + 286); - EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28147, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27679, 145 + 277); - EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27249, 145 + 272); - EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26903, 145 + 269); - EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26686, 145 + 267); - EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26638, 145 + 266); - EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26778, 145 + 268); - EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27099, 145 + 271); - EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27570, 145 + 276); - EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28149, 145 + 281); - EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28795, 145 + 288); - EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29475, 145 + 295); - EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30158, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30815, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31420, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35732, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34116, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33629, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33171, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32734, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32317, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31929, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31579, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31276, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31021, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30810, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30634, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30486, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30361, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30252, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30148, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30032, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29880, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29667, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29380, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29014, 145 + 290); + EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28585, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28120, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27653, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27227, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26885, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26673, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26631, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26777, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27103, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27577, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28158, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28805, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29484, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30166, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30821, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31424, 145 + 314); EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31945, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32367, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32672, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32863, 145 + 329); - EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32970, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33054, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33199, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33490, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33986, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34701, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35602, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36631, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37720, 145 + 377); - EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38814, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39863, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40824, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41649, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42283, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42681, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42820, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42718, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42429, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42031, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41591, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41147, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40700, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40229, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39706, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39119, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38478, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37805, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37125, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36458, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35815, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35207, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34640, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34117, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33141, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32822, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32534, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32266, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32019, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31806, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31642, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31537, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31492, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31497, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31537, 145 + 315); - EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31599, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31673, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31753, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31828, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31879, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31877, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31794, 145 + 318); - EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31605, 145 + 316); - EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31302, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30895, 145 + 309); - EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30409, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29880, 145 + 299); - EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29350, 145 + 294); - EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28870, 145 + 289); - EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28492, 145 + 285); - EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28270, 145 + 283); - EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28235, 145 + 282); - EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28391, 145 + 284); - EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28711, 145 + 287); - EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29154, 145 + 292); - EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29676, 145 + 297); - EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30240, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30820, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31395, 145 + 314); - EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31944, 145 + 319); - EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32446, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32878, 145 + 329); - EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33226, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33487, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33684, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33862, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34084, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34412, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34881, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35495, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36232, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37052, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37916, 145 + 379); - EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38787, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39629, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40405, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41072, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41583, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41899, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 42002, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41899, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41627, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41235, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40767, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40251, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39696, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39099, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38454, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37768, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37057, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36346, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35661, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35022, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34446, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(5, 170) * 1e9, 33940, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(5, 175) * 1e9, 33508, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33141, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32823, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32644, 145 + 326); - EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32502, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32382, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32287, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32233, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32240, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32322, 145 + 323); - EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32476, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32690, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32941, 145 + 329); - EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33210, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33482, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33748, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 33993, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34195, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34321, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34336, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34210, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33934, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33516, 145 + 335); - EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 32985, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32377, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31741, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31130, 145 + 311); - EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30604, 145 + 306); - EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30220, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30016, 145 + 300); - EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30001, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32364, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32667, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32856, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32962, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33046, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33193, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33488, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33990, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34710, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35616, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36649, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37741, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38836, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39886, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40846, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41668, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42300, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42694, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42830, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42725, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42435, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42036, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41595, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41151, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40705, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40234, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39712, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39126, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38485, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37812, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37131, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36462, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35819, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35209, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34641, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34116, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33140, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32820, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32531, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32261, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32013, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31798, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31633, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31528, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31482, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31487, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31526, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31586, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31658, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31736, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31808, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31856, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31852, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31767, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31576, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31272, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30865, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30380, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29852, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29325, 145 + 293); + EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28847, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28475, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28258, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28229, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28390, 145 + 284); + EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28715, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29161, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29683, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30247, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30827, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31401, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31949, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32449, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32879, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33225, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33485, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33681, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33859, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34083, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34414, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34886, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35505, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36246, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37069, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37936, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38808, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39652, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40428, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41093, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41602, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41915, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 42015, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41910, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41636, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41243, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40774, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40257, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39702, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39105, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38460, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37774, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37063, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36352, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35666, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35027, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34449, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 170) * 1e9, 33943, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 175) * 1e9, 33509, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33140, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32822, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32642, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32498, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32377, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32280, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32225, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32231, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32312, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32466, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32678, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32928, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33195, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33466, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33729, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 33971, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34170, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34294, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34306, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34179, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33902, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33485, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 32953, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32346, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31712, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31104, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30583, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30204, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30006, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 29996, 145 + 300); EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30155, 145 + 302); - EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30442, 145 + 304); - EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30822, 145 + 308); - EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31264, 145 + 313); - EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31746, 145 + 317); - EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32250, 145 + 322); - EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32759, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33249, 145 + 332); - EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33699, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34092, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34428, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34726, 145 + 347); - EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35021, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35356, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35764, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36258, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36830, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37463, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38138, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38838, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39546, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40237, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40878, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41431, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41855, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42120, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42208, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42122, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41875, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41490, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40987, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40384, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39698, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38946, 145 + 389); - EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38147, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37326, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36511, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35730, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35008, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34369, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33827, 145 + 338); - EXPECT_NEAR(get_mag_strength_tesla(10, 170) * 1e9, 33393, 145 + 334); + EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30445, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30827, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31269, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31752, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32256, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32764, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33254, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33703, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34096, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34431, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34728, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35023, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35360, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35770, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36267, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36842, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37478, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38156, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38858, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39568, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40260, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40903, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41455, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41877, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42139, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42225, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42136, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41888, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41501, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40997, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40393, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39706, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38953, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38154, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37333, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36518, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35736, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35014, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34373, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33830, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(10, 170) * 1e9, 33395, 145 + 334); EXPECT_NEAR(get_mag_strength_tesla(10, 175) * 1e9, 33064, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32823, 145 + 328); - EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33125, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33044, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33014, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33016, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33053, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33143, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33307, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33558, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33896, 145 + 339); - EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34302, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34750, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35216, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35678, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36122, 145 + 361); - EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36528, 145 + 365); - EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36868, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37104, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37196, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37114, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36845, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36401, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35810, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35118, 145 + 351); - EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34379, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33653, 145 + 337); - EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33003, 145 + 330); - EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32487, 145 + 325); - EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32144, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31982, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31987, 145 + 320); - EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32129, 145 + 321); - EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32375, 145 + 324); - EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32706, 145 + 327); - EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33104, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33557, 145 + 336); - EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34043, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34537, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35012, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35452, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35856, 145 + 359); - EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36245, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36647, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37090, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37582, 145 + 376); - EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38118, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38682, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39259, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39846, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40444, 145 + 404); - EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41051, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41652, 145 + 417); - EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42216, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42708, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43092, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43339, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43434, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43371, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43146, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42759, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42211, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41513, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40687, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39767, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38796, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37816, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36868, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35984, 145 + 360); - EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35192, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34514, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33967, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33560, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32822, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33124, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33042, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33010, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33010, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33046, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33134, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33296, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33547, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33883, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34288, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34735, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35198, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35659, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36100, 145 + 361); + EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36503, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36840, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37074, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37164, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37081, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36812, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36367, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35776, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35086, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34348, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33624, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 32979, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32468, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32130, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31974, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31983, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32128, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32377, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32709, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33109, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33562, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34050, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34544, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35019, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35458, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35862, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36252, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36655, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37098, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37593, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38131, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38696, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39276, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39864, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40465, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41074, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41677, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42243, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42735, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43117, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43362, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43456, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43390, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43163, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42773, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42224, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41524, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40697, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39776, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38804, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37824, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36874, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35990, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35197, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34518, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33970, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33561, 145 + 336); EXPECT_NEAR(get_mag_strength_tesla(15, 175) * 1e9, 33286, 145 + 333); - EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33125, 145 + 331); - EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33989, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33959, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34002, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34098, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34248, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34466, 145 + 345); - EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34772, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35178, 145 + 352); - EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35678, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36254, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36876, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37516, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38148, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38751, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39299, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39760, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40090, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40248, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40201, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39936, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39465, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38820, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38052, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37223, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36402, 145 + 364); - EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35656, 145 + 357); - EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35042, 145 + 350); - EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34595, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34322, 145 + 343); - EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34210, 145 + 342); - EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34232, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33124, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33988, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33956, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 33997, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34091, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34239, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34455, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34759, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35163, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35662, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36237, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36857, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37495, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38125, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38725, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39271, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39729, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40058, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40214, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40166, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39900, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39429, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38784, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38017, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37190, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36372, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35630, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35021, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34579, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34311, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34203, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34229, 145 + 342); EXPECT_NEAR(get_mag_strength_tesla(20, -25) * 1e9, 34368, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34605, 145 + 346); - EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34935, 145 + 349); - EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35348, 145 + 353); - EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35820, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36322, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36821, 145 + 368); - EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37298, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37755, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38211, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38689, 145 + 387); - EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39206, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39758, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40329, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40897, 145 + 409); - EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41453, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 42002, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42556, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43122, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43691, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44236, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44720, 145 + 447); - EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45108, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45373, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45495, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45460, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45251, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44854, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44257, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43467, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42510, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41433, 145 + 414); - EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40293, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39151, 145 + 392); - EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38056, 145 + 381); - EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37049, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36160, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35412, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34820, 145 + 348); - EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34392, 145 + 344); - EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34122, 145 + 341); - EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33989, 145 + 340); - EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35366, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35343, 145 + 353); - EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35423, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35586, 145 + 356); - EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35829, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36161, 145 + 362); - EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36595, 145 + 366); - EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37137, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37778, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38496, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39260, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40041, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40810, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41540, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42201, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42757, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43163, 145 + 432); - EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43375, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43360, 145 + 434); - EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43104, 145 + 431); - EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42617, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41934, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41112, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40217, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39326, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38509, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37822, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37298, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36941, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36738, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36667, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36714, 145 + 367); - EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36873, 145 + 369); - EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37143, 145 + 371); - EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37516, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37968, 145 + 380); - EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38466, 145 + 385); - EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38975, 145 + 390); - EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39474, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39961, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40452, 145 + 405); - EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40967, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41517, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42096, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42684, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43262, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43823, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44377, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44938, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45518, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46107, 145 + 461); - EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46679, 145 + 467); - EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47199, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47629, 145 + 476); - EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47938, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48102, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48098, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47904, 145 + 479); - EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47494, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46855, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45990, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44928, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43724, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42445, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41160, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39931, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38803, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37810, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36973, 145 + 370); - EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36310, 145 + 363); - EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35826, 145 + 358); - EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35517, 145 + 355); - EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35366, 145 + 354); - EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37220, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37174, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37262, 145 + 373); - EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37466, 145 + 375); - EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37783, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38212, 145 + 382); - EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38758, 145 + 388); - EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39414, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40169, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 40996, 145 + 410); - EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41865, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42744, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43605, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44419, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45153, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45769, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46222, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46471, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46481, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46238, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45752, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45057, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44208, 145 + 442); - EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43278, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42344, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41479, 145 + 415); - EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40736, 145 + 407); - EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40148, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39720, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39439, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39289, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39260, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39349, 145 + 393); - EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39558, 145 + 396); - EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39881, 145 + 399); - EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40295, 145 + 403); - EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40768, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41265, 145 + 413); - EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41761, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42251, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42748, 145 + 427); - EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43267, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43817, 145 + 438); - EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44396, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44988, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45579, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46168, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46761, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47373, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 48008, 145 + 480); - EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48658, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49293, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49877, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50370, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50737, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50950, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50979, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50797, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50379, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49710, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48797, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47670, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46386, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45016, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43636, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42309, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41087, 145 + 411); - EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 40005, 145 + 400); - EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39086, 145 + 391); - EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38345, 145 + 383); - EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37790, 145 + 378); - EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37419, 145 + 374); - EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37220, 145 + 372); - EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39521, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39439, 145 + 394); - EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39517, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39743, 145 + 397); - EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40110, 145 + 401); - EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40612, 145 + 406); - EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41240, 145 + 412); - EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 41982, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42816, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43712, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44641, 145 + 446); - EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45572, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46474, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47320, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48078, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48711, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49177, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49437, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49459, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49230, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48756, 145 + 488); - EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48071, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47227, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46294, 145 + 463); - EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45346, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44453, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43668, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43023, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42527, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42171, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41946, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34607, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34939, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35353, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35828, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36330, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36830, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37308, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37765, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38221, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38701, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39219, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39773, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40345, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40914, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41472, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 42022, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42578, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43147, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43718, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44265, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44750, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45137, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45400, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45520, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45482, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45271, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44871, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44273, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43481, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42522, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41444, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40303, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39159, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38063, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37056, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36166, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35416, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34823, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34394, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34123, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33988, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35364, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35340, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35416, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35577, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35817, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36148, 145 + 361); + EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36580, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37120, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37759, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38475, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39238, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40016, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40783, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41510, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42170, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42724, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43128, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43339, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43323, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43066, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42579, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41897, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41076, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40183, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39295, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38482, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37800, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37281, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36929, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36730, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36663, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36713, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36875, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37148, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37522, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37976, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38476, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38986, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39486, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39974, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40466, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40983, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41535, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42115, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42704, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43282, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43844, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44399, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44962, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45544, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46135, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46710, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47231, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47660, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47968, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48130, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48124, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47926, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47514, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46873, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 46005, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44942, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43736, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42455, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41170, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39940, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38811, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37817, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36979, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36314, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35829, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35518, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35364, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37218, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37170, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37254, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37456, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37770, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38196, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38739, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39394, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40146, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 40971, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41838, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42716, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43575, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44386, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45119, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45733, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46186, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46433, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46443, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46200, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45713, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45019, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44172, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43245, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42314, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41453, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40715, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40132, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39708, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39431, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39284, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39258, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39351, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39563, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39888, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40305, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40780, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41278, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41775, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42267, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42765, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43286, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43838, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44418, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 45010, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45602, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46190, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46785, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47398, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 48036, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48687, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49325, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49910, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50403, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50769, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50979, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 51005, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50821, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50400, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49729, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48813, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47684, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46398, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45028, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43646, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42319, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41096, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 40013, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39093, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38351, 145 + 384); + EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37794, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37420, 145 + 374); + EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37218, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39520, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39434, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39508, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39731, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40095, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40594, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41220, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 41959, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42790, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43684, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44612, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45540, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46441, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47286, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48042, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48674, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49139, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49398, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49420, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49191, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48718, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48034, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47193, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46262, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45318, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44429, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43649, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43008, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42516, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42164, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41942, 145 + 419); EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41842, 145 + 418); - EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41862, 145 + 419); - EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 42004, 145 + 420); - EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42262, 145 + 423); - EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42618, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43040, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43496, 145 + 435); - EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43963, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44433, 145 + 444); - EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44912, 145 + 449); - EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45412, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45945, 145 + 459); - EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46510, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47103, 145 + 471); - EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47716, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48350, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 49009, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49699, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50419, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51152, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51868, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52527, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53088, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53514, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53771, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53828, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53658, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53238, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52560, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51630, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50484, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49177, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47780, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46365, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44998, 145 + 450); - EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43731, 145 + 437); - EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42599, 145 + 426); - EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41625, 145 + 416); - EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40826, 145 + 408); - EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40208, 145 + 402); - EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39774, 145 + 398); - EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39521, 145 + 395); - EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42218, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42104, 145 + 421); - EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42164, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42394, 145 + 424); - EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42783, 145 + 428); - EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43322, 145 + 433); - EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 43993, 145 + 440); - EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44775, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45641, 145 + 456); - EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46559, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47498, 145 + 475); - EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48428, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49320, 145 + 493); - EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50147, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50879, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51483, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51923, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52165, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52182, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 51960, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51507, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50851, 145 + 509); - EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50038, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49130, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48196, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47298, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46488, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45796, 145 + 458); - EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45236, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44808, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44508, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41864, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 42009, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42271, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42629, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43053, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43512, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43980, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44451, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44932, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45434, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45968, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46534, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47127, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47741, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48374, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 49034, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49725, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50447, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51182, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51900, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52560, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53122, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53546, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53800, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53855, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53681, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53259, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52577, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51646, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50498, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49190, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47791, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46376, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 45008, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43741, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42608, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41634, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40832, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40212, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39776, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39520, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42217, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42099, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42155, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42380, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42766, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43302, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 43970, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44749, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45613, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46529, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47466, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48394, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49285, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50111, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50842, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51446, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51886, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52128, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52144, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 51923, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51471, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50816, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50006, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49102, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48171, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47277, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46471, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45783, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45227, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44802, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44504, 145 + 445); EXPECT_NEAR(get_mag_strength_tesla(40, -25) * 1e9, 44330, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44274, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44340, 145 + 443); - EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44520, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44797, 145 + 448); - EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45145, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45538, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45954, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46383, 145 + 464); - EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46830, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47303, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47814, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48368, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48968, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49613, 145 + 496); - EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50303, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51039, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51819, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52630, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53450, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54244, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54972, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55592, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56064, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56353, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56431, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56272, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55861, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55194, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54285, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53169, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51898, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50539, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49158, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47818, 145 + 478); - EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46566, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45438, 145 + 454); - EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44456, 145 + 445); - EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43635, 145 + 436); - EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42985, 145 + 430); - EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42512, 145 + 425); - EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42218, 145 + 422); - EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45210, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45076, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45117, 145 + 451); - EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45332, 145 + 453); - EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45712, 145 + 457); - EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46242, 145 + 462); - EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46903, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47669, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48509, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49392, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50285, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51161, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 51992, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52752, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53415, 145 + 534); - EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 53952, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54334, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54533, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54527, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54307, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53878, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53264, 145 + 533); - EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52504, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51649, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50756, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49880, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49066, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48345, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47737, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47246, 145 + 472); - EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46876, 145 + 469); - EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46624, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46490, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46473, 145 + 465); - EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46565, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46751, 145 + 468); - EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 47011, 145 + 470); - EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47325, 145 + 473); - EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47676, 145 + 477); - EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48055, 145 + 481); - EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48465, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48913, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49409, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49962, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50579, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51261, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 52008, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52816, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53674, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54561, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55449, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56301, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57075, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57730, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58228, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58536, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58628, 145 + 586); - EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58484, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58095, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57466, 145 + 575); - EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56614, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55574, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54396, 145 + 544); - EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53137, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51857, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50608, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49434, 145 + 494); - EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48368, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47430, 145 + 474); - EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46636, 145 + 466); - EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45996, 145 + 460); - EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45518, 145 + 455); - EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45210, 145 + 452); - EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48320, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48181, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48203, 145 + 482); - EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48385, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48720, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49195, 145 + 492); - EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49789, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50477, 145 + 505); - EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51230, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52018, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52811, 145 + 528); - EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53583, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54307, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 54961, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55522, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 55964, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56267, 145 + 563); - EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56407, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56371, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56150, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55750, 145 + 558); - EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55190, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54500, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53720, 145 + 537); - EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52896, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52070, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51283, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50563, 145 + 506); - EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49932, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49400, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44277, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44346, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44529, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44809, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45160, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45554, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45972, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46403, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46851, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47326, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47838, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48394, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48994, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49638, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50329, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51065, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51846, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52658, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53480, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54276, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 55004, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55624, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56094, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56381, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56456, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56294, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55880, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55210, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54298, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53180, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51909, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50549, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49169, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47829, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46578, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45449, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44466, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43644, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42991, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42515, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42217, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45209, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45071, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45108, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45318, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45694, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46220, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46878, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47641, 145 + 476); + EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48478, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49359, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50252, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51126, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 51956, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52716, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53379, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 53916, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54299, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54498, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54493, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54274, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53847, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53234, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52476, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51624, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50734, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49862, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49051, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48334, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47729, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47242, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46874, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46625, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46495, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46480, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46575, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46763, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 47026, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47342, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47694, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48076, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48487, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48936, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49434, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49988, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50605, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51287, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 52035, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52843, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53701, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54590, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55479, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56332, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57106, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57760, 145 + 578); + EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58256, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58561, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58649, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58502, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58110, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57478, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56624, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55584, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54405, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53147, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51867, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50619, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49447, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48380, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47442, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46646, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 46003, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45522, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45209, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48321, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48177, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48194, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48370, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48701, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49171, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49762, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50447, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51198, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 51984, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52776, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53547, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54272, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 54926, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55487, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 55931, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56234, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56376, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56341, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56122, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55724, 145 + 557); + EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55166, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54478, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53701, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52879, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52056, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51272, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50555, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49926, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49398, 145 + 494); EXPECT_NEAR(get_mag_strength_tesla(50, -30) * 1e9, 48975, 145 + 490); - EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48659, 145 + 487); - EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48453, 145 + 485); - EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48354, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48357, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48450, 145 + 484); - EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48620, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48852, 145 + 489); - EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49136, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49465, 145 + 495); - EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49840, 145 + 498); - EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50269, 145 + 503); - EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50760, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51320, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51958, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52673, 145 + 527); - EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53463, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54319, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55223, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56150, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57069, 145 + 571); - EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57941, 145 + 579); - EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58727, 145 + 587); - EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59388, 145 + 594); - EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59889, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60201, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60303, 145 + 603); - EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60182, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59835, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59272, 145 + 593); - EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58514, 145 + 585); - EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57595, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56557, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55450, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54323, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53220, 145 + 532); - EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52178, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51224, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50378, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49655, 145 + 497); - EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49065, 145 + 491); - EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48617, 145 + 486); - EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48320, 145 + 483); - EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51313, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51175, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51173, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51305, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51566, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51944, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52423, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 52982, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53595, 145 + 536); - EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54237, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54883, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55509, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56092, 145 + 561); - EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56612, 145 + 566); - EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57048, 145 + 570); - EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57382, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57596, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57675, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57608, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57391, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57029, 145 + 570); - EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56534, 145 + 565); - EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55930, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55246, 145 + 552); - EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54515, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53771, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53044, 145 + 530); - EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52362, 145 + 524); - EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51744, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48662, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48458, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48362, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48367, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48463, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48634, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48868, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49154, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49484, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49862, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50292, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50783, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51345, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51983, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52699, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53490, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54346, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55251, 145 + 553); + EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56179, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57098, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57970, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58755, 145 + 588); + EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59414, 145 + 594); + EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59913, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60222, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60320, 145 + 603); + EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60196, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59846, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59281, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58522, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57602, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56565, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55459, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54334, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53232, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52191, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51238, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50392, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49667, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49074, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48622, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48321, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51314, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51171, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51163, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51290, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51546, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51920, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52396, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 52951, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53562, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54203, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54848, 145 + 548); + EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55474, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56058, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56578, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57016, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57352, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57568, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57648, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57583, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57368, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57008, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56515, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55914, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55232, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54503, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53761, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53037, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52357, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51741, 145 + 517); EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51204, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50752, 145 + 508); - EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50394, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50133, 145 + 501); - EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49967, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49893, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49906, 145 + 499); - EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49996, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50157, 145 + 502); - EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50381, 145 + 504); - EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50666, 145 + 507); - EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 51014, 145 + 510); - EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51428, 145 + 514); - EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51916, 145 + 519); - EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52482, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53130, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53859, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54662, 145 + 547); - EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55525, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56428, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57346, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58245, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59091, 145 + 591); - EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59848, 145 + 598); - EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60482, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60963, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61268, 145 + 613); - EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61382, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61296, 145 + 613); - EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61013, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60546, 145 + 605); - EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59915, 145 + 599); - EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59151, 145 + 592); - EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58290, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57371, 145 + 574); - EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56433, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55512, 145 + 555); - EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54636, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53830, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53110, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52488, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51976, 145 + 520); - EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51582, 145 + 516); - EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51313, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53927, 145 + 539); - EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53791, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53758, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53828, 145 + 538); - EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 53996, 145 + 540); - EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54255, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54591, 145 + 546); - EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 54988, 145 + 550); - EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55430, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55896, 145 + 559); - EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56365, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56820, 145 + 568); - EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57241, 145 + 572); - EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57612, 145 + 576); - EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57916, 145 + 579); - EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58139, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58266, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58288, 145 + 583); - EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58198, 145 + 582); - EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 57993, 145 + 580); - EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57676, 145 + 577); - EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57258, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56752, 145 + 568); - EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56180, 145 + 562); - EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55564, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54928, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54297, 145 + 543); - EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53690, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50754, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50399, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50139, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49976, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49904, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49918, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 50011, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50172, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50398, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50685, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 51034, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51450, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51939, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52506, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53155, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53885, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54688, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55552, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56456, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57373, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58272, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59117, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59872, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60504, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60983, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61285, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61395, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61306, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61022, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60553, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59921, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59158, 145 + 592); + EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58298, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57380, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56444, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55525, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54651, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53845, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53124, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52501, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51986, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51588, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51314, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53929, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53787, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53749, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53814, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 53978, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54232, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54564, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 54959, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55398, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55863, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56332, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56788, 145 + 568); + EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57210, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57582, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57888, 145 + 579); + EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58112, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58242, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58266, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58178, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 57975, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57660, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57244, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56741, 145 + 567); + EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56171, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55557, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54923, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54293, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53688, 145 + 537); EXPECT_NEAR(get_mag_strength_tesla(60, -40) * 1e9, 53125, 145 + 531); - EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52617, 145 + 526); - EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52177, 145 + 522); - EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51812, 145 + 518); - EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51526, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51322, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51200, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51158, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51192, 145 + 512); - EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51300, 145 + 513); - EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51480, 145 + 515); - EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51731, 145 + 517); - EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52054, 145 + 521); - EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52452, 145 + 525); - EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52927, 145 + 529); - EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53482, 145 + 535); - EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54115, 145 + 541); - EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54822, 145 + 548); - EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55592, 145 + 556); - EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56411, 145 + 564); - EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57259, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58110, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58937, 145 + 589); - EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59709, 145 + 597); - EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60397, 145 + 604); - EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60974, 145 + 610); - EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61417, 145 + 614); - EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61707, 145 + 617); - EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61834, 145 + 618); - EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61796, 145 + 618); - EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61596, 145 + 616); - EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61246, 145 + 612); - EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60766, 145 + 608); - EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60179, 145 + 602); - EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59515, 145 + 595); - EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58802, 145 + 588); - EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58070, 145 + 581); - EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57346, 145 + 573); - EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56653, 145 + 567); - EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 56009, 145 + 560); - EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55428, 145 + 554); - EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54921, 145 + 549); - EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54497, 145 + 545); - EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54164, 145 + 542); - EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53927, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52620, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52181, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51818, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51534, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51332, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51211, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51170, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51206, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51316, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51496, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51748, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52073, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52472, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52949, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53505, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54139, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54846, 145 + 548); + EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55617, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56437, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57284, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58136, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58961, 145 + 590); + EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59732, 145 + 597); + EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60419, 145 + 604); + EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60993, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61433, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61720, 145 + 617); + EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61846, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61805, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61603, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61253, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60772, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60186, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59523, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58812, 145 + 588); + EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58082, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57360, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56668, 145 + 567); + EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 56024, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55442, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54934, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54507, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54170, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53929, 145 + 539); } diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 4707774eb0c3..acdc88ef2219 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -50,16 +50,18 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat // get indicated airspeed from input data (raw airspeed) _IAS = input_data.airspeed_indicated_raw; - update_CAS_scale_validated(input_data.lpos_valid, input_data.ground_velocity, input_data.airspeed_true_raw); + update_CAS_scale_validated(input_data.gnss_valid, input_data.ground_velocity, input_data.airspeed_true_raw); update_CAS_scale_applied(); update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius); - update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, + update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.gnss_valid, input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.q_att); update_in_fixed_wing_flight(input_data.in_fixed_wing_flight); check_airspeed_data_stuck(input_data.timestamp); check_load_factor(input_data.accel_z); - check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio, - input_data.ground_velocity, input_data.lpos_valid); + check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.hdg_test_ratio, + input_data.ground_velocity, input_data.gnss_valid); + check_first_principle(input_data.timestamp, input_data.fixed_wing_tecs_throttle, + input_data.fixed_wing_tecs_throttle_trim, input_data.tecs_timestamp, input_data.q_att); update_airspeed_valid_status(input_data.timestamp); } @@ -71,12 +73,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp) } void -AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid, +AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool gnss_valid, const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att) { _wind_estimator.update(time_now_usec); - if (lpos_valid && _in_fixed_wing_flight) { + if (gnss_valid && _in_fixed_wing_flight) { // airspeed fusion (with raw TAS) const float hor_vel_variance = lpos_evh * lpos_evh; @@ -109,9 +111,9 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) } void -AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw) +AirspeedValidator::update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw) { - if (!_in_fixed_wing_flight || !lpos_valid) { + if (!_in_fixed_wing_flight || !gnss_valid) { return; } @@ -212,7 +214,7 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now) void AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid) + float estimator_status_hdg_test_ratio, const matrix::Vector3f &vI, bool gnss_valid) { // Check normalised innovation levels with requirement for continuous data and use of hysteresis // to prevent false triggering. @@ -222,16 +224,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ } // reset states if check is disabled, we are not flying or wind estimator was just initialized/reset - if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s - || _tas_innov_integ_threshold <= 0.f) { + if (!_innovation_check_enabled || !_in_fixed_wing_flight || (time_now - _time_wind_estimator_initialized) < 5_s) { _innovations_check_failed = false; - _time_last_tas_pass = time_now; _aspd_innov_integ_state = 0.f; - } else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_test_ratio > 1.f) { + } else if (!gnss_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_hdg_test_ratio > 1.f) { //nav velocity data is likely not good //don't run the test but don't reset the check if it had previously failed when nav velocity data was still likely good - _time_last_tas_pass = time_now; _aspd_innov_integ_state = 0.f; } else { @@ -249,11 +248,7 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ _aspd_innov_integ_state = 0.f; } - if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) { - _time_last_tas_pass = time_now; - } - - _innovations_check_failed = (time_now - _time_last_tas_pass) > TAS_INNOV_FAIL_DELAY; + _innovations_check_failed = _aspd_innov_integ_state > _tas_innov_integ_threshold; } _time_last_aspd_innov_check = time_now; @@ -284,16 +279,76 @@ AirspeedValidator::check_load_factor(float accel_z) } } +void +AirspeedValidator::check_first_principle(const uint64_t timestamp, const float throttle_fw, const float throttle_trim, + const uint64_t tecs_timestamp, const Quatf &att_q) +{ + if (! _first_principle_check_enabled) { + _first_principle_check_failed = false; + _time_last_first_principle_check_passing = timestamp; + return; + } + + const float pitch = matrix::Eulerf(att_q).theta(); + const hrt_abstime tecs_dt = timestamp - tecs_timestamp; // return if TECS data is old (TECS not running) + + if (!_in_fixed_wing_flight || tecs_dt > 500_ms || !PX4_ISFINITE(_IAS) || !PX4_ISFINITE(throttle_fw) + || !PX4_ISFINITE(throttle_trim) || !PX4_ISFINITE(pitch)) { + // do not do anything in that case + return; + } + + const float dt = static_cast(timestamp - _time_last_first_principle_check) / 1_s; + _time_last_first_principle_check = timestamp; + + // update filters + if (dt < FLT_EPSILON || dt > 1.f) { + // reset if dt is too large + _IAS_derivative.reset(0.f); + _throttle_filtered.reset(throttle_fw); + _pitch_filtered.reset(pitch); + _time_last_first_principle_check_passing = timestamp; + + } else { + // update filters, with different time constant + _IAS_derivative.setParameters(dt, 5.f); + _throttle_filtered.setParameters(dt, 0.5f); + _pitch_filtered.setParameters(dt, 1.5f); + + _IAS_derivative.update(_IAS); + _throttle_filtered.update(throttle_fw); + _pitch_filtered.update(pitch); + } + + // declare high throttle if more than 5% above trim + const float high_throttle_threshold = math::min(throttle_trim + kHighThrottleDelta, _param_throttle_max); + const bool high_throttle = _throttle_filtered.getState() > high_throttle_threshold; + const bool pitching_down = _pitch_filtered.getState() < _param_psp_off; + + // check if the airspeed derivative is too low given the throttle and pitch + const bool check_failing = _IAS_derivative.getState() < kIASDerivateThreshold && high_throttle && pitching_down; + + if (!check_failing) { + _time_last_first_principle_check_passing = timestamp; + _first_principle_check_failed = false; + } + + if (timestamp - _time_last_first_principle_check_passing > _aspd_fp_t_window * 1_s) { + // only update the test_failed flag once the timeout since first principle check failing is over + _first_principle_check_failed = check_failing; + } +} void AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp) { - if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) { + if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed + || _first_principle_check_failed) { // at least one check (data stuck, innovation or load factor) failed, so record timestamp _time_checks_failed = timestamp; } else if (! _data_stuck_test_failed && !_innovations_check_failed - && !_load_factor_check_failed) { + && !_load_factor_check_failed && !_first_principle_check_failed) { // all checks(data stuck, innovation and load factor) must pass to declare airspeed good _time_checks_passed = timestamp; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index ba8f5c1a305a..74f5747989f6 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -41,6 +41,8 @@ #include #include #include +#include +#include using matrix::Dcmf; @@ -56,7 +58,7 @@ struct airspeed_validator_update_data { float airspeed_true_raw; uint64_t airspeed_timestamp; matrix::Vector3f ground_velocity; - bool lpos_valid; + bool gnss_valid; float lpos_evh; float lpos_evv; matrix::Quatf q_att; @@ -64,8 +66,11 @@ struct airspeed_validator_update_data { float air_temperature_celsius; float accel_z; float vel_test_ratio; - float mag_test_ratio; + float hdg_test_ratio; bool in_fixed_wing_flight; + float fixed_wing_tecs_throttle; + float fixed_wing_tecs_throttle_trim; + uint64_t tecs_timestamp; }; class AirspeedValidator @@ -83,6 +88,9 @@ class AirspeedValidator float get_TAS() { return _TAS; } bool get_airspeed_valid() { return _airspeed_valid; } float get_CAS_scale_validated() {return _CAS_scale_validated;} + float get_airspeed_derivative() { return _IAS_derivative.getState(); } + float get_throttle_filtered() { return _throttle_filtered.getState(); } + float get_pitch_filtered() { return _pitch_filtered.getState(); } airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); @@ -118,6 +126,10 @@ class AirspeedValidator void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; } void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; } void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; } + void set_enable_first_principle_check(bool enable) { _first_principle_check_enabled = enable; } + void set_psp_off_param(float psp_off_param) { _param_psp_off = psp_off_param; } + void set_throttle_max_param(float throttle_max_param) { _param_throttle_max = throttle_max_param; } + void set_fp_t_window(float t_window) { _aspd_fp_t_window = t_window; } private: @@ -127,10 +139,17 @@ class AirspeedValidator bool _data_stuck_check_enabled{false}; bool _innovation_check_enabled{false}; bool _load_factor_check_enabled{false}; + bool _first_principle_check_enabled{false}; // airspeed scale validity check static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°) + static constexpr float kHighThrottleDelta = + 0.05f; ///< throttle delta above trim throttle required to consider throttle high + static constexpr float kIASDerivateThreshold = + 0.1f; ///< threshold for IAS derivative to detect airspeed failure. Failure is + // detected if in a high throttle and low pitch situation and the filtered IAS derivative is below this threshold + // general states bool _in_fixed_wing_flight{false}; ///< variable to bypass innovation and load factor checks float _IAS{0.0f}; ///< indicated airsped in m/s @@ -150,9 +169,7 @@ class AirspeedValidator float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure float _tas_innov_integ_threshold{-1.0}; ///< integrator innovation error threshold for triggering innovation check failure uint64_t _time_last_aspd_innov_check{0}; ///< time airspeed innovation was last checked (uSec) - uint64_t _time_last_tas_pass{0}; ///< last time innovation checks passed float _aspd_innov_integ_state{0.0f}; ///< integral of excess normalised airspeed innovation (sec) - static constexpr uint64_t TAS_INNOV_FAIL_DELAY{1_s}; ///< time required for innovation levels to pass or fail (usec) uint64_t _time_wind_estimator_initialized{0}; ///< time last time wind estimator was initialized (uSec) // states of load factor check @@ -160,10 +177,21 @@ class AirspeedValidator float _airspeed_stall{8.0f}; ///< stall speed of aircraft used for load factor check float _load_factor_ratio{0.5f}; ///< ratio of maximum load factor predicted by stall speed to measured load factor + // first principle check + bool _first_principle_check_failed{false}; ///< first principle check has detected failure + float _aspd_fp_t_window{0.f}; ///< time window for first principle check + FilteredDerivative _IAS_derivative; ///< indicated airspeed derivative for first principle check + AlphaFilter _throttle_filtered; ///< filtered throttle for first principle check + AlphaFilter _pitch_filtered; ///< filtered pitch for first principle check + hrt_abstime _time_last_first_principle_check{0}; ///< time airspeed first principle was last checked (uSec) + hrt_abstime _time_last_first_principle_check_passing{0}; ///< time airspeed first principle was last passing (uSec) + float _param_psp_off{0.0f}; ///< parameter pitch in level flight [rad] + float _param_throttle_max{0.0f}; ///< parameter maximum throttle value + // states of airspeed valid declaration bool _airspeed_valid{true}; ///< airspeed valid (pitot or groundspeed-windspeed) - int _checks_fail_delay{3}; ///< delay for airspeed invalid declaration after single check failure (Sec) - int _checks_clear_delay{-1}; ///< delay for airspeed valid declaration after all checks passed again (Sec) + float _checks_fail_delay{2.f}; ///< delay for airspeed invalid declaration after single check failure (Sec) + float _checks_clear_delay{-1.f}; ///< delay for airspeed valid declaration after all checks passed again (Sec) uint64_t _time_checks_passed{0}; ///< time the checks have last passed (uSec) uint64_t _time_checks_failed{0}; ///< time the checks have last not passed (uSec) @@ -177,16 +205,18 @@ class AirspeedValidator void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; } - void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, + void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool gnss_valid, const matrix::Vector3f &vI, float lpos_evh, float lpos_evv, const Quatf &q_att); - void update_CAS_scale_validated(bool lpos_valid, const matrix::Vector3f &vI, float airspeed_true_raw); + void update_CAS_scale_validated(bool gnss_valid, const matrix::Vector3f &vI, float airspeed_true_raw); void update_CAS_scale_applied(); void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius); void check_airspeed_data_stuck(uint64_t timestamp); void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio, - float estimator_status_mag_test_ratio, const matrix::Vector3f &vI, bool lpos_valid); + float estimator_status_hdg_test_ratio, const matrix::Vector3f &vI, bool gnss_valid); void check_load_factor(float accel_z); + void check_first_principle(const uint64_t timestamp, const float throttle, const float throttle_trim, + const uint64_t tecs_timestamp, const Quatf &att_q); void update_airspeed_valid_status(const uint64_t timestamp); void reset(); void reset_CAS_scale_check(); diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 4877ee94b620..1880d5ab3c4d 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -112,6 +113,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; @@ -125,6 +127,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, uORB::SubscriptionMultiArray _airspeed_subs{ORB_ID::airspeed}; + tecs_status_s _tecs_status {}; estimator_status_s _estimator_status {}; vehicle_acceleration_s _accel {}; vehicle_air_data_s _vehicle_air_data {}; @@ -146,7 +149,7 @@ class AirspeedModule : public ModuleBase, public ModuleParams, int _valid_airspeed_index{-2}; /**< index of currently chosen (valid) airspeed sensor */ int _prev_airspeed_index{-2}; /**< previously chosen airspeed sensor index */ bool _initialized{false}; /**< module initialized*/ - bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */ + bool _gnss_lpos_valid{false}; /**< local position (from GNSS) valid */ bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */ float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */ float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */ @@ -162,9 +165,16 @@ class AirspeedModule : public ModuleBase, public ModuleParams, CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0), CHECK_TYPE_DATA_STUCK_BIT = (1 << 1), CHECK_TYPE_INNOVATION_BIT = (1 << 2), - CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3) + CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3), + CHECK_TYPE_FIRST_PRINCIPLE_BIT = (1 << 4) }; + + param_t _param_handle_pitch_sp_offset{PARAM_INVALID}; + float _param_pitch_sp_offset{0.0f}; + param_t _param_handle_fw_thr_max{PARAM_INVALID}; + float _param_fw_thr_max{0.0f}; + DEFINE_PARAMETERS( (ParamFloat) _param_aspd_wind_nsd, (ParamFloat) _param_aspd_scale_nsd, @@ -182,11 +192,15 @@ class AirspeedModule : public ModuleBase, public ModuleParams, (ParamFloat) _tas_innov_threshold, /**< innovation check threshold */ (ParamFloat) _tas_innov_integ_threshold, /**< innovation check integrator threshold */ - (ParamInt) _checks_fail_delay, /**< delay to declare airspeed invalid */ - (ParamInt) _checks_clear_delay, /**< delay to declare airspeed valid again */ + (ParamFloat) _checks_fail_delay, /**< delay to declare airspeed invalid */ + (ParamFloat) _checks_clear_delay, /**< delay to declare airspeed valid again */ + + (ParamFloat) _param_wind_sigma_max_synth_tas, + (ParamFloat) _aspd_fp_t_window, + // external parameters (ParamFloat) _param_fw_airspd_stall, - (ParamFloat) _param_wind_sigma_max_synth_tas + (ParamFloat) _param_fw_airspd_trim ) void init(); /**< initialization of the airspeed validator instances */ @@ -203,6 +217,8 @@ AirspeedModule::AirspeedModule(): ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) { + _param_handle_pitch_sp_offset = param_find("FW_PSP_OFF"); + _param_handle_fw_thr_max = param_find("FW_THR_MAX"); // initialise parameters update_params(); @@ -347,14 +363,17 @@ AirspeedModule::Run() struct airspeed_validator_update_data input_data = {}; input_data.timestamp = _time_now_usec; input_data.ground_velocity = vI; - input_data.lpos_valid = _vehicle_local_position_valid; + input_data.gnss_valid = _gnss_lpos_valid; input_data.lpos_evh = _vehicle_local_position.evh; input_data.lpos_evv = _vehicle_local_position.evv; input_data.q_att = _q_att; input_data.air_pressure_pa = _vehicle_air_data.baro_pressure_pa; input_data.accel_z = _accel.xyz[2]; input_data.vel_test_ratio = _estimator_status.vel_test_ratio; - input_data.mag_test_ratio = _estimator_status.mag_test_ratio; + input_data.hdg_test_ratio = _estimator_status.hdg_test_ratio; + input_data.tecs_timestamp = _tecs_status.timestamp; + input_data.fixed_wing_tecs_throttle = _tecs_status.throttle_sp; + input_data.fixed_wing_tecs_throttle_trim = _tecs_status.throttle_trim; // iterate through all airspeed sensors, poll new data from them and update their validators for (int i = 0; i < _number_of_airspeed_sensors; i++) { @@ -442,6 +461,14 @@ void AirspeedModule::update_params() { updateParams(); + if (_param_handle_pitch_sp_offset != PARAM_INVALID) { + param_get(_param_handle_pitch_sp_offset, &_param_pitch_sp_offset); + } + + if (_param_handle_fw_thr_max != PARAM_INVALID) { + param_get(_param_handle_fw_thr_max, &_param_fw_thr_max); + } + _param_airspeed_scale[0] = _param_airspeed_scale_1.get(); _param_airspeed_scale[1] = _param_airspeed_scale_2.get(); _param_airspeed_scale[2] = _param_airspeed_scale_3.get(); @@ -476,6 +503,11 @@ void AirspeedModule::update_params() CheckTypeBits::CHECK_TYPE_INNOVATION_BIT); _airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() & CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT); + _airspeed_validator[i].set_enable_first_principle_check(_param_airspeed_checks_on.get() & + CheckTypeBits::CHECK_TYPE_FIRST_PRINCIPLE_BIT); + _airspeed_validator[i].set_psp_off_param(math::radians(_param_pitch_sp_offset)); + _airspeed_validator[i].set_throttle_max_param(_param_fw_thr_max); + _airspeed_validator[i].set_fp_t_window(_aspd_fp_t_window.get()); } } @@ -501,6 +533,8 @@ void AirspeedModule::poll_topics() _vehicle_local_position_sub.update(&_vehicle_local_position); _position_setpoint_sub.update(&_position_setpoint); + _tecs_status_sub.update(&_tecs_status); + if (_vehicle_attitude_sub.updated()) { vehicle_attitude_s vehicle_attitude; _vehicle_attitude_sub.update(&vehicle_attitude); @@ -515,10 +549,10 @@ void AirspeedModule::poll_topics() } } - _vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) - && (_vehicle_local_position.timestamp > 0) - && _vehicle_local_position.v_xy_valid - && !_vehicle_local_position.dead_reckoning; + _gnss_lpos_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s) + && (_vehicle_local_position.timestamp > 0) + && _vehicle_local_position.v_xy_valid + && _estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); } void AirspeedModule::update_wind_estimator_sideslip() @@ -526,7 +560,7 @@ void AirspeedModule::update_wind_estimator_sideslip() // update wind and airspeed estimator _wind_estimator_sideslip.update(_time_now_usec); - if (_vehicle_local_position_valid + if (_gnss_lpos_valid && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_land_detected.landed) { Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz); @@ -603,8 +637,8 @@ void AirspeedModule::select_airspeed_and_publish() if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) { - // _vehicle_local_position_valid determines if ground-wind estimate is valid - if (_vehicle_local_position_valid && + // _gnss_lpos_valid determines if ground-wind estimate is valid + if (_gnss_lpos_valid && (_param_airspeed_fallback_gw.get() || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) { _valid_airspeed_index = airspeed_index::GROUND_MINUS_WIND_INDEX; @@ -661,6 +695,11 @@ void AirspeedModule::select_airspeed_and_publish() airspeed_validated.airspeed_sensor_measurement_valid = false; airspeed_validated.selected_airspeed_index = _valid_airspeed_index; + airspeed_validated.airspeed_derivative_filtered = _airspeed_validator[_valid_airspeed_index - + 1].get_airspeed_derivative(); + airspeed_validated.throttle_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_throttle_filtered(); + airspeed_validated.pitch_filtered = _airspeed_validator[_valid_airspeed_index - 1].get_pitch_filtered(); + switch (_valid_airspeed_index) { case airspeed_index::DISABLED_INDEX: break; diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 0b54d3b29760..0462fbbc34f3 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -149,11 +149,12 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0. * * @min 0 - * @max 15 + * @max 31 * @bit 0 Only data missing check (triggers if more than 1s no data) * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode) * @bit 2 Innovation check (see ASPD_FS_INNOV) * @bit 3 Load factor check (triggers if measurement is below stall speed) + * @bit 4 First principle check (airspeed change vs. throttle and pitch) * @group Airspeed Validator */ PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7); @@ -208,10 +209,10 @@ PARAM_DEFINE_FLOAT(ASPD_FS_INTEG, 10.f); * * @unit s * @group Airspeed Validator - * @min 1 - * @max 10 + * @min 0.0 + * @decimal 1 */ -PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2); +PARAM_DEFINE_FLOAT(ASPD_FS_T_STOP, 1.f); /** * Airspeed failsafe start delay @@ -221,10 +222,10 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2); * * @unit s * @group Airspeed Validator - * @min -1 - * @max 1000 + * @min -1.0 + * @decimal 1 */ -PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); +PARAM_DEFINE_FLOAT(ASPD_FS_T_START, -1.f); /** * Horizontal wind uncertainty threshold for synthetic airspeed. @@ -239,3 +240,19 @@ PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); * @group Airspeed Validator */ PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f); + +/** + * First principle airspeed check time window + * + * Window for comparing airspeed change to throttle and pitch change. + * Triggers when the airspeed change within this window is negative while throttle increases + * and the vehicle pitches down. + * Is meant to catch degrading airspeed blockages as can happen when flying through icing conditions. + * Relies on FW_THR_TRIM being set accurately. + * + * @unit s + * @min 0 + * @decimal 1 + * @group Airspeed Validator + */ +PARAM_DEFINE_FLOAT(ASPD_FP_T_WINDOW, 2.0f); diff --git a/src/modules/attitude_estimator_q/CMakeLists.txt b/src/modules/attitude_estimator_q/CMakeLists.txt index df9084fe2036..b72540872127 100644 --- a/src/modules/attitude_estimator_q/CMakeLists.txt +++ b/src/modules/attitude_estimator_q/CMakeLists.txt @@ -41,4 +41,3 @@ px4_add_module( DEPENDS world_magnetic_model ) - diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index b4da68f72275..f6b645397a3d 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -180,6 +180,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : bool AttitudeEstimatorQ::init() { + uORB::SubscriptionData vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + vehicle_attitude_sub.update(); + + if (vehicle_attitude_sub.advertised() && (hrt_elapsed_time(&vehicle_attitude_sub.get().timestamp) < 1_s)) { + PX4_ERR("init failed, vehicle_attitude already advertised"); + return false; + } + if (!_sensors_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; @@ -219,8 +227,8 @@ void AttitudeEstimatorQ::update_gps_position() if (_vehicle_gps_position_sub.update(&gps)) { if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) { // set magnetic declination automatically - update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg, - (float)gps.longitude_deg)); + float mag_decl_deg = get_mag_declination_degrees(gps.latitude_deg, gps.longitude_deg); + update_mag_declination(math::radians(mag_decl_deg)); } } } diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index 90a9342bb502..b9a3dc752d72 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -39,6 +39,16 @@ * @author Anton Babushkin */ +/** + * standalone attitude estimator enable (unsupported) + * + * Enable standalone quaternion based attitude estimator. + * + * @group Attitude Q estimator + * @boolean + */ +PARAM_DEFINE_INT32(ATT_EN, 0); + /** * Complimentary filter accelerometer weight * diff --git a/src/modules/camera_feedback/CameraFeedback.cpp b/src/modules/camera_feedback/CameraFeedback.cpp index 35ac09296dc2..a4c89f59715a 100644 --- a/src/modules/camera_feedback/CameraFeedback.cpp +++ b/src/modules/camera_feedback/CameraFeedback.cpp @@ -194,6 +194,25 @@ CameraFeedback::print_usage(const char *reason) R"DESCR_STR( ### Description +The camera_feedback module publishes `CameraCapture` UORB topics when image capture has been triggered. + +If camera capture is enabled, then trigger information from the camera capture pin is published; +otherwise trigger information at the point the camera was commanded to trigger is published +(from the `camera_trigger` module). + +The `CAMERA_IMAGE_CAPTURED` message is then emitted (by streaming code) following `CameraCapture` updates. +`CameraCapture` topics are also logged and can be used for geotagging. + +### Implementation + +`CameraTrigger` topics are published by the `camera_trigger` module (`feedback` field set `false`) +when image capture is triggered, and may also be published by the `camera_capture` driver +(with `feedback` field set `true`) if the camera capture pin is activated. + +The `camera_feedback` module subscribes to `CameraTrigger`. +It discards topics from the `camera_trigger` module if camera capture is enabled. +For the topics that are not discarded it creates a `CameraCapture` topic with the timestamp information +from the `CameraTrigger` and position information from the vehicle. )DESCR_STR"); diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index bca8ef3f68d3..2f326deb01ce 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -598,6 +598,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) { tune_negative(true); + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Resolve system health failures first\t"); + events::send(events::ID("commander_arm_denied_resolve_failures"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: Resolve system health failures first"); return TRANSITION_DENIED; } } @@ -667,9 +670,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } // update flight uuid - const int32_t flight_uuid = _param_flight_uuid.get() + 1; - _param_flight_uuid.set(flight_uuid); - _param_flight_uuid.commit_no_notification(); + const int32_t flight_uuid = _param_com_flight_uuid.get() + 1; + _param_com_flight_uuid.set(flight_uuid); + _param_com_flight_uuid.commit_no_notification(); _status_changed = true; @@ -1106,7 +1109,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: { // if no high latency telemetry exists send a failed acknowledge - if (_high_latency_datalink_heartbeat > _boot_timestamp) { + if (_high_latency_datalink_timestamp < _boot_timestamp) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t"); events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info}, @@ -1988,7 +1991,7 @@ void Commander::checkForMissionUpdate() if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) { // Transition mode to loiter or auto-mission after takeoff is completed. - if ((_param_takeoff_finished_action.get() == 1) && auto_mission_available) { + if ((_param_com_takeoff_act.get() == 1) && auto_mission_available) { _user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); } else { @@ -2233,7 +2236,7 @@ void Commander::handleAutoDisarm() if (isArmed()) { // Check for auto-disarm on landing or pre-flight - if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { + if (_param_com_disarm_land.get() > 0 || _param_com_disarm_prflt.get() > 0) { const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) && !_mission_result_sub.get().finished; @@ -2244,8 +2247,8 @@ void Commander::handleAutoDisarm() _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s); _auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time()); - } else if (_param_com_disarm_preflight.get() > 0 && !_have_taken_off_since_arming) { - _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_preflight.get() * 1_s); + } else if (_param_com_disarm_prflt.get() > 0 && !_have_taken_off_since_arming) { + _auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_prflt.get() * 1_s); _auto_disarm_landed.set_state_and_update(true, hrt_absolute_time()); } @@ -2631,7 +2634,7 @@ int Commander::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 40, - 3250, + PX4_STACK_ADJUSTED(3250), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -2669,6 +2672,28 @@ void Commander::enable_hil() void Commander::dataLinkCheck() { + // high latency data link + iridiumsbd_status_s iridium_status; + + if (_iridiumsbd_status_sub.update(&iridium_status)) { + _high_latency_datalink_timestamp = iridium_status.last_at_ok_timestamp; + + if (_vehicle_status.high_latency_data_link_lost && + (_high_latency_datalink_timestamp > _high_latency_datalink_lost) && + (_high_latency_datalink_regained == 0) + ) { + _high_latency_datalink_regained = _high_latency_datalink_timestamp; + } + + if (_vehicle_status.high_latency_data_link_lost && + (_high_latency_datalink_regained != 0) && + (hrt_elapsed_time(&_high_latency_datalink_regained) > (_param_com_hldl_reg_t.get() * 1_s)) + ) { + _vehicle_status.high_latency_data_link_lost = false; + _status_changed = true; + } + } + for (auto &telemetry_status : _telemetry_status_subs) { telemetry_status_s telemetry; @@ -2682,16 +2707,18 @@ void Commander::dataLinkCheck() break; case telemetry_status_s::LINK_TYPE_IRIDIUM: { - iridiumsbd_status_s iridium_status; - if (_iridiumsbd_status_sub.update(&iridium_status)) { - _high_latency_datalink_heartbeat = iridium_status.last_heartbeat; + if ((_high_latency_datalink_timestamp > 0) && + (hrt_elapsed_time(&_high_latency_datalink_timestamp) > (_param_com_hldl_loss_t.get() * 1_s))) { - if (_vehicle_status.high_latency_data_link_lost) { - if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) { - _vehicle_status.high_latency_data_link_lost = false; - _status_changed = true; - } + _high_latency_datalink_lost = _high_latency_datalink_timestamp; + _high_latency_datalink_regained = 0; + + if (!_vehicle_status.high_latency_data_link_lost) { + _vehicle_status.high_latency_data_link_lost = true; + mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t"); + events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost"); + _status_changed = true; } } @@ -2833,19 +2860,6 @@ void Commander::dataLinkCheck() _vehicle_status.avoidance_system_valid = false; } } - - // high latency data link loss failsafe - if (_high_latency_datalink_heartbeat > 0 - && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) { - _high_latency_datalink_lost = hrt_absolute_time(); - - if (!_vehicle_status.high_latency_data_link_lost) { - _vehicle_status.high_latency_data_link_lost = true; - mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t"); - events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost"); - _status_changed = true; - } - } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d477585c0345..7453cb2a1b7e 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -246,8 +246,9 @@ class Commander : public ModuleBase, public ModuleParams hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent - hrt_abstime _high_latency_datalink_heartbeat{0}; + hrt_abstime _high_latency_datalink_timestamp{0}; hrt_abstime _high_latency_datalink_lost{0}; + hrt_abstime _high_latency_datalink_regained{0}; hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0}; @@ -331,7 +332,7 @@ class Commander : public ModuleBase, public ModuleParams DEFINE_PARAMETERS( (ParamFloat) _param_com_disarm_land, - (ParamFloat) _param_com_disarm_preflight, + (ParamFloat) _param_com_disarm_prflt, (ParamBool) _param_com_disarm_man, (ParamInt) _param_com_dl_loss_t, (ParamInt) _param_com_hldl_loss_t, @@ -346,8 +347,8 @@ class Commander : public ModuleBase, public ModuleParams (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, - (ParamInt) _param_flight_uuid, - (ParamInt) _param_takeoff_finished_action, + (ParamInt) _param_com_flight_uuid, + (ParamInt) _param_com_takeoff_act, (ParamFloat) _param_com_cpu_max ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index a4e1873be9c0..7265e042b4c0 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -50,10 +50,12 @@ px4_add_library(health_and_arming_checks checks/gyroCheck.cpp checks/homePositionCheck.cpp checks/imuConsistencyCheck.cpp + checks/loggerCheck.cpp checks/magnetometerCheck.cpp checks/manualControlCheck.cpp checks/missionCheck.cpp checks/modeCheck.cpp + checks/navigatorCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp checks/parachuteCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 678d26636b64..796427180fee 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -284,7 +284,7 @@ bool Report::report(bool is_armed, bool force) // send all events int offset = 0; - events::EventType event; + event_s event; for (int i = 0; i < max_num_events && offset < _next_buffer_idx; ++i) { EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + offset); diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 9c16c6203d7a..da7c5c296509 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -376,7 +376,7 @@ bool Report::addEvent(uint32_t event_id, const events::LogLevels &log_levels, co Args... args) { constexpr unsigned args_size = events::util::sizeofArguments(modes, args...); - static_assert(args_size <= sizeof(events::EventType::arguments), "Too many arguments"); + static_assert(args_size <= sizeof(event_s::arguments), "Too many arguments"); unsigned total_size = sizeof(EventBufferHeader) + args_size; if (total_size > sizeof(_event_buffer) - _next_buffer_idx) { diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index b349475593fd..fdf38f5d5d77 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -49,8 +49,10 @@ #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" +#include "checks/navigatorCheck.hpp" #include "checks/gyroCheck.hpp" #include "checks/imuConsistencyCheck.hpp" +#include "checks/loggerCheck.hpp" #include "checks/magnetometerCheck.hpp" #include "checks/manualControlCheck.hpp" #include "checks/homePositionCheck.hpp" @@ -128,8 +130,10 @@ class HealthAndArmingChecks : public ModuleParams EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; + NavigatorChecks _navigator_checks; GyroChecks _gyro_checks; ImuConsistencyChecks _imu_consistency_checks; + LoggerChecks _logger_checks; MagnetometerChecks _magnetometer_checks; ManualControlChecks _manual_control_checks; HomePositionChecks _home_position_checks; @@ -165,8 +169,10 @@ class HealthAndArmingChecks : public ModuleParams &_esc_checks, &_estimator_checks, &_failure_detector_checks, + &_navigator_checks, &_gyro_checks, &_imu_consistency_checks, + &_logger_checks, &_magnetometer_checks, &_manual_control_checks, &_home_position_checks, @@ -187,4 +193,3 @@ class HealthAndArmingChecks : public ModuleParams &_vtol_checks, }; }; - diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp index 912d3bb1293b..c06886320563 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -285,4 +285,3 @@ TEST_F(ReporterTest, reporting_multiple) } } } - diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp index f13977199e7a..5e96c39633fe 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.cpp @@ -32,6 +32,7 @@ ****************************************************************************/ #include "batteryCheck.hpp" +#include #include @@ -52,7 +53,7 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat case battery_fault_reason_t::over_current: return "over current"; - case battery_fault_reason_t::fault_temperature: return "critical temperature"; + case battery_fault_reason_t::over_temperature: return "over temperature"; case battery_fault_reason_t::under_temperature: return "under temperature"; @@ -64,32 +65,19 @@ static constexpr const char *battery_fault_reason_str(battery_fault_reason_t bat case battery_fault_reason_t::hardware_fault: return "hardware fault"; - case battery_fault_reason_t::over_temperature: return "near temperature limit"; + case battery_fault_reason_t::failed_to_arm: return "failed to arm"; } return ""; }; - -using battery_mode_t = events::px4::enums::battery_mode_t; -static_assert(battery_status_s::BATTERY_MODE_COUNT == (static_cast(battery_mode_t::_max) + 1) - , "Battery mode flags mismatch!"); -static constexpr const char *battery_mode_str(battery_mode_t battery_mode) +void BatteryChecks::checkAndReport(const Context &context, Report &reporter) { - switch (battery_mode) { - case battery_mode_t::autodischarging: return "auto discharging"; - - case battery_mode_t::hotswap: return "hot-swap"; - - default: return "unknown"; + if (circuit_breaker_enabled_by_val(_param_cbrk_supply_chk.get(), CBRK_SUPPLY_CHK_KEY)) { + return; } -} - -void BatteryChecks::checkAndReport(const Context &context, Report &reporter) -{ - int battery_required_count = 0; bool battery_has_fault = false; // There are possibly multiple batteries, and we can't know which ones serve which purpose. So the safest // option is to check if ANY of them have a warning, and specifically find which one has the most @@ -101,6 +89,7 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) hrt_abstime oldest_update = hrt_absolute_time(); float worst_battery_time_s{NAN}; int num_connected_batteries{0}; + bool is_required_battery_missing{false}; for (auto &battery_sub : _battery_status_subs) { int index = battery_sub.get_instance(); @@ -110,42 +99,36 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) continue; } - if (battery.is_required) { - battery_required_count++; + if (battery.is_required && !battery.connected) { + is_required_battery_missing = true; + /* EVENT + * @description + * Make sure all required batteries are connected. + */ + reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_missing"), + events::Log::Error, "Battery {1} missing", index + 1); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i missing\t", index + 1); + } } if (!_last_armed && context.isArmed()) { _battery_connected_at_arming[index] = battery.connected; } - if (context.isArmed()) { - - if (!battery.connected && _battery_connected_at_arming[index]) { // If disconnected after arming - /* EVENT - */ - reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"), - events::Log::Emergency, "Battery {1} disconnected", index + 1); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i disconnected\t", index + 1); - } + if (context.isArmed() && !battery.connected && _battery_connected_at_arming[index]) { // If disconnected after arming + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_disconnected"), + events::Log::Emergency, "Battery {1} disconnected", index + 1); - // trigger a battery failsafe action if a battery disconnects in flight - worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL; + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %i disconnected\t", index + 1); } - if (battery.mode != 0) { - /* EVENT - */ - reporter.healthFailure(NavModes::All, health_component_t::battery, - events::ID("check_battery_mode"), - events::Log::Critical, "Battery {1} mode: {2}", index + 1, static_cast(battery.mode)); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Battery %d is in %s mode!\t", index + 1, - battery_mode_str(static_cast(battery.mode))); - } - } + // trigger a battery failsafe action if a battery disconnects in flight + worst_warning = battery_status_s::BATTERY_WARNING_CRITICAL; } if (battery.connected) { @@ -175,11 +158,10 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) /* EVENT * @description * The battery reported a failure which might be dangerous to fly with. - * Manufacturer error code: {4} */ - reporter.healthFailure + reporter.healthFailure (NavModes::All, health_component_t::battery, events::ID("check_battery_fault"), {events::Log::Emergency, events::LogInternal::Warning}, - "Battery {1}: {2}. {3}", index + 1, static_cast(fault_index), action, battery.custom_faults); + "Battery {1}: {2}. {3}", index + 1, static_cast(fault_index), action); if (reporter.mavlink_log_pub()) { mavlink_log_emergency(reporter.mavlink_log_pub(), "Battery %d: %s. %s \t", index + 1, @@ -254,16 +236,17 @@ void BatteryChecks::checkAndReport(const Context &context, Report &reporter) reporter.failsafeFlags().battery_unhealthy = // All connected batteries are regularly being published hrt_elapsed_time(&oldest_update) > 5_s - // There is at least one connected battery (in any slot) - || num_connected_batteries < battery_required_count + // There is a required battery that's missing + || is_required_battery_missing // No currently-connected batteries have any fault || battery_has_fault || reporter.failsafeFlags().battery_warning == battery_status_s::BATTERY_WARNING_FAILED; - if (reporter.failsafeFlags().battery_unhealthy && !battery_has_fault) { // faults are reported above already + if (reporter.failsafeFlags().battery_unhealthy + && !is_required_battery_missing && !battery_has_fault) { // missing batteries and faults are reported above already /* EVENT * @description - * Make sure all batteries are connected and operational. + * Make sure all batteries are operational. */ reporter.healthFailure(NavModes::All, health_component_t::battery, events::ID("check_battery_unhealthy"), events::Log::Error, "Battery unhealthy"); @@ -285,7 +268,7 @@ void BatteryChecks::rtlEstimateCheck(const Context &context, Report &reporter, f rtl_time_estimate_s rtl_time_estimate; // Compare estimate of RTL time to estimate of remaining flight time - // add hysterisis: if already in the condition, only get out of it if the remaining flight time is significantly higher again + // add hysteresis: if already in the condition, only get out of it if the remaining flight time is significantly higher again const float hysteresis_factor = reporter.failsafeFlags().battery_low_remaining_time ? 1.1f : 1.0f; reporter.failsafeFlags().battery_low_remaining_time = _rtl_time_estimate_sub.copy(&rtl_time_estimate) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp index 81f1058d3571..64cf1ed5e6ce 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/batteryCheck.hpp @@ -57,6 +57,7 @@ class BatteryChecks : public HealthAndArmingCheckBase bool _battery_connected_at_arming[battery_status_s::MAX_INSTANCES] {}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamFloat) _param_arm_battery_level_min + (ParamFloat) _param_arm_battery_level_min, + (ParamInt) _param_cbrk_supply_chk ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp index c3651910c3c5..b433e47ba428 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.cpp @@ -43,7 +43,10 @@ CpuResourceChecks::CpuResourceChecks() void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) { - if (_param_com_cpu_max.get() < FLT_EPSILON) { + const bool cpu_load_check_enabled = _param_com_cpu_max.get() > FLT_EPSILON; + const bool ram_usage_check_enabled = _param_com_ram_max.get() > FLT_EPSILON; + + if (!cpu_load_check_enabled && !ram_usage_check_enabled) { return; } @@ -54,15 +57,15 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) /* EVENT * @description * - * If the system does not provide any CPU load information, use the parameter COM_CPU_MAX - * to disable the check. + * If the system does not provide any CPU and RAM load information, use the parameters COM_CPU_MAX + * and COM_RAM_MAX to disable the checks. * */ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_missing_cpuload"), - events::Log::Error, "No CPU load information"); + events::Log::Error, "No CPU and RAM load information"); if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU load information"); + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: No CPU and RAM load information"); } } else { @@ -71,7 +74,7 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) _high_cpu_load_hysteresis.set_state_and_update(high_cpu_load, hrt_absolute_time()); // fail check if CPU load is above the threshold for 2 seconds - if (_high_cpu_load_hysteresis.get_state()) { + if (cpu_load_check_enabled && _high_cpu_load_hysteresis.get_state()) { /* EVENT * @description * The CPU load can be reduced for example by disabling unused modules (e.g. mavlink instances) or reducing the gyro update @@ -88,5 +91,26 @@ void CpuResourceChecks::checkAndReport(const Context &context, Report &reporter) mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: CPU load too high: %3.1f%%", (double)cpuload_percent); } } + + const float ram_usage_percent = cpuload.ram_usage * 100.f; + const bool high_ram_usage = ram_usage_percent > _param_com_ram_max.get(); + + if (ram_usage_check_enabled && high_ram_usage) { + /* EVENT + * @description + * The RAM usage can be reduced for example by disabling unused modules (e.g. mavlink instances). + * + * + * The threshold can be adjusted via COM_RAM_MAX parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_ram_usage_too_high"), + events::Log::Error, "RAM usage too high: {1:.1}%", ram_usage_percent); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: RAM usage too high: %3.1f%%", + (double)ram_usage_percent); + } + } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp index 10db83ad6bd7..d95c03d2908d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/cpuResourceCheck.hpp @@ -54,6 +54,7 @@ class CpuResourceChecks : public HealthAndArmingCheckBase systemlib::Hysteresis _high_cpu_load_hysteresis{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamFloat) _param_com_cpu_max + (ParamFloat) _param_com_cpu_max, + (ParamFloat) _param_com_ram_max ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index cd7d716b31c8..9554c57a4f62 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -183,4 +183,3 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e } } } - diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 9bb33f2bd24f..2b7c45318301 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -63,6 +63,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) bool pre_flt_fail_innov_heading = false; bool pre_flt_fail_innov_vel_horiz = false; + bool pre_flt_fail_innov_pos_horiz = false; bool missing_data = false; const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; @@ -90,6 +91,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) if (_estimator_status_sub.copy(&estimator_status)) { pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading; pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz; + pre_flt_fail_innov_pos_horiz = estimator_status.pre_flt_fail_innov_pos_horiz; checkEstimatorStatus(context, reporter, estimator_status, required_groups); checkEstimatorStatusFlags(context, reporter, estimator_status, lpos); @@ -99,7 +101,14 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } } - if (missing_data && _param_sys_mc_est_group.get() == 2) { + param_t param_ekf2_en_handle = param_find_no_notification("EKF2_EN"); + int32_t param_ekf2_en = 0; + + if (param_ekf2_en_handle != PARAM_INVALID) { + param_get(param_ekf2_en_handle, ¶m_ekf2_en); + } + + if (missing_data && (param_ekf2_en == 1)) { /* EVENT */ reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, @@ -116,7 +125,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter) } // set mode requirements - setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, + setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz, + lpos, vehicle_gps_position, reporter.failsafeFlags(), reporter); @@ -159,6 +169,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable"); } + } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_pos_horiz) { + /* EVENT + */ + reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, + events::ID("check_estimator_hor_pos_not_stable"), + events::Log::Error, "Horizontal position unstable"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable"); + } + } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) { /* EVENT */ @@ -201,82 +222,6 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor } } - // check vertical position innovation test ratio - if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_HGT parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_hgt_est_err"), - events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error"); - } - } - - // check velocity innovation test ratio - if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_VEL parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_vel_est_err"), - events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error"); - } - } - - // check horizontal position innovation test ratio - if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_POS parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_pos_est_err"), - events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error"); - } - } - - // check magnetometer innovation test ratio - if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) { - /* EVENT - * @description - * - * Test ratio: {1:.3}, limit: {2:.3}. - * - * This check can be configured via COM_ARM_EKF_YAW parameter. - * - */ - reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate, - events::ID("check_estimator_yaw_est_err"), - events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get()); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error"); - } - } - // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing if (_param_sys_has_gps.get()) { const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); @@ -293,7 +238,9 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor mavlink_log_warning(reporter.mavlink_log_pub(), "GNSS data fusion stopped\t"); } - events::send(events::ID("check_estimator_gnss_fusion_stopped"), {events::Log::Error, events::LogInternal::Info}, + // only report this failure as critical if not already in a local position invalid state + events::Log log_level = reporter.failsafeFlags().local_position_invalid ? events::Log::Info : events::Log::Error; + events::send(events::ID("check_estimator_gnss_fusion_stopped"), {log_level, events::LogInternal::Info}, "GNSS data fusion stopped"); } else if (!_gps_was_fused && ekf_gps_fusion) { @@ -309,6 +256,22 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor _gps_was_fused = ekf_gps_fusion; + if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) { + if (!_gnss_spoofed) { + _gnss_spoofed = true; + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS signal spoofed\t"); + } + + events::send(events::ID("check_estimator_gnss_warning_spoofing"), {events::Log::Alert, events::LogInternal::Info}, + "GNSS signal spoofed"); + } + + } else { + _gnss_spoofed = false; + } + if (!context.isArmed() && ekf_gps_check_fail) { NavModes required_groups_gps = required_groups; events::Log log_level = events::Log::Error; @@ -441,6 +404,18 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor events::ID("check_estimator_gps_vert_speed_drift_too_high"), log_level, "GPS Vertical Speed Drift too high"); + } else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_SPOOFED)) { + message = "Preflight%s: GPS signal spoofed"; + /* EVENT + * @description + * + * This check can be configured via EKF2_GPS_CHECK parameter. + * + */ + reporter.armingCheckFailure(required_groups_gps, health_component_t::gps, + events::ID("check_estimator_gps_spoofed"), + log_level, "GPS signal spoofed"); + } else { if (!ekf_gps_fusion) { // Likely cause unknown @@ -558,19 +533,6 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & estimator_status_flags_s estimator_status_flags; if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { - - bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning - || estimator_status_flags.cs_inertial_dead_reckoning; - - if (!dead_reckoning) { - // position requirements (update if not dead reckoning) - bool gps = estimator_status_flags.cs_gps; - bool optical_flow = estimator_status_flags.cs_opt_flow; - bool vision_position = estimator_status_flags.cs_ev_pos; - - _position_reliant_on_optical_flow = !gps && optical_flow && !vision_position; - } - // Check for a magnetometer fault and notify the user if (estimator_status_flags.cs_mag_fault) { /* EVENT @@ -586,7 +548,7 @@ void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report & } } - if (estimator_status_flags.cs_gps_yaw_fault) { + if (estimator_status_flags.cs_gnss_yaw_fault) { /* EVENT * @description * Land now @@ -730,7 +692,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report } void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, - bool pre_flt_fail_innov_vel_horiz, + bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, Report &reporter) { @@ -747,20 +709,11 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // Check if quality checking of position accuracy and consistency is to be performed const float lpos_eph_threshold = (_param_com_pos_fs_eph.get() < 0) ? INFINITY : _param_com_pos_fs_eph.get(); - float lpos_eph_threshold_relaxed = lpos_eph_threshold; - - // Set the allowable position uncertainty based on combination of flight and estimator state - // When we are in a operator demanded position control mode and are solely reliant on optical flow, - // do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift - if (_position_reliant_on_optical_flow) { - lpos_eph_threshold_relaxed = INFINITY; - } - bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; if (!context.isArmed()) { - if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { + if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) { xy_valid = false; } @@ -774,7 +727,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f _last_gpos_fail_time_us, !failsafe_flags.global_position_invalid); // Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period - const float eph_critical = 2.5f * _param_com_pos_fs_eph.get(); // threshold used to trigger the navigation failsafe + const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f)); estimator_status_flags_s estimator_status_flags; @@ -818,6 +771,10 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); + + // In some modes we assume that the operator will compensate for the drift so we do not need to check the position error + const float lpos_eph_threshold_relaxed = INFINITY; + failsafe_flags.local_position_invalid_relaxed = !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp, _last_lpos_relaxed_fail_time_us, !failsafe_flags.local_position_invalid_relaxed); @@ -904,4 +861,3 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat return valid; } - diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index f5b807352492..4f61df88670f 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -64,10 +64,11 @@ class EstimatorChecks : public HealthAndArmingCheckBase void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups); void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos); - void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const; - void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz, + + void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, + bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, Report &reporter); @@ -99,20 +100,14 @@ class EstimatorChecks : public HealthAndArmingCheckBase bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed - bool _position_reliant_on_optical_flow{false}; - bool _gps_was_fused{false}; + bool _gnss_spoofed{false}; bool _nav_failure_imminent_warned{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_sys_mc_est_group, (ParamInt) _param_sens_imu_mode, (ParamInt) _param_com_arm_mag_str, - (ParamFloat) _param_com_arm_ekf_hgt, - (ParamFloat) _param_com_arm_ekf_vel, - (ParamFloat) _param_com_arm_ekf_pos, - (ParamFloat) _param_com_arm_ekf_yaw, (ParamBool) _param_com_arm_wo_gps, (ParamBool) _param_sys_has_gps, (ParamFloat) _param_com_pos_fs_eph, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp index c60a9a4af05e..8ef3c5cfa966 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp @@ -64,6 +64,7 @@ int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_stat _active_registrations_mask |= 1 << free_registration_index; _registrations[free_registration_index].nav_mode_id = nav_mode_id; _registrations[free_registration_index].replaces_nav_state = replaces_nav_state; + _registrations[free_registration_index].waiting_for_first_response = true; _registrations[free_registration_index].num_no_response = 0; _registrations[free_registration_index].unresponsive = false; _registrations[free_registration_index].total_num_unresponsive = 0; @@ -230,6 +231,7 @@ void ExternalChecks::update() && _current_request_id == reply.request_id) { _reply_received_mask |= 1u << reply.registration_id; _registrations[reply.registration_id].num_no_response = 0; + _registrations[reply.registration_id].waiting_for_first_response = false; // Prevent toggling between unresponsive & responsive state if (_registrations[reply.registration_id].total_num_unresponsive <= 3) { @@ -253,7 +255,10 @@ void ExternalChecks::update() for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) { if ((1u << i) & no_reply) { - if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) { + const int max_num_no_reply = + _registrations[i].waiting_for_first_response ? NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT : NUM_NO_REPLY_UNTIL_UNRESPONSIVE; + + if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response > max_num_no_reply) { // Clear immediately if not a mode if (_registrations[i].nav_mode_id == -1) { removeRegistration(i, -1); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp index b4ee24cba6d5..7129e4620361 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/externalChecks.hpp @@ -72,6 +72,9 @@ class ExternalChecks : public HealthAndArmingCheckBase static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms; static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval"); static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL + /// Timeout directly after registering (in some cases ROS can take a while until the subscription gets the first + /// sample, around 800ms was observed) + static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE_INIT = 10; void checkNonRegisteredModes(const Context &context, Report &reporter) const; @@ -83,6 +86,7 @@ class ExternalChecks : public HealthAndArmingCheckBase int8_t nav_mode_id{-1}; ///< associated mode, -1 if none int8_t replaces_nav_state{-1}; + bool waiting_for_first_response{true}; uint8_t num_no_response{0}; bool unresponsive{false}; uint8_t total_num_unresponsive{0}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp index ca292d7b14da..ce6e8d4412ff 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/imuConsistencyCheck.cpp @@ -48,13 +48,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i]; - NavModes required_groups = NavModes::None; - if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get()) { - required_groups = NavModes::All; - } - - if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get() * 0.8f) { /* EVENT * @description * Check the calibration. @@ -66,7 +60,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report * This check can be configured via COM_ARM_IMU_ACC parameter. * */ - reporter.armingCheckFailure(required_groups, health_component_t::accel, + reporter.armingCheckFailure(NavModes::All, health_component_t::accel, events::ID("check_imu_accel_inconsistent"), events::Log::Warning, "Accel {1} inconsistent", i, accel_inconsistency_m_s_s, _param_com_arm_imu_acc.get()); @@ -85,13 +79,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i]; - NavModes required_groups = NavModes::None; - if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get()) { - required_groups = NavModes::All; - } - - if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get() * 0.5f) { /* EVENT * @description * Check the calibration. @@ -103,7 +91,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report * This check can be configured via COM_ARM_IMU_GYR parameter. * */ - reporter.armingCheckFailure(required_groups, health_component_t::gyro, + reporter.armingCheckFailure(NavModes::All, health_component_t::gyro, events::ID("check_imu_gyro_inconsistent"), events::Log::Warning, "Gyro {1} inconsistent", i, gyro_inconsistency_rad_s, _param_com_arm_imu_gyr.get()); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.cpp new file mode 100644 index 000000000000..85bd8909ae3c --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.cpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "loggerCheck.hpp" + +using namespace time_literals; + +LoggerChecks::LoggerChecks() + : _param_sdlog_mode_handle(param_find("SDLOG_MODE")) +{ + param_get(_param_sdlog_mode_handle, &_sdlog_mode); +} + +void LoggerChecks::checkAndReport(const Context &context, Report &reporter) +{ + bool active = false; + + if (_sdlog_mode >= 0) { + if (_logger_status_sub.advertised()) { + logger_status_s status; + _logger_status_sub.copy(&status); + + if (hrt_elapsed_time(&status.timestamp) < 3_s && status.is_logging) { + active = true; + } + } + } + + reporter.setHealth(health_component_t::logging, active, false, false); +} diff --git a/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.hpp similarity index 71% rename from src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp rename to src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.hpp index 99e918886f62..bcc161efcab9 100644 --- a/src/drivers/actuators/voxl_esc/voxl_esc_serial.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/loggerCheck.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2024 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,22 @@ #pragma once -#include -#include -#include -#include +#include "../Common.hpp" -#ifdef __PX4_QURT -#include -#define FAR -#endif +#include +#include +#include -class VoxlEscSerial +class LoggerChecks : public HealthAndArmingCheckBase { public: - VoxlEscSerial(); - virtual ~VoxlEscSerial(); + LoggerChecks(); + ~LoggerChecks() = default; - int uart_open(const char *dev, speed_t speed); - int uart_set_baud(speed_t speed); - int uart_close(); - int uart_write(FAR void *buf, size_t len); - int uart_read(FAR void *buf, size_t len); - bool is_open() { return _uart_fd >= 0; }; - int uart_get_baud() {return _speed; } + void checkAndReport(const Context &context, Report &reporter) override; private: - int _uart_fd = -1; - -#if ! defined(__PX4_QURT) - struct termios _orig_cfg; - struct termios _cfg; -#endif - - int _speed = -1; + uORB::Subscription _logger_status_sub{ORB_ID::logger_status}; + const param_t _param_sdlog_mode_handle; + int32_t _sdlog_mode = -1; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp index 117a23a2b8d8..6716a8e06ad2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/magnetometerCheck.cpp @@ -50,10 +50,6 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter bool is_mag_fault = false; const bool is_required = instance == 0 || isMagRequired(instance, is_mag_fault); - if (!is_required) { - continue; - } - const bool exists = _sensor_mag_sub[instance].advertised(); bool is_valid = false; bool is_calibration_valid = false; @@ -83,6 +79,11 @@ void MagnetometerChecks::checkAndReport(const Context &context, Report &reporter reporter.setIsPresent(health_component_t::magnetometer); } + // Do not raise errors if a mag is not required + if (!is_required) { + continue; + } + const bool is_sensor_ok = is_valid && is_calibration_valid && !is_mag_fault; if (!is_sensor_ok) { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp index 598531a0da9b..8e3c0e3a5a49 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp @@ -58,7 +58,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) * @description * Wait until the estimator initialized */ - reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::system, + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::attitude_estimate, events::ID("check_modes_attitude"), events::Log::Critical, "No valid attitude estimate"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude); @@ -78,7 +78,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) if (local_position_modes != NavModes::None) { /* EVENT */ - reporter.armingCheckFailure(local_position_modes, health_component_t::system, + reporter.armingCheckFailure(local_position_modes, health_component_t::local_position_estimate, events::ID("check_modes_local_pos"), events::Log::Error, "No valid local position estimate"); reporter.clearCanRunBits(local_position_modes); @@ -87,7 +87,8 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter) if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) { /* EVENT */ - reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system, + reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, + health_component_t::global_position_estimate, events::ID("check_modes_global_pos"), events::Log::Error, "No valid global position estimate"); reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position); diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp new file mode 100644 index 000000000000..2a0245bb1fbd --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "navigatorCheck.hpp" + +void NavigatorChecks::checkAndReport(const Context &context, Report &reporter) +{ + navigator_status_s status; + + if (!_navigator_status_sub.copy(&status)) { + status = {}; + } + + if (context.status().nav_state == status.nav_state) { + + reporter.failsafeFlags().navigator_failure = (status.failure != navigator_status_s::FAILURE_NONE); + + if (status.failure == navigator_status_s::FAILURE_HAGL) { + /* EVENT + */ + reporter.armingCheckFailure(NavModes::All, + health_component_t::system, + events::ID("check_navigator_failure_hagl"), + events::Log::Error, + "Waypoint above maximum height"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp new file mode 100644 index 000000000000..48c6965deaf8 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/navigatorCheck.hpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "../Common.hpp" +#include +#include + + +class NavigatorChecks : public HealthAndArmingCheckBase +{ +public: + NavigatorChecks() = default; + ~NavigatorChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _navigator_status_sub{ORB_ID(navigator_status)}; +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp index 8b40191c4212..69afd1c12cce 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.cpp @@ -74,16 +74,10 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) if (!system_power.usb_connected) { float avionics_power_rail_voltage = system_power.voltage5v_v; - const float low_error_threshold = 4.5f; - const float low_warning_threshold = 4.8f; - const float high_warning_threshold = 5.4f; + const float low_error_threshold = 4.7f; + const float high_error_threshold = 5.4f; - if (avionics_power_rail_voltage < low_warning_threshold) { - NavModes affected_groups = NavModes::None; - - if (avionics_power_rail_voltage < low_error_threshold) { - affected_groups = NavModes::All; - } + if (avionics_power_rail_voltage < low_error_threshold) { /* EVENT * @description @@ -93,16 +87,16 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) * This check can be configured via CBRK_SUPPLY_CHK parameter. * */ - reporter.healthFailure(affected_groups, health_component_t::system, + reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_low"), - events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_warning_threshold); + events::Log::Error, "Avionics Power low: {1:.2} Volt", avionics_power_rail_voltage, low_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power low: %6.2f Volt", (double)avionics_power_rail_voltage); } - } else if (avionics_power_rail_voltage > high_warning_threshold) { + } else if (avionics_power_rail_voltage > high_error_threshold) { /* EVENT * @description * Check the voltage supply to the FMU, it must be below {2:.2} Volt. @@ -113,7 +107,7 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) */ reporter.healthFailure(NavModes::All, health_component_t::system, events::ID("check_avionics_power_high"), - events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_warning_threshold); + events::Log::Error, "Avionics Power high: {1:.2} Volt", avionics_power_rail_voltage, high_error_threshold); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avionics Power high: %6.2f Volt", @@ -142,6 +136,36 @@ void PowerChecks::checkAndReport(const Context &context, Report &reporter) power_module_count, _param_com_power_count.get()); } } + + // Overcurrent detection + if (system_power.hipower_5v_oc) { + /* EVENT + * @description + * Check the power supply + */ + reporter.healthFailure(NavModes::All, health_component_t::system, + events::ID("check_power_oc_hipower"), + events::Log::Error, "Overcurrent detected for the hipower 5V supply"); + } + + if (system_power.periph_5v_oc) { + /* EVENT + * @description + * Check the power supply + */ + reporter.healthFailure(NavModes::All, health_component_t::system, + events::ID("check_power_oc_periph"), + events::Log::Error, "Overcurrent detected for the peripheral 5V supply"); + } + + if (system_power.hipower_5v_oc || system_power.periph_5v_oc) { + if (context.isArmed() && !_overcurrent_warning_sent) { + _overcurrent_warning_sent = true; + events::send(events::ID("check_power_oc_report"), + events::Log::Error, + "5V overcurrent detected, landing advised"); + } + } } } else { diff --git a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp index 72ed92839101..b81fadaee062 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/powerCheck.hpp @@ -48,6 +48,7 @@ class PowerChecks : public HealthAndArmingCheckBase private: uORB::Subscription _system_power_sub{ORB_ID(system_power)}; + bool _overcurrent_warning_sent{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_cbrk_supply_chk, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp index 660a792f857a..9f09c2a645e4 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp @@ -51,7 +51,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte if (!reporter.failsafeFlags().manual_control_signal_lost && _last_valid_manual_control_setpoint > 0) { - events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, + events::send(events::ID("commander_rc_lost"), {events::Log::Info, events::LogInternal::Info}, "Manual control lost"); } diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index ea3955e27fe2..4aa341dbcf1c 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -34,6 +34,8 @@ #include "HomePosition.hpp" +#include + #include #include "commander_helper.h" @@ -83,7 +85,8 @@ bool HomePosition::hasMovedFromCurrentHomeLocation() } } - return (home_dist_xy > eph * 2.f) || (home_dist_z > epv * 2.f); + return (home_dist_xy > fmaxf(eph * 2.f, kMinHomePositionChangeEPH)) + || (home_dist_z > fmaxf(epv * 2.f, kMinHomePositionChangeEPV)); } bool HomePosition::setHomePosition(bool force) diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 6346dac42e4f..07d6b1a63239 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -45,6 +45,8 @@ static constexpr int kHomePositionGPSRequiredFixType = 2; static constexpr float kHomePositionGPSRequiredEPH = 5.f; static constexpr float kHomePositionGPSRequiredEPV = 10.f; static constexpr float kHomePositionGPSRequiredEVH = 1.f; +static constexpr float kMinHomePositionChangeEPH = 1.f; +static constexpr float kMinHomePositionChangeEPV = 1.5f; class HomePosition { diff --git a/src/modules/commander/ModeUtil/CMakeLists.txt b/src/modules/commander/ModeUtil/CMakeLists.txt index 8713e53f6bb8..98708bba2699 100644 --- a/src/modules/commander/ModeUtil/CMakeLists.txt +++ b/src/modules/commander/ModeUtil/CMakeLists.txt @@ -36,4 +36,3 @@ add_library(mode_util mode_requirements.cpp ) add_dependencies(mode_util uorb_headers prebuild_targets) - diff --git a/src/modules/commander/ModeUtil/mode_requirements.cpp b/src/modules/commander/ModeUtil/mode_requirements.cpp index 0718804df282..32eb5b1e8116 100644 --- a/src/modules/commander/ModeUtil/mode_requirements.cpp +++ b/src/modules/commander/ModeUtil/mode_requirements.cpp @@ -82,11 +82,6 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags) setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_local_position_relaxed); setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_manual_control); - if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position); - setRequirement(vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW, flags.mode_req_global_position); - } - // NAVIGATION_STATE_AUTO_MISSION setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity); setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude); diff --git a/src/modules/commander/UserModeIntention.cpp b/src/modules/commander/UserModeIntention.cpp index 9bb3d08407f9..02b4a7f3f7e1 100644 --- a/src/modules/commander/UserModeIntention.cpp +++ b/src/modules/commander/UserModeIntention.cpp @@ -69,11 +69,17 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource } } + // never allow to change out of termination state + allow_change &= _vehicle_status.nav_state != vehicle_status_s::NAVIGATION_STATE_TERMINATION; + if (allow_change) { _had_mode_change = true; _user_intented_nav_state = user_intended_nav_state; - if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) { + // Special case termination state: even though this mode prevents arming, + // still don't switch out of it after disarm and thus store it in _nav_state_after_disarming. + if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state) + || user_intended_nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { _nav_state_after_disarming = user_intended_nav_state; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 8f2c3d51ced0..a462c8ae898c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -358,50 +358,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0); */ PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f); -/** - * Maximum EKF position innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f); - -/** - * Maximum EKF velocity innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f); - -/** - * Maximum EKF height innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f); - -/** - * Maximum EKF yaw innovation test ratio that will allow arming - * - * @group Commander - * @min 0.1 - * @max 1.0 - * @decimal 2 - * @increment 0.05 - */ -PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f); - /** * Maximum accelerometer inconsistency between IMU units that will allow arming * @@ -802,6 +758,21 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f); */ PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f); +/** + * Maximum allowed RAM usage to pass checks + * + * The check fails if the RAM usage is above this threshold. + * + * A negative value disables the check. + * + * @group Commander + * @unit % + * @min -1 + * @max 100 + * @increment 1 + */ +PARAM_DEFINE_FLOAT(COM_RAM_MAX, 95.0f); + /** * Required number of redundant power modules * diff --git a/src/modules/commander/factory_calibration_storage.cpp b/src/modules/commander/factory_calibration_storage.cpp index 450f48c49de1..698d842cf568 100644 --- a/src/modules/commander/factory_calibration_storage.cpp +++ b/src/modules/commander/factory_calibration_storage.cpp @@ -130,4 +130,3 @@ void FactoryCalibrationStorage::cleanup() param_control_autosave(true); } } - diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 9a9b1e7cad2c..11285af40352 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -472,10 +472,19 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, local_position_accuracy_low, ActionOptions(Action::RTL)); } + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || + state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Land).clearOn(ClearCondition::OnModeChangeOrDisarm)); + + } else { + CHECK_FAILSAFE(status_flags, navigator_failure, + ActionOptions(Action::Hold).clearOn(ClearCondition::OnModeChangeOrDisarm)); + } + CHECK_FAILSAFE(status_flags, geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery flight time remaining failsafe - CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(fromRemainingFlightTimeLowActParam(_param_com_fltt_low_act.get()))); @@ -512,18 +521,19 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, } - // Failure detector + // Handle fails during spoolup just after arming if ((_armed_time != 0) && (time_us < _armed_time + static_cast(_param_com_spoolup_time.get() * 1_s)) ) { CHECK_FAILSAFE(status_flags, fd_esc_arming_failure, ActionOptions(Action::Disarm).cannotBeDeferred()); + CHECK_FAILSAFE(status_flags, battery_unhealthy, ActionOptions(Action::Disarm).cannotBeDeferred()); } + // Handle fails during the early takeoff phase if ((_armed_time != 0) && (time_us < _armed_time + static_cast((_param_com_lkdown_tko.get() + _param_com_spoolup_time.get()) * 1_s)) ) { - // This handles the case where something fails during the early takeoff phase CHECK_FAILSAFE(status_flags, fd_critical_failure, ActionOptions(Action::Disarm).cannotBeDeferred()); } else if (!circuit_breaker_enabled_by_val(_param_cbrk_flightterm.get(), CBRK_FLIGHTTERM_KEY)) { diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index bb81dd02baeb..1c1b8bd86562 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -194,4 +194,3 @@ class Failsafe : public FailsafeBase ); }; - diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 4c85ee5c7214..729205ebe89e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -195,7 +195,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic_hold"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, + "Failsafe activated: switching to {2} in {3} seconds", mavlink_mode, failsafe_action, (uint16_t) delay_s); } else { @@ -204,11 +204,11 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_hold"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated due to {4}, triggering {2} in {3} seconds", mavlink_mode, failsafe_action, + "{4}: switching to {2} in {3} seconds", mavlink_mode, failsafe_action, (uint16_t) delay_s, failsafe_cause); } - mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated, entering Hold for %i seconds\t", delay_s); + mavlink_log_critical(&_mavlink_log_pub, "Failsafe activated: entering Hold for %i seconds\t", delay_s); } else { // no delay failsafe_action_t failsafe_action = (failsafe_action_t)action; @@ -222,7 +222,17 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic_warn"), {events::Log::Warning, events::LogInternal::Warning}, - "Failsafe warning triggered", mavlink_mode); + "Failsafe warning:", mavlink_mode); + + } else if (action == Action::Descend || action == Action::FallbackAltCtrl || action == Action::FallbackStab) { + /* EVENT + * @description Failsafe actions that disengage the autopilot (remove position control) + * @type append_health_and_arming_messages + */ + events::send( + events::ID("commander_failsafe_enter_autopilot_disengaged"), + {events::Log::Critical, events::LogInternal::Warning}, + "Failsafe activated: Autopilot disengaged, switching to {2}", mavlink_mode, failsafe_action); } else { /* EVENT @@ -231,7 +241,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_generic"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated, triggering {2}", mavlink_mode, failsafe_action); + "Failsafe activated: switching to {2}", mavlink_mode, failsafe_action); } } else { @@ -262,7 +272,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter_warn"), {events::Log::Warning, events::LogInternal::Warning}, - "Failsafe warning triggered due to {2}", mavlink_mode, failsafe_cause); + "Failsafe warning: {2}", mavlink_mode, failsafe_cause); } @@ -272,7 +282,7 @@ void FailsafeBase::notifyUser(uint8_t user_intended_mode, Action action, Action events::send( events::ID("commander_failsafe_enter"), {events::Log::Critical, events::LogInternal::Warning}, - "Failsafe activated due to {3}, triggering {2}", mavlink_mode, failsafe_action, failsafe_cause); + "{3}: switching to {2}", mavlink_mode, failsafe_action, failsafe_cause); } } @@ -467,7 +477,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s // Check if we should enter delayed Hold if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly - && selected_action != Action::Disarm && selected_action != Action::Terminate) { + && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold) { returned_state.delayed_action = selected_action; selected_action = Action::Hold; allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; @@ -517,6 +527,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::FallbackAltCtrl: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_ALTCTL)) { @@ -524,6 +536,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::FallbackStab: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_STAB)) { @@ -531,6 +545,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } // else: fall through here as well. If stabilized isn't available, we most certainly end up in Terminate + returned_state.cause = Cause::Generic; + // fallthrough case Action::Hold: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { @@ -538,6 +554,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::RTL: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) { @@ -545,6 +563,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Land: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) { @@ -552,6 +572,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Descend: if (modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_DESCEND)) { @@ -559,6 +581,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s break; } + returned_state.cause = Cause::Generic; + // fallthrough case Action::Terminate: selected_action = Action::Terminate; diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 07d46cbabfd6..4cd3ac5202c5 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -147,7 +147,7 @@ class FailsafeBase: public ModuleParams bool rc_sticks_takeover_request, const failsafe_flags_s &status_flags); - bool inFailsafe() const { return _selected_action != Action::None; } + bool inFailsafe() const { return (_selected_action != Action::None && _selected_action != Action::Warn); } Action selectedAction() const { return _selected_action; } @@ -283,4 +283,3 @@ class FailsafeBase: public ModuleParams ); }; - diff --git a/src/modules/commander/failsafe/parse_flags_from_msg.py b/src/modules/commander/failsafe/parse_flags_from_msg.py index 6b1ddf24b95d..487a14e049f7 100755 --- a/src/modules/commander/failsafe/parse_flags_from_msg.py +++ b/src/modules/commander/failsafe/parse_flags_from_msg.py @@ -137,4 +137,3 @@ def __init__(self, group_name): html_template_content = html_template_content.replace(html_tag, html_conditions) with open(html_output_file, 'w') as file: file.write(html_template_content) - diff --git a/src/modules/commander/lm_fit.cpp b/src/modules/commander/lm_fit.cpp index 39eb571a794e..866fa6f07192 100644 --- a/src/modules/commander/lm_fit.cpp +++ b/src/modules/commander/lm_fit.cpp @@ -353,4 +353,3 @@ int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int s return 1; } - diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 02ab015e0f17..4121549bd2ac 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -237,11 +237,8 @@ static float get_sphere_radius() if (gps_sub.copy(&gps)) { if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { - const double lat = gps.latitude_deg; - const double lon = gps.longitude_deg; - // magnetic field data returned by the geo library using the current GPS position - return get_mag_strength_gauss(lat, lon); + return get_mag_strength_gauss(gps.latitude_deg, gps.longitude_deg); } } } @@ -954,13 +951,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma return result; } -int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, float latitude, float longitude) +int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radians, + float latitude_deg, float longitude_deg) { // magnetometer quick calibration // if GPS available use world magnetic model to zero mag offsets bool mag_earth_available = false; - if (PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)) { + if (PX4_ISFINITE(latitude_deg) && PX4_ISFINITE(longitude_deg)) { mag_earth_available = true; } else { @@ -969,8 +967,8 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian if (vehicle_gps_position_sub.copy(&gps)) { if ((gps.timestamp != 0) && (gps.eph < 1000)) { - latitude = (float)gps.latitude_deg; - longitude = (float)gps.longitude_deg; + latitude_deg = (float)gps.latitude_deg; + longitude_deg = (float)gps.longitude_deg; mag_earth_available = true; } } @@ -981,14 +979,13 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian return PX4_ERROR; } else { - // magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); - const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); - const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); + const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg)); + const float field_strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg); - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, - 0, 0); + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) + * Vector3f(field_strength_gauss, 0, 0); uORB::Subscription vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; vehicle_attitude_s attitude{}; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index de028bcef0fc..e4936982ff1b 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -232,4 +232,3 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) } #endif /* DEFINE_GET_PX4_CUSTOM_MODE */ - diff --git a/src/modules/commander/worker_thread.hpp b/src/modules/commander/worker_thread.hpp index 5194c6758d71..ae271c398829 100644 --- a/src/modules/commander/worker_thread.hpp +++ b/src/modules/commander/worker_thread.hpp @@ -100,4 +100,3 @@ class WorkerThread float _longitude; }; - diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index ed4d636809d3..94f5db16f36b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -56,4 +56,3 @@ void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector 5_ms) { + if (dt > 0.005f) { do_update = true; _timestamp_sample = vehicle_thrust_setpoint.timestamp_sample; } diff --git a/src/modules/differential_drive/DifferentialDrive.cpp b/src/modules/differential_drive/DifferentialDrive.cpp deleted file mode 100644 index 70553ffeb369..000000000000 --- a/src/modules/differential_drive/DifferentialDrive.cpp +++ /dev/null @@ -1,190 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDrive.hpp" - -DifferentialDrive::DifferentialDrive() : - ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) -{ - updateParams(); -} - -bool DifferentialDrive::init() -{ - ScheduleOnInterval(10_ms); // 100 Hz - return true; -} - -void DifferentialDrive::updateParams() -{ - ModuleParams::updateParams(); - - _differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get()); - - _max_speed = _param_rdd_wheel_speed.get() * _param_rdd_wheel_radius.get(); - _differential_drive_guidance.setMaxSpeed(_max_speed); - _differential_drive_kinematics.setMaxSpeed(_max_speed); - - _max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f); - _differential_drive_guidance.setMaxAngularVelocity(_max_angular_velocity); - _differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity); -} - -void DifferentialDrive::Run() -{ - if (should_exit()) { - ScheduleClear(); - exit_and_cleanup(); - } - - hrt_abstime now = hrt_absolute_time(); - const float dt = math::min((now - _time_stamp_last), 5000_ms) / 1e6f; - _time_stamp_last = now; - - if (_parameter_update_sub.updated()) { - parameter_update_s parameter_update; - _parameter_update_sub.copy(¶meter_update); - updateParams(); - } - - if (_vehicle_control_mode_sub.updated()) { - vehicle_control_mode_s vehicle_control_mode{}; - - if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) { - _manual_driving = vehicle_control_mode.flag_control_manual_enabled; - _mission_driving = vehicle_control_mode.flag_control_auto_enabled; - } - } - - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status{}; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); - const bool spooled_up = armed && (hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s); - _differential_drive_kinematics.setArmed(spooled_up); - _acro_driving = (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO); - } - } - - if (_manual_driving) { - // Manual mode - // directly produce setpoints from the manual control setpoint (joystick) - if (_manual_control_setpoint_sub.updated()) { - manual_control_setpoint_s manual_control_setpoint{}; - - if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) { - differential_drive_setpoint_s setpoint{}; - setpoint.speed = manual_control_setpoint.throttle * math::max(0.f, _param_rdd_speed_scale.get()) * _max_speed; - setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() * _max_angular_velocity; - - // if acro mode, we activate the yaw rate control - if (_acro_driving) { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = true; - - } else { - setpoint.closed_loop_speed_control = false; - setpoint.closed_loop_yaw_rate_control = false; - } - - setpoint.timestamp = now; - _differential_drive_setpoint_pub.publish(setpoint); - } - } - - } else if (_mission_driving) { - // Mission mode - // directly receive setpoints from the guidance library - _differential_drive_guidance.computeGuidance( - _differential_drive_control.getVehicleYaw(), - _differential_drive_control.getVehicleBodyYawRate(), - dt - ); - } - - _differential_drive_control.control(dt); - _differential_drive_kinematics.allocate(); -} - -int DifferentialDrive::task_spawn(int argc, char *argv[]) -{ - DifferentialDrive *instance = new DifferentialDrive(); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - - if (instance->init()) { - return PX4_OK; - } - - } else { - PX4_ERR("alloc failed"); - } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - return PX4_ERROR; -} - -int DifferentialDrive::custom_command(int argc, char *argv[]) -{ - return print_usage("unknown command"); -} - -int DifferentialDrive::print_usage(const char *reason) -{ - if (reason) { - PX4_ERR("%s\n", reason); - } - - PRINT_MODULE_DESCRIPTION( - R"DESCR_STR( -### Description -Rover Differential Drive controller. -)DESCR_STR"); - - PRINT_MODULE_USAGE_NAME("differential_drive", "controller"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); - return 0; -} - -extern "C" __EXPORT int differential_drive_main(int argc, char *argv[]) -{ - return DifferentialDrive::main(argc, argv); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp deleted file mode 100644 index 728c5e666a10..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.cpp +++ /dev/null @@ -1,109 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDriveControl.hpp" - -using namespace matrix; - -DifferentialDriveControl::DifferentialDriveControl(ModuleParams *parent) : ModuleParams(parent) -{ - pid_init(&_pid_angular_velocity, PID_MODE_DERIVATIV_NONE, 0.001f); - pid_init(&_pid_speed, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveControl::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_pid_angular_velocity, - _param_rdd_p_gain_angular_velocity.get(), // Proportional gain - _param_rdd_i_gain_angular_velocity.get(), // Integral gain - 0, // Derivative gain - 20, // Integral limit - 200); // Output limit - - pid_set_parameters(&_pid_speed, - _param_rdd_p_gain_speed.get(), // Proportional gain - _param_rdd_i_gain_speed.get(), // Integral gain - 0, // Derivative gain - 2, // Integral limit - 200); // Output limit -} - -void DifferentialDriveControl::control(float dt) -{ - if (_vehicle_angular_velocity_sub.updated()) { - vehicle_angular_velocity_s vehicle_angular_velocity{}; - - if (_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity)) { - _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; - } - } - - if (_vehicle_attitude_sub.updated()) { - vehicle_attitude_s vehicle_attitude{}; - - if (_vehicle_attitude_sub.copy(&vehicle_attitude)) { - _vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); - _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); - } - } - - if (_vehicle_local_position_sub.updated()) { - vehicle_local_position_s vehicle_local_position{}; - - if (_vehicle_local_position_sub.copy(&vehicle_local_position)) { - Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); - Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); - _vehicle_forward_speed = velocity_in_body_frame(0); - } - } - - _differential_drive_setpoint_sub.update(&_differential_drive_setpoint); - - // PID to reach setpoint using control_output - differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint; - - if (_differential_drive_setpoint.closed_loop_speed_control) { - differential_drive_control_output.speed += - pid_calculate(&_pid_speed, _differential_drive_setpoint.speed, _vehicle_forward_speed, 0, dt); - } - - if (_differential_drive_setpoint.closed_loop_yaw_rate_control) { - differential_drive_control_output.yaw_rate += - pid_calculate(&_pid_angular_velocity, _differential_drive_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt); - } - - differential_drive_control_output.timestamp = hrt_absolute_time(); - _differential_drive_control_output_pub.publish(differential_drive_control_output); -} diff --git a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp b/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp deleted file mode 100644 index 8d3b7e1c216c..000000000000 --- a/src/modules/differential_drive/DifferentialDriveControl/DifferentialDriveControl.hpp +++ /dev/null @@ -1,91 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 DifferentialDriveControl.hpp - * - * Controller for heading rate and forward speed. - */ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class DifferentialDriveControl : public ModuleParams -{ -public: - DifferentialDriveControl(ModuleParams *parent); - ~DifferentialDriveControl() = default; - - void control(float dt); - float getVehicleBodyYawRate() const { return _vehicle_body_yaw_rate; } - float getVehicleYaw() const { return _vehicle_yaw; } - -protected: - void updateParams() override; - -private: - uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)}; - uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; - uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; - - uORB::Publication _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)}; - - differential_drive_setpoint_s _differential_drive_setpoint{}; - - matrix::Quatf _vehicle_attitude_quaternion{}; - float _vehicle_yaw{0.f}; - - // States - float _vehicle_body_yaw_rate{0.f}; - float _vehicle_forward_speed{0.f}; - - PID_t _pid_angular_velocity; ///< The PID controller for yaw rate. - PID_t _pid_speed; ///< The PID controller for velocity. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_speed, - (ParamFloat) _param_rdd_i_gain_speed, - (ParamFloat) _param_rdd_p_gain_angular_velocity, - (ParamFloat) _param_rdd_i_gain_angular_velocity - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp deleted file mode 100644 index df453ff8b5c0..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp +++ /dev/null @@ -1,138 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 "DifferentialDriveGuidance.hpp" - -#include - -using namespace matrix; - -DifferentialDriveGuidance::DifferentialDriveGuidance(ModuleParams *parent) : ModuleParams(parent) -{ - updateParams(); - - pid_init(&_heading_p_controller, PID_MODE_DERIVATIV_NONE, 0.001f); -} - -void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocity, float dt) -{ - if (_position_setpoint_triplet_sub.updated()) { - _position_setpoint_triplet_sub.copy(&_position_setpoint_triplet); - } - - if (_vehicle_global_position_sub.updated()) { - _vehicle_global_position_sub.copy(&_vehicle_global_position); - } - - matrix::Vector2d global_position(_vehicle_global_position.lat, _vehicle_global_position.lon); - matrix::Vector2d current_waypoint(_position_setpoint_triplet.current.lat, _position_setpoint_triplet.current.lon); - matrix::Vector2d next_waypoint(_position_setpoint_triplet.next.lat, _position_setpoint_triplet.next.lon); - - const float distance_to_next_wp = get_distance_to_next_waypoint(global_position(0), global_position(1), - current_waypoint(0), - current_waypoint(1)); - - float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), - current_waypoint(1)); - float heading_error = matrix::wrap_pi(desired_heading - yaw); - - if (_current_waypoint != current_waypoint) { - _currentState = GuidanceState::TURNING; - } - - // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. - if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { - _currentState = GuidanceState::GOAL_REACHED; - } - - float desired_speed = 0.f; - - switch (_currentState) { - case GuidanceState::TURNING: - desired_speed = 0.f; - - if (fabsf(heading_error) < 0.05f) { - _currentState = GuidanceState::DRIVING; - } - - break; - - case GuidanceState::DRIVING: { - const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), - _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); - _forwards_velocity_smoothing.updateDurations(max_velocity); - _forwards_velocity_smoothing.updateTraj(dt); - desired_speed = math::interpolate(abs(heading_error), 0.1f, 0.8f, - _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); - break; - } - - case GuidanceState::GOAL_REACHED: - // temporary till I find a better way to stop the vehicle - desired_speed = 0.f; - heading_error = 0.f; - angular_velocity = 0.f; - _desired_angular_velocity = 0.f; - break; - } - - // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. - float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, - dt) + heading_error; - - differential_drive_setpoint_s output{}; - output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); - output.yaw_rate = math::constrain(angular_velocity_pid, -_max_angular_velocity, _max_angular_velocity); - output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; - output.timestamp = hrt_absolute_time(); - - _differential_drive_setpoint_pub.publish(output); - - _current_waypoint = current_waypoint; -} - -void DifferentialDriveGuidance::updateParams() -{ - ModuleParams::updateParams(); - - pid_set_parameters(&_heading_p_controller, - _param_rdd_p_gain_heading.get(), // Proportional gain - 0, // Integral gain - 0, // Derivative gain - 0, // Integral limit - 200); // Output limit - - _forwards_velocity_smoothing.setMaxJerk(_param_rdd_max_jerk.get()); - _forwards_velocity_smoothing.setMaxAccel(_param_rdd_max_accel.get()); - _forwards_velocity_smoothing.setMaxVel(_max_speed); -} diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp b/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp deleted file mode 100644 index 0a5a29bbca0b..000000000000 --- a/src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp +++ /dev/null @@ -1,140 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 - -#include -#include -#include -#include -#include - -#include - - -/** - * @brief Enum class for the different states of guidance. - */ -enum class GuidanceState { - TURNING, ///< The vehicle is currently turning. - DRIVING, ///< The vehicle is currently driving straight. - GOAL_REACHED ///< The vehicle has reached its goal. -}; - -/** - * @brief Class for differential drive guidance. - */ -class DifferentialDriveGuidance : public ModuleParams -{ -public: - /** - * @brief Constructor for DifferentialDriveGuidance. - * @param parent The parent ModuleParams object. - */ - DifferentialDriveGuidance(ModuleParams *parent); - ~DifferentialDriveGuidance() = default; - - /** - * @brief Compute guidance for the vehicle. - * @param global_pos The global position of the vehicle in degrees. - * @param current_waypoint The current waypoint the vehicle is heading towards in degrees. - * @param next_waypoint The next waypoint the vehicle will head towards after reaching the current waypoint in degrees. - * @param vehicle_yaw The yaw orientation of the vehicle in radians. - * @param body_velocity The velocity of the vehicle in m/s. - * @param angular_velocity The angular velocity of the vehicle in rad/s. - * @param dt The time step in seconds. - */ - void computeGuidance(float yaw, float angular_velocity, float dt); - - /** - * @brief Set the maximum speed for the vehicle. - * @param max_speed The maximum speed in m/s. - * @return The set maximum speed in m/s. - */ - float setMaxSpeed(float max_speed) { return _max_speed = max_speed; } - - - /** - * @brief Set the maximum angular velocity for the vehicle. - * @param max_angular_velocity The maximum angular velocity in rad/s. - * @return The set maximum angular velocity in rad/s. - */ - float setMaxAngularVelocity(float max_angular_velocity) { return _max_angular_velocity = max_angular_velocity; } - -protected: - /** - * @brief Update the parameters of the module. - */ - void updateParams() override; - -private: - uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - - position_setpoint_triplet_s _position_setpoint_triplet{}; - vehicle_global_position_s _vehicle_global_position{}; - - GuidanceState _currentState; ///< The current state of guidance. - - float _desired_angular_velocity; ///< The desired angular velocity. - - float _max_speed; ///< The maximum speed. - float _max_angular_velocity; ///< The maximum angular velocity. - - matrix::Vector2d _current_waypoint; ///< The current waypoint. - - VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. - PositionSmoothing _position_smoothing; ///< The position smoothing. - - PID_t _heading_p_controller; ///< The PID controller for yaw rate. - - DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_p_gain_heading, - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rdd_max_jerk, - (ParamFloat) _param_rdd_max_accel - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp deleted file mode 100644 index 42fcbea56b66..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 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 "DifferentialDriveKinematics.hpp" - -#include - -using namespace matrix; -using namespace time_literals; - -DifferentialDriveKinematics::DifferentialDriveKinematics(ModuleParams *parent) : ModuleParams(parent) -{} - -void DifferentialDriveKinematics::allocate() -{ - hrt_abstime now = hrt_absolute_time(); - - if (_differential_drive_control_output_sub.updated()) { - _differential_drive_control_output_sub.copy(&_differential_drive_control_output); - } - - const bool setpoint_timeout = (_differential_drive_control_output.timestamp + 100_ms) < now; - - Vector2f wheel_speeds = - computeInverseKinematics(_differential_drive_control_output.speed, _differential_drive_control_output.yaw_rate); - - if (!_armed || setpoint_timeout) { - wheel_speeds = {}; // stop - } - - wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f); - - actuator_motors_s actuator_motors{}; - actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults - wheel_speeds.copyTo(actuator_motors.control); - actuator_motors.timestamp = now; - _actuator_motors_pub.publish(actuator_motors); -} - -matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate) const -{ - if (_max_speed < FLT_EPSILON) { - return Vector2f(); - } - - linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed); - yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity); - - const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate; - float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity); - - // Compute an initial gain - float gain = 1.0f; - - if (combined_velocity > _max_speed) { - float excess_velocity = fabsf(combined_velocity - _max_speed); - const float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity; - gain = adjusted_linear_velocity / fabsf(linear_velocity_x); - } - - // Apply the gain - linear_velocity_x *= gain; - - // Calculate the left and right wheel speeds - return Vector2f(linear_velocity_x - rotational_velocity, - linear_velocity_x + rotational_velocity) / _max_speed; -} diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp deleted file mode 100644 index 524a5520a78f..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematics.hpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2023-2024 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 - -/** - * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot. - * - * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics - * given linear velocity and yaw rate. - */ -class DifferentialDriveKinematics : public ModuleParams -{ -public: - DifferentialDriveKinematics(ModuleParams *parent); - ~DifferentialDriveKinematics() = default; - - /** - * @brief Sets the wheel base of the robot. - * - * @param wheel_base The distance between the centers of the wheels. - */ - void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; }; - - /** - * @brief Sets the maximum speed of the robot. - * - * @param max_speed The maximum speed of the robot. - */ - void setMaxSpeed(const float max_speed) { _max_speed = max_speed; }; - - /** - * @brief Sets the maximum angular speed of the robot. - * - * @param max_angular_speed The maximum angular speed of the robot. - */ - void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; }; - - void setArmed(const bool armed) { _armed = armed; }; - - void allocate(); - - /** - * @brief Computes the inverse kinematics for differential drive. - * - * @param linear_velocity_x Linear velocity along the x-axis. - * @param yaw_rate Yaw rate of the robot. - * @return matrix::Vector2f Motor velocities for the right and left motors. - */ - matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate) const; - -private: - uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)}; - uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; - - differential_drive_setpoint_s _differential_drive_control_output{}; - bool _armed = false; - - float _wheel_base{0.f}; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; - - DEFINE_PARAMETERS( - (ParamInt) _param_r_rev - ) -}; diff --git a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp b/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp deleted file mode 100644 index b3c4935c1d9d..000000000000 --- a/src/modules/differential_drive/DifferentialDriveKinematics/DifferentialDriveKinematicsTest.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2023-2024 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 "DifferentialDriveKinematics.hpp" -#include - -using namespace matrix; - -class DifferentialDriveKinematicsTest : public ::testing::Test -{ -public: - DifferentialDriveKinematics kinematics{nullptr}; -}; - -TEST_F(DifferentialDriveKinematicsTest, AllZeroInputCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with zero linear velocity and zero yaw rate (stationary vehicle) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, InvalidParameterCase) -{ - kinematics.setWheelBase(0.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with invalid parameters (zero wheel base and wheel radius) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f()); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(10.f); - kinematics.setMaxAngularVelocity(10.f); - - // Test with unit values for linear velocity and yaw rate - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f)); -} - - -TEST_F(DifferentialDriveKinematicsTest, UnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1)); -} - - -TEST_F(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0)); -} - -TEST_F(DifferentialDriveKinematicsTest, RandomCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Negative linear velocity for backward motion and positive yaw rate for turning right - EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f)); -} - -TEST_F(DifferentialDriveKinematicsTest, RotateInPlaceCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test rotating in place (zero linear velocity, non-zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f)); -} - -TEST_F(DifferentialDriveKinematicsTest, StraightMovementCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test moving straight (non-zero linear velocity, zero yaw rate) - EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MinInputValuesCase) -{ - kinematics.setWheelBase(FLT_MIN); - kinematics.setMaxSpeed(FLT_MIN); - kinematics.setMaxAngularVelocity(FLT_MIN); - - // Test with minimum possible input values - EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedLimitCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase) -{ - kinematics.setWheelBase(1.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f)); -} - -TEST_F(DifferentialDriveKinematicsTest, MaxAngularCase) -{ - kinematics.setWheelBase(2.f); - kinematics.setMaxSpeed(1.f); - kinematics.setMaxAngularVelocity(1.f); - - // Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed - EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f)); -} diff --git a/src/modules/differential_drive/Kconfig b/src/modules/differential_drive/Kconfig deleted file mode 100644 index a95897e91f99..000000000000 --- a/src/modules/differential_drive/Kconfig +++ /dev/null @@ -1,6 +0,0 @@ -menuconfig MODULES_DIFFERENTIAL_DRIVE - bool "differential_drive" - default n - depends on MODULES_CONTROL_ALLOCATOR - ---help--- - Enable support for control of differential drive rovers diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 3f1f9a4c991d..35c6a7b77962 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -31,8 +31,6 @@ # ############################################################################# -add_subdirectory(Utility) - option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF) # Symforce code generation TODO:fixme @@ -63,7 +61,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)) ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py DEPENDS ${EKF_DERIVATION_SRC_DIR}/derivation.py - ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + ${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py WORKING_DIRECTORY ${EKF_DERIVATION_SRC_DIR} COMMENT "Symforce code generation (default)" @@ -97,7 +95,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)) ${PYTHON_EXECUTABLE} ${EKF_DERIVATION_SRC_DIR}/derivation.py ${SYMFORCE_ARGS} DEPENDS ${EKF_DERIVATION_SRC_DIR}/derivation.py - ${EKF_DERIVATION_SRC_DIR}/derivation_utils.py + ${EKF_DERIVATION_SRC_DIR}/utils/derivation_utils.py WORKING_DIRECTORY ${EKF_DERIVATION_DST_DIR} COMMENT "Symforce code generation" @@ -113,109 +111,115 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0)) ) endif() +set(EKF_LIBS) set(EKF_SRCS) list(APPEND EKF_SRCS - EKF/bias_estimator.cpp EKF/control.cpp EKF/covariance.cpp EKF/ekf.cpp EKF/ekf_helper.cpp - EKF/EKFGSF_yaw.cpp EKF/estimator_interface.cpp - EKF/fake_height_control.cpp - EKF/fake_pos_control.cpp EKF/height_control.cpp - EKF/imu_down_sampler.cpp - EKF/output_predictor.cpp EKF/velocity_fusion.cpp EKF/position_fusion.cpp EKF/yaw_fusion.cpp - EKF/zero_innovation_heading_update.cpp + EKF/imu_down_sampler/imu_down_sampler.cpp + + EKF/aid_sources/fake_height_control.cpp + EKF/aid_sources/fake_pos_control.cpp EKF/aid_sources/ZeroGyroUpdate.cpp EKF/aid_sources/ZeroVelocityUpdate.cpp + EKF/aid_sources/zero_innovation_heading_update.cpp ) if(CONFIG_EKF2_AIRSPEED) - list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/airspeed/airspeed_fusion.cpp) endif() if(CONFIG_EKF2_AUX_GLOBAL_POSITION) - list(APPEND EKF_SRCS EKF/aux_global_position.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp) endif() if(CONFIG_EKF2_AUXVEL) - list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/auxvel/auxvel_fusion.cpp) endif() if(CONFIG_EKF2_BAROMETER) list(APPEND EKF_SRCS - EKF/baro_height_control.cpp + EKF/aid_sources/barometer/baro_height_control.cpp ) endif() if(CONFIG_EKF2_DRAG_FUSION) - list(APPEND EKF_SRCS EKF/drag_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/drag/drag_fusion.cpp) endif() if(CONFIG_EKF2_EXTERNAL_VISION) list(APPEND EKF_SRCS - EKF/ev_control.cpp - EKF/ev_height_control.cpp - EKF/ev_pos_control.cpp - EKF/ev_vel_control.cpp - EKF/ev_yaw_control.cpp + EKF/aid_sources/external_vision/ev_control.cpp + EKF/aid_sources/external_vision/ev_height_control.cpp + EKF/aid_sources/external_vision/ev_pos_control.cpp + EKF/aid_sources/external_vision/ev_vel_control.cpp + EKF/aid_sources/external_vision/ev_yaw_control.cpp ) endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS - EKF/gnss_height_control.cpp - EKF/gps_checks.cpp - EKF/gps_control.cpp + EKF/aid_sources/gnss/gnss_height_control.cpp + EKF/aid_sources/gnss/gps_checks.cpp + EKF/aid_sources/gnss/gps_control.cpp ) -endif() -if(CONFIG_EKF2_GNSS_YAW) - list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp) + if(CONFIG_EKF2_GNSS_YAW) + list(APPEND EKF_SRCS EKF/aid_sources/gnss/gnss_yaw_control.cpp) + endif() + + list(APPEND EKF_LIBS yaw_estimator) endif() if(CONFIG_EKF2_GRAVITY_FUSION) - list(APPEND EKF_SRCS EKF/gravity_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/gravity/gravity_fusion.cpp) endif() if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS - EKF/mag_3d_control.cpp - EKF/mag_control.cpp - EKF/mag_fusion.cpp - EKF/mag_heading_control.cpp + EKF/aid_sources/magnetometer/mag_control.cpp + EKF/aid_sources/magnetometer/mag_fusion.cpp ) endif() if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS - EKF/optical_flow_control.cpp - EKF/optflow_fusion.cpp + EKF/aid_sources/optical_flow/optical_flow_control.cpp + EKF/aid_sources/optical_flow/optical_flow_fusion.cpp ) endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS - EKF/range_finder_consistency_check.cpp - EKF/range_height_control.cpp - EKF/sensor_range_finder.cpp + EKF/aid_sources/range_finder/range_finder_consistency_check.cpp + EKF/aid_sources/range_finder/range_height_control.cpp + EKF/aid_sources/range_finder/range_height_fusion.cpp + EKF/aid_sources/range_finder/sensor_range_finder.cpp ) endif() if(CONFIG_EKF2_SIDESLIP) - list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) + list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp) endif() if(CONFIG_EKF2_TERRAIN) - list(APPEND EKF_SRCS EKF/terrain_estimator.cpp) + list(APPEND EKF_SRCS EKF/terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS EKF/wind.cpp) +endif () + +add_subdirectory(EKF) + px4_add_module( MODULE modules__ekf2 MAIN ekf2 @@ -242,17 +246,25 @@ px4_add_module( ${EKF_GENERATED_FILES} + MODULE_CONFIG + module.yaml + params_multi.yaml + params_volatile.yaml + params_selector.yaml + DEPENDS geo hysteresis perf - EKF2Utility px4_work_queue world_magnetic_model + + ${EKF_LIBS} + bias_estimator + output_predictor UNITY_BUILD ) if(BUILD_TESTING) - add_subdirectory(EKF) add_subdirectory(test) endif() diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index adf922907a1f..954123b250f5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -31,114 +31,132 @@ # ############################################################################ +add_subdirectory(bias_estimator) +add_subdirectory(output_predictor) + +set(EKF_LIBS) set(EKF_SRCS) list(APPEND EKF_SRCS - bias_estimator.cpp control.cpp covariance.cpp ekf.cpp ekf_helper.cpp estimator_interface.cpp - fake_height_control.cpp - fake_pos_control.cpp height_control.cpp - imu_down_sampler.cpp - output_predictor.cpp velocity_fusion.cpp position_fusion.cpp yaw_fusion.cpp - zero_innovation_heading_update.cpp + imu_down_sampler/imu_down_sampler.cpp + + aid_sources/fake_height_control.cpp + aid_sources/fake_pos_control.cpp aid_sources/ZeroGyroUpdate.cpp aid_sources/ZeroVelocityUpdate.cpp + aid_sources/zero_innovation_heading_update.cpp ) if(CONFIG_EKF2_AIRSPEED) - list(APPEND EKF_SRCS airspeed_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp) endif() if(CONFIG_EKF2_AUX_GLOBAL_POSITION) - list(APPEND EKF_SRCS aux_global_position.cpp) + list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp) endif() if(CONFIG_EKF2_AUXVEL) - list(APPEND EKF_SRCS auxvel_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp) endif() if(CONFIG_EKF2_BAROMETER) list(APPEND EKF_SRCS - baro_height_control.cpp + aid_sources/barometer/baro_height_control.cpp ) endif() if(CONFIG_EKF2_DRAG_FUSION) - list(APPEND EKF_SRCS drag_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/drag/drag_fusion.cpp) endif() if(CONFIG_EKF2_EXTERNAL_VISION) list(APPEND EKF_SRCS - ev_control.cpp - ev_height_control.cpp - ev_pos_control.cpp - ev_vel_control.cpp - ev_yaw_control.cpp + aid_sources/external_vision/ev_control.cpp + aid_sources/external_vision/ev_height_control.cpp + aid_sources/external_vision/ev_pos_control.cpp + aid_sources/external_vision/ev_vel_control.cpp + aid_sources/external_vision/ev_yaw_control.cpp ) endif() if(CONFIG_EKF2_GNSS) list(APPEND EKF_SRCS - gnss_height_control.cpp - gps_checks.cpp - gps_control.cpp - EKFGSF_yaw.cpp + aid_sources/gnss/gnss_height_control.cpp + aid_sources/gnss/gps_checks.cpp + aid_sources/gnss/gps_control.cpp ) -endif() -if(CONFIG_EKF2_GNSS_YAW) - list(APPEND EKF_SRCS gps_yaw_fusion.cpp) + if(CONFIG_EKF2_GNSS_YAW) + list(APPEND EKF_SRCS aid_sources/gnss/gnss_yaw_control.cpp) + endif() + + add_subdirectory(yaw_estimator) + list(APPEND EKF_LIBS yaw_estimator) endif() if(CONFIG_EKF2_GRAVITY_FUSION) - list(APPEND EKF_SRCS gravity_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/gravity/gravity_fusion.cpp) endif() if(CONFIG_EKF2_MAGNETOMETER) list(APPEND EKF_SRCS - mag_3d_control.cpp - mag_control.cpp - mag_fusion.cpp - mag_heading_control.cpp + aid_sources/magnetometer/mag_control.cpp + aid_sources/magnetometer/mag_fusion.cpp ) endif() if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS - optical_flow_control.cpp - optflow_fusion.cpp + aid_sources/optical_flow/optical_flow_control.cpp + aid_sources/optical_flow/optical_flow_fusion.cpp ) endif() if(CONFIG_EKF2_RANGE_FINDER) list(APPEND EKF_SRCS - range_finder_consistency_check.cpp - range_height_control.cpp - sensor_range_finder.cpp + aid_sources/range_finder/range_finder_consistency_check.cpp + aid_sources/range_finder/range_height_control.cpp + aid_sources/range_finder/range_height_fusion.cpp + aid_sources/range_finder/sensor_range_finder.cpp ) endif() if(CONFIG_EKF2_SIDESLIP) - list(APPEND EKF_SRCS sideslip_fusion.cpp) + list(APPEND EKF_SRCS aid_sources/sideslip/sideslip_fusion.cpp) endif() if(CONFIG_EKF2_TERRAIN) - list(APPEND EKF_SRCS terrain_estimator.cpp) + list(APPEND EKF_SRCS terrain_control.cpp) endif() +if(CONFIG_EKF2_WIND) + list(APPEND EKF_SRCS wind.cpp) +endif () + +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_library(ecl_EKF ${EKF_SRCS} ) add_dependencies(ecl_EKF prebuild_targets) target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PATH}) -target_link_libraries(ecl_EKF PRIVATE geo world_magnetic_model) + +target_link_libraries(ecl_EKF + PRIVATE + bias_estimator + geo + output_predictor + world_magnetic_model + ${EKF_LIBS} +) + target_compile_options(ecl_EKF PRIVATE -fno-associative-math) diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp index 48d07d583dac..052e37312c02 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.cpp @@ -42,13 +42,13 @@ ZeroVelocityUpdate::ZeroVelocityUpdate() void ZeroVelocityUpdate::reset() { - _time_last_zero_velocity_fuse = 0; + _time_last_fuse = 0; } bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delayed) { // Fuse zero velocity at a limited rate (every 200 milliseconds) - const bool zero_velocity_update_data_ready = (_time_last_zero_velocity_fuse + 200'000 < imu_delayed.time_us); + const bool zero_velocity_update_data_ready = (_time_last_fuse + 200'000 < imu_delayed.time_us); if (zero_velocity_update_data_ready) { const bool continuing_conditions_passing = ekf.control_status_flags().vehicle_at_rest @@ -69,7 +69,7 @@ bool ZeroVelocityUpdate::update(Ekf &ekf, const estimator::imuSample &imu_delaye ekf.fuseDirectStateMeasurement(innovation, innov_var(i), obs_var, State::vel.idx + i); } - _time_last_zero_velocity_fuse = imu_delayed.time_us; + _time_last_fuse = imu_delayed.time_us; return true; } diff --git a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp index 0016c584c6ec..a591d9dc96f1 100644 --- a/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp +++ b/src/modules/ekf2/EKF/aid_sources/ZeroVelocityUpdate.hpp @@ -45,9 +45,11 @@ class ZeroVelocityUpdate : public EstimatorAidSource void reset() override; bool update(Ekf &ekf, const estimator::imuSample &imu_delayed) override; + const auto &time_last_fuse() const { return _time_last_fuse; } + private: - uint64_t _time_last_zero_velocity_fuse{0}; ///< last time of zero velocity update (uSec) + uint64_t _time_last_fuse{0}; ///< last time of zero velocity update (uSec) }; diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp similarity index 84% rename from src/modules/ekf2/EKF/airspeed_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 7f8539b1cb0e..5cfb1210795d 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -62,7 +62,12 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _control_status.flags.wind = false; } + if (_control_status.flags.wind && _external_wind_init) { + _external_wind_init = false; + } + #if defined(CONFIG_EKF2_GNSS) + // clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active) if (_control_status.flags.fixed_wing) { if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) { @@ -74,6 +79,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _yawEstimator.setTrueAirspeed(0.f); } } + #endif // CONFIG_EKF2_GNSS if (_params.arsp_thr <= 0.f) { @@ -87,13 +93,18 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) updateAirspeed(airspeed_sample, _aid_src_airspeed); - _innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag + _innov_check_fail_status.flags.reject_airspeed = + _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag + + const bool continuing_conditions_passing = _control_status.flags.in_air + && _control_status.flags.fixed_wing + && !_control_status.flags.fake_pos; - const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos; const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f); - const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant - && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); + const bool starting_conditions_passing = continuing_conditions_passing + && is_airspeed_significant + && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning); if (_control_status.flags.fuse_aspd) { if (continuing_conditions_passing) { @@ -121,10 +132,11 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) if (_control_status.flags.inertial_dead_reckoning && !is_airspeed_consistent) { resetVelUsingAirspeed(airspeed_sample); - } else if (!_control_status.flags.wind || getWindVelocityVariance().longerThan(_params.initial_wind_uncertainty)) { - // If starting wind state estimation, reset the wind states and covariances before fusing any data - // Also catch the case where sideslip fusion enabled wind estimation recently and didn't converge yet. + } else if (!_external_wind_init + && (!_control_status.flags.wind + || getWindVelocityVariance().longerThan(sq(_params.initial_wind_uncertainty)))) { resetWindUsingAirspeed(airspeed_sample); + _aid_src_airspeed.time_last_fuse = _time_delayed_us; } _control_status.flags.wind = true; @@ -139,26 +151,22 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const { - // reset flags - resetEstimatorAidStatus(aid_src); - // Variance for true airspeed measurement - (m/sec)^2 const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); float innov = 0.f; float innov_var = 0.f; - sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var); - - aid_src.observation = airspeed_sample.true_airspeed; - aid_src.observation_variance = R; - aid_src.innovation = innov; - aid_src.innovation_variance = innov_var; - - aid_src.timestamp_sample = airspeed_sample.time_us; - - const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(aid_src, innov_gate); + sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, + &innov, &innov_var); + + updateAidSourceStatus(aid_src, + airspeed_sample.time_us, // sample timestamp + airspeed_sample.true_airspeed, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.tas_innov_gate, 1.f)); // innovation gate } void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) @@ -214,6 +222,10 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour if (is_fused) { aid_src.time_last_fuse = _time_delayed_us; + + if (!update_wind_only) { + _time_last_hor_vel_fuse = _time_delayed_us; + } } } @@ -221,7 +233,6 @@ void Ekf::stopAirspeedFusion() { if (_control_status.flags.fuse_aspd) { ECL_INFO("stopping airspeed fusion"); - resetEstimatorAidStatus(_aid_src_airspeed); _control_status.flags.fuse_aspd = false; #if defined(CONFIG_EKF2_GNSS) @@ -235,16 +246,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample) constexpr float sideslip_var = sq(math::radians(15.0f)); const float euler_yaw = getEulerYaw(_R_to_earth); - const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); + const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) + * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f)); matrix::SquareMatrix P_wind; - sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind); + sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), + getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind); resetStateCovariance(P_wind); ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1)); - _aid_src_airspeed.time_last_fuse = _time_delayed_us; + resetAidSourceStatusZeroInnovation(_aid_src_airspeed); } void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample) diff --git a/src/modules/ekf2/EKF/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp similarity index 86% rename from src/modules/ekf2/EKF/aux_global_position.cpp rename to src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index f64a990bae84..6efd4fabe66b 100644 --- a/src/modules/ekf2/EKF/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -33,7 +33,7 @@ #include "ekf.h" -#include "aux_global_position.hpp" +#include "aid_sources/aux_global_position/aux_global_position.hpp" #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) @@ -80,23 +80,27 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed position = ekf.global_origin().project(sample.latitude, sample.longitude); //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate - float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get()); + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); const float pos_var = sq(pos_noise); const Vector2f pos_obs_var(pos_var, pos_var); - ekf.updateHorizontalPositionAidSrcStatus(sample.time_us, - position, // observation - pos_obs_var, // observation variance - math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate - aid_src); + + ekf.updateAidSourceStatus(aid_src, + sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(ekf.state().pos) - position, // innovation + Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance + math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate } const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) - && ekf.control_status_flags().yaw_align; + && ekf.control_status_flags().yaw_align; const bool continuing_conditions = starting_conditions && ekf.global_origin_valid(); switch (_state) { case State::stopped: + /* FALLTHROUGH */ case State::starting: if (starting_conditions) { @@ -116,6 +120,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } } } + break; case State::active: @@ -124,8 +129,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { + ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); - aid_src.time_last_fuse = imu_delayed.time_us; + + ekf.resetAidSourceStatusZeroInnovation(aid_src); + _reset_counters.lat_lon = sample.lat_lon_reset_counter; } @@ -133,6 +141,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed ekf.disableControlStatusAuxGpos(); _state = State::stopped; } + break; default: @@ -142,7 +151,10 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed #if defined(MODULE_NAME) aid_src.timestamp = hrt_absolute_time(); _estimator_aid_src_aux_global_position_pub.publish(aid_src); + + _test_ratio_filtered = math::max(fabsf(aid_src.test_ratio_filtered[0]), fabsf(aid_src.test_ratio_filtered[1])); #endif // MODULE_NAME + } else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) { ekf.disableControlStatusAuxGpos(); _state = State::stopped; diff --git a/src/modules/ekf2/EKF/aux_global_position.hpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp similarity index 95% rename from src/modules/ekf2/EKF/aux_global_position.hpp rename to src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp index 4246477350aa..e5bc78026a68 100644 --- a/src/modules/ekf2/EKF/aux_global_position.hpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.hpp @@ -42,8 +42,8 @@ // WelfordMean for init? // WelfordMean for rate -#include "common.h" -#include "RingBuffer.h" +#include "../../common.h" +#include "../../RingBuffer.h" #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) @@ -74,6 +74,8 @@ class AuxGlobalPosition : public ModuleParams updateParams(); } + float test_ratio_filtered() const { return _test_ratio_filtered; } + private: bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t time_delayed_us, uint64_t timeout_period) const { @@ -94,8 +96,8 @@ class AuxGlobalPosition : public ModuleParams uint64_t _time_last_buffer_push{0}; enum class Ctrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1) + HPOS = (1 << 0), + VPOS = (1 << 1) }; enum class State { @@ -106,6 +108,8 @@ class AuxGlobalPosition : public ModuleParams State _state{State::stopped}; + float _test_ratio_filtered{INFINITY}; + #if defined(MODULE_NAME) struct reset_counters_s { uint8_t lat_lon{}; diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp similarity index 73% rename from src/modules/ekf2/EKF/auxvel_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp index 4ef94e31ee9e..6531c5c65a07 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp @@ -33,16 +33,20 @@ #include "ekf.h" -void Ekf::controlAuxVelFusion() +void Ekf::controlAuxVelFusion(const imuSample &imu_sample) { if (_auxvel_buffer) { - auxVelSample auxvel_sample_delayed; + auxVelSample sample; - if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) { + if (_auxvel_buffer->pop_first_older_than(imu_sample.time_us, &sample)) { - resetEstimatorAidStatus(_aid_src_aux_vel); - - updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); + updateAidSourceStatus(_aid_src_aux_vel, + sample.time_us, // sample timestamp + sample.vel, // observation + sample.velVar, // observation variance + Vector2f(_state.vel.xy()) - sample.vel, // innovation + Vector2f(getStateVariance()) + sample.velVar, // innovation variance + math::max(_params.auxvel_gate, 1.f)); // innovation gate if (isHorizontalAidingActive()) { fuseHorizontalVelocity(_aid_src_aux_vel); @@ -55,5 +59,4 @@ void Ekf::stopAuxVelFusion() { ECL_INFO("stopping aux vel fusion"); //_control_status.flags.aux_vel = false; - resetEstimatorAidStatus(_aid_src_aux_vel); } diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp similarity index 68% rename from src/modules/ekf2/EKF/baro_height_control.cpp rename to src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index def9a885f348..dc6d36bcb5c7 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlBaroHeightFusion() +void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "baro"; @@ -49,18 +49,16 @@ void Ekf::controlBaroHeightFusion() baroSample baro_sample; - if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { + if (_baro_buffer && _baro_buffer->pop_first_older_than(imu_sample.time_us, &baro_sample)) { #if defined(CONFIG_EKF2_BARO_COMPENSATION) - const float measurement = compensateBaroForDynamicPressure(baro_sample.hgt); + const float measurement = compensateBaroForDynamicPressure(imu_sample, baro_sample.hgt); #else const float measurement = baro_sample.hgt; #endif const float measurement_var = sq(_params.baro_noise); - const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); if (measurement_valid) { @@ -80,11 +78,11 @@ void Ekf::controlBaroHeightFusion() } // vertical position innovation - baro measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(baro_sample.time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + updateVerticalPositionAidStatus(aid_src, + baro_sample.time_us, + -(measurement - bias_est.getBias()), // observation + measurement_var + bias_est.getBiasVar(), // observation variance + math::max(_params.baro_innov_gate, 1.f)); // innovation gate // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. @@ -128,7 +126,7 @@ void Ekf::controlBaroHeightFusion() const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::BARO)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); @@ -136,16 +134,21 @@ void Ekf::controlBaroHeightFusion() resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); - // reset vertical velocity - resetVerticalVelocityToZero(); + // reset vertical velocity if no valid sources available + if (!isVerticalVelocityAidingActive()) { + resetVerticalVelocityToZero(); + } - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; } else if (is_fusion_failing) { - // Some other height source is still working ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); stopBaroHgtFusion(); - _baro_hgt_faulty = true; + + if (isRecent(_time_last_hgt_fuse, _params.hgt_fusion_timeout_max)) { + // Some other height source is still working + _baro_hgt_faulty = true; + } } } else { @@ -168,7 +171,7 @@ void Ekf::controlBaroHeightFusion() bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); } - aid_src.time_last_fuse = _time_delayed_us; + aid_src.time_last_fuse = imu_sample.time_us; bias_est.setFusionActive(); _control_status.flags.baro_hgt = true; } @@ -191,8 +194,44 @@ void Ekf::stopBaroHgtFusion() } _baro_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_baro_hgt); _control_status.flags.baro_hgt = false; } } + +#if defined(CONFIG_EKF2_BARO_COMPENSATION) +float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const float baro_alt_uncompensated) const +{ + if (_control_status.flags.wind && local_position_is_valid()) { + // calculate static pressure error = Pmeas - Ptruth + // model position error sensitivity as a body fixed ellipse with a different scale in the positive and + // negative X and Y directions. Used to correct baro data for positional errors + + // Calculate airspeed in body frame + const Vector3f angular_velocity = (imu_sample.delta_ang / imu_sample.delta_ang_dt) - _state.gyro_bias; + const Vector3f vel_imu_rel_body_ned = _R_to_earth * (angular_velocity % _params.imu_pos_body); + const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned; + + const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f); + + const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth; + + const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth); + + const Vector3f K_pstatic_coef( + airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn, + airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn, + _params.static_pressure_coef_z); + + const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed)); + + const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef)); + + // correct baro measurement using pressure error estimate and assuming sea level gravity + return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); + } + + // otherwise return the uncorrected baro measurement + return baro_alt_uncompensated; +} +#endif // CONFIG_EKF2_BARO_COMPENSATION diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp similarity index 80% rename from src/modules/ekf2/EKF/drag_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp index 5b4baedf123f..41c7dc2f12c8 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp @@ -48,9 +48,11 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed) if ((_params.drag_ctrl > 0) && _drag_buffer) { if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) { - // reset the wind states and covariances when starting drag accel fusion _control_status.flags.wind = true; - resetWindToZero(); + + if (!_external_wind_init) { + resetWindCov(); + } } dragSample drag_sample; @@ -105,11 +107,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } - _aid_src_drag.timestamp_sample = drag_sample.time_us; - _aid_src_drag.fused = false; - bool fused[] {false, false}; + Vector2f observation{}; + Vector2f observation_variance{R_ACC, R_ACC}; + Vector2f innovation{}; + Vector2f innovation_variance{}; + + // Apply an innovation consistency check with a 5 Sigma threshold + const float innov_gate = 5.f; + VectorState H; // perform sequential fusion of XY specific forces @@ -120,16 +127,15 @@ void Ekf::fuseDrag(const dragSample &drag_sample) // Drag is modelled as an arbitrary combination of bluff body drag that proportional to // equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed // parallel to the rotor disc and mass flow through the rotor disc. - const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected; + const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed + - rel_wind_body(axis_index) * mcoef_corrrected; - _aid_src_drag.observation[axis_index] = mea_acc; - _aid_src_drag.observation_variance[axis_index] = R_ACC; - _aid_src_drag.innovation[axis_index] = pred_acc - mea_acc; - _aid_src_drag.innovation_variance[axis_index] = NAN; // reset + observation(axis_index) = mea_acc; + innovation(axis_index) = pred_acc - mea_acc; if (axis_index == 0) { sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &H); + &innovation_variance(axis_index), &H); if (!using_bcoef_x && !using_mcoef) { continue; @@ -137,35 +143,41 @@ void Ekf::fuseDrag(const dragSample &drag_sample) } else if (axis_index == 1) { sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON, - &_aid_src_drag.innovation_variance[axis_index], &H); + &innovation_variance(axis_index), &H); if (!using_bcoef_y && !using_mcoef) { continue; } } - if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) { + if (innovation_variance(axis_index) < R_ACC) { // calculation is badly conditioned - return; + break; } - // Apply an innovation consistency check with a 5 Sigma threshold - const float innov_gate = 5.f; - setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate); + const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index)); if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos - && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index]) - && (_aid_src_drag.test_ratio[axis_index] < 1.f) + && PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index)) + && (test_ratio < 1.f) ) { - VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index]; + VectorState K = P * H / innovation_variance(axis_index); - if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) { + if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) { fused[axis_index] = true; } } } + updateAidSourceStatus(_aid_src_drag, + drag_sample.time_us, // sample timestamp + observation, // observation + observation_variance, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate + if (fused[0] && fused[1]) { _aid_src_drag.fused = true; _aid_src_drag.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp similarity index 77% rename from src/modules/ekf2/EKF/ev_control.cpp rename to src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp index 0780f4111814..68022677ba64 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp @@ -38,7 +38,7 @@ #include "ekf.h" -void Ekf::controlExternalVisionFusion() +void Ekf::controlExternalVisionFusion(const imuSample &imu_sample) { _ev_pos_b_est.predict(_dt_ekf_avg); _ev_hgt_b_est.predict(_dt_ekf_avg); @@ -46,7 +46,7 @@ void Ekf::controlExternalVisionFusion() // Check for new external vision data extVisionSample ev_sample; - if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(_time_delayed_us, &ev_sample)) { + if (_ext_vision_buffer && _ext_vision_buffer->pop_first_older_than(imu_sample.time_us, &ev_sample)) { bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter); @@ -55,23 +55,23 @@ void Ekf::controlExternalVisionFusion() const bool starting_conditions_passing = quality_sufficient && ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL) - && ((_params.ev_quality_minimum <= 0) || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient - && ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient + && ((_params.ev_quality_minimum <= 0) + || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient + && ((_params.ev_quality_minimum <= 0) + || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient && isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL); updateEvAttitudeErrorFilter(ev_sample, ev_reset); - controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); - controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); - controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); - controlEvHeightFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_hgt); + controlEvYawFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw); + controlEvVelFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel); + controlEvPosFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos); + controlEvHeightFusion(imu_sample, ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, + _aid_src_ev_hgt); if (quality_sufficient) { _ev_sample_prev = ev_sample; } - // record corresponding yaw state for future EV delta heading innovation (logging only) - _ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal); - } else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw || _control_status.flags.ev_hgt) && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) { @@ -84,7 +84,6 @@ void Ekf::controlExternalVisionFusion() _ev_q_error_initialized = false; - _warning_events.flags.vision_data_stopped = true; ECL_WARN("vision data stopped"); } } diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp similarity index 81% rename from src/modules/ekf2/EKF/ev_height_control.cpp rename to src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 0552ace53c76..440c0fe7acf6 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -38,8 +38,9 @@ #include "ekf.h" -void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) +void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV height"; @@ -77,19 +78,21 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f)); #if defined(CONFIG_EKF2_GNSS) + // increase minimum variance if GPS active if (_control_status.flags.gps_hgt) { measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise)); } + #endif // CONFIG_EKF2_GNSS const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); - updateVerticalPositionAidSrcStatus(ev_sample.time_us, - measurement - bias_est.getBias(), - measurement_var + bias_est.getBiasVar(), - math::max(_params.ev_pos_innov_gate, 1.f), - aid_src); + updateVerticalPositionAidStatus(aid_src, + ev_sample.time_us, + measurement - bias_est.getBias(), // observation + measurement_var + bias_est.getBiasVar(), // observation variance + math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -139,40 +142,13 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired() && quality_sufficient) { + if (isHeightResetRequired() && quality_sufficient && (_height_sensor_ref == HeightSensor::EV)) { // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); bias_est.setBias(-_state.pos(2) + measurement); - // reset vertical velocity - if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast(EvCtrl::VEL))) { - - // correct velocity for offset relative to IMU - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - switch (ev_sample.vel_frame) { - case VelocityFrame::LOCAL_FRAME_NED: - case VelocityFrame::LOCAL_FRAME_FRD: { - const Vector3f reset_vel = ev_sample.vel - vel_offset_earth; - resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise))); - } - break; - - case VelocityFrame::BODY_FRAME_FRD: { - const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise))); - } - break; - } - - } else { - resetVerticalVelocityToZero(); - } - aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { @@ -220,7 +196,6 @@ void Ekf::stopEvHgtFusion() } _ev_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_ev_hgt); _control_status.flags.ev_hgt = false; } diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp similarity index 89% rename from src/modules/ekf2/EKF/ev_pos_control.cpp rename to src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 3f6027400ade..590a289da394 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -41,8 +41,9 @@ static constexpr const char *EV_AID_SRC_NAME = "EV position"; -void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src) +void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source2d_s &aid_src) { const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); @@ -126,12 +127,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } #if defined(CONFIG_EKF2_GNSS) + // increase minimum variance if GPS active (position reference) if (_control_status.flags.gps) { for (int i = 0; i < 2; i++) { pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise)); } } + #endif // CONFIG_EKF2_GNSS const Vector2f measurement{pos(0), pos(1)}; @@ -155,18 +158,24 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } } - updateHorizontalPositionAidSrcStatus(ev_sample.time_us, - measurement - _ev_pos_b_est.getBias(), // observation - measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance - math::max(_params.ev_pos_innov_gate, 1.f), // innovation gate - aid_src); + const Vector2f position = measurement - _ev_pos_b_est.getBias(); + const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar(); + + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance())); + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), + measurement_var + Vector2f(getStateVariance())); } if (!measurement_valid) { @@ -197,7 +206,8 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } } -void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src) +void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, + estimator_aid_source2d_s &aid_src) { // activate fusion // TODO: (_params.position_sensor_ref == PositionSensor::EV) @@ -221,7 +231,8 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem _control_status.flags.ev_pos = true; } -void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src) +void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, + bool reset, estimator_aid_source2d_s &aid_src) { if (reset) { @@ -297,8 +308,6 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure void Ekf::stopEvPosFusion() { if (_control_status.flags.ev_pos) { - resetEstimatorAidStatus(_aid_src_ev_pos); - _control_status.flags.ev_pos = false; } } diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp new file mode 100644 index 000000000000..b94f24f45e9f --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp @@ -0,0 +1,347 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 ev_vel_control.cpp + * Control functions for ekf external vision velocity fusion + */ + +#include "ekf.h" +#include +#include +#include + +void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source3d_s &aid_src) +{ + static constexpr const char *AID_SRC_NAME = "EV velocity"; + + const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) + || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + + // determine if we should use EV velocity aiding + bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) + && _control_status.flags.tilt_align + && ev_sample.vel.isAllFinite(); + + // correct velocity for offset relative to IMU + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; + const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; + const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; + + // rotate measurement into correct earth frame if required + Vector3f measurement{}; + Vector3f measurement_var{}; + + float minimum_variance = math::max(sq(0.01f), sq(_params.ev_vel_noise)); + + switch (ev_sample.vel_frame) { + case VelocityFrame::LOCAL_FRAME_NED: + if (_control_status.flags.yaw_align) { + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; + + } else { + continuing_conditions_passing = false; + } + + break; + + case VelocityFrame::LOCAL_FRAME_FRD: + if (_control_status.flags.ev_yaw) { + // using EV frame + measurement = ev_sample.vel - vel_offset_earth; + measurement_var = ev_sample.velocity_var; + + } else { + // rotate EV to the EKF reference frame + const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); + + measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; + measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * + R_ev_to_ekf.transpose()).diag(); + minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max()); + } + + break; + + case VelocityFrame::BODY_FRAME_FRD: { + + // currently it is assumed that the orientation of the EV frame and the body frame are the same + measurement = ev_sample.vel - vel_offset_body; + measurement_var = ev_sample.velocity_var; + break; + } + + default: + continuing_conditions_passing = false; + break; + } + +#if defined(CONFIG_EKF2_GNSS) + + // increase minimum variance if GPS active (position reference) + if (_control_status.flags.gps) { + for (int i = 0; i < 2; i++) { + measurement_var(i) = math::max(measurement_var(i), sq(_params.gps_vel_noise)); + } + } + +#endif // CONFIG_EKF2_GNSS + + measurement_var = Vector3f{ + math::max(measurement_var(0), minimum_variance), + math::max(measurement_var(1), minimum_variance), + math::max(measurement_var(2), minimum_variance) + }; + continuing_conditions_passing &= measurement.isAllFinite() && measurement_var.isAllFinite(); + + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + const Vector3f measurement_ekf_frame = _R_to_earth * measurement; + const uint64_t t = aid_src.timestamp_sample; + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement_ekf_frame, // observation + measurement_var_ekf_frame, // observation variance + _state.vel - measurement_ekf_frame, // innovation + getVelocityVariance() + measurement_var_ekf_frame, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + aid_src.timestamp_sample = t; + measurement.copyTo(aid_src.observation); + measurement_var.copyTo(aid_src.observation_variance); + + } else { + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + measurement, // observation + measurement_var, // observation variance + _state.vel - measurement, // innovation + getVelocityVariance() + measurement_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + } + + + const bool starting_conditions_passing = common_starting_conditions_passing + && continuing_conditions_passing + && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); + + if (_control_status.flags.ev_vel) { + if (continuing_conditions_passing) { + if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { + if (quality_sufficient) { + ECL_INFO("reset to %s", AID_SRC_NAME); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + resetAidSourceStatusZeroInnovation(aid_src); + + } else { + // EV has reset, but quality isn't sufficient + // we have no choice but to stop EV and try to resume once quality is acceptable + stopEvVelFusion(); + return; + } + + } else if (quality_sufficient) { + fuseEvVelocity(aid_src, ev_sample); + + } else { + aid_src.innovation_rejected = true; + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second + + if (is_fusion_failing) { + + if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) { + // Data seems good, attempt a reset + _information_events.flags.reset_vel_to_vision = true; + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + resetAidSourceStatusZeroInnovation(aid_src); + + if (_control_status.flags.in_air) { + _nb_ev_vel_reset_available--; + } + + } else { + // differ warning message based on whether the starting conditions are passing + if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + //_control_status.flags.ev_vel_fault = true; + ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + } + + stopEvVelFusion(); + } + + } else if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_ekf_frame = _R_to_earth * measurement; + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2)); + + } else { + resetVerticalVelocityTo(measurement(2), measurement_var(2)); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); + stopEvVelFusion(); + } + + } else { + if (starting_conditions_passing) { + // activate fusion, only reset if necessary + if (!isHorizontalAidingActive() || yaw_alignment_changed) { + ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, + (double)measurement(0), (double)measurement(1), (double)measurement(2)); + _information_events.flags.reset_vel_to_vision = true; + resetVelocityToEV(measurement, measurement_var, ev_sample.vel_frame); + resetAidSourceStatusZeroInnovation(aid_src); + + _control_status.flags.ev_vel = true; + + } else if (fuseEvVelocity(aid_src, ev_sample)) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + _control_status.flags.ev_vel = true; + } + + if (_control_status.flags.ev_vel) { + _nb_ev_vel_reset_available = 5; + _information_events.flags.starting_vision_vel_fusion = true; + } + } + } +} + +bool Ekf::fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample) +{ + if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) { + + VectorState H; + estimator_aid_source1d_s current_aid_src; + const auto state_vector = _state.vector(); + + for (uint8_t index = 0; index <= 2; index++) { + current_aid_src.timestamp_sample = aid_src.timestamp_sample; + + if (index == 0) { + sym::ComputeEvBodyVelHx(state_vector, &H); + + } else if (index == 1) { + sym::ComputeEvBodyVelHy(state_vector, &H); + + } else { + sym::ComputeEvBodyVelHz(state_vector, &H); + } + + const float innov_var = (H.T() * P * H)(0, 0) + aid_src.observation_variance[index]; + const float innov = (_R_to_earth.transpose() * _state.vel - Vector3f(aid_src.observation))(index, 0); + + updateAidSourceStatus(current_aid_src, + ev_sample.time_us, // sample timestamp + aid_src.observation[index], // observation + aid_src.observation_variance[index], // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate + + if (!current_aid_src.innovation_rejected) { + fuseBodyVelocity(current_aid_src, current_aid_src.innovation_variance, H); + + } + + aid_src.innovation[index] = current_aid_src.innovation; + aid_src.innovation_variance[index] = current_aid_src.innovation_variance; + aid_src.test_ratio[index] = current_aid_src.test_ratio; + aid_src.fused = current_aid_src.fused; + aid_src.innovation_rejected |= current_aid_src.innovation_rejected; + + if (aid_src.fused) { + aid_src.time_last_fuse = _time_delayed_us; + } + + } + + if (aid_src.fused) { + _time_last_hor_vel_fuse = _time_delayed_us; + _time_last_ver_vel_fuse = _time_delayed_us; + } + + aid_src.timestamp_sample = current_aid_src.timestamp_sample; + return !aid_src.innovation_rejected; + + } else { + return fuseVelocity(aid_src); + } +} + +void Ekf::stopEvVelFusion() +{ + if (_control_status.flags.ev_vel) { + + _control_status.flags.ev_vel = false; + } +} + +void Ekf::resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, + const VelocityFrame &vel_frame) +{ + if (vel_frame == VelocityFrame::BODY_FRAME_FRD) { + const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var); + resetVelocityTo(_R_to_earth * measurement, measurement_var_ekf_frame); + + } else { + resetVelocityTo(measurement, measurement_var); + } + +} + +Vector3f Ekf::rotateVarianceToEkf(const Vector3f &measurement_var) +{ + // rotate the covariance matrix into the EKF frame + const matrix::SquareMatrix R_cov = _R_to_earth * matrix::diag(measurement_var) * _R_to_earth.transpose(); + return R_cov.diag(); +} diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp similarity index 81% rename from src/modules/ekf2/EKF/ev_yaw_control.cpp rename to src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp index 3afb3b78fced..ad15c4f6bb33 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp @@ -38,16 +38,28 @@ #include "ekf.h" -void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src) +void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src) { static constexpr const char *AID_SRC_NAME = "EV yaw"; - resetEstimatorAidStatus(aid_src); - aid_src.timestamp_sample = ev_sample.time_us; - aid_src.observation = getEulerYaw(ev_sample.quat); - aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f)); - aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation); + float obs = getEulerYaw(ev_sample.quat); + float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f)); + + float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs); + float innov_var = 0.f; + + VectorState H_YAW; + computeYawInnovVarAndH(obs_var, innov_var, H_YAW); + + updateAidSourceStatus(aid_src, + ev_sample.time_us, // sample timestamp + obs, // observation + obs_var, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate if (ev_reset) { _control_status.flags.ev_yaw_fault = false; @@ -65,10 +77,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED) ) { continuing_conditions_passing = false; - - // use delta yaw for innovation logging - aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev) - - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat))); } const bool starting_conditions_passing = common_starting_conditions_passing @@ -94,7 +102,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseYaw(aid_src); + fuseYaw(aid_src, H_YAW); } else { aid_src.innovation_rejected = true; @@ -141,18 +149,25 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) { if (_control_status.flags.yaw_align) { - ECL_INFO("starting %s fusion", AID_SRC_NAME); + + if (fuseYaw(aid_src, H_YAW)) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + _information_events.flags.starting_vision_yaw_fusion = true; + + _control_status.flags.ev_yaw = true; + } } else { // reset yaw to EV and set yaw_align ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); + _information_events.flags.starting_vision_yaw_fusion = true; + resetQuatStateYaw(aid_src.observation, aid_src.observation_variance); _control_status.flags.yaw_align = true; - } + _control_status.flags.ev_yaw = true; - aid_src.time_last_fuse = _time_delayed_us; - _information_events.flags.starting_vision_yaw_fusion = true; - _control_status.flags.ev_yaw = true; + aid_src.time_last_fuse = _time_delayed_us; + } } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { // turn on fusion of external vision yaw measurements @@ -177,10 +192,11 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common void Ekf::stopEvYawFusion() { #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { - resetEstimatorAidStatus(_aid_src_ev_yaw); _control_status.flags.ev_yaw = false; } + #endif // CONFIG_EKF2_EXTERNAL_VISION } diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp similarity index 90% rename from src/modules/ekf2/EKF/fake_height_control.cpp rename to src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index 8b7eba566413..d9635c8e4aa6 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -51,8 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src); - + updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate); const bool continuing_conditions_passing = !isVerticalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing @@ -64,7 +63,13 @@ void Ekf::controlFakeHgtFusion() // always protect against extreme values that could result in a NaN if (aid_src.test_ratio < sq(100.0f / innov_gate)) { - fuseVerticalPosition(aid_src); + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, + State::pos.idx + 2) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + } } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); @@ -113,7 +118,5 @@ void Ekf::stopFakeHgtFusion() if (_control_status.flags.fake_hgt) { ECL_INFO("stop fake height fusion"); _control_status.flags.fake_hgt = false; - - resetEstimatorAidStatus(_aid_src_fake_hgt); } } diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp similarity index 74% rename from src/modules/ekf2/EKF/fake_pos_control.cpp rename to src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index cb3d246f235b..df7257d9b038 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -63,26 +63,39 @@ void Ekf::controlFakePosFusion() obs_var(0) = obs_var(1) = sq(0.5f); } - const float innov_gate = 3.f; - - updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src); + const Vector2f position(_last_known_pos); + const float innov_gate = 3.f; - const bool continuing_conditions_passing = !isHorizontalAidingActive() - && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) - && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest); + updateAidSourceStatus(aid_src, + _time_delayed_us, + position, // observation + obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate - const bool starting_conditions_passing = continuing_conditions_passing - && _horizontal_deadreckon_time_exceeded; + const bool enable_conditions_passing = !isHorizontalAidingActive() + && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest) + && (!(_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest) + && _horizontal_deadreckon_time_exceeded; if (_control_status.flags.fake_pos) { - if (continuing_conditions_passing) { + if (enable_conditions_passing) { // always protect against extreme values that could result in a NaN if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate)) && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) ) { - fuseHorizontalPosition(aid_src); + if (!aid_src.innovation_rejected + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::pos.idx + 1) + ) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + } } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); @@ -97,14 +110,13 @@ void Ekf::controlFakePosFusion() } } else { - if (starting_conditions_passing) { + if (enable_conditions_passing) { ECL_INFO("start fake position fusion"); _control_status.flags.fake_pos = true; resetFakePosFusion(); if (_control_status.flags.tilt_align) { // The fake position fusion is not started for initial alignement - _warning_events.flags.stopping_navigation = true; ECL_WARN("stopping navigation"); } } @@ -131,7 +143,5 @@ void Ekf::stopFakePosFusion() if (_control_status.flags.fake_pos) { ECL_INFO("stop fake position fusion"); _control_status.flags.fake_pos = false; - - resetEstimatorAidStatus(_aid_src_fake_pos); } } diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp similarity index 88% rename from src/modules/ekf2/EKF/gnss_height_control.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index aafefd8bff1c..91a8685cebae 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -67,16 +67,14 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const float measurement = gnss_alt - getEkfGlobalOriginAltitude(); const float measurement_var = sq(noise); - const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f); - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); // GNSS position, vertical position GNSS measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(gps_sample.time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); + updateVerticalPositionAidStatus(aid_src, + gps_sample.time_us, + -(measurement - bias_est.getBias()), + measurement_var + bias_est.getBiasVar(), + math::max(_params.gps_pos_innov_gate, 1.f)); // update the bias estimator before updating the main filter but after // using its current state to compute the vertical position innovation @@ -102,23 +100,14 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - if (isHeightResetRequired()) { + if (isHeightResetRequired() && (_height_sensor_ref == HeightSensor::GNSS)) { // All height sources are failing ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var); + resetVerticalPositionTo(aid_src.observation, measurement_var); bias_est.setBias(_state.pos(2) + measurement); - // reset vertical velocity - if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & static_cast(GnssCtrl::VEL))) { - // use 1.5 as a typical ratio of vacc/hacc - resetVerticalVelocityTo(gps_sample.vel(2), sq(math::max(1.5f * gps_sample.sacc, _params.gps_vel_noise))); - - } else { - resetVerticalVelocityToZero(); - } - aid_src.time_last_fuse = _time_delayed_us; } else if (is_fusion_failing) { @@ -171,7 +160,6 @@ void Ekf::stopGpsHgtFusion() } _gps_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_gnss_hgt); _control_status.flags.gps_hgt = false; } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp new file mode 100644 index 000000000000..456691635a00 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_yaw_control.cpp @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (c) 2018-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 gnss_yaw_control.cpp + * Definition of functions required to use yaw obtained from GNSS dual antenna measurements. + * Equations generated using src/modules/ekf2/EKF/python/ekf_derivation/derivation.py + * + */ + +#include "ekf.h" + +#include +#include + +#include + +void Ekf::controlGnssYawFusion(const gnssSample &gnss_sample) +{ + if (!(_params.gnss_ctrl & static_cast(GnssCtrl::YAW)) + || _control_status.flags.gnss_yaw_fault) { + + stopGnssYawFusion(); + return; + } + + const bool is_new_data_available = PX4_ISFINITE(gnss_sample.yaw); + + if (is_new_data_available) { + + updateGnssYaw(gnss_sample); + + const bool continuing_conditions_passing = _control_status.flags.tilt_align; + + const bool is_gnss_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, + 2 * GNSS_YAW_MAX_INTERVAL); + + const bool starting_conditions_passing = continuing_conditions_passing + && _gps_checks_passed + && !is_gnss_yaw_data_intermittent + && !_gps_intermittent; + + if (_control_status.flags.gnss_yaw) { + if (continuing_conditions_passing) { + + fuseGnssYaw(gnss_sample.yaw_offset); + + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); + + if (is_fusion_failing) { + stopGnssYawFusion(); + + // Before takeoff, we do not want to continue to rely on the current heading + // if we had to stop the fusion + if (!_control_status.flags.in_air) { + ECL_INFO("clearing yaw alignment"); + _control_status.flags.yaw_align = false; + } + } + + } else { + // Stop GNSS yaw fusion but do not declare it faulty + stopGnssYawFusion(); + } + + } else { + if (starting_conditions_passing) { + // Try to activate GNSS yaw fusion + const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos; + + if (!_control_status.flags.in_air + || !_control_status.flags.yaw_align + || not_using_ne_aiding) { + + // Reset before starting the fusion + if (resetYawToGnss(gnss_sample.yaw, gnss_sample.yaw_offset)) { + + resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw); + + _control_status.flags.gnss_yaw = true; + _control_status.flags.yaw_align = true; + } + + } else if (!_aid_src_gnss_yaw.innovation_rejected) { + // Do not force a reset but wait for the consistency check to pass + _control_status.flags.gnss_yaw = true; + fuseGnssYaw(gnss_sample.yaw_offset); + } + + if (_control_status.flags.gnss_yaw) { + ECL_INFO("starting GNSS yaw fusion"); + } + } + } + + } else if (_control_status.flags.gnss_yaw + && !isNewestSampleRecent(_time_last_gnss_yaw_buffer_push, _params.reset_timeout_max)) { + + // No yaw data in the message anymore. Stop until it comes back. + stopGnssYawFusion(); + } +} + +void Ekf::updateGnssYaw(const gnssSample &gnss_sample) +{ + // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement + const float measured_hdg = wrap_pi(gnss_sample.yaw + gnss_sample.yaw_offset); + + const float yaw_acc = PX4_ISFINITE(gnss_sample.yaw_acc) ? gnss_sample.yaw_acc : 0.f; + const float R_YAW = sq(fmaxf(yaw_acc, _params.gnss_heading_noise)); + + float heading_pred; + float heading_innov_var; + + VectorState H; + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gnss_sample.yaw_offset, R_YAW, FLT_EPSILON, + &heading_pred, &heading_innov_var, &H); + + updateAidSourceStatus(_aid_src_gnss_yaw, + gnss_sample.time_us, // sample timestamp + measured_hdg, // observation + R_YAW, // observation variance + wrap_pi(heading_pred - measured_hdg), // innovation + heading_innov_var, // innovation variance + math::max(_params.heading_innov_gate, 1.f)); // innovation gate +} + +void Ekf::fuseGnssYaw(float antenna_yaw_offset) +{ + auto &aid_src = _aid_src_gnss_yaw; + + if (aid_src.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + return; + } + + if (!PX4_ISFINITE(antenna_yaw_offset)) { + antenna_yaw_offset = 0.f; + } + + float heading_pred; + float heading_innov_var; + VectorState H; + + // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H + // making a separate function just for H uses more flash space without reducing CPU load significantly + sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, aid_src.observation_variance, FLT_EPSILON, + &heading_pred, &heading_innov_var, &H); + + // check if the innovation variance calculation is badly conditioned + if (aid_src.innovation_variance < aid_src.observation_variance) { + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.flags.bad_hdg = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + ECL_ERR("GNSS yaw numerical error - covariance reset"); + stopGnssYawFusion(); + return; + } + + _fault_status.flags.bad_hdg = false; + _innov_check_fail_status.flags.reject_yaw = false; + + if ((fabsf(aid_src.test_ratio_filtered) > 0.2f) + && !_control_status.flags.in_air && isTimedOut(aid_src.time_last_fuse, (uint64_t)1e6) + ) { + // A constant large signed test ratio is a sign of wrong gyro bias + // Reset the yaw gyro variance to converge faster and avoid + // being stuck on a previous bad estimate + resetGyroBiasZCov(); + } + + // calculate the Kalman gains + // only calculate gains for states we are using + VectorState Kfusion = P * H / aid_src.innovation_variance; + + const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation); + _fault_status.flags.bad_hdg = !is_fused; + aid_src.fused = is_fused; + + if (is_fused) { + _time_last_heading_fuse = _time_delayed_us; + aid_src.time_last_fuse = _time_delayed_us; + } +} + +bool Ekf::resetYawToGnss(const float gnss_yaw, const float gnss_yaw_offset) +{ + // define the predicted antenna array vector and rotate into earth frame + const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f}; + const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; + + // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return false; + } + + // GNSS yaw measurement is already compensated for antenna offset in the driver + const float measured_yaw = gnss_yaw; + + const float yaw_variance = sq(fmaxf(_params.gnss_heading_noise, 1.e-2f)); + resetQuatStateYaw(measured_yaw, yaw_variance); + + return true; +} + +void Ekf::stopGnssYawFusion() +{ + if (_control_status.flags.gnss_yaw) { + + _control_status.flags.gnss_yaw = false; + + ECL_INFO("stopping GNSS yaw fusion"); + } +} diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp similarity index 77% rename from src/modules/ekf2/EKF/gps_checks.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 71018df1c708..94fa6f04f381 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -48,15 +48,16 @@ #include // GPS pre-flight check bit locations -#define MASK_GPS_NSATS (1<<0) -#define MASK_GPS_PDOP (1<<1) -#define MASK_GPS_HACC (1<<2) -#define MASK_GPS_VACC (1<<3) -#define MASK_GPS_SACC (1<<4) -#define MASK_GPS_HDRIFT (1<<5) -#define MASK_GPS_VDRIFT (1<<6) -#define MASK_GPS_HSPD (1<<7) -#define MASK_GPS_VSPD (1<<8) +#define MASK_GPS_NSATS (1<<0) +#define MASK_GPS_PDOP (1<<1) +#define MASK_GPS_HACC (1<<2) +#define MASK_GPS_VACC (1<<3) +#define MASK_GPS_SACC (1<<4) +#define MASK_GPS_HDRIFT (1<<5) +#define MASK_GPS_VDRIFT (1<<6) +#define MASK_GPS_HSPD (1<<7) +#define MASK_GPS_VSPD (1<<8) +#define MASK_GPS_SPOOFED (1<<9) void Ekf::collect_gps(const gnssSample &gps) { @@ -97,40 +98,8 @@ void Ekf::collect_gps(const gnssSample &gps) const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) { - - // If we have good GPS data set the origin's WGS-84 position to the last gps fix - const double lat = gps.lat; - -#if defined(CONFIG_EKF2_MAGNETOMETER) - const double lon = gps.lon; - - // set the magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = get_mag_declination_radians(lat, lon); - const float mag_inclination_gps = get_mag_inclination_radians(lat, lon); - const float mag_strength_gps = get_mag_strength_gauss(lat, lon); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - - const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); - const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); - - if ((_wmm_gps_time_last_set == 0) - || !PX4_ISFINITE(_mag_declination_gps) - || !PX4_ISFINITE(_mag_inclination_gps) - || !PX4_ISFINITE(_mag_strength_gps) - || mag_declination_changed - || mag_inclination_changed - ) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } - } -#endif // CONFIG_EKF2_MAGNETOMETER - - _earth_rate_NED = calcEarthRateNED((float)math::radians(lat)); + updateWmm(gps.lat, gps.lon); + _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } _wmm_gps_time_last_checked = _time_delayed_us; @@ -139,6 +108,8 @@ void Ekf::collect_gps(const gnssSample &gps) bool Ekf::runGnssChecks(const gnssSample &gps) { + _gps_check_fail_status.flags.spoofed = gps.spoofed; + // Check the fix type _gps_check_fail_status.flags.fix = (gps.fix_type < 3); @@ -157,7 +128,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Calculate time lapsed since last update, limit to prevent numerical errors and calculate a lowpass filter coefficient constexpr float filt_time_const = 10.0f; - const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) * 1e-6f, 0.001f, filt_time_const); + const float dt = math::constrain(float(int64_t(gps.time_us) - int64_t(_gps_pos_prev.getProjectionReferenceTimestamp())) + * 1e-6f, 0.001f, filt_time_const); const float filter_coef = dt / filt_time_const; // The following checks are only valid when the vehicle is at rest @@ -187,15 +159,15 @@ bool Ekf::runGnssChecks(const gnssSample &gps) // Apply a low pass filter _gps_pos_deriv_filt = pos_derived * filter_coef + _gps_pos_deriv_filt * (1.0f - filter_coef); - // Calculate the horizontal drift speed and fail if too high + // hdrift: calculate the horizontal drift speed and fail if too high _gps_horizontal_position_drift_rate_m_s = Vector2f(_gps_pos_deriv_filt.xy()).norm(); _gps_check_fail_status.flags.hdrift = (_gps_horizontal_position_drift_rate_m_s > _params.req_hdrift); - // Fail if the vertical drift speed is too high + // vdrift: fail if the vertical drift speed is too high _gps_vertical_position_drift_rate_m_s = fabsf(_gps_pos_deriv_filt(2)); _gps_check_fail_status.flags.vdrift = (_gps_vertical_position_drift_rate_m_s > _params.req_vdrift); - // Check the magnitude of the filtered horizontal GPS velocity + // hspeed: check the magnitude of the filtered horizontal GNSS velocity const Vector2f gps_velNE = matrix::constrain(Vector2f(gps.vel.xy()), -10.0f * _params.req_hdrift, 10.0f * _params.req_hdrift); @@ -203,12 +175,20 @@ bool Ekf::runGnssChecks(const gnssSample &gps) _gps_filtered_horizontal_velocity_m_s = _gps_velNE_filt.norm(); _gps_check_fail_status.flags.hspeed = (_gps_filtered_horizontal_velocity_m_s > _params.req_hdrift); + // vspeed: check the magnitude of the filtered vertical GNSS velocity + const float gnss_vz_limit = 10.f * _params.req_vdrift; + const float gnss_vz = math::constrain(gps.vel(2), -gnss_vz_limit, gnss_vz_limit); + _gps_vel_d_filt = gnss_vz * filter_coef + _gps_vel_d_filt * (1.f - filter_coef); + + _gps_check_fail_status.flags.vspeed = (fabsf(_gps_vel_d_filt) > _params.req_vdrift); + } else if (_control_status.flags.in_air) { // These checks are always declared as passed when flying // If on ground and moving, the last result before movement commenced is kept _gps_check_fail_status.flags.hdrift = false; _gps_check_fail_status.flags.vdrift = false; _gps_check_fail_status.flags.hspeed = false; + _gps_check_fail_status.flags.vspeed = false; resetGpsDriftCheckFilters(); @@ -221,12 +201,6 @@ bool Ekf::runGnssChecks(const gnssSample &gps) _gps_pos_prev.initReference(lat, lon, gps.time_us); _gps_alt_prev = gps.alt; - // Check the filtered difference between GPS and EKF vertical velocity - const float vz_diff_limit = 10.0f * _params.req_vdrift; - const float vertVel = math::constrain(gps.vel(2) - _state.vel(2), -vz_diff_limit, vz_diff_limit); - _gps_velD_diff_filt = vertVel * filter_coef + _gps_velD_diff_filt * (1.0f - filter_coef); - _gps_check_fail_status.flags.vspeed = (fabsf(_gps_velD_diff_filt) > _params.req_vdrift); - // assume failed first time through if (_last_gps_fail_us == 0) { _last_gps_fail_us = _time_delayed_us; @@ -243,7 +217,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps) (_gps_check_fail_status.flags.hdrift && (_params.gps_check_mask & MASK_GPS_HDRIFT)) || (_gps_check_fail_status.flags.vdrift && (_params.gps_check_mask & MASK_GPS_VDRIFT)) || (_gps_check_fail_status.flags.hspeed && (_params.gps_check_mask & MASK_GPS_HSPD)) || - (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) + (_gps_check_fail_status.flags.vspeed && (_params.gps_check_mask & MASK_GPS_VSPD)) || + (_gps_check_fail_status.flags.spoofed && (_params.gps_check_mask & MASK_GPS_SPOOFED)) ) { _last_gps_fail_us = _time_delayed_us; return false; @@ -257,6 +232,8 @@ bool Ekf::runGnssChecks(const gnssSample &gps) void Ekf::resetGpsDriftCheckFilters() { _gps_velNE_filt.setZero(); + _gps_vel_d_filt = 0.f; + _gps_pos_deriv_filt.setZero(); _gps_horizontal_position_drift_rate_m_s = NAN; diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp similarity index 70% rename from src/modules/ekf2/EKF/gps_control.cpp rename to src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index cf2f9eb7b454..7bfb2e283107 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -51,7 +51,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } // run EKF-GSF yaw estimator once per imu_delayed update - _yawEstimator.predict(imu_delayed, _control_status.flags.in_air && !_control_status.flags.vehicle_at_rest); + _yawEstimator.predict(imu_delayed.delta_ang, imu_delayed.delta_ang_dt, + imu_delayed.delta_vel, imu_delayed.delta_vel_dt, + (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest)); _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GNSS_MAX_INTERVAL); @@ -61,7 +63,8 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_gps_data_ready) { const gnssSample &gnss_sample = _gps_sample_delayed; - if (runGnssChecks(gnss_sample) && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us / 2)) { + if (runGnssChecks(gnss_sample) + && isTimedOut(_last_gps_fail_us, max((uint64_t)1e6, (uint64_t)_min_gps_health_time_us / 10))) { if (isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) { // First time checks are passing, latching. _gps_checks_passed = true; @@ -75,7 +78,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_control_status.flags.gps && isTimedOut(_last_gps_pass_us, _params.reset_timeout_max)) { stopGpsFusion(); - _warning_events.flags.gps_quality_poor = true; ECL_WARN("GPS quality poor - stopping use"); } } @@ -84,12 +86,11 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) updateGnssPos(gnss_sample, _aid_src_gnss_pos); } - updateGnssVel(gnss_sample, _aid_src_gnss_vel); + updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); } else if (_control_status.flags.gps) { if (!isNewestSampleRecent(_time_last_gps_buffer_push, _params.reset_timeout_max)) { stopGpsFusion(); - _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); } } @@ -97,7 +98,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (_gps_data_ready) { #if defined(CONFIG_EKF2_GNSS_YAW) const gnssSample &gnss_sample = _gps_sample_delayed; - controlGpsYawFusion(gnss_sample); + controlGnssYawFusion(gnss_sample); #endif // CONFIG_EKF2_GNSS_YAW controlGnssYawEstimator(_aid_src_gnss_vel); @@ -123,25 +124,31 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) bool do_vel_pos_reset = shouldResetGpsFusion(); - if (isYawFailure() - && _control_status.flags.in_air + if (_control_status.flags.in_air + && isYawFailure() && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay) && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) { do_vel_pos_reset = tryYawEmergencyReset(); } if (do_vel_pos_reset) { - ECL_WARN("GPS fusion timeout, resetting velocity / position"); + ECL_WARN("GPS fusion timeout, resetting"); + } - if (gnss_vel_enabled) { + if (gnss_vel_enabled) { + if (do_vel_pos_reset) { resetVelocityToGnss(_aid_src_gnss_vel); - } - if (gnss_pos_enabled) { - resetHorizontalPositionToGnss(_aid_src_gnss_pos); + } else if (isHeightResetRequired()) { + // reset vertical velocity if height is failing + resetVerticalVelocityTo(_aid_src_gnss_vel.observation[2], _aid_src_gnss_vel.observation_variance[2]); } } + if (gnss_pos_enabled && do_vel_pos_reset) { + resetHorizontalPositionToGnss(_aid_src_gnss_pos); + } + } else { stopGpsFusion(); } @@ -172,22 +179,36 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } -void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) +void Ekf::updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src) { // correct velocity for offset relative to IMU const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; + const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias; + const Vector3f vel_offset_body = angular_velocity % pos_offset_body; const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; const Vector3f velocity = gnss_sample.vel - vel_offset_earth; - const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise)); + const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f)); const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f)); - updateVelocityAidSrcStatus(gnss_sample.time_us, - velocity, // observation - vel_obs_var, // observation variance - math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate - aid_src); + + const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + velocity, // observation + vel_obs_var, // observation variance + _state.vel - velocity, // innovation + getVelocityVariance() + vel_obs_var, // innovation variance + innovation_gate); // innovation gate + + // vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable, + // but limit innovation to prevent spikes that could destabilise the filter + if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) { + const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]); + aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } } void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src) @@ -208,13 +229,16 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s } } - const float pos_var = sq(pos_noise); + const float pos_var = math::max(sq(pos_noise), sq(0.01f)); const Vector2f pos_obs_var(pos_var, pos_var); - updateHorizontalPositionAidSrcStatus(gnss_sample.time_us, - position, // observation - pos_obs_var, // observation variance - math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate - aid_src); + + updateAidSourceStatus(aid_src, + gnss_sample.time_us, // sample timestamp + position, // observation + pos_obs_var, // observation variance + Vector2f(_state.pos) - position, // innovation + Vector2f(getStateVariance()) + pos_obs_var, // innovation variance + math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel) @@ -246,7 +270,7 @@ bool Ekf::tryYawEmergencyReset() bool success = false; /* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously - * fails while the difference between the yaw emergency estimator and the yas estimate is large. + * fails while the difference between the yaw emergency estimator and the yaw estimate is large. * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was * present before flight to prevent triggering due to GPS glitches or other sensor errors. */ @@ -257,14 +281,12 @@ bool Ekf::tryYawEmergencyReset() // stop using the magnetometer in the main EKF otherwise its fusion could drag the yaw around // and cause another navigation failure _control_status.flags.mag_fault = true; - _warning_events.flags.emergency_yaw_reset_mag_stopped = true; } #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; + if (_control_status.flags.gnss_yaw) { + _control_status.flags.gnss_yaw_fault = true; } #endif // CONFIG_EKF2_GNSS_YAW @@ -287,7 +309,8 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) { _information_events.flags.reset_vel_to_gps = true; resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance)); - aid_src.time_last_fuse = _time_delayed_us; + + resetAidSourceStatusZeroInnovation(aid_src); } void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) @@ -295,7 +318,8 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty - aid_src.time_last_fuse = _time_delayed_us; + + resetAidSourceStatusZeroInnovation(aid_src); } bool Ekf::shouldResetGpsFusion() const @@ -318,7 +342,8 @@ bool Ekf::shouldResetGpsFusion() const #endif // CONFIG_EKF2_OPTICAL_FLOW const bool is_reset_required = has_horizontal_aiding_timed_out - || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max); + || (isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max) + && (_params.gnss_ctrl & static_cast(GnssCtrl::HPOS))); const bool is_inflight_nav_failure = _control_status.flags.in_air && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max) @@ -329,118 +354,11 @@ bool Ekf::shouldResetGpsFusion() const return (is_reset_required || is_inflight_nav_failure); } -#if defined(CONFIG_EKF2_GNSS_YAW) -void Ekf::controlGpsYawFusion(const gnssSample &gps_sample) -{ - if (!(_params.gnss_ctrl & static_cast(GnssCtrl::YAW)) - || _control_status.flags.gps_yaw_fault) { - - stopGpsYawFusion(); - return; - } - - updateGpsYaw(gps_sample); - - const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw); - - if (is_new_data_available) { - - const bool continuing_conditions_passing = _control_status.flags.tilt_align; - - const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, - 2 * GNSS_YAW_MAX_INTERVAL); - - const bool starting_conditions_passing = continuing_conditions_passing - && _gps_checks_passed - && !is_gps_yaw_data_intermittent - && !_gps_intermittent; - - if (_control_status.flags.gps_yaw) { - - if (continuing_conditions_passing) { - - fuseGpsYaw(gps_sample.yaw_offset); - - const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max); - - if (is_fusion_failing) { - if (_nb_gps_yaw_reset_available > 0) { - // Data seems good, attempt a reset - resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset); - - if (_control_status.flags.in_air) { - _nb_gps_yaw_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - _control_status.flags.gps_yaw_fault = true; - stopGpsYawFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - stopGpsYawFusion(); - } - - // TODO: should we give a new reset credit when the fusion does not fail for some time? - } - - } else { - // Stop GPS yaw fusion but do not declare it faulty - stopGpsYawFusion(); - } - - } else { - if (starting_conditions_passing) { - // Try to activate GPS yaw fusion - if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) { - ECL_INFO("starting GPS yaw fusion"); - - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; - _control_status.flags.gps_yaw = true; - _control_status.flags.yaw_align = true; - - _nb_gps_yaw_reset_available = 1; - } - } - } - - } else if (_control_status.flags.gps_yaw - && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) { - - // No yaw data in the message anymore. Stop until it comes back. - stopGpsYawFusion(); - } -} - -void Ekf::stopGpsYawFusion() -{ - if (_control_status.flags.gps_yaw) { - - _control_status.flags.gps_yaw = false; - resetEstimatorAidStatus(_aid_src_gnss_yaw); - - // Before takeoff, we do not want to continue to rely on the current heading - // if we had to stop the fusion - if (!_control_status.flags.in_air) { - ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment"); - _control_status.flags.yaw_align = false; - - } else { - ECL_INFO("stopping GPS yaw fusion"); - } - } -} -#endif // CONFIG_EKF2_GNSS_YAW - void Ekf::stopGpsFusion() { if (_control_status.flags.gps) { ECL_INFO("stopping GPS position and velocity fusion"); - resetEstimatorAidStatus(_aid_src_gnss_pos); - resetEstimatorAidStatus(_aid_src_gnss_vel); + _last_gps_fail_us = 0; _last_gps_pass_us = 0; @@ -449,7 +367,7 @@ void Ekf::stopGpsFusion() stopGpsHgtFusion(); #if defined(CONFIG_EKF2_GNSS_YAW) - stopGpsYawFusion(); + stopGnssYawFusion(); #endif // CONFIG_EKF2_GNSS_YAW _yawEstimator.reset(); diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp similarity index 85% rename from src/modules/ekf2/EKF/gravity_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp index 70a0a8ded200..e6961ed17c31 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp @@ -54,7 +54,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f; const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f; - const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) && (_accel_magnitude_filt < upper_accel_limit); + const bool accel_lpf_norm_good = (_accel_magnitude_filt > lower_accel_limit) + && (_accel_magnitude_filt < upper_accel_limit); // fuse gravity observation if our overall acceleration isn't too big _control_status.flags.gravity_vector = (_params.imu_ctrl & static_cast(ImuCtrl::GravityVector)) @@ -69,19 +70,13 @@ void Ekf::controlGravityFusion(const imuSample &imu) sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H); // fill estimator aid source status - resetEstimatorAidStatus(_aid_src_gravity); - _aid_src_gravity.timestamp_sample = imu.time_us; - measurement.copyTo(_aid_src_gravity.observation); - - for (auto &var : _aid_src_gravity.observation_variance) { - var = measurement_var; - } - - innovation.copyTo(_aid_src_gravity.innovation); - innovation_variance.copyTo(_aid_src_gravity.innovation_variance); - - float innovation_gate = 0.25f; - setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); + updateAidSourceStatus(_aid_src_gravity, + imu.time_us, // sample timestamp + measurement, // observation + Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance + innovation, // innovation + innovation_variance, // innovation variance + 0.25f); // innovation gate bool fused[3] {false, false, false}; @@ -96,14 +91,16 @@ void Ekf::controlGravityFusion(const imuSample &imu) sym::ComputeGravityYInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, + -1.f))(index) - measurement(index); } else if (index == 2) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) sym::ComputeGravityZInnovVarAndH(state_vector, P, measurement_var, &_aid_src_gravity.innovation_variance[index], &H); // recalculate innovation using the updated state - _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, -1.f))(index) - measurement(index); + _aid_src_gravity.innovation[index] = _state.quat_nominal.rotateVectorInverse(Vector3f(0.f, 0.f, + -1.f))(index) - measurement(index); } VectorState K = P * H / _aid_src_gravity.innovation_variance[index]; @@ -111,7 +108,8 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); + fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], + _aid_src_gravity.innovation[index]); } } diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp similarity index 53% rename from src/modules/ekf2/EKF/mag_control.cpp rename to src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index b491228c3999..0e42006b249d 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -39,36 +39,30 @@ #include "ekf.h" #include -void Ekf::controlMagFusion() +#include + +void Ekf::controlMagFusion(const imuSample &imu_sample) { + static constexpr const char *AID_SRC_NAME = "mag"; + estimator_aid_source3d_s &aid_src = _aid_src_mag; + // reset the flight alignment flag so that the mag fields will be // re-initialised next time we achieve flight altitude if (!_control_status_prev.flags.in_air && _control_status.flags.in_air) { _control_status.flags.mag_aligned_in_flight = false; } - checkYawAngleObservability(); - checkMagHeadingConsistency(); - if (_params.mag_fusion_type == MagFuseType::NONE) { stopMagFusion(); - stopMagHdgFusion(); return; } - // stop mag (require a reset before using again) if there was an external yaw reset (yaw estimator, GPS yaw, etc) - if (_mag_decl_cov_reset && (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat)) { - ECL_INFO("yaw reset, stopping mag fusion to force reinitialization"); - stopMagFusion(); - resetMagCov(); - } - magSample mag_sample; - if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { + if (_mag_buffer && _mag_buffer->pop_first_older_than(imu_sample.time_us, &mag_sample)) { if (mag_sample.reset || (_mag_counter == 0)) { - // sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion) + // sensor or calibration has changed, reset low pass filter _control_status.flags.mag_fault = false; _state.mag_B.zero(); @@ -82,13 +76,6 @@ void Ekf::controlMagFusion() _mag_counter++; } - const bool starting_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) - && checkMagField(mag_sample.mag) - && (_mag_counter > 5) // wait until we have more than a few samples through the filter - && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame - && (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset - && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); - // if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value. // this is useful if there is a lot of interference on the sensor measurement. if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) @@ -105,19 +92,222 @@ void Ekf::controlMagFusion() _control_status.flags.synthetic_mag_z = false; } - controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag); - controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading); + // reset flags + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + + // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations + const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); + + // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains + Vector3f mag_innov; + Vector3f innov_var; + + // Observation jacobian and Kalman gain vectors + VectorState H; + sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); + + updateAidSourceStatus(aid_src, + mag_sample.time_us, // sample timestamp + mag_sample.mag, // observation + Vector3f(R_MAG, R_MAG, R_MAG), // observation variance + mag_innov, // innovation + innov_var, // innovation variance + math::max(_params.mag_innov_gate, 1.f)); // innovation gate + + // Perform an innovation consistency check and report the result + _innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f); + _innov_check_fail_status.flags.reject_mag_y = (aid_src.test_ratio[1] > 1.f); + _innov_check_fail_status.flags.reject_mag_z = (aid_src.test_ratio[2] > 1.f); + + // determine if we should use mag fusion + bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::INIT) + || (_params.mag_fusion_type == MagFuseType::AUTO) + || (_params.mag_fusion_type == MagFuseType::HEADING)) + && _control_status.flags.tilt_align + && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && mag_sample.mag.longerThan(0.f) + && mag_sample.mag.isAllFinite(); + + const bool starting_conditions_passing = continuing_conditions_passing + && checkMagField(mag_sample.mag) + && (_mag_counter > 3) // wait until we have more than a few samples through the filter + && (_control_status.flags.yaw_align == _control_status_prev.flags.yaw_align) // no yaw alignment change this frame + && (_state_reset_status.reset_count.quat == + _state_reset_count_prev.quat) // don't allow starting on same frame as yaw reset + && isNewestSampleRecent(_time_last_mag_buffer_push, MAG_MAX_INTERVAL); + + checkMagHeadingConsistency(mag_sample); + + // WMM update can occur on the last epoch, just after mag fusion + const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); + const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + + + { + const bool mag_consistent_or_no_ne_aiding = _control_status.flags.mag_heading_consistent || !using_ne_aiding; + const bool common_conditions_passing = _control_status.flags.mag + && ((_control_status.flags.yaw_align && mag_consistent_or_no_ne_aiding) + || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) + && !_control_status.flags.mag_fault + && !_control_status.flags.mag_field_disturbed + && !_control_status.flags.ev_yaw + && !_control_status.flags.gnss_yaw; + + _control_status.flags.mag_3D = common_conditions_passing + && (_params.mag_fusion_type == MagFuseType::AUTO) + && _control_status.flags.mag_aligned_in_flight; + + _control_status.flags.mag_hdg = common_conditions_passing + && ((_params.mag_fusion_type == MagFuseType::HEADING) + || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)); + } + + // TODO: allow clearing mag_fault if mag_3d is good? + + if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { + ECL_INFO("starting mag 3D fusion"); + + } else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) { + ECL_INFO("stopping mag 3D fusion"); + } + + // if we are using 3-axis magnetometer fusion, but without external NE aiding, + // then the declination must be fused as an observation to prevent long term heading drift + const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.in_air; + _control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff; + + if (_control_status.flags.mag) { + + if (continuing_conditions_passing && _control_status.flags.yaw_align) { + + if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) { + ECL_INFO("reset to %s", AID_SRC_NAME); + resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); + aid_src.time_last_fuse = imu_sample.time_us; + + } else { + // The normal sequence is to fuse the magnetometer data first before fusing + // declination angle at a higher uncertainty to allow some learning of + // declination angle over time. + const bool update_all_states = _control_status.flags.mag_3D || _control_status.flags.mag_hdg; + const bool update_tilt = _control_status.flags.mag_3D; + fuseMag(mag_sample.mag, R_MAG, H, aid_src, update_all_states, update_tilt); + + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + if (update_all_states && update_tilt) { + _fault_status.flags.bad_mag_x = (aid_src.innovation_variance[0] < aid_src.observation_variance[0]); + _fault_status.flags.bad_mag_y = (aid_src.innovation_variance[1] < aid_src.observation_variance[1]); + _fault_status.flags.bad_mag_z = (aid_src.innovation_variance[2] < aid_src.observation_variance[2]); + } + + if (_control_status.flags.mag_dec) { + fuseDeclination(0.5f); + } + } + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); + + if (is_fusion_failing) { + if (no_ne_aiding_or_pre_takeoff) { + ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); + resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); + aid_src.time_last_fuse = imu_sample.time_us; + + } else { + ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); + stopMagFusion(); + } + } + + } else { + // Stop fusion but do not declare it faulty + ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME); + stopMagFusion(); + } + + } else { + if (starting_conditions_passing) { + + // activate fusion, reset mag states and initialize variance if first init or in flight reset + if (!_control_status.flags.yaw_align + || wmm_updated + || !_state.mag_I.longerThan(0.f) + || (getStateVariance().min() < kMagVarianceMin) + || (getStateVariance().min() < kMagVarianceMin) + ) { + ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); + + bool reset_heading = !_control_status.flags.yaw_align; + + resetMagStates(_mag_lpf.getState(), reset_heading); + aid_src.time_last_fuse = imu_sample.time_us; + + if (reset_heading) { + _control_status.flags.yaw_align = true; + resetAidSourceStatusZeroInnovation(aid_src); + } + + _control_status.flags.mag = true; + + } else { + if (fuseMag(mag_sample.mag, R_MAG, H, aid_src)) { + ECL_INFO("starting %s fusion", AID_SRC_NAME); + _control_status.flags.mag = true; + } + } + } + } } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - stopMagHdgFusion(); stopMagFusion(); } } +void Ekf::stopMagFusion() +{ + if (_control_status.flags.mag) { + ECL_INFO("stopping mag fusion"); + + if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) { + // reset yaw alignment from mag unless using GNSS aiding + const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + + if (!using_ne_aiding) { + _control_status.flags.yaw_align = false; + } + } + + _control_status.flags.mag = false; + _control_status.flags.mag_dec = false; + + if (_control_status.flags.mag_3D) { + ECL_INFO("stopping mag 3D fusion"); + _control_status.flags.mag_3D = false; + } + + if (_control_status.flags.mag_hdg) { + ECL_INFO("stopping mag heading fusion"); + _control_status.flags.mag_hdg = false; + _fault_status.flags.bad_hdg = false; + } + + _control_status.flags.mag_aligned_in_flight = false; + + _fault_status.flags.bad_mag_x = false; + _fault_status.flags.bad_mag_y = false; + _fault_status.flags.bad_mag_z = false; + + _fault_status.flags.bad_mag_decl = false; + } +} + bool Ekf::checkHaglYawResetReq() const { #if defined(CONFIG_EKF2_TERRAIN) + // We need to reset the yaw angle after climbing away from the ground to enable // recovery from ground level magnetic interference. if (_control_status.flags.in_air && _control_status.flags.yaw_align && !_control_status.flags.mag_aligned_in_flight) { @@ -127,6 +317,7 @@ bool Ekf::checkHaglYawResetReq() const const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; } + #endif // CONFIG_EKF2_TERRAIN return false; @@ -149,21 +340,7 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) * Vector3f(_mag_strength_gps, 0, 0); // mag_B: reset -#if defined(CONFIG_EKF2_GNSS) - if (isYawEmergencyEstimateAvailable()) { - - const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); - const Dcmf R_to_body = R_to_earth.transpose(); - - // mag_B: reset using WMM and yaw estimator - _state.mag_B = mag - (R_to_body * mag_earth_pred); - - ECL_INFO("resetMagStates using yaw estimator"); - - } else if (!reset_heading && _control_status.flags.yaw_align) { -#else if (!reset_heading && _control_status.flags.yaw_align) { -#endif // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); _state.mag_B = mag - (R_to_body * mag_earth_pred); @@ -179,9 +356,6 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) resetMagHeading(mag); } - // earth field was reset to WMM, skip initial declination fusion - _mag_decl_cov_reset = true; - } else { // mag_B: reset _state.mag_B.zero(); @@ -222,27 +396,39 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) } } -void Ekf::checkYawAngleObservability() +void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) { - if (_control_status.flags.gps) { - // Check if there has been enough change in horizontal velocity to make yaw observable - // Apply hysteresis to check to avoid rapid toggling - if (_yaw_angle_observable) { - _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate; + // use mag bias if variance good + Vector3f mag_bias{0.f, 0.f, 0.f}; + const Vector3f mag_bias_var = getMagBiasVariance(); - } else { - _yaw_angle_observable = _accel_lpf_NE.norm() > _params.mag_acc_gate * 2.f; - } + if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) { + mag_bias = _state.mag_B; + } + + // calculate mag heading + // Rotate the measurements into earth frame using the zero yaw angle + const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); + + // the angle of the projection onto the horizontal gives the yaw angle + // calculate the yaw innovation and wrap to the interval between +-pi + const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias); + const float declination = getMagDeclination(); + const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination; + + if (_control_status.flags.yaw_align) { + const float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); + _mag_heading_innov_lpf.update(innovation); } else { - _yaw_angle_observable = false; + _mag_heading_innov_lpf.reset(0.f); } -} -void Ekf::checkMagHeadingConsistency() -{ if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { - if (_yaw_angle_observable) { + // Check if there has been enough change in horizontal velocity to make yaw observable + const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; + + if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) { // yaw angle must be observable to consider consistency _control_status.flags.mag_heading_consistent = true; } @@ -349,7 +535,6 @@ void Ekf::resetMagHeading(const Vector3f &mag) // update quaternion states and corresponding covarainces resetQuatStateYaw(yaw_new, yaw_new_variance); - _mag_heading_last_declination = declination; _time_last_heading_fuse = _time_delayed_us; @@ -374,4 +559,3 @@ float Ekf::getMagDeclination() // otherwise use the parameter value return math::radians(_params.mag_declination_deg); } - diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp new file mode 100644 index 000000000000..acd16578c54f --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp @@ -0,0 +1,210 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 heading_fusion.cpp + * Magnetometer fusion methods. + * Equations generated using EKF/python/ekf_derivation/main.py + * + * @author Roman Bast + * @author Paul Riseborough + * + */ + +#include "ekf.h" + +#include +#include + +#include + +#include + +bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, + bool update_all_states, bool update_tilt) +{ + // if any axis failed, abort the mag fusion + if (aid_src.innovation_rejected) { + return false; + } + + const auto state_vector = _state.vector(); + + bool fused[3] {false, false, false}; + + // update the states and covariance using sequential fusion of the magnetometer components + for (uint8_t index = 0; index <= 2; index++) { + // Calculate Kalman gains and observation jacobians + if (index == 0) { + // everything was already computed + + } else if (index == 1) { + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); + + // recalculate innovation using the updated state + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( + index); + + } else if (index == 2) { + // we do not fuse synthesized magnetomter measurements when doing 3D fusion + if (_control_status.flags.synthetic_mag_z) { + fused[2] = true; + continue; + } + + // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) + sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src.innovation_variance[index], &H); + + // recalculate innovation using the updated state + aid_src.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag( + index); + } + + if (aid_src.innovation_variance[index] < R_MAG) { + ECL_ERR("mag numerical error covariance reset"); + + // we need to re-initialise covariances and abort this fusion step + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + + return false; + } + + VectorState Kfusion = P * H / aid_src.innovation_variance[index]; + + if (update_all_states) { + if (!update_tilt) { + Kfusion(State::quat_nominal.idx + 0) = 0.f; + Kfusion(State::quat_nominal.idx + 1) = 0.f; + } + + } else { + // zero non-mag Kalman gains if not updating all states + + // copy mag_I and mag_B Kalman gains + const Vector3f K_mag_I = Kfusion.slice(State::mag_I.idx, 0); + const Vector3f K_mag_B = Kfusion.slice(State::mag_B.idx, 0); + + // zero all Kalman gains, then restore mag + Kfusion.setZero(); + Kfusion.slice(State::mag_I.idx, 0) = K_mag_I; + Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; + } + + if (measurementUpdate(Kfusion, H, aid_src.observation_variance[index], aid_src.innovation[index])) { + fused[index] = true; + } + } + + if (update_all_states) { + _fault_status.flags.bad_mag_x = !fused[0]; + _fault_status.flags.bad_mag_y = !fused[1]; + _fault_status.flags.bad_mag_z = !fused[2]; + } + + if (fused[0] && fused[1] && fused[2]) { + aid_src.fused = true; + aid_src.time_last_fuse = _time_delayed_us; + + if (update_all_states) { + _time_last_heading_fuse = _time_delayed_us; + } + + return true; + } + + return false; +} + +bool Ekf::fuseDeclination(float decl_sigma) +{ + float decl_measurement = NAN; + + if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) + && PX4_ISFINITE(_mag_declination_gps) + ) { + decl_measurement = _mag_declination_gps; + + } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) + && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) + ) { + decl_measurement = math::radians(_params.mag_declination_deg); + } + + if (PX4_ISFINITE(decl_measurement)) { + + // observation variance (rad**2) + const float R_DECL = sq(decl_sigma); + + VectorState H; + float decl_pred; + float innovation_variance; + + sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, + &H); + + const float innovation = wrap_pi(decl_pred - decl_measurement); + + if (innovation_variance < R_DECL) { + // variance calculation is badly conditioned + return false; + } + + // Calculate the Kalman gains + VectorState Kfusion = P * H / innovation_variance; + + const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); + + _fault_status.flags.bad_mag_decl = !is_fused; + + return is_fused; + } + + return false; +} + +float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted) +{ + // theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge + // of the earth magnetic field vector at the current location + const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); + + // calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component + const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2)); + + return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs; +} diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp new file mode 100644 index 000000000000..3299577a2875 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 optical_flow_control.cpp + * Control functions for optical flow fusion + */ + +#include "ekf.h" + +#include + +void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) +{ + if (!_flow_buffer || (_params.flow_ctrl != 1)) { + stopFlowFusion(); + return; + } + + VectorState H; + + // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon + if (_flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed)) { + + // flow gyro has opposite sign convention + _ref_body_rate = -(imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias()); + + // ensure valid flow sample gyro rate before proceeding + switch (static_cast(_params.flow_gyro_src)) { + default: + + /* FALLTHROUGH */ + case FlowGyroSource::Auto: + if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) { + _flow_sample_delayed.gyro_rate = _ref_body_rate; + } + + if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) { + // Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro + _flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2); + } + + break; + + case FlowGyroSource::Internal: + _flow_sample_delayed.gyro_rate = _ref_body_rate; + break; + } + + const flowSample &flow_sample = _flow_sample_delayed; + + const int32_t min_quality = _control_status.flags.in_air + ? _params.flow_qual_min + : _params.flow_qual_min_gnd; + + const bool is_quality_good = (flow_sample.quality >= min_quality); + + bool is_tilt_good = true; + +#if defined(CONFIG_EKF2_RANGE_FINDER) + is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); +#endif // CONFIG_EKF2_RANGE_FINDER + + calcOptFlowBodyRateComp(flow_sample); + + // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed + // correct for gyro bias errors in the data used to do the motion compensation + // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. + const Vector3f flow_gyro_corrected = flow_sample.gyro_rate - _flow_gyro_bias; + const Vector2f flow_compensated = flow_sample.flow_rate - flow_gyro_corrected.xy(); + + // calculate the optical flow observation variance + const float R_LOS = calcOptFlowMeasVar(flow_sample); + + const float epsilon = 1e-3f; + Vector2f innov_var; + sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, R_LOS, epsilon, &innov_var, &H); + + // run the innovation consistency check and record result + updateAidSourceStatus(_aid_src_optical_flow, + flow_sample.time_us, // sample timestamp + flow_compensated, // observation + Vector2f{R_LOS, R_LOS}, // observation variance + predictFlow(flow_gyro_corrected) - flow_compensated, // innovation + innov_var, // innovation variance + math::max(_params.flow_innov_gate, 1.f)); // innovation gate + + // logging + _flow_rate_compensated = flow_compensated; + + // compute the velocities in body and local frames from corrected optical flow measurement for logging only + const float range = predictFlowRange(); + _flow_vel_body(0) = -flow_compensated(1) * range; + _flow_vel_body(1) = flow_compensated(0) * range; + _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); + + + // Check if we are in-air and require optical flow to control position drift + bool is_flow_required = _control_status.flags.in_air + && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently + || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)); + + const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance); + + const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite() + && !flow_sample.flow_rate.longerThan(_flow_max_rate) + && !flow_compensated.longerThan(_flow_max_rate); + + const bool continuing_conditions_passing = (_params.flow_ctrl == 1) + && _control_status.flags.tilt_align + && is_within_sensor_dist; + + const bool starting_conditions_passing = continuing_conditions_passing + && is_quality_good + && is_magnitude_good + && is_tilt_good + && (isTerrainEstimateValid() || isHorizontalAidingActive()) + && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching + + // If the height is relative to the ground, terrain height cannot be observed. + _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); + + if (_control_status.flags.opt_flow) { + if (continuing_conditions_passing) { + + if (is_quality_good && is_magnitude_good && is_tilt_good) { + fuseOptFlow(H, _control_status.flags.opt_flow_terrain); + } + + // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period + if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) { + if (is_flow_required && is_quality_good && is_magnitude_good) { + resetFlowFusion(); + + if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) { + resetTerrainToFlow(); + } + + } else { + stopFlowFusion(); + } + } + + } else { + stopFlowFusion(); + } + + } else { + if (starting_conditions_passing) { + // If the height is relative to the ground, terrain height cannot be observed. + _control_status.flags.opt_flow_terrain = (_height_sensor_ref != HeightSensor::RANGE); + + if (isHorizontalAidingActive()) { + if (fuseOptFlow(H, _control_status.flags.opt_flow_terrain)) { + ECL_INFO("starting optical flow"); + _control_status.flags.opt_flow = true; + + } else if (_control_status.flags.opt_flow_terrain && !_control_status.flags.rng_terrain) { + ECL_INFO("starting optical flow, resetting terrain"); + resetTerrainToFlow(); + _control_status.flags.opt_flow = true; + } + + } else { + if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) { + ECL_INFO("starting optical flow, resetting"); + resetFlowFusion(); + _control_status.flags.opt_flow = true; + + } else if (_control_status.flags.opt_flow_terrain) { + ECL_INFO("starting optical flow, resetting terrain"); + resetTerrainToFlow(); + _control_status.flags.opt_flow = true; + } + } + + _control_status.flags.opt_flow_terrain = _control_status.flags.opt_flow && !(_height_sensor_ref == HeightSensor::RANGE); + } + } + + } else if (_control_status.flags.opt_flow && isTimedOut(_flow_sample_delayed.time_us, _params.reset_timeout_max)) { + stopFlowFusion(); + } +} + +void Ekf::resetFlowFusion() +{ + ECL_INFO("reset velocity to flow"); + _information_events.flags.reset_vel_to_flow = true; + resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed)); + + // reset position, estimate is relative to initial position in this mode, so we start with zero error + if (!_control_status.flags.in_air) { + ECL_INFO("reset position to zero"); + resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); + } + + resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); + + _innov_check_fail_status.flags.reject_optflow_X = false; + _innov_check_fail_status.flags.reject_optflow_Y = false; +} + +void Ekf::resetTerrainToFlow() +{ + ECL_INFO("reset hagl to flow"); + + // TODO: use the flow data + const float new_terrain = fmaxf(0.0f, _state.pos(2)); + const float delta_terrain = new_terrain - _state.terrain; + _state.terrain = new_terrain; + P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); + + resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); + + _innov_check_fail_status.flags.reject_optflow_X = false; + _innov_check_fail_status.flags.reject_optflow_Y = false; + + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_terrain; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_terrain; + } + + _state_reset_status.reset_count.hagl++; +} + +void Ekf::stopFlowFusion() +{ + if (_control_status.flags.opt_flow) { + ECL_INFO("stopping optical flow fusion"); + _control_status.flags.opt_flow = false; + _control_status.flags.opt_flow_terrain = false; + + _fault_status.flags.bad_optflow_X = false; + _fault_status.flags.bad_optflow_Y = false; + + _innov_check_fail_status.flags.reject_optflow_X = false; + _innov_check_fail_status.flags.reject_optflow_Y = false; + } +} + +void Ekf::calcOptFlowBodyRateComp(const flowSample &flow_sample) +{ + // calculate the bias estimate using a combined LPF and spike filter + _flow_gyro_bias = 0.99f * _flow_gyro_bias + + 0.01f * matrix::constrain(flow_sample.gyro_rate - _ref_body_rate, -0.1f, 0.1f); +} diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp similarity index 53% rename from src/modules/ekf2/EKF/optflow_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index 5dd257c7251d..e95df3dbc2d1 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -33,12 +33,6 @@ /** * @file optflow_fusion.cpp - * Function for fusing gps and baro measurements/ - * equations generated using EKF/python/ekf_derivation/main.py - * - * @author Paul Riseborough - * @author Siddharth Bharat Purohit - * */ #include "ekf.h" @@ -48,105 +42,52 @@ #include #include -void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) +bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) { - resetEstimatorAidStatus(aid_src); - aid_src.timestamp_sample = _flow_sample_delayed.time_us; - - const Vector2f vel_body = predictFlowVelBody(); - const float range = predictFlowRange(); - - // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed - // correct for gyro bias errors in the data used to do the motion compensation - // Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis. - const Vector2f opt_flow_rate = _flow_rate_compensated; - - // compute the velocities in body and local frames from corrected optical flow measurement for logging only - _flow_vel_body(0) = -opt_flow_rate(1) * range; - _flow_vel_body(1) = opt_flow_rate(0) * range; - _flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f)); - - aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis - aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis - - aid_src.innovation[0] = (vel_body(1) / range) - aid_src.observation[0]; - aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1]; - - // calculate the optical flow observation variance - const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); - aid_src.observation_variance[0] = R_LOS; - aid_src.observation_variance[1] = R_LOS; - - Vector2f innov_var; - VectorState H; - sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(aid_src.innovation_variance); - - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f)); -} - -void Ekf::fuseOptFlow() -{ - const float R_LOS = _aid_src_optical_flow.observation_variance[0]; - - // calculate the height above the ground of the optical flow camera. Since earth frame is NED - // a positive offset in earth frame leads to a smaller height above the ground. - float range = predictFlowRange(); - const auto state_vector = _state.vector(); - Vector2f innov_var; - VectorState H; - sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(_aid_src_optical_flow.innovation_variance); - - if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS) - || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - initialiseCovariance(); - return; - } - - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f)); - _innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f); _innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f); // if either axis fails we abort the fusion if (_aid_src_optical_flow.innovation_rejected) { - return; + return false; } bool fused[2] {false, false}; // fuse observation axes sequentially for (uint8_t index = 0; index <= 1; index++) { + + if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) { + // we need to reinitialise the covariance matrix and abort this fusion step + ECL_ERR("Opt flow error - covariance reset"); + initialiseCovariance(); + return false; + } + if (index == 0) { // everything was already computed above } else if (index == 1) { // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::ComputeFlowYInnovVarAndH(state_vector, P, range, R_LOS, FLT_EPSILON, &_aid_src_optical_flow.innovation_variance[1], &H); + const float R_LOS = _aid_src_optical_flow.observation_variance[1]; + const float epsilon = 1e-3f; + sym::ComputeFlowYInnovVarAndH(state_vector, P, R_LOS, epsilon, &_aid_src_optical_flow.innovation_variance[1], &H); // recalculate the innovation using the updated state - const Vector2f vel_body = predictFlowVelBody(); - range = predictFlowRange(); - _aid_src_optical_flow.innovation[1] = (-vel_body(0) / range) - _aid_src_optical_flow.observation[1]; - - if (_aid_src_optical_flow.innovation_variance[1] < R_LOS) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - initialiseCovariance(); - return; - } + const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; + _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1]; } VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; - if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) { + if (!update_terrain) { + Kfusion(State::terrain.idx) = 0.f; + } + + if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], + _aid_src_optical_flow.innovation[index])) { fused[index] = true; } } @@ -157,10 +98,20 @@ void Ekf::fuseOptFlow() if (fused[0] && fused[1]) { _aid_src_optical_flow.time_last_fuse = _time_delayed_us; _aid_src_optical_flow.fused = true; + + _time_last_hor_vel_fuse = _time_delayed_us; + + if (update_terrain) { + _time_last_terrain_fuse = _time_delayed_us; + } + + return true; } + + return false; } -float Ekf::predictFlowRange() +float Ekf::predictFlowRange() const { // calculate the sensor position relative to the IMU const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; @@ -170,30 +121,41 @@ float Ekf::predictFlowRange() // calculate the height above the ground of the optical flow camera. Since earth frame is NED // a positive offset in earth frame leads to a smaller height above the ground. - const float height_above_gnd_est = math::max(_terrain_vpos - _state.pos(2) - pos_offset_earth(2), fmaxf(_params.rng_gnd_clearance, 0.01f)); + const float height_above_gnd_est = getHagl() - pos_offset_earth(2); // calculate range from focal point to centre of image - return height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view + float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view + + // avoid the flow prediction singularity at range = 0 + if (fabsf(flow_range) < FLT_EPSILON) { + flow_range = signNoZero(flow_range) * FLT_EPSILON; + } + + return flow_range; } -Vector2f Ekf::predictFlowVelBody() +Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const { // calculate the sensor position relative to the IMU const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body; // calculate the velocity of the sensor relative to the imu in body frame - // Note: _flow_sample_delayed.gyro_rate is the negative of the body angular velocity, thus use minus sign - const Vector3f vel_rel_imu_body = Vector3f(-(_flow_sample_delayed.gyro_rate - _flow_gyro_bias)) % pos_offset_body; + // Note: flow gyro is the negative of the body angular velocity, thus use minus sign + const Vector3f vel_rel_imu_body = -flow_gyro % pos_offset_body; // calculate the velocity of the sensor in the earth frame const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body; // rotate into body frame - return _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy(); + const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy(); + + // calculate range from focal point to centre of image + const float range = predictFlowRange(); + + return Vector2f(vel_body(1) / range, -vel_body(0) / range); } -// calculate the measurement variance for the optical flow sensor (rad/sec)^2 -float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) +float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const { // calculate the observation noise variance - scaling noise linearly across flow quality range const float R_LOS_best = fmaxf(_params.flow_noise, 0.05f); diff --git a/src/modules/ekf2/EKF/Sensor.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp similarity index 99% rename from src/modules/ekf2/EKF/Sensor.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp index e1b83e708b4d..8134418fabf2 100644 --- a/src/modules/ekf2/EKF/Sensor.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/Sensor.hpp @@ -42,7 +42,7 @@ #ifndef EKF_SENSOR_HPP #define EKF_SENSOR_HPP -#include "common.h" +#include namespace estimator { diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp similarity index 88% rename from src/modules/ekf2/EKF/range_finder_consistency_check.cpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index 453da560b421..918ab523c0d6 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -35,9 +35,10 @@ * @file range_finder_consistency_check.cpp */ -#include "range_finder_consistency_check.hpp" +#include -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us) +void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, + bool horizontal_motion, uint64_t time_us) { if (horizontal_motion) { _time_last_horizontal_motion = time_us; @@ -55,7 +56,8 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt; _innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down - const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 + // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2 + const float var = 2.f * dist_bottom_var / (dt * dt); _innov_var = var + vz_var; const float normalized_innov_sq = (_innov * _innov) / _innov_var; @@ -84,8 +86,9 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) } } else { - if (_test_ratio < 1.f - && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) { + if ((_test_ratio < 1.f) + && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us) + ) { _is_kinematically_consistent = true; } } diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp similarity index 100% rename from src/modules/ekf2/EKF/range_finder_consistency_check.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp new file mode 100644 index 000000000000..0aafe8a1b00d --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -0,0 +1,336 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 range_height_control.cpp + * Control functions for ekf range finder height fusion + */ + +#include "ekf.h" +#include "ekf_derivation/generated/compute_hagl_innov_var.h" + +void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) +{ + static constexpr const char *HGT_SRC_NAME = "RNG"; + + bool rng_data_ready = false; + + if (_range_buffer) { + // Get range data from buffer and check validity + rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); + _range_sensor.setDataReadiness(rng_data_ready); + + // update range sensor angle parameters in case they have changed + _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); + + if (_range_sensor.isDataHealthy()) { + // correct the range data for position offset relative to the IMU + const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + + if (_control_status.flags.in_air) { + const bool horizontal_motion = _control_status.flags.fixed_wing + || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); + + const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); + const float var = sq(_params.range_noise) + dist_dependant_var; + + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), + P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us); + } + + } else { + // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements + // and are on the ground, then synthesise a measurement at the expected on ground value + if (!_control_status.flags.in_air + && _range_sensor.isRegularlySendingData() + && _range_sensor.isDataReady()) { + + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setValidity(true); // bypass the checks + } + } + + _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + + } else { + return; + } + + auto &aid_src = _aid_src_rng_hgt; + + if (rng_data_ready && _range_sensor.getSampleAddress()) { + + updateRangeHagl(aid_src); + const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); + + const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) + || (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) + && _control_status.flags.tilt_align + && measurement_valid + && _range_sensor.isDataHealthy() + && _rng_consistency_check.isKinematicallyConsistent(); + + const bool starting_conditions_passing = continuing_conditions_passing + && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) + && _range_sensor.isRegularlySendingData(); + + + const bool do_conditional_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) + && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) + && isConditionalRangeAidSuitable(); + + const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) + && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); + + if (_control_status.flags.rng_hgt) { + if (!(do_conditional_range_aid || do_range_aid)) { + ECL_INFO("stopping %s fusion", HGT_SRC_NAME); + stopRngHgtFusion(); + } + + } else { + if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { + if (do_conditional_range_aid) { + // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. + ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); + _height_sensor_ref = HeightSensor::RANGE; + + _control_status.flags.rng_hgt = true; + stopRngTerrFusion(); + + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + resetTerrainToRng(aid_src); + } + + } else if (do_range_aid) { + // Range finder is the primary height source, the ground is now the datum used + // to compute the local vertical position + ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + _height_sensor_ref = HeightSensor::RANGE; + + _information_events.flags.reset_hgt_to_rng = true; + resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance); + _state.terrain = 0.f; + _control_status.flags.rng_hgt = true; + stopRngTerrFusion(); + + aid_src.time_last_fuse = imu_sample.time_us; + } + + } else { + if (do_conditional_range_aid || do_range_aid) { + ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + _control_status.flags.rng_hgt = true; + + if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { + resetTerrainToRng(aid_src); + } + } + } + } + + if (_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) { + if (continuing_conditions_passing) { + + fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); + + const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); + + if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { + // All height sources are failing + ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + + _information_events.flags.reset_hgt_to_rng = true; + resetVerticalPositionTo(-(aid_src.observation - _state.terrain)); + + // reset vertical velocity if no valid sources available + if (!isVerticalVelocityAidingActive()) { + resetVerticalVelocityToZero(); + } + + aid_src.time_last_fuse = imu_sample.time_us; + + } else if (is_fusion_failing) { + // Some other height source is still working + if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) { + ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME); + stopRngHgtFusion(); + stopRngTerrFusion(); + + } else { + resetTerrainToRng(aid_src); + } + } + + } else { + ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); + stopRngHgtFusion(); + stopRngTerrFusion(); + } + + } else { + if (starting_conditions_passing) { + if (_control_status.flags.opt_flow_terrain) { + if (!aid_src.innovation_rejected) { + _control_status.flags.rng_terrain = true; + fuseHaglRng(aid_src, _control_status.flags.rng_hgt, _control_status.flags.rng_terrain); + } + + } else { + if (aid_src.innovation_rejected) { + resetTerrainToRng(aid_src); + } + + _control_status.flags.rng_terrain = true; + } + } + } + + } else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) + && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { + // No data anymore. Stop until it comes back. + ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME); + stopRngHgtFusion(); + stopRngTerrFusion(); + } +} + +void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) +{ + const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); + const float measurement_variance = getRngVar(); + + float innovation_variance; + sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance); + + const float innov_gate = math::max(_params.range_innov_gate, 1.f); + updateAidSourceStatus(aid_src, + _range_sensor.getSampleAddress()->time_us, // sample timestamp + measurement, // observation + measurement_variance, // observation variance + getHagl() - measurement, // innovation + innovation_variance, // innovation variance + innov_gate); // innovation gate + + // z special case if there is bad vertical acceleration data, then don't reject measurement, + // but limit innovation to prevent spikes that could destabilise the filter + if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { + const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); + aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); + aid_src.innovation_rejected = false; + } +} + +float Ekf::getRngVar() const +{ + return fmaxf( + P(State::pos.idx + 2, State::pos.idx + 2) + + sq(_params.range_noise) + + sq(_params.range_noise_scaler * _range_sensor.getRange()), + 0.f); +} + +void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) +{ + const float new_terrain = _state.pos(2) + aid_src.observation; + const float delta_terrain = new_terrain - _state.terrain; + + _state.terrain = new_terrain; + P.uncorrelateCovarianceSetVariance(State::terrain.idx, aid_src.observation_variance); + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_terrain; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_terrain; + } + + _state_reset_status.reset_count.hagl++; + + + aid_src.time_last_fuse = _time_delayed_us; +} + +bool Ekf::isConditionalRangeAidSuitable() +{ + // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching + // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice + float range_hagl_max = _params.max_hagl_for_range_aid; + float max_vel_xy = _params.max_vel_for_range_aid; + + const float hagl_test_ratio = _aid_src_rng_hgt.test_ratio; + + bool is_hagl_stable = (hagl_test_ratio < 1.f); + + if (!_control_status.flags.rng_hgt) { + range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; + max_vel_xy = 0.7f * _params.max_vel_for_range_aid; + is_hagl_stable = (hagl_test_ratio < 0.01f); + } + + const bool is_in_range = (getHagl() < range_hagl_max); + + bool is_below_max_speed = true; + + if (isHorizontalAidingActive()) { + is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy); + } + + return is_in_range && is_hagl_stable && is_below_max_speed; +} + +void Ekf::stopRngHgtFusion() +{ + if (_control_status.flags.rng_hgt) { + + if (_height_sensor_ref == HeightSensor::RANGE) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + + _control_status.flags.rng_hgt = false; + } +} + +void Ekf::stopRngTerrFusion() +{ + _control_status.flags.rng_terrain = false; +} diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp new file mode 100644 index 000000000000..c2fd63672003 --- /dev/null +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_fusion.cpp @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "ekf.h" +#include "ekf_derivation/generated/compute_hagl_h.h" + +bool Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain) +{ + if (aid_src.innovation_rejected) { + _innov_check_fail_status.flags.reject_hagl = true; + return false; + } + + VectorState H; + + sym::ComputeHaglH(&H); + + // calculate the Kalman gain + VectorState K = P * H / aid_src.innovation_variance; + + if (!update_terrain) { + K(State::terrain.idx) = 0.f; + } + + if (!update_height) { + const float k_terrain = K(State::terrain.idx); + K.zero(); + K(State::terrain.idx) = k_terrain; + } + + measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); + + // record last successful fusion event + _innov_check_fail_status.flags.reject_hagl = false; + + aid_src.time_last_fuse = _time_delayed_us; + aid_src.fused = true; + + if (update_terrain) { + _time_last_terrain_fuse = _time_delayed_us; + } + + return true; +} diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp similarity index 94% rename from src/modules/ekf2/EKF/sensor_range_finder.cpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp index c47cf0f74d00..359d10ca5242 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp @@ -38,20 +38,22 @@ * */ -#include "sensor_range_finder.hpp" +#include + +#include namespace estimator { namespace sensor { -void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth) +void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth) { updateSensorToEarthRotation(R_to_earth); updateValidity(current_time_us); } -void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth) +void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth) { // calculate 2,2 element of rotation matrix from sensor frame to earth frame // this is required for use of range finder and flow data @@ -114,7 +116,7 @@ inline bool SensorRangeFinder::isDataInRange() const void SensorRangeFinder::updateStuckCheck() { - if(!isStuckDetectorEnabled()){ + if (!isStuckDetectorEnabled()) { // Stuck detector disabled _is_stuck = false; return; diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp similarity index 93% rename from src/modules/ekf2/EKF/sensor_range_finder.hpp rename to src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp index ec642f8e98eb..f3c59be54135 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp @@ -42,6 +42,7 @@ #define EKF_SENSOR_RANGE_FINDER_HPP #include "Sensor.hpp" + #include namespace estimator @@ -49,6 +50,15 @@ namespace estimator namespace sensor { +struct rangeSample { + uint64_t time_us{}; ///< timestamp of the measurement (uSec) + float rng{}; ///< range (distance to ground) measurement (m) + int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. +}; + +static constexpr uint64_t RNG_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) + class SensorRangeFinder : public Sensor { public: diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp similarity index 76% rename from src/modules/ekf2/EKF/sideslip_fusion.cpp rename to src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp index c35501f241c9..193af436f329 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp @@ -49,8 +49,10 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) { - _control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing - && _control_status.flags.in_air && !_control_status.flags.fake_pos; + _control_status.flags.fuse_beta = _params.beta_fusion_enabled + && _control_status.flags.fixed_wing + && _control_status.flags.in_air + && !_control_status.flags.fake_pos; if (_control_status.flags.fuse_beta) { @@ -62,47 +64,40 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed) updateSideslip(_aid_src_sideslip); _innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected; - // If starting wind state estimation, reset the wind states and covariances before fusing any data - if (!_control_status.flags.wind) { - // activate the wind states + if (fuseSideslip(_aid_src_sideslip)) { _control_status.flags.wind = true; - // reset the timeout timers to prevent repeated resets - _aid_src_sideslip.time_last_fuse = imu_delayed.time_us; - resetWindToZero(); - } - fuseSideslip(_aid_src_sideslip); + } else if (!_external_wind_init && !_control_status.flags.wind) { + resetWindCov(); + } } } } -void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const +void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const { - // reset flags - resetEstimatorAidStatus(sideslip); - - const float R = sq(_params.beta_noise); // observation noise variance - - float innov = 0.f; - float innov_var = 0.f; - sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var); - - sideslip.observation = 0.f; - sideslip.observation_variance = R; - sideslip.innovation = innov; - sideslip.innovation_variance = innov_var; - - sideslip.timestamp_sample = _time_delayed_us; - - const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(sideslip, innov_gate); + float observation = 0.f; + const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance + const float epsilon = 1e-3f; + float innov; + float innov_var; + sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, epsilon, &innov, &innov_var); + + updateAidSourceStatus(aid_src, + _time_delayed_us, // sample timestamp + observation, // observation + R, // observation variance + innov, // innovation + innov_var, // innovation variance + math::max(_params.beta_innov_gate, 1.f)); // innovation gate } -void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) +bool Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) { if (sideslip.innovation_rejected) { - return; + return false; } + // determine if we need the sideslip fusion to correct states other than wind bool update_wind_only = !_control_status.flags.wind_dead_reckoning; @@ -115,7 +110,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) const char *action_string = nullptr; if (update_wind_only) { - resetWind(); + resetWindCov(); action_string = "wind"; } else { @@ -126,15 +121,16 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) ECL_ERR("sideslip badly conditioned - %s covariance reset", action_string); - return; + return false; } _fault_status.flags.bad_sideslip = false; + const float epsilon = 1e-3f; VectorState H; // Observation jacobian VectorState K; // Kalman gain vector - sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); + sym::ComputeSideslipHAndK(_state.vector(), P, sideslip.innovation_variance, epsilon, &H, &K); if (update_wind_only) { const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); @@ -150,4 +146,6 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) if (is_fused) { sideslip.time_last_fuse = _time_delayed_us; } + + return is_fused; } diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp similarity index 92% rename from src/modules/ekf2/EKF/zero_innovation_heading_update.cpp rename to src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp index 1bae77c7c8b7..fded4cfbcc13 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/aid_sources/zero_innovation_heading_update.cpp @@ -41,10 +41,10 @@ void Ekf::controlZeroInnovationHeadingUpdate() { const bool yaw_aiding = _control_status.flags.mag_hdg || _control_status.flags.mag_3D - || _control_status.flags.ev_yaw || _control_status.flags.gps_yaw; + || _control_status.flags.ev_yaw || _control_status.flags.gnss_yaw; // fuse zero innovation at a limited rate if the yaw variance is too large - if(!yaw_aiding + if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { // Use an observation variance larger than usual but small enough @@ -60,7 +60,8 @@ void Ekf::controlZeroInnovationHeadingUpdate() computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); - if (!_control_status.flags.tilt_align || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { + if (!_control_status.flags.tilt_align + || (aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { // The yaw variance is too large, fuse fake measurement fuseYaw(aid_src_status, H_YAW); } diff --git a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt b/src/modules/ekf2/EKF/bias_estimator/CMakeLists.txt similarity index 88% rename from src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt rename to src/modules/ekf2/EKF/bias_estimator/CMakeLists.txt index 4610fa6c3d4d..b2346919e15a 100644 --- a/src/modules/differential_drive/DifferentialDriveControl/CMakeLists.txt +++ b/src/modules/ekf2/EKF/bias_estimator/CMakeLists.txt @@ -31,9 +31,11 @@ # ############################################################################ -px4_add_library(DifferentialDriveControl - DifferentialDriveControl.cpp +add_library(bias_estimator + bias_estimator.cpp + bias_estimator.hpp + height_bias_estimator.hpp + position_bias_estimator.hpp ) -target_link_libraries(DifferentialDriveControl PUBLIC pid) -target_include_directories(DifferentialDriveControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +add_dependencies(bias_estimator prebuild_targets) diff --git a/src/modules/ekf2/EKF/bias_estimator.cpp b/src/modules/ekf2/EKF/bias_estimator/bias_estimator.cpp similarity index 100% rename from src/modules/ekf2/EKF/bias_estimator.cpp rename to src/modules/ekf2/EKF/bias_estimator/bias_estimator.cpp diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator/bias_estimator.hpp similarity index 100% rename from src/modules/ekf2/EKF/bias_estimator.hpp rename to src/modules/ekf2/EKF/bias_estimator/bias_estimator.hpp diff --git a/src/modules/ekf2/EKF/height_bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator/height_bias_estimator.hpp similarity index 99% rename from src/modules/ekf2/EKF/height_bias_estimator.hpp rename to src/modules/ekf2/EKF/bias_estimator/height_bias_estimator.hpp index 5cb03cbfdd13..9fe1ee7dacfc 100644 --- a/src/modules/ekf2/EKF/height_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator/height_bias_estimator.hpp @@ -39,7 +39,7 @@ #define EKF_HEIGHT_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" -#include "common.h" +#include "../common.h" class HeightBiasEstimator: public BiasEstimator { diff --git a/src/modules/ekf2/EKF/position_bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator/position_bias_estimator.hpp similarity index 99% rename from src/modules/ekf2/EKF/position_bias_estimator.hpp rename to src/modules/ekf2/EKF/bias_estimator/position_bias_estimator.hpp index 0cba8fcd996b..629bb538a143 100644 --- a/src/modules/ekf2/EKF/position_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator/position_bias_estimator.hpp @@ -39,7 +39,6 @@ #define EKF_POSITION_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" -#include "common.h" class PositionBiasEstimator { diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 880df1d4fec7..af03d2a8db84 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -66,19 +66,26 @@ using math::Utilities::sq; using math::Utilities::updateYawInRotMat; // maximum sensor intervals in usec -static constexpr uint64_t BARO_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec) -static constexpr uint64_t EV_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) -static constexpr uint64_t GNSS_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) -static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) -static constexpr uint64_t RNG_MAX_INTERVAL = 200e3; ///< Maximum allowable time interval between range finder measurements (uSec) -static constexpr uint64_t MAG_MAX_INTERVAL = 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) +static constexpr uint64_t BARO_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between pressure altitude measurements (uSec) +static constexpr uint64_t EV_MAX_INTERVAL = + 200e3; ///< Maximum allowable time interval between external vision system measurements (uSec) +static constexpr uint64_t GNSS_MAX_INTERVAL = + 500e3; ///< Maximum allowable time interval between GNSS measurements (uSec) +static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = + 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec) +static constexpr uint64_t MAG_MAX_INTERVAL = + 500e3; ///< Maximum allowable time interval between magnetic field measurements (uSec) // bad accelerometer detection and mitigation -static constexpr uint64_t BADACC_PROBATION = 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) -static constexpr float BADACC_BIAS_PNOISE = 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) +static constexpr uint64_t BADACC_PROBATION = + 10e6; ///< Period of time that accel data declared bad must continuously pass checks to be declared good again (uSec) +static constexpr float BADACC_BIAS_PNOISE = + 4.9f; ///< The delta velocity process noise is set to this when accel data is declared bad (m/sec**2) // ground effect compensation -static constexpr uint64_t GNDEFFECT_TIMEOUT = 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) +static constexpr uint64_t GNDEFFECT_TIMEOUT = + 10e6; ///< Maximum period of time that ground effect protection will be active after it was last turned on (uSec) enum class PositionFrame : uint8_t { LOCAL_FRAME_NED = 0, @@ -94,21 +101,21 @@ enum class VelocityFrame : uint8_t { #if defined(CONFIG_EKF2_MAGNETOMETER) enum GeoDeclinationMask : uint8_t { // Bit locations for mag_declination_source - USE_GEO_DECL = (1<<0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value - SAVE_GEO_DECL = (1<<1), ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library - FUSE_DECL = (1<<2) ///< set to true if the declination is always fused as an observation to constrain drift when 3-axis fusion is performed + USE_GEO_DECL = (1 << 0), ///< set to true to use the declination from the geo library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value + SAVE_GEO_DECL = (1 << 1) ///< set to true to set the EKF2_MAG_DECL parameter to the value returned by the geo library }; enum MagFuseType : uint8_t { // Integer definitions for mag_fusion_type AUTO = 0, ///< The selection of either heading or 3D magnetometer fusion will be automatic HEADING = 1, ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg - NONE = 5 ///< Do not use magnetometer under any circumstance.. + NONE = 5, ///< Do not use magnetometer under any circumstance. + INIT = 6 ///< Use the mag for heading initialization only. }; #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_TERRAIN) -enum TerrainFusionMask : uint8_t { +enum class TerrainFusionMask : uint8_t { TerrainFuseRangeFinder = (1 << 0), TerrainFuseOpticalFlow = (1 << 1) }; @@ -129,16 +136,16 @@ enum class PositionSensor : uint8_t { }; enum class ImuCtrl : uint8_t { - GyroBias = (1<<0), - AccelBias = (1<<1), - GravityVector = (1<<2), + GyroBias = (1 << 0), + AccelBias = (1 << 1), + GravityVector = (1 << 2), }; enum class GnssCtrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1), - VEL = (1<<2), - YAW = (1<<3) + HPOS = (1 << 0), + VPOS = (1 << 1), + VEL = (1 << 2), + YAW = (1 << 3) }; enum class RngCtrl : uint8_t { @@ -148,10 +155,10 @@ enum class RngCtrl : uint8_t { }; enum class EvCtrl : uint8_t { - HPOS = (1<<0), - VPOS = (1<<1), - VEL = (1<<2), - YAW = (1<<3) + HPOS = (1 << 0), + VPOS = (1 << 1), + VEL = (1 << 2), + YAW = (1 << 3) }; enum class MagCheckMask : uint8_t { @@ -160,6 +167,11 @@ enum class MagCheckMask : uint8_t { FORCE_WMM = (1 << 2) }; +enum class FlowGyroSource : uint8_t { + Auto = 0, + Internal = 1 +}; + struct imuSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) Vector3f delta_ang{}; ///< delta angle in body frame (integrated gyro measurements) (rad) @@ -184,6 +196,7 @@ struct gnssSample { float yaw{}; ///< yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI]) float yaw_acc{}; ///< 1-std yaw error (rad) float yaw_offset{}; ///< Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET + bool spoofed{}; ///< true if GNSS data is spoofed }; struct magSample { @@ -198,12 +211,6 @@ struct baroSample { bool reset{false}; }; -struct rangeSample { - uint64_t time_us{}; ///< timestamp of the measurement (uSec) - float rng{}; ///< range (distance to ground) measurement (m) - int8_t quality{}; ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. -}; - struct airspeedSample { uint64_t time_us{}; ///< timestamp of the measurement (uSec) float true_airspeed{}; ///< true airspeed measurement (m/sec) @@ -266,7 +273,7 @@ struct parameters { int32_t height_sensor_ref{static_cast(HeightSensor::BARO)}; int32_t position_sensor_ref{static_cast(PositionSensor::GNSS)}; - int32_t sensor_interval_max_ms{10}; ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec) + float delay_max_ms{110.f}; ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec) // input noise float gyro_noise{1.5e-2f}; ///< IMU angular rate noise used for covariance prediction (rad/sec) @@ -277,7 +284,7 @@ struct parameters { float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3) #if defined(CONFIG_EKF2_WIND) - const float initial_wind_uncertainty{1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) + const float initial_wind_uncertainty {1.0f}; ///< 1-sigma initial uncertainty in wind velocity (m/sec) float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity #endif // CONFIG_EKF2_WIND @@ -288,7 +295,7 @@ struct parameters { float initial_tilt_err{0.1f}; ///< 1-sigma tilt error after initial alignment using gravity vector (rad) #if defined(CONFIG_EKF2_BAROMETER) - int32_t baro_ctrl{1}; + int32_t baro_ctrl {1}; float baro_delay_ms{0.0f}; ///< barometer height measurement delay relative to the IMU (mSec) float baro_noise{2.0f}; ///< observation noise for barometric height fusion (m) float baro_bias_nsd{0.13f}; ///< process noise for barometric height bias estimation (m/s/sqrt(Hz)) @@ -311,7 +318,7 @@ struct parameters { #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) - int32_t gnss_ctrl{static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; + int32_t gnss_ctrl {static_cast(GnssCtrl::HPOS) | static_cast(GnssCtrl::VEL)}; float gps_delay_ms{110.0f}; ///< GPS measurement delay relative to the IMU (mSec) Vector3f gps_pos_body{}; ///< xyz position of the GPS antenna in body frame (m) @@ -336,7 +343,7 @@ struct parameters { # if defined(CONFIG_EKF2_GNSS_YAW) // GNSS heading fusion - float gps_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) + float gnss_heading_noise{0.1f}; ///< measurement noise standard deviation used for GNSS heading fusion (rad) # endif // CONFIG_EKF2_GNSS_YAW // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value @@ -352,7 +359,7 @@ struct parameters { float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) #if defined(CONFIG_EKF2_MAGNETOMETER) - float mag_delay_ms{0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) + float mag_delay_ms {0.0f}; ///< magnetometer measurement delay relative to the IMU (mSec) float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec) float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec) @@ -361,7 +368,7 @@ struct parameters { float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetometer fusion (Gauss) float mag_declination_deg{0.0f}; ///< magnetic declination (degrees) float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD) - int32_t mag_declination_source{7}; ///< bitmask used to control the handling of declination data + int32_t mag_declination_source{3}; ///< bitmask used to control the handling of declination data int32_t mag_fusion_type{0}; ///< integer used to specify the type of magnetometer fusion used float mag_acc_gate{0.5f}; ///< when in auto select mode, heading fusion will be used when manoeuvre accel is lower than this (m/sec**2) @@ -389,16 +396,13 @@ struct parameters { #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_TERRAIN) - int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | - TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator - - float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec) + float terrain_p_noise {5.0f}; ///< process noise for terrain offset (m/sec) float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m) const float terrain_timeout{10.f}; ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s) #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) - float rng_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) + float rng_gnd_clearance {0.1f}; ///< minimum valid value for range when on ground (m) #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -408,10 +412,8 @@ struct parameters { float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) - const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion @@ -444,7 +446,8 @@ struct parameters { #endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_OPTICAL_FLOW) - int32_t flow_ctrl{0}; + int32_t flow_ctrl {0}; + int32_t flow_gyro_src {static_cast(FlowGyroSource::Auto)}; float flow_delay_ms{5.0f}; ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval // optical flow fusion @@ -505,7 +508,8 @@ union fault_status_u { bool bad_hdg : 1; ///< 3 - true if the fusion of the heading angle has encountered a numerical error bool bad_mag_decl : 1; ///< 4 - true if the fusion of the magnetic declination has encountered a numerical error bool bad_airspeed : 1; ///< 5 - true if fusion of the airspeed has encountered a numerical error - bool bad_sideslip : 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool bad_sideslip : + 1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error bool bad_optflow_X : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error bool bad_optflow_Y : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error bool bad_acc_bias : 1; ///< 9 - true if bad delta velocity bias estimates have been detected @@ -528,7 +532,7 @@ union innovation_fault_status_u { bool reject_yaw : 1; ///< 7 - true if the yaw observation has been rejected bool reject_airspeed : 1; ///< 8 - true if the airspeed observation has been rejected bool reject_sideslip : 1; ///< 9 - true if the synthetic sideslip observation has been rejected - bool reject_hagl : 1; ///< 10 - true if the height above ground observation has been rejected + bool reject_hagl : 1; ///< 10 - unused bool reject_optflow_X : 1; ///< 11 - true if the X optical flow observation has been rejected bool reject_optflow_Y : 1; ///< 12 - true if the Y optical flow observation has been rejected } flags; @@ -548,6 +552,7 @@ union gps_check_fail_status_u { uint16_t vdrift : 1; ///< 7 - true if vertical drift is excessive (can only be used when stationary on ground) uint16_t hspeed : 1; ///< 8 - true if horizontal speed is excessive (can only be used when stationary on ground) uint16_t vspeed : 1; ///< 9 - true if vertical speed error is excessive + uint16_t spoofed: 1; ///< 10 - true if the GNSS data is spoofed } flags; uint16_t value; }; @@ -565,7 +570,8 @@ union filter_control_status_u { uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference - uint64_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference +uint64_t rng_hgt : + 1; ///< 10 - true when range finder height is being fused as a primary height reference uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended @@ -573,61 +579,46 @@ union filter_control_status_u { uint64_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused uint64_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength uint64_t fixed_wing : 1; ///< 17 - true when the vehicle is operating as a fixed wing vehicle - uint64_t mag_fault : 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used +uint64_t mag_fault : + 1; ///< 18 - true when the magnetometer has been declared faulty and is no longer being used uint64_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused - uint64_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active - uint64_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough - uint64_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +uint64_t gnd_effect : + 1; ///< 20 - true when protection from ground effect induced static pressure rise is active +uint64_t rng_stuck : + 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +uint64_t gnss_yaw : + 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended uint64_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed - uint64_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended - uint64_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint64_t ev_vel : + 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended +uint64_t synthetic_mag_z : + 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component uint64_t vehicle_at_rest : 1; ///< 26 - true when the vehicle is at rest - uint64_t gps_yaw_fault : 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used - uint64_t rng_fault : 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used - uint64_t inertial_dead_reckoning : 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift +uint64_t gnss_yaw_fault : + 1; ///< 27 - true when the GNSS heading has been declared faulty and is no longer being used +uint64_t rng_fault : + 1; ///< 28 - true when the range finder has been declared faulty and is no longer being used +uint64_t inertial_dead_reckoning : + 1; ///< 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift uint64_t wind_dead_reckoning : 1; ///< 30 - true if we are navigationg reliant on wind relative measurements uint64_t rng_kin_consistent : 1; ///< 31 - true when the range finder kinematic consistency check is passing uint64_t fake_pos : 1; ///< 32 - true when fake position measurements are being fused uint64_t fake_hgt : 1; ///< 33 - true when fake height measurements are being fused uint64_t gravity_vector : 1; ///< 34 - true when gravity vector measurements are being fused - uint64_t mag : 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended - uint64_t ev_yaw_fault : 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used - uint64_t mag_heading_consistent : 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter - uint64_t aux_gpos : 1; +uint64_t mag : + 1; ///< 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended +uint64_t ev_yaw_fault : + 1; ///< 36 - true when the EV heading has been declared faulty and is no longer being used +uint64_t mag_heading_consistent : + 1; ///< 37 - true when the heading obtained from mag data is declared consistent with the filter + uint64_t aux_gpos : 1; ///< 38 - true if auxiliary global position measurement fusion is intended + uint64_t rng_terrain : 1; ///< 39 - true if we are fusing range finder data for terrain + uint64_t opt_flow_terrain : 1; ///< 40 - true if we are fusing flow data for terrain } flags; uint64_t value; }; -// Mavlink bitmask containing state of estimator solution -union ekf_solution_status_u { - struct { - uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good - uint16_t velocity_horiz : 1; ///< 1 - True if the horizontal velocity estimate is good - uint16_t velocity_vert : 1; ///< 2 - True if the vertical velocity estimate is good - uint16_t pos_horiz_rel : 1; ///< 3 - True if the horizontal position (relative) estimate is good - uint16_t pos_horiz_abs : 1; ///< 4 - True if the horizontal position (absolute) estimate is good - uint16_t pos_vert_abs : 1; ///< 5 - True if the vertical position (absolute) estimate is good - uint16_t pos_vert_agl : 1; ///< 6 - True if the vertical position (above ground) estimate is good - uint16_t const_pos_mode : 1; ///< 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) - uint16_t pred_pos_horiz_rel : 1; ///< 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate - uint16_t pred_pos_horiz_abs : 1; ///< 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate - uint16_t gps_glitch : 1; ///< 10 - True if the EKF has detected a GPS glitch - uint16_t accel_error : 1; ///< 11 - True if the EKF has detected bad accelerometer data - } flags; - uint16_t value; -}; - -#if defined(CONFIG_EKF2_TERRAIN) -union terrain_fusion_status_u { - struct { - bool range_finder : 1; ///< 0 - true if we are fusing range finder data - bool flow : 1; ///< 1 - true if we are fusing flow data - } flags; - uint8_t value; -}; -#endif // CONFIG_EKF2_TERRAIN - // define structure used to communicate information events union information_event_status_u { struct { @@ -639,35 +630,23 @@ union information_event_status_u { bool reset_pos_to_last_known : 1; ///< 5 - true when the position states are reset to the last known position bool reset_pos_to_gps : 1; ///< 6 - true when the position states are reset to the gps measurement bool reset_pos_to_vision : 1; ///< 7 - true when the position states are reset to the vision system measurement - bool starting_gps_fusion : 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates - bool starting_vision_pos_fusion : 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates - bool starting_vision_vel_fusion : 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates - bool starting_vision_yaw_fusion : 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates - bool yaw_aligned_to_imu_gps : 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data +bool starting_gps_fusion : + 1; ///< 8 - true when the filter starts using gps measurements to correct the state estimates +bool starting_vision_pos_fusion : + 1; ///< 9 - true when the filter starts using vision system position measurements to correct the state estimates +bool starting_vision_vel_fusion : + 1; ///< 10 - true when the filter starts using vision system velocity measurements to correct the state estimates +bool starting_vision_yaw_fusion : + 1; ///< 11 - true when the filter starts using vision system yaw measurements to correct the state estimates +bool yaw_aligned_to_imu_gps : + 1; ///< 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data bool reset_hgt_to_baro : 1; ///< 13 - true when the vertical position state is reset to the baro measurement bool reset_hgt_to_gps : 1; ///< 14 - true when the vertical position state is reset to the gps measurement bool reset_hgt_to_rng : 1; ///< 15 - true when the vertical position state is reset to the rng measurement bool reset_hgt_to_ev : 1; ///< 16 - true when the vertical position state is reset to the ev measurement - bool reset_pos_to_ext_obs : 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning - } flags; - uint32_t value; -}; - -// define structure used to communicate information events -union warning_event_status_u { - struct { - bool gps_quality_poor : 1; ///< 0 - true when the gps is failing quality checks - bool gps_fusion_timout : 1; ///< 1 - true when the gps data has not been used to correct the state estimates for a significant time period - bool gps_data_stopped : 1; ///< 2 - true when the gps data has stopped for a significant time period - bool gps_data_stopped_using_alternate : 1; ///< 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation - bool height_sensor_timeout : 1; ///< 4 - true when the height sensor has not been used to correct the state estimates for a significant time period - bool stopping_navigation : 1; ///< 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation - bool invalid_accel_bias_cov_reset : 1; ///< 6 - true when the filter has detected bad acceerometer bias state estimates and has reset the corresponding covariance matrix elements - bool bad_yaw_using_gps_course : 1; ///< 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course - bool stopping_mag_use : 1; ///< 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data - bool vision_data_stopped : 1; ///< 9 - true when the vision system data has stopped for a significant time period - bool emergency_yaw_reset_mag_stopped : 1; ///< 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data - bool emergency_yaw_reset_gps_yaw_stopped: 1; ///< 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data +bool reset_pos_to_ext_obs : + 1; ///< 17 - true when horizontal position was reset to an external observation while deadreckoning + bool reset_wind_to_ext_obs : 1; ///< 18 - true when wind states were reset to an external observation } flags; uint32_t value; }; diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 34d26273e78b..b8aca027e769 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -102,7 +102,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding - controlMagFusion(); + controlMagFusion(imu_delayed); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -137,13 +137,17 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_EXTERNAL_VISION) // Additional data odometry data from an external estimator can be fused. - controlExternalVisionFusion(); + controlExternalVisionFusion(imu_delayed); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) // Additional horizontal velocity data from an auxiliary sensor can be fused - controlAuxVelFusion(); + controlAuxVelFusion(imu_delayed); #endif // CONFIG_EKF2_AUXVEL + // +#if defined(CONFIG_EKF2_TERRAIN) + controlTerrainFakeFusion(); +#endif // CONFIG_EKF2_TERRAIN controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index c6ba03f25622..787d1636e2ab 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -53,7 +53,7 @@ void Ekf::initialiseCovariance() { P.zero(); - resetQuatCov(); + resetQuatCov(0.f); // Start with no initial uncertainty to improve fine leveling through zero vel/pos fusion // velocity #if defined(CONFIG_EKF2_GNSS) @@ -76,14 +76,17 @@ void Ekf::initialiseCovariance() if (_control_status.flags.gps_hgt) { z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } + #else const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f)); #endif #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); } + #endif // CONFIG_EKF2_RANGE_FINDER P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); @@ -99,6 +102,11 @@ void Ekf::initialiseCovariance() #if defined(CONFIG_EKF2_WIND) resetWindCov(); #endif // CONFIG_EKF2_WIND + +#if defined(CONFIG_EKF2_TERRAIN) + // use the ground clearance value as our uncertainty + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); +#endif // CONFIG_EKF2_TERRAIN } void Ekf::predictCovariance(const imuSample &imu_delayed) @@ -107,11 +115,11 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) const float dt = 0.5f * (imu_delayed.delta_vel_dt + imu_delayed.delta_ang_dt); // gyro noise variance - float gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); + float gyro_noise = _params.gyro_noise; const float gyro_var = sq(gyro_noise); // accel noise variance - float accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); + float accel_noise = _params.accel_noise; Vector3f accel_var; for (unsigned i = 0; i < 3; i++) { @@ -126,36 +134,36 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states P = sym::PredictCovariance(_state.vector(), P, - imu_delayed.delta_vel / math::max(imu_delayed.delta_vel_dt, FLT_EPSILON), accel_var, - imu_delayed.delta_ang / math::max(imu_delayed.delta_ang_dt, FLT_EPSILON), gyro_var, + imu_delayed.delta_vel / imu_delayed.delta_vel_dt, accel_var, + imu_delayed.delta_ang / imu_delayed.delta_ang_dt, gyro_var, dt); // Construct the process noise variance diagonal for those states with a stationary process model // These are kinematic states and their error growth is controlled separately by the IMU noise variances - // gyro bias: add process noise unless state is inhibited + // gyro bias: add process noise { - const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f); + const float gyro_bias_sig = dt * _params.gyro_bias_p_noise; const float gyro_bias_process_noise = sq(gyro_bias_sig); for (unsigned index = 0; index < State::gyro_bias.dof; index++) { const unsigned i = State::gyro_bias.idx + index; - if (!_gyro_bias_inhibit[index]) { + if (P(i, i) < gyro_var) { P(i, i) += gyro_bias_process_noise; } } } - // accel bias: add process noise unless state is inhibited + // accel bias: add process noise { - const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f); + const float accel_bias_sig = dt * _params.accel_bias_p_noise; const float accel_bias_process_noise = sq(accel_bias_sig); for (unsigned index = 0; index < State::accel_bias.dof; index++) { const unsigned i = State::accel_bias.idx + index; - if (!_accel_bias_inhibit[index]) { + if (P(i, i) < accel_var(index)) { P(i, i) += accel_bias_process_noise; } } @@ -163,41 +171,63 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag) { - // mag_I: add process noise - float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); - float mag_I_process_noise = sq(mag_I_sig); + // mag_I: add process noise + float mag_I_sig = dt * _params.mage_p_noise; + float mag_I_process_noise = sq(mag_I_sig); - for (unsigned index = 0; index < State::mag_I.dof; index++) { - const unsigned i = State::mag_I.idx + index; + for (unsigned index = 0; index < State::mag_I.dof; index++) { + const unsigned i = State::mag_I.idx + index; + + if (P(i, i) < sq(_params.mag_noise)) { P(i, i) += mag_I_process_noise; } + } + + // mag_B: add process noise + float mag_B_sig = dt * _params.magb_p_noise; + float mag_B_process_noise = sq(mag_B_sig); - // mag_B: add process noise - float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); - float mag_B_process_noise = sq(mag_B_sig); + for (unsigned index = 0; index < State::mag_B.dof; index++) { + const unsigned i = State::mag_B.idx + index; - for (unsigned index = 0; index < State::mag_B.dof; index++) { - const unsigned i = State::mag_B.idx + index; + if (P(i, i) < sq(_params.mag_noise)) { P(i, i) += mag_B_process_noise; } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - if (_control_status.flags.wind) { - // wind vel: add process noise - float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; - for (unsigned index = 0; index < State::wind_vel.dof; index++) { - const unsigned i = State::wind_vel.idx + index; + // wind vel: add process noise + float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); + float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + const unsigned i = State::wind_vel.idx + index; + + if (P(i, i) < sq(_params.initial_wind_uncertainty)) { P(i, i) += wind_vel_process_noise; } } + #endif // CONFIG_EKF2_WIND +#if defined(CONFIG_EKF2_TERRAIN) + + if (_height_sensor_ref != HeightSensor::RANGE) { + // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle + // process noise due to errors in vehicle height estimate + float terrain_process_noise = sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); + + // process noise due to terrain gradient + terrain_process_noise += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( + 1))); + P(State::terrain.idx, State::terrain.idx) += terrain_process_noise; + } + +#endif // CONFIG_EKF2_TERRAIN // covariance matrix is symmetrical, so copy upper half to lower half for (unsigned row = 0; row < State::size; row++) { @@ -224,17 +254,25 @@ void Ekf::constrainStateVariances() constrainStateVarLimitRatio(State::accel_bias, kAccelBiasVarianceMin, 1.f); #if defined(CONFIG_EKF2_MAGNETOMETER) + if (_control_status.flags.mag) { constrainStateVarLimitRatio(State::mag_I, kMagVarianceMin, 1.f); constrainStateVarLimitRatio(State::mag_B, kMagVarianceMin, 1.f); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + if (_control_status.flags.wind) { constrainStateVarLimitRatio(State::wind_vel, 1e-6f, 1e6f); } + #endif // CONFIG_EKF2_WIND + +#if defined(CONFIG_EKF2_TERRAIN) + constrainStateVarLimitRatio(State::terrain, 0.f, 1e4f); +#endif // CONFIG_EKF2_TERRAIN } void Ekf::constrainStateVar(const IdxDof &state, float min, float max) @@ -271,7 +309,7 @@ void Ekf::resetQuatCov(const float yaw_noise) // update the yaw angle variance using the variance of the measurement if (PX4_ISFINITE(yaw_noise)) { // using magnetic heading tuning parameter - yaw_var = math::max(sq(yaw_noise), yaw_var); + yaw_var = sq(yaw_noise); } resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var)); @@ -279,24 +317,34 @@ void Ekf::resetQuatCov(const float yaw_noise) void Ekf::resetQuatCov(const Vector3f &rot_var_ned) { - matrix::SquareMatrix q_cov_ned = diag(rot_var_ned); - resetStateCovariance(_R_to_earth.T() * q_cov_ned * _R_to_earth); + P.uncorrelateCovarianceSetVariance(State::quat_nominal.idx, rot_var_ned); +} + +void Ekf::resetGyroBiasCov() +{ + // Zero the corresponding covariances and set + // variances to the values use for initial alignment + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); +} + +void Ekf::resetGyroBiasZCov() +{ + P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias)); +} + +void Ekf::resetAccelBiasCov() +{ + // Zero the corresponding covariances and set + // variances to the values use for initial alignment + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); } #if defined(CONFIG_EKF2_MAGNETOMETER) void Ekf::resetMagCov() { - if (_mag_decl_cov_reset) { - ECL_INFO("reset mag covariance"); - _mag_decl_cov_reset = false; - } + ECL_INFO("reset mag covariance"); P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); } #endif // CONFIG_EKF2_MAGNETOMETER - -void Ekf::resetGyroBiasZCov() -{ - P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias)); -} diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index b6de89f544bb..9f6a0ab40b66 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -71,6 +71,11 @@ void Ekf::reset() #if defined(CONFIG_EKF2_WIND) _state.wind_vel.setZero(); #endif // CONFIG_EKF2_WIND + // +#if defined(CONFIG_EKF2_TERRAIN) + // assume a ground clearance + _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); @@ -84,8 +89,6 @@ void Ekf::reset() _control_status.flags.in_air = true; _control_status_prev.flags.in_air = true; - _ang_rate_delayed_raw.zero(); - _fault_status.value = 0; _innov_check_fail_status.value = 0; @@ -107,6 +110,7 @@ void Ekf::reset() _time_last_hor_vel_fuse = 0; _time_last_ver_vel_fuse = 0; _time_last_heading_fuse = 0; + _time_last_terrain_fuse = 0; _last_known_pos.setZero(); @@ -122,57 +126,14 @@ void Ekf::reset() _time_bad_vert_accel = 0; _time_good_vert_accel = 0; - _clip_counter = 0; - -#if defined(CONFIG_EKF2_BAROMETER) - resetEstimatorAidStatus(_aid_src_baro_hgt); -#endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_AIRSPEED) - resetEstimatorAidStatus(_aid_src_airspeed); -#endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_SIDESLIP) - resetEstimatorAidStatus(_aid_src_sideslip); -#endif // CONFIG_EKF2_SIDESLIP - - resetEstimatorAidStatus(_aid_src_fake_pos); - resetEstimatorAidStatus(_aid_src_fake_hgt); -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - resetEstimatorAidStatus(_aid_src_ev_hgt); - resetEstimatorAidStatus(_aid_src_ev_pos); - resetEstimatorAidStatus(_aid_src_ev_vel); - resetEstimatorAidStatus(_aid_src_ev_yaw); -#endif // CONFIG_EKF2_EXTERNAL_VISION - -#if defined(CONFIG_EKF2_GNSS) - resetEstimatorAidStatus(_aid_src_gnss_hgt); - resetEstimatorAidStatus(_aid_src_gnss_pos); - resetEstimatorAidStatus(_aid_src_gnss_vel); - -# if defined(CONFIG_EKF2_GNSS_YAW) - resetEstimatorAidStatus(_aid_src_gnss_yaw); -# endif // CONFIG_EKF2_GNSS_YAW -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_MAGNETOMETER) - resetEstimatorAidStatus(_aid_src_mag_heading); - resetEstimatorAidStatus(_aid_src_mag); -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_AUXVEL) - resetEstimatorAidStatus(_aid_src_aux_vel); -#endif // CONFIG_EKF2_AUXVEL - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - resetEstimatorAidStatus(_aid_src_optical_flow); - resetEstimatorAidStatus(_aid_src_terrain_optical_flow); -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_RANGE_FINDER) - resetEstimatorAidStatus(_aid_src_rng_hgt); -#endif // CONFIG_EKF2_RANGE_FINDER + for (auto &clip_count : _clip_counter) { + clip_count = 0; + } _zero_velocity_update.reset(); + + updateParameters(); } bool Ekf::update() @@ -208,12 +169,8 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); -#if defined(CONFIG_EKF2_TERRAIN) - // run a separate filter for terrain estimation - runTerrainEstimator(imu_sample_delayed); -#endif // CONFIG_EKF2_TERRAIN - - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, + _state.gyro_bias, _state.accel_bias); return true; } @@ -248,11 +205,6 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); -#if defined(CONFIG_EKF2_TERRAIN) - // Initialise the terrain estimator - initHagl(); -#endif // CONFIG_EKF2_TERRAIN - // reset the output predictor state history to match the EKF initial values _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); @@ -313,13 +265,6 @@ void Ekf::predictState(const imuSample &imu_delayed) _state.vel = matrix::constrain(_state.vel, -1000.f, 1000.f); _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); - // some calculations elsewhere in code require a raw angular rate vector so calculate here to avoid duplication - // protect against possible small timesteps resulting from timing slip on previous frame that can drive spikes into the rate - // due to insufficient averaging - if (imu_delayed.delta_ang_dt > 0.25f * _dt_ekf_avg) { - _ang_rate_delayed_raw = imu_delayed.delta_ang / imu_delayed.delta_ang_dt; - } - // calculate a filtered horizontal acceleration with a 1 sec time constant // this are used for manoeuvre detection elsewhere @@ -331,25 +276,87 @@ void Ekf::predictState(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } -void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation) +bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, + uint64_t timestamp_observation) { - if (!_pos_ref.isInitialized()) { - return; + ECL_WARN("unable to reset global position, position reference not initialized"); + return false; + } + + Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg); + + // apply a first order correction using velocity at the delayed time horizon and the delta time + if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) { + + timestamp_observation = math::min(_time_latest_us, timestamp_observation); + + float diff_us = 0.f; + + if (_time_delayed_us >= timestamp_observation) { + diff_us = static_cast(_time_delayed_us - timestamp_observation); + + } else { + diff_us = -static_cast(timestamp_observation - _time_delayed_us); + } + + const float dt_s = diff_us * 1e-6f; + pos_corrected += _state.vel.xy() * dt_s; } - // apply a first order correction using velocity at the delated time horizon and the delta time - timestamp_observation = math::min(_time_latest_us, timestamp_observation); - const float dt = _time_delayed_us > timestamp_observation ? static_cast(_time_delayed_us - timestamp_observation) - * 1e-6f : -static_cast(timestamp_observation - _time_delayed_us) * 1e-6f; + const float obs_var = math::max(sq(accuracy), sq(0.01f)); + + const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected; + const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; - Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt; + const float sq_gate = sq(5.f); // magic hardcoded gate + const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)), + sq(innov(1)) / (sq_gate * innov_var(1))}; + + const bool innov_rejected = (test_ratio.max() > 1.f); + + if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) { + // when on ground or accuracy chosen to be very low, we hard reset position + // this allows the user to still send hard resets at any time + ECL_INFO("reset position to external observation"); + _information_events.flags.reset_pos_to_ext_obs = true; + + resetHorizontalPositionTo(pos_corrected, obs_var); + _last_known_pos.xy() = _state.pos.xy(); + return true; - resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON)); + } else { + if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0) + && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1) + ) { + ECL_INFO("fused external observation as position measurement"); + _state_reset_status.reset_count.posNE++; + _time_last_hor_pos_fuse = _time_delayed_us; + _last_known_pos.xy() = _state.pos.xy(); + return true; + } + } + + return false; } void Ekf::updateParameters() { + _params.gyro_noise = math::constrain(_params.gyro_noise, 0.f, 1.f); + _params.accel_noise = math::constrain(_params.accel_noise, 0.f, 1.f); + + _params.gyro_bias_p_noise = math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f); + _params.accel_bias_p_noise = math::constrain(_params.accel_bias_p_noise, 0.f, 1.f); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + _params.mage_p_noise = math::constrain(_params.mage_p_noise, 0.f, 1.f); + _params.magb_p_noise = math::constrain(_params.magb_p_noise, 0.f, 1.f); +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_WIND) + _params.wind_vel_nsd = math::constrain(_params.wind_vel_nsd, 0.f, 1.f); +#endif // CONFIG_EKF2_WIND + #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) _aux_global_position.updateParameters(); #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION @@ -371,40 +378,49 @@ void Ekf::print_status() printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6); printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n", State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1, - (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3), - (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), + (double)_state.quat_nominal(3), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), + (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()), + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::vel.idx, State::vel.idx + State::vel.dof - 1, (double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::pos.idx, State::pos.idx + State::pos.dof - 1, (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1, (double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n", State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1, (double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); #if defined(CONFIG_EKF2_MAGNETOMETER) printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1, (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), - (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) + (double)getStateVariance()(0), (double)getStateVariance()(1), + (double)getStateVariance()(2) ); printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", @@ -423,6 +439,14 @@ void Ekf::print_status() ); #endif // CONFIG_EKF2_WIND +#if defined(CONFIG_EKF2_TERRAIN) + printf("Terrain position (%d): %.3f var: %.1e\n", + State::terrain.idx, + (double)_state.terrain, + (double)getStateVariance()(0) + ); +#endif // CONFIG_EKF2_TERRAIN + printf("\nP:\n"); P.print(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index edffd17974d0..a7c43567df52 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -46,12 +46,12 @@ #include "estimator_interface.h" #if defined(CONFIG_EKF2_GNSS) -# include "EKFGSF_yaw.h" +# include "yaw_estimator/EKFGSF_yaw.h" #endif // CONFIG_EKF2_GNSS -#include "bias_estimator.hpp" -#include "height_bias_estimator.hpp" -#include "position_bias_estimator.hpp" +#include "bias_estimator/bias_estimator.hpp" +#include "bias_estimator/height_bias_estimator.hpp" +#include "bias_estimator/position_bias_estimator.hpp" #include @@ -63,7 +63,7 @@ #include "aid_sources/ZeroVelocityUpdate.hpp" #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) -# include "aux_global_position.hpp" +# include "aid_sources/aux_global_position/aux_global_position.hpp" #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION enum class Likelihood { LOW, MEDIUM, HIGH }; @@ -73,7 +73,6 @@ class Ekf final : public EstimatorInterface public: typedef matrix::Vector VectorState; typedef matrix::SquareMatrix SquareMatrixState; - typedef matrix::SquareMatrix Matrix2f; Ekf() { @@ -101,30 +100,17 @@ class Ekf final : public EstimatorInterface // terrain estimate bool isTerrainEstimateValid() const; - uint8_t getTerrainEstimateSensorBitfield() const { return _hagl_sensor_status.value; } - // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _terrain_vpos; }; - - // get the number of times the vertical terrain position has been reset - uint8_t getTerrainVertPosResetCounter() const { return _terrain_vpos_reset_counter; }; + float getTerrainVertPos() const { return _state.terrain; }; + float getHagl() const { return _state.terrain - _state.pos(2); } // get the terrain variance - float get_terrain_var() const { return _terrain_var; } - -# if defined(CONFIG_EKF2_RANGE_FINDER) - const auto &aid_src_terrain_range_finder() const { return _aid_src_terrain_range_finder; } -# endif // CONFIG_EKF2_RANGE_FINDER - -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } -# endif // CONFIG_EKF2_OPTICAL_FLOW + float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - const BiasEstimator::status &getRngHgtBiasEstimatorStatus() const { return _rng_hgt_b_est.getStatus(); } const auto &aid_src_rng_hgt() const { return _aid_src_rng_hgt; } float getHaglRateInnov() const { return _rng_consistency_check.getInnov(); } @@ -143,31 +129,33 @@ class Ekf final : public EstimatorInterface const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_rate; } const Vector3f &getFlowGyroBias() const { return _flow_gyro_bias; } - const Vector3f &getRefBodyRate() const { return _ref_body_rate; } + const Vector3f &getFlowRefBodyRate() const { return _ref_body_rate; } #endif // CONFIG_EKF2_OPTICAL_FLOW float getHeadingInnov() const { #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg) { - return _aid_src_mag_heading.innovation; - } - if (_control_status.flags.mag_3D) { + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -176,25 +164,27 @@ class Ekf final : public EstimatorInterface float getHeadingInnovVar() const { #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg) { - return _aid_src_mag_heading.innovation_variance; - } - if (_control_status.flags.mag_3D) { + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.innovation_variance).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.innovation_variance; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.innovation_variance; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -203,25 +193,27 @@ class Ekf final : public EstimatorInterface float getHeadingInnovRatio() const { #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg) { - return _aid_src_mag_heading.test_ratio; - } - if (_control_status.flags.mag_3D) { + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { return Vector3f(_aid_src_mag.test_ratio).max(); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { + + if (_control_status.flags.gnss_yaw) { return _aid_src_gnss_yaw.test_ratio; } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { return _aid_src_ev_yaw.test_ratio; } + #endif // CONFIG_EKF2_EXTERNAL_VISION return 0.f; @@ -254,7 +246,7 @@ class Ekf final : public EstimatorInterface // get the diagonal elements of the covariance matrix matrix::Vector covariances_diagonal() const { return P.diag(); } - matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + matrix::Vector3f getRotVarBody() const; matrix::Vector3f getRotVarNed() const; float getYawVar() const; float getTiltVariance() const; @@ -267,6 +259,7 @@ class Ekf final : public EstimatorInterface // return true if the origin is valid bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f); + void updateWmm(double lat, double lon); // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -277,12 +270,13 @@ class Ekf final : public EstimatorInterface // get the 1-sigma horizontal and vertical velocity uncertainty void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const; - // get the vehicle control limits required by the estimator to keep within sensor limitations + // Returns the following vehicle control limits required by the estimator to keep within sensor limitations. + // vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. + // vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. + // hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. + // hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const; - // Reset all IMU bias states and covariances to initial alignment values. - void resetImuBias(); - void resetGyroBias(); void resetGyroBiasCov(); @@ -362,6 +356,13 @@ class Ekf final : public EstimatorInterface *counter = _state_reset_status.reset_count.posD; } + uint8_t get_hagl_reset_count() const { return _state_reset_status.reset_count.hagl; } + void get_hagl_reset(float *delta, uint8_t *counter) const + { + *delta = _state_reset_status.hagl_change; + *counter = _state_reset_status.reset_count.hagl; + } + // return the amount the local vertical velocity changed in the last reset and the number of reset events uint8_t get_velD_reset_count() const { return _state_reset_status.reset_count.velD; } void get_velD_reset(float *delta, uint8_t *counter) const @@ -394,16 +395,21 @@ class Ekf final : public EstimatorInterface *counter = _state_reset_status.reset_count.quat; } - // get EKF innovation consistency check status information comprising of: - // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check - // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. - // A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF - // Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. - void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) const; + float getHeadingInnovationTestRatio() const; + + float getHorizontalVelocityInnovationTestRatio() const; + float getVerticalVelocityInnovationTestRatio() const; + + float getHorizontalPositionInnovationTestRatio() const; + float getVerticalPositionInnovationTestRatio() const; - // return a bitmask integer that describes which state estimates can be used for flight control - void get_ekf_soln_status(uint16_t *status) const; + float getAirspeedInnovationTestRatio() const; + float getSyntheticSideslipInnovationTestRatio() const; + + float getHeightAboveGroundInnovationTestRatio() const; + + // return a bitmask integer that describes which state estimates are valid + uint16_t get_ekf_soln_status() const; HeightSensor getHeightSensorRef() const { return _height_sensor_ref; } @@ -460,7 +466,6 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) - const auto &aid_src_mag_heading() const { return _aid_src_mag_heading; } const auto &aid_src_mag() const { return _aid_src_mag; } #endif // CONFIG_EKF2_MAGNETOMETER @@ -485,9 +490,14 @@ class Ekf final : public EstimatorInterface #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update - VectorState PH = P * H; + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major + VectorState PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { @@ -496,7 +506,7 @@ class Ekf final : public EstimatorInterface } // Step 2: stabilized update - PH = P * H; + PH = P * H; // H is stored as a column vector. H is in fact H.T for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { @@ -504,6 +514,7 @@ class Ekf final : public EstimatorInterface P(j, i) = P(i, j); } } + #endif constrainStateVariances(); @@ -513,7 +524,20 @@ class Ekf final : public EstimatorInterface return true; } - void resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation); + bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, + uint64_t timestamp_observation); + + /** + * @brief Resets the wind states to an external observation + * + * @param wind_speed The wind speed in m/s + * @param wind_direction The azimuth (from true north) to where the wind is heading in radians + * @param wind_speed_accuracy The 1 sigma accuracy of the wind speed estimate in m/s + * @param wind_direction_accuracy The 1 sigma accuracy of the wind direction estimate in radians + */ + void resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, + float wind_direction_accuracy); + bool _external_wind_init{false}; void updateParameters(); @@ -545,6 +569,7 @@ class Ekf final : public EstimatorInterface uint8_t posNE{0}; ///< number of horizontal position reset events (allow to wrap if count exceeds 255) uint8_t posD{0}; ///< number of vertical position reset events (allow to wrap if count exceeds 255) uint8_t quat{0}; ///< number of quaternion reset events (allow to wrap if count exceeds 255) + uint8_t hagl{0}; ///< number of height above ground level reset events (allow to wrap if count exceeds 255) }; struct StateResets { @@ -553,6 +578,7 @@ class Ekf final : public EstimatorInterface Vector2f posNE_change; ///< North, East position change due to last reset (m) float posD_change; ///< Down position change due to last reset (m) Quatf quat_change; ///< quaternion delta due to last reset - multiply pre-reset quaternion by this to get post-reset quaternion + float hagl_change; ///< Height above ground level change due to last reset (m) StateResetCounts reset_count{}; }; @@ -560,8 +586,6 @@ class Ekf final : public EstimatorInterface StateResets _state_reset_status{}; ///< reset event monitoring structure containing velocity, position, height and yaw reset information StateResetCounts _state_reset_count_prev{}; - Vector3f _ang_rate_delayed_raw{}; ///< uncorrected angular rate vector at fusion time horizon (rad/sec) - StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised @@ -575,6 +599,7 @@ class Ekf final : public EstimatorInterface uint64_t _time_last_hor_vel_fuse{0}; ///< time the last fusion of horizontal velocity measurements was performed (uSec) uint64_t _time_last_ver_vel_fuse{0}; ///< time the last fusion of verticalvelocity measurements was performed (uSec) uint64_t _time_last_heading_fuse{0}; + uint64_t _time_last_terrain_fuse{0}; Vector3f _last_known_pos{}; ///< last known local position vector (m) @@ -590,36 +615,20 @@ class Ekf final : public EstimatorInterface SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) - estimator_aid_source2d_s _aid_src_drag{}; + estimator_aid_source2d_s _aid_src_drag {}; #endif // CONFIG_EKF2_DRAG_FUSION #if defined(CONFIG_EKF2_TERRAIN) // Terrain height state estimation - float _terrain_vpos{0.0f}; ///< estimated vertical position of the terrain underneath the vehicle in local NED frame (m) - float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) - uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset - - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) - -# if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_terrain_range_finder{}; - uint64_t _time_last_healthy_rng_data{0}; -# endif // CONFIG_EKF2_RANGE_FINDER - -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - estimator_aid_source2d_s _aid_src_terrain_optical_flow{}; -# endif // CONFIG_EKF2_OPTICAL_FLOW - #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; - HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; + estimator_aid_source1d_s _aid_src_rng_hgt {}; #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - estimator_aid_source2d_s _aid_src_optical_flow{}; + estimator_aid_source2d_s _aid_src_optical_flow {}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -628,41 +637,37 @@ class Ekf final : public EstimatorInterface Vector3f _ref_body_rate{}; Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive - - bool _flow_data_ready{false}; ///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) - estimator_aid_source1d_s _aid_src_airspeed{}; + estimator_aid_source1d_s _aid_src_airspeed {}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_SIDESLIP) - estimator_aid_source1d_s _aid_src_sideslip{}; + estimator_aid_source1d_s _aid_src_sideslip {}; #endif // CONFIG_EKF2_SIDESLIP estimator_aid_source2d_s _aid_src_fake_pos{}; estimator_aid_source1d_s _aid_src_fake_hgt{}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) - estimator_aid_source1d_s _aid_src_ev_hgt{}; + estimator_aid_source1d_s _aid_src_ev_hgt {}; estimator_aid_source2d_s _aid_src_ev_pos{}; estimator_aid_source3d_s _aid_src_ev_vel{}; estimator_aid_source1d_s _aid_src_ev_yaw{}; - float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m) - uint8_t _nb_ev_pos_reset_available{0}; uint8_t _nb_ev_vel_reset_available{0}; uint8_t _nb_ev_yaw_reset_available{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused + bool _gps_data_ready {false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused // variables used for the GPS quality checks Vector3f _gps_pos_deriv_filt{}; ///< GPS NED position derivative (m/sec) Vector2f _gps_velNE_filt{}; ///< filtered GPS North and East velocity (m/sec) - float _gps_velD_diff_filt{0.0f}; ///< GPS filtered Down velocity (m/sec) + float _gps_vel_d_filt{0.0f}; ///< GNSS filtered Down velocity (m/sec) uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time @@ -679,17 +684,16 @@ class Ekf final : public EstimatorInterface estimator_aid_source3d_s _aid_src_gnss_vel{}; # if defined(CONFIG_EKF2_GNSS_YAW) - estimator_aid_source1d_s _aid_src_gnss_yaw{}; - uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + estimator_aid_source1d_s _aid_src_gnss_yaw {}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_GRAVITY_FUSION) - estimator_aid_source3d_s _aid_src_gravity{}; + estimator_aid_source3d_s _aid_src_gravity {}; #endif // CONFIG_EKF2_GRAVITY_FUSION #if defined(CONFIG_EKF2_AUXVEL) - estimator_aid_source2d_s _aid_src_aux_vel{}; + estimator_aid_source2d_s _aid_src_aux_vel {}; #endif // CONFIG_EKF2_AUXVEL // Variables used by the initial filter alignment @@ -698,7 +702,7 @@ class Ekf final : public EstimatorInterface AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) #if defined(CONFIG_EKF2_BAROMETER) - estimator_aid_source1d_s _aid_src_baro_hgt{}; + estimator_aid_source1d_s _aid_src_baro_hgt {}; // Variables used to perform in flight resets and switch between height sources AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) @@ -710,19 +714,10 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_MAGNETOMETER) - float _mag_heading_prev{}; ///< previous value of mag heading (rad) - float _mag_heading_pred_prev{}; ///< previous value of yaw state used by mag heading fusion (rad) - // used by magnetometer fusion mode selection - bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable AlphaFilter _mag_heading_innov_lpf{0.1f}; - float _mag_heading_last_declination{}; ///< last magnetic field declination used for heading fusion (rad) - bool _mag_decl_cov_reset{false}; ///< true after the fuseDeclination() function has been used to modify the earth field covariances after a magnetic field reset event. - uint8_t _nb_mag_heading_reset_available{0}; - uint8_t _nb_mag_3d_reset_available{0}; uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time - estimator_aid_source1d_s _aid_src_mag_heading{}; estimator_aid_source3d_s _aid_src_mag{}; AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) @@ -742,7 +737,7 @@ class Ekf final : public EstimatorInterface // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) uint64_t _time_good_vert_accel{0}; ///< last time a good vertical accel was detected (uSec) - uint16_t _clip_counter{0}; ///< counter that increments when clipping ad decrements when not + uint16_t _clip_counter[3]; ///< counter per axis that increments when clipping ad decrements when not // initialise filter states of both the delayed ekf and the real time complementary filter bool initialiseFilter(void); @@ -764,7 +759,6 @@ class Ekf final : public EstimatorInterface } // update quaternion states and covariances using an innovation, observation variance and Jacobian vector - bool fuseYaw(estimator_aid_source1d_s &aid_src_status); bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const; @@ -772,14 +766,13 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); + bool fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estimator_aid_source3d_s &aid_src, + bool update_all_states = false, bool update_tilt = false); // fuse magnetometer declination measurement // argument passed in is the declination uncertainty in radians bool fuseDeclination(float decl_sigma); - // apply sensible limits to the declination and length of the NE mag field states estimates - void limitDeclination(); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) @@ -802,7 +795,7 @@ class Ekf final : public EstimatorInterface // fuse synthetic zero sideslip measurement void updateSideslip(estimator_aid_source1d_s &_aid_src_sideslip) const; - void fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); + bool fuseSideslip(estimator_aid_source1d_s &_aid_src_sideslip); #endif // CONFIG_EKF2_SIDESLIP #if defined(CONFIG_EKF2_DRAG_FUSION) @@ -821,12 +814,15 @@ class Ekf final : public EstimatorInterface void resetHorizontalVelocityToZero(); void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); + + void resetHorizontalPositionToLastKnown(); - void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); + bool isHeightResetRequired() const; void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); @@ -834,80 +830,65 @@ class Ekf final : public EstimatorInterface void resetVerticalVelocityToZero(); // horizontal and vertical position aid source - void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; - void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const; - - // 2d & 3d velocity aid source - void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const; - void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const; + void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, + const float observation, const float observation_variance, const float innovation_gate = 1.f) const; // horizontal and vertical position fusion - void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); - void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); + bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src); + bool fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src); // 2d & 3d velocity fusion - void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); - void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); + bool fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src); + bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src); #if defined(CONFIG_EKF2_TERRAIN) - // terrain vertical position estimator - void initHagl(); - void runTerrainEstimator(const imuSample &imu_delayed); - void predictHagl(const imuSample &imu_delayed); - - float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } - - void controlHaglFakeFusion(); - void terrainHandleVerticalPositionReset(float delta_z); + void initTerrain(); + float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; } + void controlTerrainFakeFusion(); # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder - void controlHaglRngFusion(); - void updateHaglRng(estimator_aid_source1d_s &aid_src) const; - void fuseHaglRng(estimator_aid_source1d_s &aid_src); - void resetHaglRng(); - void stopHaglRngFusion(); + bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain); + void updateRangeHagl(estimator_aid_source1d_s &aid_src); + void resetTerrainToRng(estimator_aid_source1d_s &aid_src); float getRngVar() const; # endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) - // update the terrain vertical position estimate using an optical flow measurement - void controlHaglFlowFusion(); - void resetHaglFlow(); - void stopHaglFlowFusion(); - void fuseFlowForTerrain(estimator_aid_source2d_s &flow); + void resetTerrainToFlow(); # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - void controlRangeHeightFusion(); + void controlRangeHaglFusion(const imuSample &imu_delayed); bool isConditionalRangeAidSuitable(); void stopRngHgtFusion(); + void stopRngTerrFusion(); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) // control fusion of optical flow observations void controlOpticalFlowFusion(const imuSample &imu_delayed); - void startFlowFusion(); void resetFlowFusion(); void stopFlowFusion(); void updateOnGroundMotionForOpticalFlowChecks(); void resetOnGroundMotionForOpticalFlowChecks(); - // calculate the measurement variance for the optical flow sensor - float calcOptFlowMeasVar(const flowSample &flow_sample); + // calculate the measurement variance for the optical flow sensor (rad/sec)^2 + float calcOptFlowMeasVar(const flowSample &flow_sample) const; // calculate optical flow body angular rate compensation - void calcOptFlowBodyRateComp(const imuSample &imu_delayed); + void calcOptFlowBodyRateComp(const flowSample &flow_sample); + + float predictFlowRange() const; + Vector2f predictFlow(const Vector3f &flow_gyro) const; // fuse optical flow line of sight rate measurements - void updateOptFlow(estimator_aid_source2d_s &aid_src); - void fuseOptFlow(); - float predictFlowRange(); - Vector2f predictFlowVelBody(); + bool fuseOptFlow(VectorState &H, bool update_terrain); + #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -930,6 +911,7 @@ class Ekf final : public EstimatorInterface } #if defined(CONFIG_EKF2_MAGNETOMETER) + if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_I.dof; i++) { K(State::mag_I.idx + i) = 0.f; @@ -941,14 +923,17 @@ class Ekf final : public EstimatorInterface K(State::mag_B.idx + i) = 0.f; } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + if (!_control_status.flags.wind) { for (unsigned i = 0; i < State::wind_vel.dof; i++) { K(State::wind_vel.idx + i) = 0.f; } } + #endif // CONFIG_EKF2_WIND } @@ -962,10 +947,6 @@ class Ekf final : public EstimatorInterface // and a scalar innovation value void fuse(const VectorState &K, float innovation); -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const; -#endif // CONFIG_EKF2_BARO_COMPENSATION - // calculate the earth rotation vector from a given latitude Vector3f calcEarthRateNED(float lat_rad) const; @@ -974,26 +955,43 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_EXTERNAL_VISION) // control fusion of external vision observations - void controlExternalVisionFusion(); + void controlExternalVisionFusion(const imuSample &imu_sample); void updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset); - void controlEvHeightFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); - void controlEvPosFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source2d_s &aid_src); - void controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src); - void controlEvYawFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, estimator_aid_source1d_s &aid_src); + void controlEvHeightFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src); + void controlEvPosFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source2d_s &aid_src); + void controlEvVelFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source3d_s &aid_src); + void controlEvYawFusion(const imuSample &imu_sample, const extVisionSample &ev_sample, + const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient, + estimator_aid_source1d_s &aid_src); + void resetVelocityToEV(const Vector3f &measurement, const Vector3f &measurement_var, const VelocityFrame &vel_frame); + Vector3f rotateVarianceToEkf(const Vector3f &measurement_var); void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); - void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, + bool reset, estimator_aid_source2d_s &aid_src); void stopEvPosFusion(); void stopEvHgtFusion(); void stopEvVelFusion(); void stopEvYawFusion(); + bool fuseEvVelocity(estimator_aid_source3d_s &aid_src, const extVisionSample &ev_sample); + void fuseBodyVelocity(estimator_aid_source1d_s &aid_src, float &innov_var, VectorState &H) + { + VectorState Kfusion = P * H / innov_var; + aid_src.fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation); + } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) // control fusion of GPS observations void controlGpsFusion(const imuSample &imu_delayed); void stopGpsFusion(); - void updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); + void updateGnssVel(const imuSample &imu_sample, const gnssSample &gnss_sample, estimator_aid_source3d_s &aid_src); void updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src); void controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel); bool tryYawEmergencyReset(); @@ -1014,17 +1012,17 @@ class Ekf final : public EstimatorInterface void resetGpsDriftCheckFilters(); # if defined(CONFIG_EKF2_GNSS_YAW) - void controlGpsYawFusion(const gnssSample &gps_sample); - void stopGpsYawFusion(); + void controlGnssYawFusion(const gnssSample &gps_sample); + void stopGnssYawFusion(); // fuse the yaw angle obtained from a dual antenna GPS unit - void fuseGpsYaw(float antenna_yaw_offset); + void fuseGnssYaw(float antenna_yaw_offset); // reset the quaternions states using the yaw angle obtained from a dual antenna GPS unit // return true if the reset was successful - bool resetYawToGps(float gnss_yaw, float gnss_yaw_offset); + bool resetYawToGnss(float gnss_yaw, float gnss_yaw_offset); - void updateGpsYaw(const gnssSample &gps_sample); + void updateGnssYaw(const gnssSample &gps_sample); # endif // CONFIG_EKF2_GNSS_YAW @@ -1042,9 +1040,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) // control fusion of magnetometer observations - void controlMagFusion(); - void controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source1d_s &aid_src); - void controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, estimator_aid_source3d_s &aid_src); + void controlMagFusion(const imuSample &imu_sample); bool checkHaglYawResetReq() const; @@ -1052,13 +1048,11 @@ class Ekf final : public EstimatorInterface void resetMagStates(const Vector3f &mag, bool reset_heading = true); bool haglYawResetReq(); - void checkYawAngleObservability(); - void checkMagHeadingConsistency(); + void checkMagHeadingConsistency(const magSample &mag_sample); bool checkMagField(const Vector3f &mag); static bool isMeasuredMatchingExpected(float measured, float expected, float gate); - void stopMagHdgFusion(); void stopMagFusion(); // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter @@ -1079,7 +1073,7 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_AUXVEL) // control fusion of auxiliary velocity observations - void controlAuxVelFusion(); + void controlAuxVelFusion(const imuSample &imu_sample); void stopAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL @@ -1092,10 +1086,15 @@ class Ekf final : public EstimatorInterface void checkHeightSensorRefFallback(); #if defined(CONFIG_EKF2_BAROMETER) - void controlBaroHeightFusion(); + void controlBaroHeightFusion(const imuSample &imu_sample); void stopBaroHgtFusion(); void updateGroundEffect(); + +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + float compensateBaroForDynamicPressure(const imuSample &imu_sample, float baro_alt_uncompensated) const; +# endif // CONFIG_EKF2_BARO_COMPENSATION + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GRAVITY_FUSION) @@ -1104,24 +1103,19 @@ class Ekf final : public EstimatorInterface #endif // CONFIG_EKF2_GRAVITY_FUSION void resetQuatCov(const float yaw_noise = NAN); - void resetQuatCov(const Vector3f &euler_noise_ned); + void resetQuatCov(const Vector3f &rot_var_ned); #if defined(CONFIG_EKF2_MAGNETOMETER) void resetMagCov(); #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) - // perform a reset of the wind states and related covariances - void resetWind(); void resetWindCov(); void resetWindToZero(); #endif // CONFIG_EKF2_WIND void resetGyroBiasZCov(); - // uncorrelate quaternion states from other states - void uncorrelateQuatFromOtherStates(); - bool isTimedOut(uint64_t last_sensor_timestamp, uint64_t timeout_period) const { return (last_sensor_timestamp == 0) || (last_sensor_timestamp + timeout_period < _time_delayed_us); @@ -1149,101 +1143,213 @@ class Ekf final : public EstimatorInterface PositionSensor _position_sensor_ref{PositionSensor::GNSS}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) - HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; + HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref}; AlphaFilter _ev_q_error_filt{0.001f}; bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION - void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const + // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) + void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; + status.time_last_fuse = _time_delayed_us; - // preserve status.time_last_fuse + status.innovation = 0.f; + status.innovation_filtered = 0.f; + status.innovation_variance = status.observation_variance; - status.observation = 0; - status.observation_variance = 0; + status.test_ratio = 0.f; + status.test_ratio_filtered = 0.f; - status.innovation = 0; - status.innovation_variance = 0; - status.test_ratio = INFINITY; - - status.innovation_rejected = true; - status.fused = false; - } + status.innovation_rejected = false; + status.fused = true; } - template - void resetEstimatorAidStatus(T &status) const + // helper used for populating and filtering estimator aid source struct for logging + void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, + const float &observation, const float &observation_variance, + const float &innovation, const float &innovation_variance, + float innovation_gate = 1.f) const { - // only bother resetting if timestamp_sample is set - if (status.timestamp_sample != 0) { - status.timestamp_sample = 0; + bool innovation_rejected = false; - // preserve status.time_last_fuse + const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance); - for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { - status.observation[i] = 0; - status.observation_variance[i] = 0; + if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) { - status.innovation[i] = 0; - status.innovation_variance[i] = 0; - status.test_ratio[i] = INFINITY; + const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f); + + static constexpr float tau = 0.5f; + const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f); + + // test_ratio_filtered + if (PX4_ISFINITE(status.test_ratio_filtered)) { + status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered); + + } else { + // otherwise, init the filtered test ratio + status.test_ratio_filtered = test_ratio; + } + + // innovation_filtered + if (PX4_ISFINITE(status.innovation_filtered)) { + status.innovation_filtered += alpha * (innovation - status.innovation_filtered); + + } else { + // otherwise, init the filtered innovation + status.innovation_filtered = innovation; } - status.innovation_rejected = true; - status.fused = false; + + // limit extremes in filtered values + static constexpr float kNormalizedInnovationLimit = 2.f; + static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit); + + if (test_ratio > kTestRatioLimit) { + + status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit); + + const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance); + status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit); + } + + } else { + // invalid timestamp_sample, reset + status.test_ratio_filtered = test_ratio; + status.innovation_filtered = innovation; } + + status.test_ratio = test_ratio; + + status.observation = observation; + status.observation_variance = observation_variance; + + status.innovation = innovation; + status.innovation_variance = innovation_variance; + + if ((test_ratio > 1.f) + || !PX4_ISFINITE(test_ratio) + || !PX4_ISFINITE(status.innovation) + || !PX4_ISFINITE(status.innovation_variance) + ) { + innovation_rejected = true; + } + + status.timestamp_sample = timestamp_sample; + + // if any of the innovations are rejected, then the overall innovation is rejected + status.innovation_rejected = innovation_rejected; + + // reset + status.fused = false; } - void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const + // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) + template + void resetAidSourceStatusZeroInnovation(T &status) const { - if (PX4_ISFINITE(status.innovation) - && PX4_ISFINITE(status.innovation_variance) - && (status.innovation_variance > 0.f) - ) { - status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance); - status.innovation_rejected = (status.test_ratio > 1.f); + status.time_last_fuse = _time_delayed_us; - } else { - status.test_ratio = INFINITY; - status.innovation_rejected = true; + for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { + status.innovation[i] = 0.f; + status.innovation_filtered[i] = 0.f; + status.innovation_variance[i] = status.observation_variance[i]; + + status.test_ratio[i] = 0.f; + status.test_ratio_filtered[i] = 0.f; } + + status.innovation_rejected = false; + status.fused = true; } - template - void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const + // helper used for populating and filtering estimator aid source struct for logging + template + void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample, + const S &observation, const S &observation_variance, + const S &innovation, const S &innovation_variance, + float innovation_gate = 1.f) const { bool innovation_rejected = false; - for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) { - if (PX4_ISFINITE(status.innovation[i]) - && PX4_ISFINITE(status.innovation_variance[i]) - && (status.innovation_variance[i] > 0.f) - ) { - status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]); + const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f); + + static constexpr float tau = 0.5f; + const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f); + + for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) { - if (status.test_ratio[i] > 1.f) { - innovation_rejected = true; + const float test_ratio = sq(innovation(i)) / (sq(innovation_gate) * innovation_variance(i)); + + if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) { + + // test_ratio_filtered + if (PX4_ISFINITE(status.test_ratio_filtered[i])) { + status.test_ratio_filtered[i] += alpha * (matrix::sign(innovation(i)) * test_ratio - status.test_ratio_filtered[i]); + + } else { + // otherwise, init the filtered test ratio + status.test_ratio_filtered[i] = test_ratio; + } + + // innovation_filtered + if (PX4_ISFINITE(status.innovation_filtered[i])) { + status.innovation_filtered[i] += alpha * (innovation(i) - status.innovation_filtered[i]); + + } else { + // otherwise, init the filtered innovation + status.innovation_filtered[i] = innovation(i); + } + + // limit extremes in filtered values + static constexpr float kNormalizedInnovationLimit = 2.f; + static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit); + + if (test_ratio > kTestRatioLimit) { + + status.test_ratio_filtered[i] = math::constrain(status.test_ratio_filtered[i], -kTestRatioLimit, kTestRatioLimit); + + const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance(i)); + status.innovation_filtered[i] = math::constrain(status.innovation_filtered[i], -innov_limit, innov_limit); } } else { - status.test_ratio[i] = INFINITY; + // invalid timestamp_sample, reset + status.test_ratio_filtered[i] = test_ratio; + status.innovation_filtered[i] = innovation(i); + } + + status.test_ratio[i] = test_ratio; + + status.observation[i] = observation(i); + status.observation_variance[i] = observation_variance(i); + + status.innovation[i] = innovation(i); + status.innovation_variance[i] = innovation_variance(i); + + if ((test_ratio > 1.f) + || !PX4_ISFINITE(test_ratio) + || !PX4_ISFINITE(status.innovation[i]) + || !PX4_ISFINITE(status.innovation_variance[i]) + ) { innovation_rejected = true; } } + status.timestamp_sample = timestamp_sample; + // if any of the innovations are rejected, then the overall innovation is rejected status.innovation_rejected = innovation_rejected; + + // reset + status.fused = false; } ZeroGyroUpdate _zero_gyro_update{}; ZeroVelocityUpdate _zero_velocity_update{}; #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) - AuxGlobalPosition _aux_global_position{}; + AuxGlobalPosition _aux_global_position {}; #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION }; diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 032d35fb31a6..ae3e9e355e0a 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -56,43 +56,6 @@ bool Ekf::isHeightResetRequired() const return (continuous_bad_accel_hgt || hgt_fusion_timeout); } -#if defined(CONFIG_EKF2_BARO_COMPENSATION) -float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const -{ - if (_control_status.flags.wind && local_position_is_valid()) { - // calculate static pressure error = Pmeas - Ptruth - // model position error sensitivity as a body fixed ellipse with a different scale in the positive and - // negative X and Y directions. Used to correct baro data for positional errors - - // Calculate airspeed in body frame - const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body); - const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned; - - const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f); - - const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth; - - const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth); - - const Vector3f K_pstatic_coef( - airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn, - airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn, - _params.static_pressure_coef_z); - - const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed)); - - const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef)); - - // correct baro measurement using pressure error estimate and assuming sea level gravity - return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); - } - - // otherwise return the uncorrected baro measurement - return baro_alt_uncompensated; -} -#endif // CONFIG_EKF2_BARO_COMPENSATION - -// calculate the earth rotation vector Vector3f Ekf::calcEarthRateNED(float lat_rad) const { return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad), @@ -109,13 +72,14 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo return _NED_origin_initialised; } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, const float epv) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, + const float epv) { // sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90) - && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) - && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) - ) { + && PX4_ISFINITE(longitude) && (abs(longitude) <= 180) + && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f) + ) { bool current_pos_available = false; double current_lat = static_cast(NAN); double current_lon = static_cast(NAN); @@ -126,55 +90,37 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons current_pos_available = true; } - const float gps_alt_ref_prev = getEkfGlobalOriginAltitude(); + const float gps_alt_ref_prev = _gps_alt_ref; // reinitialize map projection to latitude, longitude, altitude, and reset position _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; -#if defined(CONFIG_EKF2_MAGNETOMETER) - const float mag_declination_gps = get_mag_declination_radians(latitude, longitude); - const float mag_inclination_gps = get_mag_inclination_radians(latitude, longitude); - const float mag_strength_gps = get_mag_strength_gauss(latitude, longitude); - - if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { - _mag_declination_gps = mag_declination_gps; - _mag_inclination_gps = mag_inclination_gps; - _mag_strength_gps = mag_strength_gps; - - _wmm_gps_time_last_set = _time_delayed_us; - } -#endif // CONFIG_EKF2_MAGNETOMETER + updateWmm(current_lat, current_lon); _gpos_origin_eph = eph; _gpos_origin_epv = epv; _NED_origin_initialised = true; - // minimum change in position or height that triggers a reset - static constexpr float MIN_RESET_DIST_M = 0.01f; - if (current_pos_available) { - // reset horizontal position + // reset horizontal position if we already have a global origin Vector2f position = _pos_ref.project(current_lat, current_lon); - - if (Vector2f(position - Vector2f(_state.pos)).longerThan(MIN_RESET_DIST_M)) { - resetHorizontalPositionTo(position); - } + resetHorizontalPositionTo(position); } - // reset vertical position (if there's any change) - if (fabsf(altitude - gps_alt_ref_prev) > MIN_RESET_DIST_M) { + if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) { // determine current z - float current_alt = -_state.pos(2) + gps_alt_ref_prev; - + const float z_prev = _state.pos(2); + const float current_alt = -z_prev + gps_alt_ref_prev; #if defined(CONFIG_EKF2_GNSS) const float gps_hgt_bias = _gps_hgt_b_est.getBias(); #endif // CONFIG_EKF2_GNSS resetVerticalPositionTo(_gps_alt_ref - current_alt); - + ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, + (double)_state.pos(2)); #if defined(CONFIG_EKF2_GNSS) - // preserve GPS height bias + // adjust existing GPS height bias _gps_hgt_b_est.setBias(gps_hgt_bias); #endif // CONFIG_EKF2_GNSS } @@ -185,6 +131,39 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons return false; } +void Ekf::updateWmm(const double lat, const double lon) +{ +#if defined(CONFIG_EKF2_MAGNETOMETER) + + // set the magnetic field data returned by the geo library using the current GPS position + const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon)); + const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon)); + const float mag_strength_gps = get_mag_strength_gauss(lat, lon); + + if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) { + + const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f)); + const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f)); + + if ((_wmm_gps_time_last_set == 0) + || !PX4_ISFINITE(_mag_declination_gps) + || !PX4_ISFINITE(_mag_inclination_gps) + || !PX4_ISFINITE(_mag_strength_gps) + || mag_declination_changed + || mag_inclination_changed + ) { + _mag_declination_gps = mag_declination_gps; + _mag_inclination_gps = mag_inclination_gps; + _mag_strength_gps = mag_strength_gps; + + _wmm_gps_time_last_set = _time_delayed_us; + } + } + +#endif // CONFIG_EKF2_MAGNETOMETER +} + + void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { float eph = INFINITY; @@ -219,15 +198,19 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const // and using state variances for accuracy reporting is overly optimistic in these situations if (_horizontal_deadreckon_time_exceeded) { #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_gnss_pos.innovation).norm()); } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { hpos_err = math::max(hpos_err, Vector2f(_aid_src_ev_pos.innovation).norm()); } + #endif // CONFIG_EKF2_EXTERNAL_VISION } @@ -235,7 +218,6 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2)); } -// get the 1-sigma horizontal and vertical velocity uncertainty void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const { float hvel_err = sqrtf(P.trace<2>(State::vel.idx)); @@ -247,19 +229,24 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const float vel_err_conservative = 0.0f; #if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (_control_status.flags.opt_flow) { float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f); - vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); + vel_err_conservative = math::max(getHagl(), gndclearance) * Vector2f(_aid_src_optical_flow.innovation).norm(); } + #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_gnss_pos.innovation).norm()); } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_pos) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_pos.innovation).norm()); } @@ -267,6 +254,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const if (_control_status.flags.ev_vel) { vel_err_conservative = math::max(vel_err_conservative, Vector2f(_aid_src_ev_vel.innovation).norm()); } + #endif // CONFIG_EKF2_EXTERNAL_VISION hvel_err = math::max(hvel_err, vel_err_conservative); @@ -276,13 +264,6 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } -/* -Returns the following vehicle control limits required by the estimator to keep within sensor limitations. -vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed. -vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed. -hagl_min : Minimum height above ground (meters). NaN when limiting is not needed. -hagl_max : Maximum height above ground (meters). NaN when limiting is not needed. -*/ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const { // Do not require limiting by default @@ -295,8 +276,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // Calculate range finder limits const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); - // Allow use of 75% of rangefinder maximum range to allow for angular motion - const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal(); + // Allow use of 90% of rangefinder maximum range to allow for angular motion + const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal(); // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); @@ -313,10 +294,18 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl if (relying_on_optical_flow) { // Calculate optical flow limits - const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance); - const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance); + float flow_hagl_min = _flow_min_distance; + float flow_hagl_max = _flow_max_distance; + + // only limit optical flow height is dependent on range finder or terrain estimate invalid (precaution) + if ((!_control_status.flags.opt_flow_terrain && _control_status.flags.rng_terrain) + || !isTerrainEstimateValid() + ) { + flow_hagl_min = math::max(flow_hagl_min, rangefinder_hagl_min); + flow_hagl_max = math::min(flow_hagl_max, rangefinder_hagl_max); + } - const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max); + const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max); // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; @@ -325,17 +314,12 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = flow_hagl_min; *hagl_max = flow_hagl_max; } + # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_RANGE_FINDER } -void Ekf::resetImuBias() -{ - resetGyroBias(); - resetAccelBias(); -} - void Ekf::resetGyroBias() { // Zero the gyro bias states @@ -344,13 +328,6 @@ void Ekf::resetGyroBias() resetGyroBiasCov(); } -void Ekf::resetGyroBiasCov() -{ - // Zero the corresponding covariances and set - // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); -} - void Ekf::resetAccelBias() { // Zero the accel bias states @@ -359,200 +336,339 @@ void Ekf::resetAccelBias() resetAccelBiasCov(); } -void Ekf::resetAccelBiasCov() -{ - // Zero the corresponding covariances and set - // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); -} - -// get EKF innovation consistency check status information comprising of: -// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check -// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold. -// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF -// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned. -void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas, - float &hagl, float &beta) const +float Ekf::getHeadingInnovationTestRatio() const { - // return the integer bitmask containing the consistency check pass/fail status - status = _innov_check_fail_status.value; - - // return the largest magnetometer innovation test ratio - mag = 0.f; + // return the largest heading innovation test ratio + float test_ratio = -1.f; #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg) { - mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio)); - } - if (_control_status.flags.mag_3D) { - mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max())); + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + for (auto &test_ratio_filtered : _aid_src_mag.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) - if (_control_status.flags.gps_yaw) { - mag = math::max(mag, sqrtf(_aid_src_gnss_yaw.test_ratio)); + + if (_control_status.flags.gnss_yaw) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_yaw.test_ratio_filtered)); } + #endif // CONFIG_EKF2_GNSS_YAW #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_yaw) { - mag = math::max(mag, sqrtf(_aid_src_ev_yaw.test_ratio)); + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_yaw.test_ratio_filtered)); } + #endif // CONFIG_EKF2_EXTERNAL_VISION - // return the largest velocity and position innovation test ratio - vel = NAN; - pos = NAN; + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; +} + +float Ekf::getHorizontalVelocityInnovationTestRatio() const +{ + // return the largest velocity innovation test ratio + float test_ratio = -1.f; #if defined(CONFIG_EKF2_GNSS) - if (_control_status.flags.gps) { - float gps_vel = sqrtf(Vector3f(_aid_src_gnss_vel.test_ratio).max()); - vel = math::max(gps_vel, FLT_MIN); - float gps_pos = sqrtf(Vector2f(_aid_src_gnss_pos.test_ratio).max()); - pos = math::max(gps_pos, FLT_MIN); + if (_control_status.flags.gps) { + for (int i = 0; i < 2; i++) { // only xy + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); + } } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_vel) { - float ev_vel = sqrtf(Vector3f(_aid_src_ev_vel.test_ratio).max()); - vel = math::max(vel, ev_vel, FLT_MIN); + for (int i = 0; i < 2; i++) { // only xy + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); + } } - if (_control_status.flags.ev_pos) { - float ev_pos = sqrtf(Vector2f(_aid_src_ev_pos.test_ratio).max()); - pos = math::max(pos, ev_pos, FLT_MIN); - } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) + if (isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)) { - float of_vel = sqrtf(Vector2f(_aid_src_optical_flow.test_ratio).max()); - vel = math::max(of_vel, FLT_MIN); + for (auto &test_ratio_filtered : _aid_src_optical_flow.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } } + #endif // CONFIG_EKF2_OPTICAL_FLOW + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; +} + +float Ekf::getVerticalVelocityInnovationTestRatio() const +{ + // return the largest velocity innovation test ratio + float test_ratio = -1.f; + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2])); + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_vel) { + test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[2])); + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; +} + +float Ekf::getHorizontalPositionInnovationTestRatio() const +{ + // return the largest position innovation test ratio + float test_ratio = -1.f; + +#if defined(CONFIG_EKF2_GNSS) + + if (_control_status.flags.gps) { + for (auto &test_ratio_filtered : _aid_src_gnss_pos.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_pos) { + for (auto &test_ratio_filtered : _aid_src_ev_pos.test_ratio_filtered) { + test_ratio = math::max(test_ratio, fabsf(test_ratio_filtered)); + } + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + +#if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME) + + if (_control_status.flags.aux_gpos) { + test_ratio = math::max(test_ratio, fabsf(_aux_global_position.test_ratio_filtered())); + } + +#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION + + if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { + return sqrtf(test_ratio); + } + + return NAN; +} + +float Ekf::getVerticalPositionInnovationTestRatio() const +{ // return the combined vertical position innovation test ratio float hgt_sum = 0.f; int n_hgt_sources = 0; #if defined(CONFIG_EKF2_BAROMETER) + if (_control_status.flags.baro_hgt) { - hgt_sum += sqrtf(_aid_src_baro_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_baro_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps_hgt) { - hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_gnss_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { - hgt_sum += sqrtf(_aid_src_rng_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_hgt) { - hgt_sum += sqrtf(_aid_src_ev_hgt.test_ratio); + hgt_sum += sqrtf(fabsf(_aid_src_ev_hgt.test_ratio_filtered)); n_hgt_sources++; } + #endif // CONFIG_EKF2_EXTERNAL_VISION if (n_hgt_sources > 0) { - hgt = math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); - - } else { - hgt = NAN; + return math::max(hgt_sum / static_cast(n_hgt_sources), FLT_MIN); } + return NAN; +} + +float Ekf::getAirspeedInnovationTestRatio() const +{ #if defined(CONFIG_EKF2_AIRSPEED) - // return the airspeed fusion innovation test ratio - tas = sqrtf(_aid_src_airspeed.test_ratio); + + if (_control_status.flags.fuse_aspd) { + // return the airspeed fusion innovation test ratio + return sqrtf(fabsf(_aid_src_airspeed.test_ratio_filtered)); + } + #endif // CONFIG_EKF2_AIRSPEED - hagl = NAN; -#if defined(CONFIG_EKF2_TERRAIN) -# if defined(CONFIG_EKF2_RANGE_FINDER) - if (_hagl_sensor_status.flags.range_finder) { - // return the terrain height innovation test ratio - hagl = sqrtf(_aid_src_terrain_range_finder.test_ratio); + return NAN; +} + +float Ekf::getSyntheticSideslipInnovationTestRatio() const +{ +#if defined(CONFIG_EKF2_SIDESLIP) + + if (_control_status.flags.fuse_beta) { + // return the synthetic sideslip innovation test ratio + return sqrtf(fabsf(_aid_src_sideslip.test_ratio_filtered)); } -#endif // CONFIG_EKF2_RANGE_FINDER + +#endif // CONFIG_EKF2_SIDESLIP + + return NAN; +} + +float Ekf::getHeightAboveGroundInnovationTestRatio() const +{ + // return the combined HAGL innovation test ratio + float hagl_sum = 0.f; + int n_hagl_sources = 0; + +#if defined(CONFIG_EKF2_TERRAIN) # if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_hagl_sensor_status.flags.flow) { - // return the terrain height innovation test ratio - hagl = sqrtf(math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1])); + + if (_control_status.flags.opt_flow_terrain) { + hagl_sum += sqrtf(math::max(fabsf(_aid_src_optical_flow.test_ratio_filtered[0]), + _aid_src_optical_flow.test_ratio_filtered[1])); + n_hagl_sources++; } + # endif // CONFIG_EKF2_OPTICAL_FLOW + +# if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_control_status.flags.rng_terrain) { + hagl_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered)); + n_hagl_sources++; + } + +# endif // CONFIG_EKF2_RANGE_FINDER + #endif // CONFIG_EKF2_TERRAIN -#if defined(CONFIG_EKF2_SIDESLIP) - // return the synthetic sideslip innovation test ratio - beta = sqrtf(_aid_src_sideslip.test_ratio); -#endif // CONFIG_EKF2_SIDESLIP + if (n_hagl_sources > 0) { + return math::max(hagl_sum / static_cast(n_hagl_sources), FLT_MIN); + } + + return NAN; } -// return a bitmask integer that describes which state estimates are valid -void Ekf::get_ekf_soln_status(uint16_t *status) const +uint16_t Ekf::get_ekf_soln_status() const { - ekf_solution_status_u soln_status{}; - // TODO: Is this accurate enough? + // LEGACY Mavlink bitmask containing state of estimator solution (see Mavlink ESTIMATOR_STATUS_FLAGS) + union ekf_solution_status_u { + struct { + uint16_t attitude : 1; + uint16_t velocity_horiz : 1; + uint16_t velocity_vert : 1; + uint16_t pos_horiz_rel : 1; + uint16_t pos_horiz_abs : 1; + uint16_t pos_vert_abs : 1; + uint16_t pos_vert_agl : 1; + uint16_t const_pos_mode : 1; + uint16_t pred_pos_horiz_rel : 1; + uint16_t pred_pos_horiz_abs : 1; + uint16_t gps_glitch : 1; + uint16_t accel_error : 1; + } flags; + uint16_t value; + } soln_status{}; + + // 1 ESTIMATOR_ATTITUDE True if the attitude estimate is good soln_status.flags.attitude = attitude_valid(); - soln_status.flags.velocity_horiz = (isHorizontalAidingActive() || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0); - soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); - soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); - soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; + + // 2 ESTIMATOR_VELOCITY_HORIZ True if the horizontal velocity estimate is good + soln_status.flags.velocity_horiz = local_position_is_valid(); + + // 4 ESTIMATOR_VELOCITY_VERT True if the vertical velocity estimate is good + soln_status.flags.velocity_vert = isLocalVerticalVelocityValid() || isLocalVerticalPositionValid(); + + // 8 ESTIMATOR_POS_HORIZ_REL True if the horizontal position (relative) estimate is good + soln_status.flags.pos_horiz_rel = local_position_is_valid(); + + // 16 ESTIMATOR_POS_HORIZ_ABS True if the horizontal position (absolute) estimate is good + soln_status.flags.pos_horiz_abs = global_position_is_valid(); + + // 32 ESTIMATOR_POS_VERT_ABS True if the vertical position (absolute) estimate is good + soln_status.flags.pos_vert_abs = isVerticalAidingActive(); + + // 64 ESTIMATOR_POS_VERT_AGL True if the vertical position (above ground) estimate is good #if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); #endif // CONFIG_EKF2_TERRAIN - soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; - soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; - soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; - bool mag_innov_good = true; + // 128 ESTIMATOR_CONST_POS_MODE True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) + soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest; -#if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag_hdg) { - if (_aid_src_mag_heading.test_ratio < 1.f) { - mag_innov_good = false; - } + // 256 ESTIMATOR_PRED_POS_HORIZ_REL True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate + soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive(); - } else if (_control_status.flags.mag_3D) { - if (Vector3f(_aid_src_mag.test_ratio).max() < 1.f) { - mag_innov_good = false; - } - } -#endif // CONFIG_EKF2_MAGNETOMETER + // 512 ESTIMATOR_PRED_POS_HORIZ_ABS True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate + soln_status.flags.pred_pos_horiz_abs = _control_status.flags.gps || _control_status.flags.aux_gpos; + // 1024 ESTIMATOR_GPS_GLITCH True if the EKF has detected a GPS glitch #if defined(CONFIG_EKF2_GNSS) const bool gps_vel_innov_bad = Vector3f(_aid_src_gnss_vel.test_ratio).max() > 1.f; const bool gps_pos_innov_bad = Vector2f(_aid_src_gnss_pos.test_ratio).max() > 1.f; - - soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad) && mag_innov_good; -#else - (void)mag_innov_good; + soln_status.flags.gps_glitch = (gps_vel_innov_bad || gps_pos_innov_bad); #endif // CONFIG_EKF2_GNSS - soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical; - *status = soln_status.value; + // 2048 ESTIMATOR_ACCEL_ERROR True if the EKF has detected bad accelerometer data + soln_status.flags.accel_error = _fault_status.flags.bad_acc_vertical || _fault_status.flags.bad_acc_clipping; + + return soln_status.value; } void Ekf::fuse(const VectorState &K, float innovation) { // quat_nominal - Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, 0) * (-1.f * innovation))); - _state.quat_nominal *= delta_quat; + Quatf delta_quat(matrix::AxisAnglef(K.slice(State::quat_nominal.idx, + 0) * (-1.f * innovation))); + _state.quat_nominal = delta_quat * _state.quat_nominal; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); @@ -563,32 +679,40 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); // gyro_bias - _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, 0) * innovation, - -getGyroBiasLimit(), getGyroBiasLimit()); + _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, + 0) * innovation, + -getGyroBiasLimit(), getGyroBiasLimit()); // accel_bias - _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, 0) * innovation, - -getAccelBiasLimit(), getAccelBiasLimit()); + _state.accel_bias = matrix::constrain(_state.accel_bias - K.slice(State::accel_bias.idx, + 0) * innovation, + -getAccelBiasLimit(), getAccelBiasLimit()); #if defined(CONFIG_EKF2_MAGNETOMETER) + // mag_I, mag_B if (_control_status.flags.mag) { - _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, 1.f); - _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, -getMagBiasLimit(), getMagBiasLimit()); + _state.mag_I = matrix::constrain(_state.mag_I - K.slice(State::mag_I.idx, 0) * innovation, -1.f, + 1.f); + _state.mag_B = matrix::constrain(_state.mag_B - K.slice(State::mag_B.idx, 0) * innovation, + -getMagBiasLimit(), getMagBiasLimit()); } + #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_WIND) + // wind_vel if (_control_status.flags.wind) { - _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, 0) * innovation, -1.e2f, 1.e2f); + _state.wind_vel = matrix::constrain(_state.wind_vel - K.slice(State::wind_vel.idx, + 0) * innovation, -1.e2f, 1.e2f); } + #endif // CONFIG_EKF2_WIND -} -void Ekf::uncorrelateQuatFromOtherStates() -{ - P.uncorrelateCovarianceBlock(State::quat_nominal.idx); +#if defined(CONFIG_EKF2_TERRAIN) + _state.terrain = math::constrain(_state.terrain - K(State::terrain.idx) * innovation, -1e4f, 1e4f); +#endif // CONFIG_EKF2_TERRAIN } void Ekf::updateDeadReckoningStatus() @@ -599,44 +723,95 @@ void Ekf::updateDeadReckoningStatus() void Ekf::updateHorizontalDeadReckoningstatus() { - const bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.aux_gpos) - && (isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) - || isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max)); + bool inertial_dead_reckoning = true; + bool aiding_expected_in_air = false; + + // velocity aiding active + if ((_control_status.flags.gps || _control_status.flags.ev_vel) + && isRecent(_time_last_hor_vel_fuse, _params.no_aid_timeout_max) + ) { + inertial_dead_reckoning = false; + } + + // position aiding active + if ((_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.aux_gpos) + && isRecent(_time_last_hor_pos_fuse, _params.no_aid_timeout_max) + ) { + inertial_dead_reckoning = false; + } - bool optFlowAiding = false; #if defined(CONFIG_EKF2_OPTICAL_FLOW) - optFlowAiding = _control_status.flags.opt_flow && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max); -#endif // CONFIG_EKF2_OPTICAL_FLOW - bool airDataAiding = false; + // optical flow active + if (_control_status.flags.opt_flow + && isRecent(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) + ) { + inertial_dead_reckoning = false; + + } else { + if (!_control_status.flags.in_air && (_params.flow_ctrl == 1) + && isRecent(_aid_src_optical_flow.timestamp_sample, _params.no_aid_timeout_max) + ) { + // currently landed, but optical flow aiding should be possible once in air + aiding_expected_in_air = true; + } + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AIRSPEED) - airDataAiding = _control_status.flags.wind && - isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max) && - isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max); - _control_status.flags.wind_dead_reckoning = !velPosAiding && !optFlowAiding && airDataAiding; -#else - _control_status.flags.wind_dead_reckoning = false; + // air data aiding active + if ((_control_status.flags.fuse_aspd && isRecent(_aid_src_airspeed.time_last_fuse, _params.no_aid_timeout_max)) + && (_control_status.flags.fuse_beta && isRecent(_aid_src_sideslip.time_last_fuse, _params.no_aid_timeout_max)) + ) { + // wind_dead_reckoning: no other aiding but air data + _control_status.flags.wind_dead_reckoning = inertial_dead_reckoning; + + // air data aiding is active, we're not inertial dead reckoning + inertial_dead_reckoning = false; + + } else { + _control_status.flags.wind_dead_reckoning = false; + + if (!_control_status.flags.in_air && _control_status.flags.fixed_wing + && (_params.beta_fusion_enabled == 1) + && (_params.arsp_thr > 0.f) && isRecent(_aid_src_airspeed.timestamp_sample, _params.no_aid_timeout_max) + ) { + // currently landed, but air data aiding should be possible once in air + aiding_expected_in_air = true; + } + } + #endif // CONFIG_EKF2_AIRSPEED - _control_status.flags.inertial_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding; + // zero velocity update + if (isRecent(_zero_velocity_update.time_last_fuse(), _params.no_aid_timeout_max)) { + // only respect as a valid aiding source now if we expect to have another valid source once in air + if (aiding_expected_in_air) { + inertial_dead_reckoning = false; + } + } - if (!_control_status.flags.inertial_dead_reckoning) { + if (inertial_dead_reckoning) { + if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) { + // deadreckon time exceeded + if (!_horizontal_deadreckon_time_exceeded) { + ECL_WARN("horizontal dead reckon time exceeded"); + _horizontal_deadreckon_time_exceeded = true; + } + } + + } else { if (_time_delayed_us > _params.no_aid_timeout_max) { _time_last_horizontal_aiding = _time_delayed_us - _params.no_aid_timeout_max; } - } - // report if we have been deadreckoning for too long, initial state is deadreckoning until aiding is present - bool deadreckon_time_exceeded = isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max); + _horizontal_deadreckon_time_exceeded = false; - if (!_horizontal_deadreckon_time_exceeded && deadreckon_time_exceeded) { - // deadreckon time now exceeded - ECL_WARN("dead reckon time exceeded"); } - _horizontal_deadreckon_time_exceeded = deadreckon_time_exceeded; + _control_status.flags.inertial_dead_reckoning = inertial_dead_reckoning; } void Ekf::updateVerticalDeadReckoningStatus() @@ -660,10 +835,16 @@ void Ekf::updateVerticalDeadReckoningStatus() } } -Vector3f Ekf::getRotVarNed() const +Vector3f Ekf::getRotVarBody() const { const matrix::SquareMatrix3f rot_cov_body = getStateCovariance(); - return matrix::SquareMatrix(_R_to_earth * rot_cov_body * _R_to_earth.T()).diag(); + return matrix::SquareMatrix3f(_R_to_earth.T() * rot_cov_body * _R_to_earth).diag(); +} + +Vector3f Ekf::getRotVarNed() const +{ + const matrix::SquareMatrix3f rot_cov_ned = getStateCovariance(); + return rot_cov_ned.diag(); } float Ekf::getYawVar() const @@ -682,19 +863,20 @@ void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { #if defined(CONFIG_EKF2_TERRAIN) + if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid - float height = _terrain_vpos - _state.pos(2); + float height = getHagl(); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); } else #endif // CONFIG_EKF2_TERRAIN - if (_control_status.flags.gnd_effect) { - // Turn off ground effect compensation if it times out - if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { - _control_status.flags.gnd_effect = false; + if (_control_status.flags.gnd_effect) { + // Turn off ground effect compensation if it times out + if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { + _control_status.flags.gnd_effect = false; + } } - } } else { _control_status.flags.gnd_effect = false; @@ -702,89 +884,6 @@ void Ekf::updateGroundEffect() } #endif // CONFIG_EKF2_BAROMETER -void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) -{ - // save a copy of the quaternion state for later use in calculating the amount of reset change - const Quatf quat_before_reset = _state.quat_nominal; - - // save a copy of covariance in NED frame to restore it after the quat reset - Vector3f rot_var_ned_before_reset = getRotVarNed(); - - // update the yaw angle variance - if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { - rot_var_ned_before_reset(2) = yaw_variance; - } - - // update transformation matrix from body to world frame using the current estimate - // update the rotation matrix using the new yaw value - _R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal)); - - // calculate the amount that the quaternion has changed by - const Quatf quat_after_reset(_R_to_earth); - const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized()); - - // update quaternion states - _state.quat_nominal = quat_after_reset; - uncorrelateQuatFromOtherStates(); - - // restore covariance - resetQuatCov(rot_var_ned_before_reset); - - // add the reset amount to the output observer buffered data - _output_predictor.resetQuaternion(q_error); - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - // update EV attitude error filter - if (_ev_q_error_initialized) { - const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized(); - _ev_q_error_filt.reset(ev_q_error_updated); - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - - // record the state change - if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) { - _state_reset_status.quat_change = q_error; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.quat_change = q_error * _state_reset_status.quat_change; - _state_reset_status.quat_change.normalize(); - } - - _state_reset_status.reset_count.quat++; - - _time_last_heading_fuse = _time_delayed_us; -} - -#if defined(CONFIG_EKF2_WIND) -void Ekf::resetWind() -{ -#if defined(CONFIG_EKF2_AIRSPEED) - if (_control_status.flags.fuse_aspd && isRecent(_airspeed_sample_delayed.time_us, 1e6)) { - resetWindUsingAirspeed(_airspeed_sample_delayed); - return; - } -#endif // CONFIG_EKF2_AIRSPEED - - resetWindToZero(); -} - -void Ekf::resetWindToZero() -{ - ECL_INFO("reset wind to zero"); - - // If we don't have an airspeed measurement, then assume the wind is zero - _state.wind_vel.setZero(); - - resetWindCov(); -} - -void Ekf::resetWindCov() -{ - // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); -} -#endif // CONFIG_EKF2_WIND void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { @@ -793,7 +892,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) { const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f); const float beta = 1.f - alpha; - _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, + beta * _ang_rate_magnitude_filt); } { @@ -818,8 +918,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) // accel bias inhibit const bool do_inhibit_all_accel_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::AccelBias)) - || is_manoeuvre_level_high - || _fault_status.flags.bad_acc_vertical; + || is_manoeuvre_level_high + || _fault_status.flags.bad_acc_vertical; for (unsigned index = 0; index < State::accel_bias.dof; index++) { bool is_bias_observable = true; @@ -865,18 +965,24 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c #else // Efficient implementation of the Joseph stabilized covariance update // Based on "G. J. Bierman. Factorization Methods for Discrete Sequential Estimation. Academic Press, Dover Publications, New York, 1977, 2006" + // P = (I - K * H) * P * (I - K * H).T + K * R * K.T + // = P_temp * (I - H.T * K.T) + K * R * K.T + // = P_temp - P_temp * H.T * K.T + K * R * K.T // Step 1: conventional update + // Compute P_temp and store it in P to avoid allocating more memory + // P is symmetric, so PH == H.T * P.T == H.T * P. Taking the row is faster as matrices are row-major VectorState PH = P.row(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j < State::size; j++) { - P(i, j) -= K(i) * PH(j); // P is now not symmetrical if K is not optimal (e.g.: some gains have been zeroed) + P(i, j) -= K(i) * PH(j); // P is now not symmetric if K is not optimal (e.g.: some gains have been zeroed) } } // Step 2: stabilized update - PH = P.row(state_index); + // P (or "P_temp") is not symmetric so we must take the column + PH = P.col(state_index); for (unsigned i = 0; i < State::size; i++) { for (unsigned j = 0; j <= i; j++) { @@ -884,6 +990,7 @@ bool Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c P(j, i) = P(i, j); } } + #endif constrainStateVariances(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 0b384bfb5649..f7a6c70dec39 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -86,14 +86,25 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) _time_latest_us = imu_sample.time_us; // the output observer always runs - _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, imu_sample.delta_vel, imu_sample.delta_vel_dt); + _output_predictor.calculateOutputStates(imu_sample.time_us, imu_sample.delta_ang, imu_sample.delta_ang_dt, + imu_sample.delta_vel, imu_sample.delta_vel_dt); // accumulate and down-sample imu data and push to the buffer when new downsampled data becomes available if (_imu_down_sampler.update(imu_sample)) { _imu_updated = true; - _imu_buffer.push(_imu_down_sampler.getDownSampledImuAndTriggerReset()); + imuSample imu_downsampled = _imu_down_sampler.getDownSampledImuAndTriggerReset(); + + // as a precaution constrain the integration delta time to prevent numerical problems + const float filter_update_period_s = _params.filter_update_interval_us * 1e-6f; + const float imu_min_dt = 0.5f * filter_update_period_s; + const float imu_max_dt = 2.0f * filter_update_period_s; + + imu_downsampled.delta_ang_dt = math::constrain(imu_downsampled.delta_ang_dt, imu_min_dt, imu_max_dt); + imu_downsampled.delta_vel_dt = math::constrain(imu_downsampled.delta_vel_dt, imu_min_dt, imu_max_dt); + + _imu_buffer.push(imu_downsampled); // get the oldest data from the buffer _time_delayed_us = _imu_buffer.get_oldest().time_us; @@ -141,7 +152,8 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) _time_last_mag_buffer_push = _time_latest_us; } else { - ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("mag data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _mag_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_MAGNETOMETER @@ -179,13 +191,16 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample) _time_last_gps_buffer_push = _time_latest_us; #if defined(CONFIG_EKF2_GNSS_YAW) + if (PX4_ISFINITE(gnss_sample.yaw)) { - _time_last_gps_yaw_buffer_push = _time_latest_us; + _time_last_gnss_yaw_buffer_push = _time_latest_us; } + #endif // CONFIG_EKF2_GNSS_YAW } else { - ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("GPS data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _gps_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_GNSS @@ -223,7 +238,8 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) _time_last_baro_buffer_push = _time_latest_us; } else { - ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("baro data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _baro_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_BAROMETER @@ -260,13 +276,14 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) _airspeed_buffer->push(airspeed_sample_new); } else { - ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("airspeed data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _airspeed_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) -void EstimatorInterface::setRangeData(const rangeSample &range_sample) +void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample) { if (!_initialised) { return; @@ -274,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) // Allocate the required buffer size if not previously done if (_range_buffer == nullptr) { - _range_buffer = new RingBuffer(_obs_buffer_length); + _range_buffer = new RingBuffer(_obs_buffer_length); if (_range_buffer == nullptr || !_range_buffer->valid()) { delete _range_buffer; @@ -291,14 +308,15 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample) // limit data rate to prevent data being lost if (time_us >= static_cast(_range_buffer->get_newest().time_us + _min_obs_interval_us)) { - rangeSample range_sample_new{range_sample}; + sensor::rangeSample range_sample_new{range_sample}; range_sample_new.time_us = time_us; _range_buffer->push(range_sample_new); _time_last_range_buffer_push = _time_latest_us; } else { - ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("range data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _range_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_RANGE_FINDER @@ -335,7 +353,8 @@ void EstimatorInterface::setOpticalFlowData(const flowSample &flow) _flow_buffer->push(optflow_sample_new); } else { - ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("optical flow data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _flow_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -374,7 +393,8 @@ void EstimatorInterface::setExtVisionData(const extVisionSample &evdata) _time_last_ext_vision_buffer_push = _time_latest_us; } else { - ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("EV data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _ext_vision_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -411,7 +431,8 @@ void EstimatorInterface::setAuxVelData(const auxVelSample &auxvel_sample) _auxvel_buffer->push(auxvel_sample_new); } else { - ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_WARN("aux velocity data too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _auxvel_buffer->get_newest().time_us, + _min_obs_interval_us); } } #endif // CONFIG_EKF2_AUXVEL @@ -446,7 +467,8 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) _system_flag_buffer->push(system_flags_new); } else { - ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); + ECL_DEBUG("system flag update too fast %" PRIi64 " < %" PRIu64 " + %d", time_us, + _system_flag_buffer->get_newest().time_us, _min_obs_interval_us); } } @@ -516,71 +538,15 @@ void EstimatorInterface::setDragData(const imuSample &imu) bool EstimatorInterface::initialise_interface(uint64_t timestamp) { - // find the maximum time delay the buffers are required to handle - - // it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used - float max_time_delay_ms = _params.sensor_interval_max_ms; - - // aux vel -#if defined(CONFIG_EKF2_AUXVEL) - max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms); -#endif // CONFIG_EKF2_AUXVEL - -#if defined(CONFIG_EKF2_BAROMETER) - // using baro - if (_params.baro_ctrl > 0) { - max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_AIRSPEED) - // using airspeed - if (_params.arsp_thr > FLT_EPSILON) { - max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_AIRSPEED - -#if defined(CONFIG_EKF2_MAGNETOMETER) - // mag mode - if (_params.mag_fusion_type != MagFuseType::NONE) { - max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_MAGNETOMETER - -#if defined(CONFIG_EKF2_RANGE_FINDER) - // using range finder - if ((_params.rng_ctrl != static_cast(RngCtrl::DISABLED))) { - max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_GNSS) - if (_params.gnss_ctrl > 0) { - max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_GNSS - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - if (_params.flow_ctrl > 0) { - max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_params.ev_ctrl > 0) { - max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms); - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f; // calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter - _imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms)); + _imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms)); // set the observation buffer length to handle the minimum time of arrival between observations in combination // with the worst case delay from current time to ekf fusion time // allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter - const float ekf_delay_ms = max_time_delay_ms * 1.5f; + const float ekf_delay_ms = _params.delay_max_ms * 1.5f; _obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms); // limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate) diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e94d14b5e6cb..75e65268304d 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -63,12 +63,12 @@ #include "common.h" #include "RingBuffer.h" -#include "imu_down_sampler.hpp" -#include "output_predictor.h" +#include "imu_down_sampler/imu_down_sampler.hpp" +#include "output_predictor/output_predictor.h" #if defined(CONFIG_EKF2_RANGE_FINDER) -# include "range_finder_consistency_check.hpp" -# include "sensor_range_finder.hpp" +# include "aid_sources/range_finder/range_finder_consistency_check.hpp" +# include "aid_sources/range_finder/sensor_range_finder.hpp" #endif // CONFIG_EKF2_RANGE_FINDER #include @@ -107,7 +107,7 @@ class EstimatorInterface #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_RANGE_FINDER) - void setRangeData(const rangeSample &range_sample); + void setRangeData(const estimator::sensor::rangeSample &range_sample); // set sensor limitations reported by the rangefinder void set_rangefinder_limits(float min_distance, float max_distance) @@ -115,7 +115,7 @@ class EstimatorInterface _range_sensor.setLimits(min_distance, max_distance); } - const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } + const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -293,10 +293,6 @@ class EstimatorInterface const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; } const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; } - const warning_event_status_u &warning_event_status() const { return _warning_events; } - const decltype(warning_event_status_u::flags) &warning_event_flags() const { return _warning_events.flags; } - void clear_warning_events() { _warning_events.value = 0; } - const information_event_status_u &information_event_status() const { return _information_events; } const decltype(information_event_status_u::flags) &information_event_flags() const { return _information_events.flags; } void clear_information_events() { _information_events.value = 0; } @@ -348,15 +344,15 @@ class EstimatorInterface OutputPredictor _output_predictor{}; #if defined(CONFIG_EKF2_AIRSPEED) - airspeedSample _airspeed_sample_delayed{}; + airspeedSample _airspeed_sample_delayed {}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) - extVisionSample _ev_sample_prev{}; + extVisionSample _ev_sample_prev {}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_RANGE_FINDER) - RingBuffer *_range_buffer{nullptr}; + RingBuffer *_range_buffer {nullptr}; uint64_t _time_last_range_buffer_push{0}; sensor::SensorRangeFinder _range_sensor{}; @@ -364,7 +360,7 @@ class EstimatorInterface #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) - RingBuffer *_flow_buffer{nullptr}; + RingBuffer *_flow_buffer {nullptr}; flowSample _flow_sample_delayed{}; @@ -387,7 +383,7 @@ class EstimatorInterface float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin #if defined(CONFIG_EKF2_GNSS) - RingBuffer *_gps_buffer{nullptr}; + RingBuffer *_gps_buffer {nullptr}; uint64_t _time_last_gps_buffer_push{0}; gnssSample _gps_sample_delayed{}; @@ -401,13 +397,12 @@ class EstimatorInterface # if defined(CONFIG_EKF2_GNSS_YAW) // innovation consistency check monitoring ratios - AlphaFilter _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state - uint64_t _time_last_gps_yaw_buffer_push{0}; + uint64_t _time_last_gnss_yaw_buffer_push{0}; # endif // CONFIG_EKF2_GNSS_YAW #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_DRAG_FUSION) - RingBuffer *_drag_buffer{nullptr}; + RingBuffer *_drag_buffer {nullptr}; dragSample _drag_down_sampled{}; // down sampled drag specific force data (filter prediction rate -> observation rate) #endif // CONFIG_EKF2_DRAG_FUSION @@ -425,25 +420,25 @@ class EstimatorInterface RingBuffer _imu_buffer{kBufferLengthDefault}; #if defined(CONFIG_EKF2_MAGNETOMETER) - RingBuffer *_mag_buffer{nullptr}; + RingBuffer *_mag_buffer {nullptr}; uint64_t _time_last_mag_buffer_push{0}; #endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) - RingBuffer *_airspeed_buffer{nullptr}; + RingBuffer *_airspeed_buffer {nullptr}; #endif // CONFIG_EKF2_AIRSPEED #if defined(CONFIG_EKF2_EXTERNAL_VISION) - RingBuffer *_ext_vision_buffer{nullptr}; + RingBuffer *_ext_vision_buffer {nullptr}; uint64_t _time_last_ext_vision_buffer_push{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_AUXVEL) - RingBuffer *_auxvel_buffer{nullptr}; + RingBuffer *_auxvel_buffer {nullptr}; #endif // CONFIG_EKF2_AUXVEL - RingBuffer *_system_flag_buffer{nullptr}; + RingBuffer *_system_flag_buffer {nullptr}; #if defined(CONFIG_EKF2_BAROMETER) - RingBuffer *_baro_buffer{nullptr}; + RingBuffer *_baro_buffer {nullptr}; uint64_t _time_last_baro_buffer_push{0}; #endif // CONFIG_EKF2_BAROMETER @@ -458,7 +453,7 @@ class EstimatorInterface uint64_t _wmm_gps_time_last_set{0}; // time WMM last set #if defined(CONFIG_EKF2_MAGNETOMETER) - float _mag_declination_gps{NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) + float _mag_declination_gps {NAN}; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_inclination_gps{NAN}; // magnetic inclination returned by the geo library using the last valid GPS position (rad) float _mag_strength_gps{NAN}; // magnetic strength returned by the geo library using the last valid GPS position (T) @@ -474,7 +469,6 @@ class EstimatorInterface // these are used to record single frame events for external monitoring and should NOT be used for // state logic becasue they will be cleared externally after being read. - warning_event_status_u _warning_events{}; information_event_status_u _information_events{}; unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec) diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp deleted file mode 100644 index aff345ac1140..000000000000 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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 ev_vel_control.cpp - * Control functions for ekf external vision velocity fusion - */ - -#include "ekf.h" - -void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common_starting_conditions_passing, - const bool ev_reset, const bool quality_sufficient, estimator_aid_source3d_s &aid_src) -{ - static constexpr const char *AID_SRC_NAME = "EV velocity"; - - const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw) - || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); - - // determine if we should use EV velocity aiding - bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VEL)) - && _control_status.flags.tilt_align - && ev_sample.vel.isAllFinite(); - - // correct velocity for offset relative to IMU - const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body; - const Vector3f vel_offset_body = _ang_rate_delayed_raw % pos_offset_body; - const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body; - - // rotate measurement into correct earth frame if required - Vector3f vel{NAN, NAN, NAN}; - Matrix3f vel_cov{}; - - switch (ev_sample.vel_frame) { - case VelocityFrame::LOCAL_FRAME_NED: - if (_control_status.flags.yaw_align) { - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); - - } else { - continuing_conditions_passing = false; - } - - break; - - case VelocityFrame::LOCAL_FRAME_FRD: - if (_control_status.flags.ev_yaw) { - // using EV frame - vel = ev_sample.vel - vel_offset_earth; - vel_cov = matrix::diag(ev_sample.velocity_var); - - } else { - // rotate EV to the EKF reference frame - const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); - - vel = R_ev_to_ekf * ev_sample.vel - vel_offset_earth; - vel_cov = R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) * R_ev_to_ekf.transpose(); - - // increase minimum variance to include EV orientation variance - // TODO: do this properly - const float orientation_var_max = ev_sample.orientation_var.max(); - - for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), orientation_var_max); - } - } - - break; - - case VelocityFrame::BODY_FRAME_FRD: - vel = _R_to_earth * (ev_sample.vel - vel_offset_body); - vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose(); - break; - - default: - continuing_conditions_passing = false; - break; - } - -#if defined(CONFIG_EKF2_GNSS) - // increase minimum variance if GPS active (position reference) - if (_control_status.flags.gps) { - for (int i = 0; i < 2; i++) { - vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise)); - } - } -#endif // CONFIG_EKF2_GNSS - - const Vector3f measurement{vel}; - - const Vector3f measurement_var{ - math::max(vel_cov(0, 0), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(1, 1), sq(_params.ev_vel_noise), sq(0.01f)), - math::max(vel_cov(2, 2), sq(_params.ev_vel_noise), sq(0.01f)) - }; - - const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite(); - - updateVelocityAidSrcStatus(ev_sample.time_us, - measurement, // observation - measurement_var, // observation variance - math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate - aid_src); - - if (!measurement_valid) { - continuing_conditions_passing = false; - } - - const bool starting_conditions_passing = common_starting_conditions_passing - && continuing_conditions_passing - && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); - - if (_control_status.flags.ev_vel) { - - if (continuing_conditions_passing) { - - if ((ev_reset && isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.ev_vel)) || yaw_alignment_changed) { - - if (quality_sufficient) { - ECL_INFO("reset to %s", AID_SRC_NAME); - _information_events.flags.reset_vel_to_vision = true; - resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _time_delayed_us; - - } else { - // EV has reset, but quality isn't sufficient - // we have no choice but to stop EV and try to resume once quality is acceptable - stopEvVelFusion(); - return; - } - - } else if (quality_sufficient) { - fuseVelocity(aid_src); - - } else { - aid_src.innovation_rejected = true; - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.no_aid_timeout_max); // 1 second - - if (is_fusion_failing) { - - if ((_nb_ev_vel_reset_available > 0) && quality_sufficient) { - // Data seems good, attempt a reset - _information_events.flags.reset_vel_to_vision = true; - ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetVelocityTo(measurement, measurement_var); - aid_src.time_last_fuse = _time_delayed_us; - - if (_control_status.flags.in_air) { - _nb_ev_vel_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - //_control_status.flags.ev_vel_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopEvVelFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); - stopEvVelFusion(); - } - } - - } else { - // Stop fusion but do not declare it faulty - ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); - stopEvVelFusion(); - } - - } else { - if (starting_conditions_passing) { - // activate fusion, only reset if necessary - if (!isHorizontalAidingActive() || yaw_alignment_changed) { - ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2)); - _information_events.flags.reset_vel_to_vision = true; - resetVelocityTo(measurement, measurement_var); - - } else { - ECL_INFO("starting %s fusion", AID_SRC_NAME); - } - - aid_src.time_last_fuse = _time_delayed_us; - - _nb_ev_vel_reset_available = 5; - _information_events.flags.starting_vision_vel_fusion = true; - _control_status.flags.ev_vel = true; - } - } -} - -void Ekf::stopEvVelFusion() -{ - if (_control_status.flags.ev_vel) { - resetEstimatorAidStatus(_aid_src_ev_vel); - - _control_status.flags.ev_vel = false; - } -} diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp deleted file mode 100644 index 13c057f26653..000000000000 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ /dev/null @@ -1,168 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018-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 gps_yaw_fusion.cpp - * Definition of functions required to use yaw obtained from GPS dual antenna measurements. - * Equations generated using EKF/python/ekf_derivation/main.py - * - * @author Paul Riseborough - * - */ - -#include "ekf.h" -#include - -#include -#include - -void Ekf::updateGpsYaw(const gnssSample &gps_sample) -{ - if (PX4_ISFINITE(gps_sample.yaw)) { - - auto &gnss_yaw = _aid_src_gnss_yaw; - resetEstimatorAidStatus(gnss_yaw); - - // initially populate for estimator_aid_src_gnss_yaw logging - - // calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement - const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset); - - const float yaw_acc = PX4_ISFINITE(gps_sample.yaw_acc) ? gps_sample.yaw_acc : 0.f; - const float R_YAW = sq(fmaxf(yaw_acc, _params.gps_heading_noise)); - - float heading_pred; - float heading_innov_var; - - { - VectorState H; - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); - } - - gnss_yaw.observation = measured_hdg; - gnss_yaw.observation_variance = R_YAW; - gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); - gnss_yaw.innovation_variance = heading_innov_var; - - gnss_yaw.timestamp_sample = gps_sample.time_us; - - const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); - setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate); - } -} - -void Ekf::fuseGpsYaw(float antenna_yaw_offset) -{ - auto &gnss_yaw = _aid_src_gnss_yaw; - - if (gnss_yaw.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - return; - } - - if (!PX4_ISFINITE(antenna_yaw_offset)) { - antenna_yaw_offset = 0.f; - } - - VectorState H; - - { - float heading_pred; - float heading_innov_var; - - // Note: we recompute innov and innov_var because it doesn't cost much more than just computing H - // making a separate function just for H uses more flash space without reducing CPU load significantly - sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); - } - - // check if the innovation variance calculation is badly conditioned - if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_hdg = true; - - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - ECL_ERR("GPS yaw numerical error - covariance reset"); - return; - } - - _fault_status.flags.bad_hdg = false; - _innov_check_fail_status.flags.reject_yaw = false; - - _gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio); - - if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f) - && !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) { - - // A constant large signed test ratio is a sign of wrong gyro bias - // Reset the yaw gyro variance to converge faster and avoid - // being stuck on a previous bad estimate - resetGyroBiasZCov(); - } - - // calculate the Kalman gains - // only calculate gains for states we are using - VectorState Kfusion = P * H / gnss_yaw.innovation_variance; - - const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation); - _fault_status.flags.bad_hdg = !is_fused; - gnss_yaw.fused = is_fused; - - if (is_fused) { - _time_last_heading_fuse = _time_delayed_us; - gnss_yaw.time_last_fuse = _time_delayed_us; - } -} - -bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset) -{ - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(gnss_yaw_offset), sinf(gnss_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - - // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { - return false; - } - - // GPS yaw measurement is alreday compensated for antenna offset in the driver - const float measured_yaw = gnss_yaw; - - const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance); - - _aid_src_gnss_yaw.time_last_fuse = _time_delayed_us; - _gnss_yaw_signed_test_ratio_lpf.reset(0.f); - - return true; -} diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index 9d07c9bc580d..4db232ca051f 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -45,7 +45,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_BAROMETER) updateGroundEffect(); - controlBaroHeightFusion(); + controlBaroHeightFusion(imu_delayed); #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) @@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) - controlRangeHeightFusion(); + controlRangeHaglFusion(imu_delayed); #endif // CONFIG_EKF2_RANGE_FINDER checkHeightSensorRefFallback(); @@ -216,7 +216,6 @@ void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed) _time_acc_bias_check = imu_delayed.time_us; _fault_status.flags.bad_acc_bias = false; - _warning_events.flags.invalid_accel_bias_cov_reset = true; ECL_WARN("invalid accel bias - covariance reset"); } } @@ -230,22 +229,32 @@ void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed) Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood(); - // Check for more than 50% clipping affected IMU samples within the past 1 second - const uint16_t clip_count_limit = 1.f / _dt_ekf_avg; - const bool is_clipping = imu_delayed.delta_vel_clipping[0] || - imu_delayed.delta_vel_clipping[1] || - imu_delayed.delta_vel_clipping[2]; + const uint16_t kClipCountLimit = 1.f / _dt_ekf_avg; - if (is_clipping && _clip_counter < clip_count_limit) { - _clip_counter++; + bool acc_clip_warning[3] {}; + bool acc_clip_critical[3] {}; - } else if (_clip_counter > 0) { - _clip_counter--; + for (int axis = 0; axis < 3; axis++) { + if (imu_delayed.delta_vel_clipping[axis] && (_clip_counter[axis] < kClipCountLimit)) { + _clip_counter[axis]++; + + } else if (_clip_counter[axis] > 0) { + _clip_counter[axis]--; + } + + // warning if more than 50% clipping affected IMU samples within the past 1 second + acc_clip_warning[axis] = _clip_counter[axis] >= kClipCountLimit / 2; + acc_clip_critical[axis] = _clip_counter[axis] >= kClipCountLimit; } - _fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2; + // bad_acc_clipping if ALL axes are reporting warning or if ANY axis is critical + const bool all_axis_warning = (acc_clip_warning[0] && acc_clip_warning[1] && acc_clip_warning[2]); + const bool any_axis_critical = (acc_clip_critical[0] || acc_clip_critical[1] || acc_clip_critical[2]); - const bool is_clipping_frequently = _clip_counter > 0; + _fault_status.flags.bad_acc_clipping = all_axis_warning || any_axis_critical; + + // if Z axis is warning or any other axis critical + const bool is_clipping_frequently = acc_clip_warning[2] || _fault_status.flags.bad_acc_clipping; // Do not require evidence of clipping if the likelihood of having the INS falling is high const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM)) @@ -284,12 +293,15 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const } checks[6] {}; #if defined(CONFIG_EKF2_BAROMETER) + if (_control_status.flags.baro_hgt) { checks[0] = {ReferenceType::PRESSURE, _aid_src_baro_hgt.innovation, _aid_src_baro_hgt.innovation_variance}; } + #endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_GNSS) + if (_control_status.flags.gps_hgt) { checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; } @@ -297,15 +309,20 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.gps) { checks[2] = {ReferenceType::GNSS, _aid_src_gnss_vel.innovation[2], _aid_src_gnss_vel.innovation_variance[2]}; } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) + if (_control_status.flags.rng_hgt) { - checks[3] = {ReferenceType::GROUND, _aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance}; + // Range is a distance to ground measurement, not a direct height observation and has an opposite sign + checks[3] = {ReferenceType::GROUND, -_aid_src_rng_hgt.innovation, _aid_src_rng_hgt.innovation_variance}; } + #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) + if (_control_status.flags.ev_hgt) { checks[4] = {ReferenceType::GROUND, _aid_src_ev_hgt.innovation, _aid_src_ev_hgt.innovation_variance}; } @@ -313,6 +330,7 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const if (_control_status.flags.ev_vel) { checks[5] = {ReferenceType::GROUND, _aid_src_ev_vel.innovation[2], _aid_src_ev_vel.innovation_variance[2]}; } + #endif // CONFIG_EKF2_EXTERNAL_VISION // Compute the check based on innovation ratio for all the sources diff --git a/src/modules/ekf2/EKF/imu_down_sampler.cpp b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp similarity index 98% rename from src/modules/ekf2/EKF/imu_down_sampler.cpp rename to src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp index 02f43a12f54c..0054f917f3b4 100644 --- a/src/modules/ekf2/EKF/imu_down_sampler.cpp +++ b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.cpp @@ -1,4 +1,4 @@ -#include "imu_down_sampler.hpp" +#include "imu_down_sampler/imu_down_sampler.hpp" #include diff --git a/src/modules/ekf2/EKF/imu_down_sampler.hpp b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp similarity index 99% rename from src/modules/ekf2/EKF/imu_down_sampler.hpp rename to src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp index 29732bd584c6..8f0c4211bedf 100644 --- a/src/modules/ekf2/EKF/imu_down_sampler.hpp +++ b/src/modules/ekf2/EKF/imu_down_sampler/imu_down_sampler.hpp @@ -41,7 +41,7 @@ #include #include -#include "common.h" +#include "../common.h" using namespace estimator; diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp deleted file mode 100644 index 89f502ac93f8..000000000000 --- a/src/modules/ekf2/EKF/mag_3d_control.cpp +++ /dev/null @@ -1,208 +0,0 @@ -/**************************************************************************** - * - * 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 mag_3d_control.cpp - * Control functions for ekf mag 3D fusion - */ - -#include "ekf.h" - -void Ekf::controlMag3DFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, - estimator_aid_source3d_s &aid_src) -{ - static constexpr const char *AID_SRC_NAME = "mag"; - - resetEstimatorAidStatus(aid_src); - - const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse); // WMM update can occur on the last epoch, just after mag fusion - - // determine if we should use mag fusion - bool continuing_conditions_passing = (_params.mag_fusion_type != MagFuseType::NONE) - && _control_status.flags.tilt_align - && (_control_status.flags.yaw_align || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && mag_sample.mag.longerThan(0.f) - && mag_sample.mag.isAllFinite(); - - const bool starting_conditions_passing = common_starting_conditions_passing - && continuing_conditions_passing; - - // For the first few seconds after in-flight alignment we allow the magnetic field state estimates to stabilise - // before they are used to constrain heading drift - _control_status.flags.mag_3D = (_params.mag_fusion_type == MagFuseType::AUTO) - && _control_status.flags.mag - && _control_status.flags.mag_aligned_in_flight - && (_control_status.flags.mag_heading_consistent || !_control_status.flags.gps) - && !_control_status.flags.mag_fault - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw; - - // TODO: allow clearing mag_fault if mag_3d is good? - - if (_control_status.flags.mag_3D && !_control_status_prev.flags.mag_3D) { - ECL_INFO("starting mag 3D fusion"); - - } else if (!_control_status.flags.mag_3D && _control_status_prev.flags.mag_3D) { - ECL_INFO("stopping mag 3D fusion"); - } - - // if we are using 3-axis magnetometer fusion, but without external NE aiding, - // then the declination must be fused as an observation to prevent long term heading drift - // fusing declination when gps aiding is available is optional, but recommended to prevent - // problem if the vehicle is static for extended periods of time - const bool mag_decl_user_selected = (_params.mag_declination_source & GeoDeclinationMask::FUSE_DECL); - const bool not_using_ne_aiding = !_control_status.flags.gps; - _control_status.flags.mag_dec = (_control_status.flags.mag && (not_using_ne_aiding || mag_decl_user_selected)); - - if (_control_status.flags.mag) { - aid_src.timestamp_sample = mag_sample.time_us; - - if (continuing_conditions_passing && _control_status.flags.yaw_align) { - - if (mag_sample.reset || checkHaglYawResetReq() || wmm_updated) { - ECL_INFO("reset to %s", AID_SRC_NAME); - resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; - - } else { - if (!_mag_decl_cov_reset) { - // After any magnetic field covariance reset event the earth field state - // covariances need to be corrected to incorporate knowledge of the declination - // before fusing magnetometer data to prevent rapid rotation of the earth field - // states for the first few observations. - fuseDeclination(0.02f); - _mag_decl_cov_reset = true; - fuseMag(mag_sample.mag, aid_src, false); - - } else { - // The normal sequence is to fuse the magnetometer data first before fusing - // declination angle at a higher uncertainty to allow some learning of - // declination angle over time. - const bool update_all_states = _control_status.flags.mag_3D; - fuseMag(mag_sample.mag, aid_src, update_all_states); - - if (_control_status.flags.mag_dec) { - fuseDeclination(0.5f); - } - } - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max); - - if (is_fusion_failing) { - if (_nb_mag_3d_reset_available > 0) { - // Data seems good, attempt a reset (mag states only unless mag_3D currently active) - ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D); - aid_src.time_last_fuse = _time_delayed_us; - - if (_control_status.flags.in_air) { - _nb_mag_3d_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - //_control_status.flags.mag_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopMagFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); - stopMagFusion(); - } - } - - } else { - // Stop fusion but do not declare it faulty - ECL_DEBUG("stopping %s fusion, continuing conditions no longer passing", AID_SRC_NAME); - stopMagFusion(); - } - - } else { - if (starting_conditions_passing) { - - _control_status.flags.mag = true; - - // activate fusion, reset mag states and initialize variance if first init or in flight reset - if (!_control_status.flags.yaw_align - || wmm_updated - || !_mag_decl_cov_reset - || !_state.mag_I.longerThan(0.f) - || (getStateVariance().min() < kMagVarianceMin) - || (getStateVariance().min() < kMagVarianceMin) - ) { - ECL_INFO("starting %s fusion, resetting states", AID_SRC_NAME); - - bool reset_heading = !_control_status.flags.yaw_align; - - resetMagStates(_mag_lpf.getState(), reset_heading); - - if (reset_heading) { - _control_status.flags.yaw_align = true; - } - - } else { - ECL_INFO("starting %s fusion", AID_SRC_NAME); - fuseMag(mag_sample.mag, aid_src, false); - } - - aid_src.time_last_fuse = _time_delayed_us; - - _nb_mag_3d_reset_available = 2; - } - } -} - -void Ekf::stopMagFusion() -{ - if (_control_status.flags.mag) { - ECL_INFO("stopping mag fusion"); - - _control_status.flags.mag = false; - _control_status.flags.mag_dec = false; - - if (_control_status.flags.mag_3D) { - ECL_INFO("stopping mag 3D fusion"); - _control_status.flags.mag_3D = false; - } - - _fault_status.flags.bad_mag_x = false; - _fault_status.flags.bad_mag_y = false; - _fault_status.flags.bad_mag_z = false; - - _fault_status.flags.bad_mag_decl = false; - } -} diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp deleted file mode 100644 index b1a8cc68fe29..000000000000 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ /dev/null @@ -1,359 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-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 heading_fusion.cpp - * Magnetometer fusion methods. - * Equations generated using EKF/python/ekf_derivation/main.py - * - * @author Roman Bast - * @author Paul Riseborough - * - */ - -#include "ekf.h" - -#include -#include -#include - -#include - -#include - -bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states) -{ - // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations - const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f)); - - // calculate intermediate variables used for X axis innovation variance, observation Jacobians and Kalman gains - Vector3f mag_innov; - Vector3f innov_var; - - // Observation jacobian and Kalman gain vectors - VectorState H; - const auto state_vector = _state.vector(); - sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion - if (_control_status.flags.synthetic_mag_z) { - mag_innov(2) = 0.0f; - } - - for (int i = 0; i < 3; i++) { - aid_src_mag.observation[i] = mag(i) - _state.mag_B(i); - aid_src_mag.observation_variance[i] = R_MAG; - aid_src_mag.innovation[i] = mag_innov(i); - aid_src_mag.innovation_variance[i] = innov_var(i); - } - - const float innov_gate = math::max(_params.mag_innov_gate, 1.f); - setEstimatorAidStatusTestRatio(aid_src_mag, innov_gate); - - if (update_all_states) { - _fault_status.flags.bad_mag_x = (aid_src_mag.innovation_variance[0] < aid_src_mag.observation_variance[0]); - _fault_status.flags.bad_mag_y = (aid_src_mag.innovation_variance[1] < aid_src_mag.observation_variance[1]); - _fault_status.flags.bad_mag_z = (aid_src_mag.innovation_variance[2] < aid_src_mag.observation_variance[2]); - - } else { - _fault_status.flags.bad_mag_x = false; - _fault_status.flags.bad_mag_y = false; - _fault_status.flags.bad_mag_z = false; - } - - // Perform an innovation consistency check and report the result - _innov_check_fail_status.flags.reject_mag_x = (aid_src_mag.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_mag_y = (aid_src_mag.test_ratio[1] > 1.f); - _innov_check_fail_status.flags.reject_mag_z = (aid_src_mag.test_ratio[2] > 1.f); - - const char *numerical_error_covariance_reset_string = "numerical error - covariance reset"; - - // check innovation variances for being badly conditioned - if (innov_var.min() < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("mag %s", numerical_error_covariance_reset_string); - return false; - } - - // if any axis fails, abort the mag fusion - if (aid_src_mag.innovation_rejected) { - return false; - } - - bool fused[3] {false, false, false}; - - // update the states and covariance using sequential fusion of the magnetometer components - for (uint8_t index = 0; index <= 2; index++) { - // Calculate Kalman gains and observation jacobians - if (index == 0) { - // everything was already computed above - - } else if (index == 1) { - // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::ComputeMagYInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - - // recalculate innovation using the updated state - aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - - if (aid_src_mag.innovation_variance[index] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_y = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("magY %s", numerical_error_covariance_reset_string); - return false; - } - - } else if (index == 2) { - // we do not fuse synthesized magnetomter measurements when doing 3D fusion - if (_control_status.flags.synthetic_mag_z) { - fused[2] = true; - continue; - } - - // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::ComputeMagZInnovVarAndH(state_vector, P, R_MAG, FLT_EPSILON, &aid_src_mag.innovation_variance[index], &H); - - // recalculate innovation using the updated state - aid_src_mag.innovation[index] = _state.quat_nominal.rotateVectorInverse(_state.mag_I)(index) + _state.mag_B(index) - mag(index); - - if (aid_src_mag.innovation_variance[index] < R_MAG) { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.flags.bad_mag_z = true; - - // we need to re-initialise covariances and abort this fusion step - if (update_all_states) { - resetQuatCov(_params.mag_heading_noise); - } - - resetMagCov(); - - ECL_ERR("magZ %s", numerical_error_covariance_reset_string); - return false; - } - } - - VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; - - if (!update_all_states) { - // zero non-mag Kalman gains if not updating all states - - // copy mag_I and mag_B Kalman gains - const Vector3f K_mag_I = Kfusion.slice(State::mag_I.idx, 0); - const Vector3f K_mag_B = Kfusion.slice(State::mag_B.idx, 0); - - // zero all Kalman gains, then restore mag - Kfusion.setZero(); - Kfusion.slice(State::mag_I.idx, 0) = K_mag_I; - Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; - } - - if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) { - fused[index] = true; - limitDeclination(); - - } else { - fused[index] = false; - } - } - - if (update_all_states) { - _fault_status.flags.bad_mag_x = !fused[0]; - _fault_status.flags.bad_mag_y = !fused[1]; - _fault_status.flags.bad_mag_z = !fused[2]; - } - - if (fused[0] && fused[1] && fused[2]) { - aid_src_mag.fused = true; - aid_src_mag.time_last_fuse = _time_delayed_us; - - if (update_all_states) { - _time_last_heading_fuse = _time_delayed_us; - } - - return true; - } - - aid_src_mag.fused = false; - return false; -} - -bool Ekf::fuseDeclination(float decl_sigma) -{ - if (!_control_status.flags.mag) { - return false; - } - - float decl_measurement = NAN; - - if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) - && PX4_ISFINITE(_mag_declination_gps) - ) { - decl_measurement = _mag_declination_gps; - - } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) - && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) - ) { - decl_measurement = math::radians(_params.mag_declination_deg); - } - - if (PX4_ISFINITE(decl_measurement)) { - - // observation variance (rad**2) - const float R_DECL = sq(decl_sigma); - - VectorState H; - float decl_pred; - float innovation_variance; - - sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); - - const float innovation = wrap_pi(decl_pred - decl_measurement); - - if (innovation_variance < R_DECL) { - // variance calculation is badly conditioned - return false; - } - - // Calculate the Kalman gains - VectorState Kfusion = P * H / innovation_variance; - - const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); - - _fault_status.flags.bad_mag_decl = !is_fused; - - if (is_fused) { - limitDeclination(); - } - - return is_fused; - } - - return false; -} - -void Ekf::limitDeclination() -{ - const Vector3f mag_I_before = _state.mag_I; - - // get a reference value for the earth field declinaton and minimum plausible horizontal field strength - float decl_reference = NAN; - float h_field_min = 0.001f; - - if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL - && (PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps) && PX4_ISFINITE(_mag_inclination_gps)) - ) { - decl_reference = _mag_declination_gps; - - // set to 50% of the horizontal strength from geo tables if location is known - h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); - - } else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) - && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f) - ) { - // use parameter value if GPS isn't available - decl_reference = math::radians(_params.mag_declination_deg); - } - - if (!PX4_ISFINITE(decl_reference)) { - return; - } - - // do not allow the horizontal field length to collapse - this will make the declination fusion badly conditioned - // and can result in a reversal of the NE field states which the filter cannot recover from - // apply a circular limit - float h_field = sqrtf(_state.mag_I(0) * _state.mag_I(0) + _state.mag_I(1) * _state.mag_I(1)); - - if (h_field < h_field_min) { - if (h_field > 0.001f * h_field_min) { - const float h_scaler = h_field_min / h_field; - _state.mag_I(0) *= h_scaler; - _state.mag_I(1) *= h_scaler; - - } else { - // too small to scale radially so set to expected value - _state.mag_I(0) = 2.0f * h_field_min * cosf(decl_reference); - _state.mag_I(1) = 2.0f * h_field_min * sinf(decl_reference); - } - - h_field = h_field_min; - } - - // do not allow the declination estimate to vary too much relative to the reference value - constexpr float decl_tolerance = 0.5f; - const float decl_max = decl_reference + decl_tolerance; - const float decl_min = decl_reference - decl_tolerance; - const float decl_estimate = atan2f(_state.mag_I(1), _state.mag_I(0)); - - if (decl_estimate > decl_max) { - _state.mag_I(0) = h_field * cosf(decl_max); - _state.mag_I(1) = h_field * sinf(decl_max); - - } else if (decl_estimate < decl_min) { - _state.mag_I(0) = h_field * cosf(decl_min); - _state.mag_I(1) = h_field * sinf(decl_min); - } - - if ((mag_I_before - _state.mag_I).longerThan(0.01f)) { - ECL_DEBUG("declination limited mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f] (decl: %.3f)", - (double)mag_I_before(0), (double)mag_I_before(1), (double)mag_I_before(2), - (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), - (double)decl_reference - ); - } -} - -float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted) -{ - // theoretical magnitude of the magnetometer Z component value given X and Y sensor measurement and our knowledge - // of the earth magnetic field vector at the current location - const float mag_z_abs = sqrtf(math::max(sq(mag_earth_predicted.length()) - sq(mag_meas(0)) - sq(mag_meas(1)), 0.0f)); - - // calculate sign of synthetic magnetomter Z component based on the sign of the predicted magnetometer Z component - const float mag_z_body_pred = mag_earth_predicted.dot(_R_to_earth.col(2)); - - return (mag_z_body_pred < 0) ? -mag_z_abs : mag_z_abs; -} diff --git a/src/modules/ekf2/EKF/mag_heading_control.cpp b/src/modules/ekf2/EKF/mag_heading_control.cpp deleted file mode 100644 index 612474937e10..000000000000 --- a/src/modules/ekf2/EKF/mag_heading_control.cpp +++ /dev/null @@ -1,204 +0,0 @@ -/**************************************************************************** - * - * 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 mag_heading_control.cpp - * Control functions for ekf mag heading fusion - */ - -#include "ekf.h" - -void Ekf::controlMagHeadingFusion(const magSample &mag_sample, const bool common_starting_conditions_passing, - estimator_aid_source1d_s &aid_src) -{ - static constexpr const char *AID_SRC_NAME = "mag heading"; - - // use mag bias if variance good - Vector3f mag_bias{0.f, 0.f, 0.f}; - const Vector3f mag_bias_var = getMagBiasVariance(); - - if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) { - mag_bias = _state.mag_B; - } - - // calculate mag heading - // Rotate the measurements into earth frame using the zero yaw angle - const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); - - // the angle of the projection onto the horizontal gives the yaw angle - // calculate the yaw innovation and wrap to the interval between +-pi - const Vector3f mag_earth_pred = R_to_earth * (mag_sample.mag - mag_bias); - const float declination = getMagDeclination(); - const float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination; - - resetEstimatorAidStatus(aid_src); - aid_src.observation = measured_hdg; - aid_src.observation_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f)); - - if (_control_status.flags.yaw_align) { - // mag heading - aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation); - _mag_heading_innov_lpf.update(aid_src.innovation); - - } else { - // mag heading delta (logging only) - aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _mag_heading_pred_prev) - - wrap_pi(measured_hdg - _mag_heading_prev)); - _mag_heading_innov_lpf.reset(0.f); - } - - // determine if we should use mag heading aiding - const bool mag_consistent_or_no_gnss = _control_status.flags.mag_heading_consistent || !_control_status.flags.gps; - - bool continuing_conditions_passing = ((_params.mag_fusion_type == MagFuseType::HEADING) - || (_params.mag_fusion_type == MagFuseType::AUTO && !_control_status.flags.mag_3D)) - && _control_status.flags.tilt_align - && ((_control_status.flags.yaw_align && mag_consistent_or_no_gnss) - || (!_control_status.flags.ev_yaw && !_control_status.flags.yaw_align)) - && !_control_status.flags.mag_fault - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.ev_yaw - && !_control_status.flags.gps_yaw - && PX4_ISFINITE(aid_src.observation) - && PX4_ISFINITE(aid_src.observation_variance); - - const bool starting_conditions_passing = common_starting_conditions_passing - && continuing_conditions_passing - && isTimedOut(aid_src.time_last_fuse, 3e6); - - if (_control_status.flags.mag_hdg) { - aid_src.timestamp_sample = mag_sample.time_us; - - if (continuing_conditions_passing && _control_status.flags.yaw_align) { - - const bool declination_changed = _control_status_prev.flags.mag_hdg - && (fabsf(declination - _mag_heading_last_declination) > math::radians(3.f)); - bool skip_timeout_check = false; - - if (mag_sample.reset || declination_changed) { - if (declination_changed) { - ECL_INFO("reset to %s, declination changed %.5f->%.5f", - AID_SRC_NAME, (double)_mag_heading_last_declination, (double)declination); - - } else { - ECL_INFO("reset to %s", AID_SRC_NAME); - } - - resetMagHeading(_mag_lpf.getState()); - aid_src.time_last_fuse = _time_delayed_us; - - } else { - VectorState H_YAW; - computeYawInnovVarAndH(aid_src.observation_variance, aid_src.innovation_variance, H_YAW); - - if ((aid_src.innovation_variance - aid_src.observation_variance) > sq(_params.mag_heading_noise / 2.f)) { - // Only fuse mag to constrain the yaw variance to a safe value - fuseYaw(aid_src, H_YAW); - - } else { - // Yaw variance is low enough, stay in mag heading mode but don't fuse the data and skip the fusion timeout check - skip_timeout_check = true; - aid_src.test_ratio = 0.f; // required for preflight checks to pass - } - } - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max) && !skip_timeout_check; - - if (is_fusion_failing) { - if (_nb_mag_heading_reset_available > 0) { - // Data seems good, attempt a reset - ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME); - resetMagHeading(_mag_lpf.getState()); - aid_src.time_last_fuse = _time_delayed_us; - - if (_control_status.flags.in_air) { - _nb_mag_heading_reset_available--; - } - - } else if (starting_conditions_passing) { - // Data seems good, but previous reset did not fix the issue - // something else must be wrong, declare the sensor faulty and stop the fusion - _control_status.flags.mag_fault = true; - ECL_WARN("stopping %s fusion, starting conditions failing", AID_SRC_NAME); - stopMagHdgFusion(); - - } else { - // A reset did not fix the issue but all the starting checks are not passing - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - ECL_WARN("stopping %s, fusion failing", AID_SRC_NAME); - stopMagHdgFusion(); - } - } - - } else { - // Stop fusion but do not declare it faulty - ECL_WARN("stopping %s fusion, continuing conditions failing", AID_SRC_NAME); - stopMagHdgFusion(); - } - - } else { - if (starting_conditions_passing) { - // activate fusion - ECL_INFO("starting %s fusion", AID_SRC_NAME); - - if (!_control_status.flags.yaw_align) { - // reset heading - resetMagHeading(_mag_lpf.getState()); - _control_status.flags.yaw_align = true; - } - - _control_status.flags.mag_hdg = true; - aid_src.time_last_fuse = _time_delayed_us; - - _nb_mag_heading_reset_available = 1; - } - } - - // record corresponding mag heading and yaw state for future mag heading delta heading innovation (logging only) - _mag_heading_prev = measured_hdg; - _mag_heading_pred_prev = getEulerYaw(_state.quat_nominal); - - _mag_heading_last_declination = getMagDeclination(); -} - -void Ekf::stopMagHdgFusion() -{ - if (_control_status.flags.mag_hdg) { - ECL_INFO("stopping mag heading fusion"); - resetEstimatorAidStatus(_aid_src_mag_heading); - - _control_status.flags.mag_hdg = false; - - _fault_status.flags.bad_hdg = false; - } -} diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp deleted file mode 100644 index f8218e883b24..000000000000 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ /dev/null @@ -1,187 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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 optical_flow_control.cpp - * Control functions for optical flow fusion - */ - -#include "ekf.h" - -void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) -{ - if (_flow_buffer) { - _flow_data_ready = _flow_buffer->pop_first_older_than(imu_delayed.time_us, &_flow_sample_delayed); - } - - // New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon - if (_flow_data_ready) { - const int32_t min_quality = _control_status.flags.in_air - ? _params.flow_qual_min - : _params.flow_qual_min_gnd; - - const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality); - const bool is_magnitude_good = _flow_sample_delayed.flow_rate.isAllFinite() - && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate); - const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); - - if (is_quality_good - && is_magnitude_good - && is_tilt_good) { - // compensate for body motion to give a LOS rate - calcOptFlowBodyRateComp(imu_delayed); - _flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy(); - - } else { - // don't use this flow data and wait for the next data to arrive - _flow_data_ready = false; - _flow_rate_compensated.setZero(); - } - } - - if (_flow_data_ready) { - updateOptFlow(_aid_src_optical_flow); - - // Check if we are in-air and require optical flow to control position drift - bool is_flow_required = _control_status.flags.in_air - && (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently - || isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)); - - // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently - // use a relaxed time criteria to enable it to coast through bad range finder data - const bool terrain_available = isTerrainEstimateValid() || isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6); - - const bool continuing_conditions_passing = (_params.flow_ctrl == 1) - && _control_status.flags.tilt_align - && (terrain_available || is_flow_required); - - const bool starting_conditions_passing = continuing_conditions_passing - && isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching - - if (_control_status.flags.opt_flow) { - if (continuing_conditions_passing) { - fuseOptFlow(); - - // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period - if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) { - if (is_flow_required) { - resetFlowFusion(); - - } else { - stopFlowFusion(); - } - } - - } else { - stopFlowFusion(); - } - - } else { - if (starting_conditions_passing) { - startFlowFusion(); - } - } - - } else if (_control_status.flags.opt_flow && isTimedOut(_flow_sample_delayed.time_us, _params.reset_timeout_max)) { - stopFlowFusion(); - } -} - -void Ekf::startFlowFusion() -{ - ECL_INFO("starting optical flow fusion"); - - if (!_aid_src_optical_flow.innovation_rejected && isHorizontalAidingActive()) { - // Consistent with the current velocity state, simply fuse the data without reset - fuseOptFlow(); - _control_status.flags.opt_flow = true; - - } else if (!isHorizontalAidingActive()) { - resetFlowFusion(); - _control_status.flags.opt_flow = true; - - } else { - ECL_INFO("optical flow fusion failed to start"); - _control_status.flags.opt_flow = false; - } -} - -void Ekf::resetFlowFusion() -{ - ECL_INFO("reset velocity to flow"); - _information_events.flags.reset_vel_to_flow = true; - resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed)); - - // reset position, estimate is relative to initial position in this mode, so we start with zero error - if (!_control_status.flags.in_air) { - ECL_INFO("reset position to zero"); - resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); - } - - updateOptFlow(_aid_src_optical_flow); - _innov_check_fail_status.flags.reject_optflow_X = false; - _innov_check_fail_status.flags.reject_optflow_Y = false; - - _aid_src_optical_flow.time_last_fuse = _time_delayed_us; -} - -void Ekf::stopFlowFusion() -{ - if (_control_status.flags.opt_flow) { - ECL_INFO("stopping optical flow fusion"); - _control_status.flags.opt_flow = false; - - resetEstimatorAidStatus(_aid_src_optical_flow); - } -} - -void Ekf::calcOptFlowBodyRateComp(const imuSample &imu_delayed) -{ - if (imu_delayed.delta_ang_dt > FLT_EPSILON) { - _ref_body_rate = -imu_delayed.delta_ang / imu_delayed.delta_ang_dt - getGyroBias(); // flow gyro has opposite sign convention - - } else { - _ref_body_rate.zero(); - } - - if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(0)) || !PX4_ISFINITE(_flow_sample_delayed.gyro_rate(1))) { - _flow_sample_delayed.gyro_rate = _ref_body_rate; - - } else if (!PX4_ISFINITE(_flow_sample_delayed.gyro_rate(2))) { - // Some flow modules only provide X ind Y angular rates. If this is the case, complete the vector with our own Z gyro - _flow_sample_delayed.gyro_rate(2) = _ref_body_rate(2); - } - - // calculate the bias estimate using a combined LPF and spike filter - _flow_gyro_bias = _flow_gyro_bias * 0.99f + matrix::constrain(_flow_sample_delayed.gyro_rate - _ref_body_rate, -0.1f, 0.1f) * 0.01f; -} diff --git a/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt new file mode 100644 index 000000000000..701a5cb13f65 --- /dev/null +++ b/src/modules/ekf2/EKF/output_predictor/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +add_library(output_predictor + output_predictor.cpp + output_predictor.h +) + +add_dependencies(output_predictor prebuild_targets) diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp similarity index 98% rename from src/modules/ekf2/EKF/output_predictor.cpp rename to src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index 93086dc7e6b6..d16e20a79095 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4. All rights reserved. + * Copyright (c) 2022-2024 PX4. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -261,7 +261,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) + const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, + const matrix::Vector3f &accel_bias) { // calculate an average filter update time if (_time_last_correct_states_us != 0) { @@ -365,7 +366,8 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre next_state.vert_vel += vert_vel_correction; // position is propagated forward using the corrected velocity and a trapezoidal integrator - next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * next_state.dt; + next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * + next_state.dt; // advance the index index = (index + 1) % size; diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h similarity index 97% rename from src/modules/ekf2/EKF/output_predictor.h rename to src/modules/ekf2/EKF/output_predictor/output_predictor.h index ee5cc0f7105b..07248f3dce72 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4. All rights reserved. + * Copyright (c) 2022-2024 PX4. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,8 +36,7 @@ #include -#include "common.h" -#include "RingBuffer.h" +#include "../RingBuffer.h" #include @@ -68,7 +67,8 @@ class OutputPredictor const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); void correctOutputStates(const uint64_t time_delayed_us, - const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); void resetQuaternion(const matrix::Quatf &quat_change); diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index a1d2f85c273d..cf8e7de04b33 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -33,55 +33,34 @@ #include "ekf.h" -void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const +void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, + const float observation, const float observation_variance, const float innovation_gate) const { - resetEstimatorAidStatus(aid_src); + float innovation = _state.pos(2) - observation; + float innovation_variance = getStateVariance()(2) + observation_variance; - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::pos.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, - const float innov_gate, estimator_aid_source1d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - aid_src.observation = obs; - aid_src.innovation = _state.pos(2) - aid_src.observation; - - aid_src.observation_variance = math::max(sq(0.01f), obs_var); - aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); + updateAidSourceStatus(aid_src, time_us, + observation, observation_variance, + innovation, innovation_variance, + innovation_gate); // z special case if there is bad vertical acceleration data, then don't reject measurement, // but limit innovation to prevent spikes that could destabilise the filter if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance); + const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance); aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit); aid_src.innovation_rejected = false; } - - aid_src.timestamp_sample = time_us; } -void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) +bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::pos.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::pos.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::pos.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -91,13 +70,16 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } -void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) +bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, State::pos.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation, aid_src.innovation_variance, aid_src.observation_variance, + State::pos.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -107,6 +89,8 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) @@ -180,13 +164,21 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #if defined(CONFIG_EKF2_GNSS) _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); #endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_RANGE_FINDER) - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_TERRAIN) - terrainHandleVerticalPositionReset(delta_z); -#endif + _state.terrain += delta_z; + + // record the state change + if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { + _state_reset_status.hagl_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.hagl_change += delta_z; + } + + _state_reset_status.reset_count.hagl++; +#endif // CONFIG_EKF2_TERRAIN // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; @@ -200,11 +192,3 @@ void Ekf::resetHorizontalPositionToLastKnown() // Used when falling back to non-aiding mode of operation resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); } - -void Ekf::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy) -{ - ECL_INFO("reset position to external observation"); - _information_events.flags.reset_pos_to_ext_obs = true; - - resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy)); -} diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 5cbe1a127f3b..df08b048ee50 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -48,7 +48,7 @@ from symforce.values import Values import sympy as sp -from derivation_utils import * +from utils.derivation_utils import * # Initialize parser parser = argparse.ArgumentParser() @@ -68,7 +68,8 @@ accel_bias = sf.V3(), mag_I = sf.V3(), mag_B = sf.V3(), - wind_vel = sf.V2() + wind_vel = sf.V2(), + terrain = sf.V1() ) if args.disable_mag: @@ -132,7 +133,8 @@ def predict_covariance( accel_bias = sf.V3.symbolic("delta_a_b"), mag_I = sf.V3.symbolic("mag_I"), mag_B = sf.V3.symbolic("mag_B"), - wind_vel = sf.V2.symbolic("wind_vel") + wind_vel = sf.V2.symbolic("wind_vel"), + terrain = sf.V1.symbolic("terrain") ) if args.disable_mag: @@ -148,7 +150,7 @@ def predict_covariance( for key in state.keys(): if key == "quat_nominal": # Create true quaternion using small angle approximation of the error rotation - state_t["quat_nominal"] = state["quat_nominal"] * sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) + state_t["quat_nominal"] = sf.Rot3(sf.Quaternion(xyz=(state_error["theta"] / 2), w=1)) * state["quat_nominal"] else: state_t[key] = state[key] + state_error[key] @@ -184,7 +186,7 @@ def predict_covariance( state_error_pred = Values() for key in state_error.keys(): if key == "theta": - delta_q = sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() * sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage()) + delta_q = sf.Quaternion.from_storage(state_t_pred["quat_nominal"].to_storage()) * sf.Quaternion.from_storage(state_pred["quat_nominal"].to_storage()).conj() state_error_pred["theta"] = 2 * sf.V3(delta_q.x, delta_q.y, delta_q.z) # Use small angle approximation to obtain a simpler jacobian else: state_error_pred[key] = state_t_pred[key] - state_pred[key] @@ -216,6 +218,36 @@ def predict_covariance( return P_new +def jacobian_chain_rule(expr: sf.Scalar , state: State): + # First compute the jacobian in the parameter space + dh_dx = sf.V1(expr).jacobian(state, tangent_space=False) + + class MStorageTangent(sf.Matrix): + SHAPE = (State.storage_dim(), State.tangent_dim()) + + # Then compute the jarobian mapping infinitesimal elements of the parameter space to the error state + # Note that this jacobian only depends on the structure of the EKF + dx_derror = MStorageTangent() + q = sf.Quaternion.from_storage(state["quat_nominal"].to_storage()) + p = sf.Quaternion.symbolic('p') + + pq = p * q + qR = sf.M41(pq.to_storage()).jacobian(sf.M41(p.to_storage())) # Right quaternion product matrix + dx_derror[0:4, 0:3] = qR / 2 * sf.M43([[1, 0, 0], + [0, 1, 0], + [0, 0, 1], + [0, 0, 0]]) + + # The rest of the matrix is trivial + for i in range(4, State.storage_dim()): + for j in range(3, State.tangent_dim()): + if (i == j+1): + dx_derror[i, j] = 1 + + # Finally use the chain rule: dh/derror = dh/dx * dx/derror + H = dh_dx * dx_derror + return H + def compute_airspeed_innov_and_innov_var( state: VState, P: MTangent, @@ -231,7 +263,7 @@ def compute_airspeed_innov_and_innov_var( innov = airspeed_pred - airspeed - H = sf.V1(airspeed_pred).jacobian(state) + H = jacobian_chain_rule(airspeed_pred, state) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) @@ -247,7 +279,7 @@ def compute_airspeed_h_and_k( wind = sf.V3(state["wind_vel"][0], state["wind_vel"][1], 0.0) vel_rel = state["vel"] - wind airspeed_pred = vel_rel.norm(epsilon=epsilon) - H = sf.V1(airspeed_pred).jacobian(state) + H = jacobian_chain_rule(airspeed_pred, state) K = P * H.T / sf.Max(innov_var, epsilon) @@ -282,6 +314,20 @@ def compute_wind_init_and_cov_from_airspeed( wind = wind.subs({sideslip: 0.0}) return (wind, P) +def compute_wind_init_and_cov_from_wind_speed_and_direction( + wind_speed: sf.Scalar, + wind_direction: sf.Scalar, + wind_speed_var: sf.Scalar, + wind_direction_var: sf.Scalar +)-> (sf.V2, sf.V2): + wind = sf.V2(wind_speed * sf.cos(wind_direction), wind_speed * sf.sin(wind_direction)) + H = wind.jacobian([wind_speed, wind_direction]) + R = sf.Matrix.diag([wind_speed_var, wind_direction_var]) + + P = H * R * H.T + P_diag = sf.V2(P[0,0], P[1,1]) + return (wind, P_diag) + def predict_sideslip( state: State, epsilon: sf.Scalar @@ -309,7 +355,7 @@ def compute_sideslip_innov_and_innov_var( innov = sideslip_pred - 0.0 - H = sf.V1(sideslip_pred).jacobian(state) + H = jacobian_chain_rule(sideslip_pred, state) innov_var = (H * P * H.T + R)[0,0] return (innov, innov_var) @@ -324,12 +370,46 @@ def compute_sideslip_h_and_k( state = vstate_to_state(state) sideslip_pred = predict_sideslip(state, epsilon); - H = sf.V1(sideslip_pred).jacobian(state) + H = jacobian_chain_rule(sideslip_pred, state) K = P * H.T / sf.Max(innov_var, epsilon) return (H.T, K) +def predict_vel_body( + state: VState +) -> (sf.V3): + vel = state["vel"] + R_to_body = state["quat_nominal"].inverse() + return R_to_body * vel + +def compute_ev_body_vel_hx( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state) + Hx = jacobian_chain_rule(meas_pred[0], state) + return (Hx.T) + +def compute_ev_body_vel_hy( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[1] + Hy = jacobian_chain_rule(meas_pred, state) + return (Hy.T) + +def compute_ev_body_vel_hz( + state: VState, +) -> (VTangent): + + state = vstate_to_state(state) + meas_pred = predict_vel_body(state)[2] + Hz = jacobian_chain_rule(meas_pred, state) + return (Hz.T) + def predict_mag_body(state) -> sf.V3: mag_field_earth = state["mag_I"] mag_bias_body = state["mag_B"] @@ -351,11 +431,11 @@ def compute_mag_innov_innov_var_and_hx( innov = meas_pred - meas innov_var = sf.V3() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var[1] = (Hy * P * Hy.T + R)[0,0] - Hz = sf.V1(meas_pred[2]).jacobian(state) + Hz = jacobian_chain_rule(meas_pred[2], state) innov_var[2] = (Hz * P * Hz.T + R)[0,0] return (innov, innov_var, Hx.T) @@ -370,7 +450,7 @@ def compute_mag_y_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[1]).jacobian(state) + H = jacobian_chain_rule(meas_pred[1], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -385,7 +465,7 @@ def compute_mag_z_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_mag_body(state); - H = sf.V1(meas_pred[2]).jacobian(state) + H = jacobian_chain_rule(meas_pred[2], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -402,8 +482,11 @@ def compute_yaw_innov_var_and_h( delta_q = q * r.conj() # create a quaternion error of the measurement at the origin delta_meas_pred = 2 * delta_q.z # Use small angle approximation to obtain a simpler jacobian - H = sf.V1(delta_meas_pred).jacobian(state) + H = jacobian_chain_rule(delta_meas_pred, state) H = H.subs({r.w: q.w, r.x: q.x, r.y: q.y, r.z: q.z}) # assume innovation is small + + for i in range(State.tangent_dim()): + H[i] = sp.factor(H[i]).subs(q.w**2 + q.x**2 + q.y**2 + q.z**2, 1) # unit norm quaternion innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -418,12 +501,15 @@ def compute_mag_declination_pred_innov_var_and_h( state = vstate_to_state(state) meas_pred = sf.atan2(state["mag_I"][1], state["mag_I"][0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = jacobian_chain_rule(meas_pred, state) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) -def predict_opt_flow(state, distance, epsilon): +def predict_hagl(state): + return state["terrain"][0] - state["pos"][2] + +def predict_opt_flow(state, epsilon): R_to_body = state["quat_nominal"].inverse() # Calculate earth relative velocity in a non-rotating sensor frame @@ -431,28 +517,28 @@ def predict_opt_flow(state, distance, epsilon): # Divide by range to get predicted angular LOS rates relative to X and Y # axes. Note these are rates in a non-rotating sensor frame + hagl = predict_hagl(state) + hagl = add_epsilon_sign(hagl, hagl, epsilon) + R_to_earth = state["quat_nominal"].to_rotation_matrix() flow_pred = sf.V2() - flow_pred[0] = rel_vel_sensor[1] / distance - flow_pred[1] = -rel_vel_sensor[0] / distance - flow_pred = add_epsilon_sign(flow_pred, distance, epsilon) + flow_pred[0] = rel_vel_sensor[1] / hagl * R_to_earth[2, 2] + flow_pred[1] = -rel_vel_sensor[0] / hagl * R_to_earth[2, 2] return flow_pred - def compute_flow_xy_innov_var_and_hx( state: VState, P: MTangent, - distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.V2, VTangent): state = vstate_to_state(state) - meas_pred = predict_opt_flow(state, distance, epsilon); + meas_pred = predict_opt_flow(state, epsilon) innov_var = sf.V2() - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var[0] = (Hx * P * Hx.T + R)[0,0] - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var[1] = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hx.T) @@ -460,18 +546,40 @@ def compute_flow_xy_innov_var_and_hx( def compute_flow_y_innov_var_and_h( state: VState, P: MTangent, - distance: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar ) -> (sf.Scalar, VTangent): state = vstate_to_state(state) - meas_pred = predict_opt_flow(state, distance, epsilon); + meas_pred = predict_opt_flow(state, epsilon) - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) +def compute_hagl_innov_var( + P: MTangent, + R: sf.Scalar, +) -> (sf.Scalar): + state = VState.symbolic("state") + state = vstate_to_state(state) + meas_pred = predict_hagl(state) + + H = jacobian_chain_rule(meas_pred, state) + innov_var = (H * P * H.T + R)[0,0] + + return (innov_var) + +def compute_hagl_h( +) -> (VTangent): + state = VState.symbolic("state") + state = vstate_to_state(state) + meas_pred = predict_hagl(state) + + H = jacobian_chain_rule(meas_pred, state) + + return (H.T) + def compute_gnss_yaw_pred_innov_var_and_h( state: VState, P: MTangent, @@ -492,7 +600,7 @@ def compute_gnss_yaw_pred_innov_var_and_h( # Calculate the yaw angle from the projection meas_pred = sf.atan2(ant_vec_ef[1], ant_vec_ef[0], epsilon=epsilon) - H = sf.V1(meas_pred).jacobian(state) + H = jacobian_chain_rule(meas_pred, state) innov_var = (H * P * H.T + R)[0,0] return (meas_pred, innov_var, H.T) @@ -529,7 +637,7 @@ def compute_drag_x_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hx = sf.V1(meas_pred[0]).jacobian(state) + Hx = jacobian_chain_rule(meas_pred[0], state) innov_var = (Hx * P * Hx.T + R)[0,0] return (innov_var, Hx.T) @@ -546,7 +654,7 @@ def compute_drag_y_innov_var_and_h( state = vstate_to_state(state) meas_pred = predict_drag(state, rho, cd, cm, epsilon) - Hy = sf.V1(meas_pred[1]).jacobian(state) + Hy = jacobian_chain_rule(meas_pred[1], state) innov_var = (Hy * P * Hy.T + R)[0,0] return (innov_var, Hy.T) @@ -575,7 +683,7 @@ def compute_gravity_xyz_innov_var_and_hx( # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) # for each axis for i in range(3): - H[i] = sf.V1(meas_pred[i]).jacobian(state) + H[i] = jacobian_chain_rule(meas_pred[i], state) innov_var[i] = (H[i] * P * H[i].T + R)[0,0] return (innov_var, H[0].T) @@ -590,7 +698,7 @@ def compute_gravity_y_innov_var_and_h( meas_pred = predict_gravity_direction(state) # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) - H = sf.V1(meas_pred[1]).jacobian(state) + H = jacobian_chain_rule(meas_pred[1], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -605,7 +713,7 @@ def compute_gravity_z_innov_var_and_h( meas_pred = predict_gravity_direction(state) # calculate observation jacobian (H), kalman gain (K), and innovation variance (S) - H = sf.V1(meas_pred[2]).jacobian(state) + H = jacobian_chain_rule(meas_pred[2], state) innov_var = (H * P * H.T + R)[0,0] return (innov_var, H.T) @@ -627,13 +735,19 @@ def compute_gravity_z_innov_var_and_h( generate_px4_function(compute_sideslip_h_and_k, output_names=["H", "K"]) generate_px4_function(compute_sideslip_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_wind_init_and_cov_from_airspeed, output_names=["wind", "P_wind"]) + generate_px4_function(compute_wind_init_and_cov_from_wind_speed_and_direction, output_names=["wind", "P_wind"]) generate_px4_function(compute_yaw_innov_var_and_h, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) generate_px4_function(compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) +generate_px4_function(compute_hagl_innov_var, output_names=["innov_var"]) +generate_px4_function(compute_hagl_h, output_names=["H"]) generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas_pred", "innov_var", "H"]) generate_px4_function(compute_gravity_xyz_innov_var_and_hx, output_names=["innov_var", "Hx"]) generate_px4_function(compute_gravity_y_innov_var_and_h, output_names=["innov_var", "Hy"]) generate_px4_function(compute_gravity_z_innov_var_and_h, output_names=["innov_var", "Hz"]) +generate_px4_function(compute_ev_body_vel_hx, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hy, output_names=["H"]) +generate_px4_function(compute_ev_body_vel_hz, output_names=["H"]) generate_px4_state(State, tangent_idx) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py deleted file mode 100755 index 2d321eb6ed00..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_terrain_estimator.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -""" - Copyright (c) 2023 PX4 Development Team - 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: derivation_terrain_estimator.py -Description: -""" - -import symforce -symforce.set_epsilon_to_symbol() - -import symforce.symbolic as sf -from derivation_utils import * - -def predict_opt_flow( - terrain_vpos: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - epsilon : sf.Scalar -): - R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix() - flow_pred = sf.V2() - dist = - (pos_z - terrain_vpos) - dist = add_epsilon_sign(dist, dist, epsilon) - flow_pred[0] = -v[1] / dist * R_to_earth[2, 2] - flow_pred[1] = v[0] / dist * R_to_earth[2, 2] - return flow_pred - -def terr_est_compute_flow_xy_innov_var_and_hx( - terrain_vpos: sf.Scalar, - P: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - R: sf.Scalar, - epsilon : sf.Scalar -): - flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon) - Hx = sf.V1(flow_pred[0]).jacobian(terrain_vpos) - Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos) - - innov_var = sf.V2() - innov_var[0] = (Hx * P * Hx.T + R)[0,0] - innov_var[1] = (Hy * P * Hy.T + R)[0,0] - - return (innov_var, Hx[0, 0]) - -def terr_est_compute_flow_y_innov_var_and_h( - terrain_vpos: sf.Scalar, - P: sf.Scalar, - q_att: sf.V4, - v: sf.V3, - pos_z: sf.Scalar, - R: sf.Scalar, - epsilon : sf.Scalar -): - flow_pred = predict_opt_flow(terrain_vpos, q_att, v, pos_z, epsilon) - Hy = sf.V1(flow_pred[1]).jacobian(terrain_vpos) - - innov_var = (Hy * P * Hy.T + R)[0,0] - - return (innov_var, Hy[0, 0]) - -print("Derive terrain estimator equations...") -generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"]) -generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h index cc2c18cd6394..52bb16353d36 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_h_and_k.h @@ -16,21 +16,21 @@ namespace sym { * Symbolic function: compute_airspeed_h_and_k * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * innov_var: Scalar * epsilon: Scalar * * Outputs: - * H: Matrix23_1 - * K: Matrix23_1 + * H: Matrix24_1 + * K: Matrix24_1 */ template -void ComputeAirspeedHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 246 +void ComputeAirspeedHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 256 // Input arrays @@ -47,7 +47,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, // Output terms (2) if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); @@ -59,7 +59,7 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k(0, 0) = _tmp6 * (-P(0, 21) * _tmp3 - P(0, 22) * _tmp4 + P(0, 3) * _tmp3 + P(0, 4) * _tmp4 + P(0, 5) * _tmp5); @@ -107,6 +107,8 @@ void ComputeAirspeedHAndK(const matrix::Matrix& state, P(21, 4) * _tmp4 + P(21, 5) * _tmp5); _k(22, 0) = _tmp6 * (-P(22, 21) * _tmp3 - P(22, 22) * _tmp4 + P(22, 3) * _tmp3 + P(22, 4) * _tmp4 + P(22, 5) * _tmp5); + _k(23, 0) = _tmp6 * (-P(23, 21) * _tmp3 - P(23, 22) * _tmp4 + P(23, 3) * _tmp3 + + P(23, 4) * _tmp4 + P(23, 5) * _tmp5); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h index 34c059b56208..e97088d173cf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_airspeed_innov_and_innov_var.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_airspeed_innov_and_innov_var * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * airspeed: Scalar * R: Scalar * epsilon: Scalar @@ -27,8 +27,8 @@ namespace sym { * innov_var: Scalar */ template -void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar airspeed, +void ComputeAirspeedInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar airspeed, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h index 4a08ef76f169..6dba76add33f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_h.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_drag_x_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * rho: Scalar * cd: Scalar * cm: Scalar @@ -26,103 +26,116 @@ namespace sym { * * Outputs: * innov_var: Scalar - * Hx: Matrix23_1 + * Hx: Matrix24_1 */ template -void ComputeDragXInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar rho, +void ComputeDragXInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const Hx = nullptr) { - // Total ops: 317 + matrix::Matrix* const Hx = nullptr) { + // Total ops: 357 // Input arrays - // Intermediate terms (73) - const Scalar _tmp0 = 2 * state(3, 0); - const Scalar _tmp1 = _tmp0 * state(0, 0); - const Scalar _tmp2 = 2 * state(2, 0); - const Scalar _tmp3 = _tmp2 * state(1, 0); + // Intermediate terms (79) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(1, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); const Scalar _tmp4 = _tmp1 + _tmp3; const Scalar _tmp5 = _tmp4 * cm; - const Scalar _tmp6 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp6; - const Scalar _tmp8 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp9 = -2 * _tmp8; - const Scalar _tmp10 = _tmp7 + _tmp9 + 1; - const Scalar _tmp11 = -state(22, 0) + state(4, 0); - const Scalar _tmp12 = -state(23, 0) + state(5, 0); - const Scalar _tmp13 = _tmp2 * state(0, 0); - const Scalar _tmp14 = -_tmp13; - const Scalar _tmp15 = _tmp0 * state(1, 0); - const Scalar _tmp16 = _tmp14 + _tmp15; - const Scalar _tmp17 = _tmp12 * _tmp4 + _tmp16 * state(6, 0); - const Scalar _tmp18 = _tmp10 * _tmp11 + _tmp17; - const Scalar _tmp19 = 2 * _tmp18; - const Scalar _tmp20 = _tmp19 * _tmp4; - const Scalar _tmp21 = _tmp0 * state(2, 0); - const Scalar _tmp22 = 2 * state(0, 0) * state(1, 0); - const Scalar _tmp23 = -_tmp22; - const Scalar _tmp24 = _tmp21 + _tmp23; - const Scalar _tmp25 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp26 = 1 - 2 * _tmp25; - const Scalar _tmp27 = _tmp26 + _tmp9; - const Scalar _tmp28 = _tmp13 + _tmp15; - const Scalar _tmp29 = _tmp11 * _tmp28 + _tmp12 * _tmp24; - const Scalar _tmp30 = _tmp27 * state(6, 0) + _tmp29; + const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = _tmp6 + _tmp7 + 1; + const Scalar _tmp9 = -state(22, 0) + state(4, 0); + const Scalar _tmp10 = -state(23, 0) + state(5, 0); + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = _tmp11 * state(0, 0); + const Scalar _tmp13 = _tmp2 * state(3, 0); + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _tmp10 * _tmp4 + _tmp14 * state(6, 0) + _tmp8 * _tmp9; + const Scalar _tmp16 = 2 * _tmp15; + const Scalar _tmp17 = _tmp16 * _tmp4; + const Scalar _tmp18 = _tmp11 * state(3, 0); + const Scalar _tmp19 = _tmp2 * state(0, 0); + const Scalar _tmp20 = _tmp18 - _tmp19; + const Scalar _tmp21 = _tmp12 + _tmp13; + const Scalar _tmp22 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp23 = _tmp22 + _tmp7; + const Scalar _tmp24 = _tmp10 * _tmp20 + _tmp21 * _tmp9 + _tmp23 * state(6, 0); + const Scalar _tmp25 = 2 * _tmp24; + const Scalar _tmp26 = _tmp20 * _tmp25; + const Scalar _tmp27 = _tmp22 + _tmp6; + const Scalar _tmp28 = -_tmp1 + _tmp3; + const Scalar _tmp29 = _tmp18 + _tmp19; + const Scalar _tmp30 = _tmp10 * _tmp27 + _tmp28 * _tmp9 + _tmp29 * state(6, 0); const Scalar _tmp31 = 2 * _tmp30; - const Scalar _tmp32 = _tmp24 * _tmp31; - const Scalar _tmp33 = _tmp26 + _tmp7; - const Scalar _tmp34 = -_tmp1; - const Scalar _tmp35 = _tmp3 + _tmp34; - const Scalar _tmp36 = _tmp21 + _tmp22; - const Scalar _tmp37 = _tmp11 * _tmp35 + _tmp36 * state(6, 0); - const Scalar _tmp38 = _tmp12 * _tmp33 + _tmp37; - const Scalar _tmp39 = 2 * _tmp38; - const Scalar _tmp40 = _tmp33 * _tmp39; - const Scalar _tmp41 = std::sqrt(Scalar(std::pow(_tmp18, Scalar(2)) + std::pow(_tmp30, Scalar(2)) + - std::pow(_tmp38, Scalar(2)) + epsilon)); - const Scalar _tmp42 = cd * rho; - const Scalar _tmp43 = Scalar(0.25) * _tmp18 * _tmp42 / _tmp41; - const Scalar _tmp44 = Scalar(0.5) * _tmp41 * _tmp42; - const Scalar _tmp45 = _tmp4 * _tmp44; - const Scalar _tmp46 = -_tmp43 * (_tmp20 + _tmp32 + _tmp40) - _tmp45 - _tmp5; - const Scalar _tmp47 = -_tmp25; - const Scalar _tmp48 = _tmp47 + _tmp6; - const Scalar _tmp49 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp50 = -_tmp49; - const Scalar _tmp51 = _tmp50 + _tmp8; - const Scalar _tmp52 = -_tmp3; - const Scalar _tmp53 = -_tmp15; - const Scalar _tmp54 = -_tmp6; - const Scalar _tmp55 = _tmp12 * (_tmp47 + _tmp49 + _tmp54 + _tmp8) + _tmp37; - const Scalar _tmp56 = -_tmp43 * (_tmp19 * _tmp55 + _tmp39 * (_tmp11 * (_tmp48 + _tmp51) + - _tmp12 * (_tmp34 + _tmp52) + - state(6, 0) * (_tmp13 + _tmp53))) - - _tmp44 * _tmp55 - _tmp55 * cm; - const Scalar _tmp57 = -_tmp43 * (-_tmp20 - _tmp32 - _tmp40) + _tmp45 + _tmp5; - const Scalar _tmp58 = _tmp10 * cm; - const Scalar _tmp59 = _tmp10 * _tmp19; - const Scalar _tmp60 = _tmp28 * _tmp31; - const Scalar _tmp61 = _tmp35 * _tmp39; - const Scalar _tmp62 = _tmp10 * _tmp44; - const Scalar _tmp63 = -_tmp43 * (-_tmp59 - _tmp60 - _tmp61) + _tmp58 + _tmp62; - const Scalar _tmp64 = -_tmp8; - const Scalar _tmp65 = -_tmp21; - const Scalar _tmp66 = _tmp49 + _tmp64; - const Scalar _tmp67 = - _tmp43 * (_tmp31 * (_tmp11 * (_tmp1 + _tmp52) + _tmp12 * (_tmp25 + _tmp50 + _tmp6 + _tmp64) + - state(6, 0) * (_tmp23 + _tmp65)) + - _tmp39 * (_tmp29 + state(6, 0) * (_tmp48 + _tmp66))); - const Scalar _tmp68 = _tmp25 + _tmp54; - const Scalar _tmp69 = - _tmp11 * (_tmp14 + _tmp53) + _tmp12 * (_tmp22 + _tmp65) + state(6, 0) * (_tmp51 + _tmp68); - const Scalar _tmp70 = - -_tmp43 * (_tmp19 * _tmp69 + _tmp31 * (_tmp11 * (_tmp66 + _tmp68) + _tmp17)) - - _tmp44 * _tmp69 - _tmp69 * cm; - const Scalar _tmp71 = -_tmp43 * (_tmp59 + _tmp60 + _tmp61) - _tmp58 - _tmp62; - const Scalar _tmp72 = -_tmp16 * _tmp44 - _tmp16 * cm - - _tmp43 * (_tmp16 * _tmp19 + _tmp27 * _tmp31 + _tmp36 * _tmp39); + const Scalar _tmp32 = _tmp27 * _tmp31; + const Scalar _tmp33 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp24, Scalar(2)) + + std::pow(_tmp30, Scalar(2)) + epsilon)); + const Scalar _tmp34 = cd * rho; + const Scalar _tmp35 = Scalar(0.25) * _tmp15 * _tmp34 / _tmp33; + const Scalar _tmp36 = Scalar(0.5) * _tmp33 * _tmp34; + const Scalar _tmp37 = _tmp36 * _tmp4; + const Scalar _tmp38 = -_tmp35 * (-_tmp17 - _tmp26 - _tmp32) + _tmp37 + _tmp5; + const Scalar _tmp39 = -_tmp35 * (_tmp17 + _tmp26 + _tmp32) - _tmp37 - _tmp5; + const Scalar _tmp40 = _tmp8 * cm; + const Scalar _tmp41 = _tmp16 * _tmp8; + const Scalar _tmp42 = _tmp21 * _tmp25; + const Scalar _tmp43 = _tmp28 * _tmp31; + const Scalar _tmp44 = _tmp36 * _tmp8; + const Scalar _tmp45 = -_tmp35 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44; + const Scalar _tmp46 = 2 * state(3, 0); + const Scalar _tmp47 = _tmp10 * _tmp46; + const Scalar _tmp48 = 2 * state(6, 0); + const Scalar _tmp49 = _tmp48 * state(2, 0); + const Scalar _tmp50 = _tmp47 - _tmp49; + const Scalar _tmp51 = _tmp10 * _tmp2; + const Scalar _tmp52 = _tmp11 * _tmp9; + const Scalar _tmp53 = _tmp46 * _tmp9; + const Scalar _tmp54 = _tmp2 * state(6, 0); + const Scalar _tmp55 = + -_tmp35 * (_tmp16 * _tmp50 + _tmp25 * (-_tmp51 + _tmp52) + _tmp31 * (-_tmp53 + _tmp54)) - + _tmp36 * _tmp50 - _tmp50 * cm; + const Scalar _tmp56 = (Scalar(1) / Scalar(2)) * _tmp55; + const Scalar _tmp57 = _tmp10 * _tmp11; + const Scalar _tmp58 = _tmp48 * state(3, 0); + const Scalar _tmp59 = _tmp57 + _tmp58; + const Scalar _tmp60 = _tmp0 * _tmp10; + const Scalar _tmp61 = 4 * state(6, 0); + const Scalar _tmp62 = 4 * _tmp10; + const Scalar _tmp63 = _tmp48 * state(0, 0); + const Scalar _tmp64 = + -_tmp35 * (_tmp16 * _tmp59 + _tmp25 * (_tmp53 - _tmp60 - _tmp61 * state(1, 0)) + + _tmp31 * (_tmp52 - _tmp62 * state(1, 0) + _tmp63)) - + _tmp36 * _tmp59 - _tmp59 * cm; + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp66 = 4 * _tmp9; + const Scalar _tmp67 = _tmp54 + _tmp60 - _tmp66 * state(3, 0); + const Scalar _tmp68 = _tmp2 * _tmp9; + const Scalar _tmp69 = _tmp0 * _tmp9; + const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp35 * + (_tmp16 * _tmp67 + _tmp25 * (_tmp57 + _tmp68) + + _tmp31 * (_tmp49 - _tmp62 * state(3, 0) - _tmp69)) - + Scalar(1) / Scalar(2) * _tmp36 * _tmp67 - + Scalar(1) / Scalar(2) * _tmp67 * cm; + const Scalar _tmp71 = _tmp51 - _tmp63 - _tmp66 * state(2, 0); + const Scalar _tmp72 = -Scalar(1) / Scalar(2) * _tmp35 * + (_tmp16 * _tmp71 + _tmp25 * (_tmp47 - _tmp61 * state(2, 0) + _tmp69) + + _tmp31 * (_tmp58 + _tmp68)) - + Scalar(1) / Scalar(2) * _tmp36 * _tmp71 - + Scalar(1) / Scalar(2) * _tmp71 * cm; + const Scalar _tmp73 = + -_tmp56 * state(3, 0) - _tmp64 * _tmp65 + _tmp70 * state(0, 0) + _tmp72 * state(1, 0); + const Scalar _tmp74 = (Scalar(1) / Scalar(2)) * _tmp64; + const Scalar _tmp75 = + -_tmp55 * _tmp65 - _tmp70 * state(1, 0) + _tmp72 * state(0, 0) + _tmp74 * state(3, 0); + const Scalar _tmp76 = + -_tmp56 * state(1, 0) + _tmp70 * state(2, 0) - _tmp72 * state(3, 0) + _tmp74 * state(0, 0); + const Scalar _tmp77 = -_tmp35 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44; + const Scalar _tmp78 = -_tmp14 * _tmp36 - _tmp14 * cm - + _tmp35 * (_tmp14 * _tmp16 + _tmp23 * _tmp25 + _tmp29 * _tmp31); // Output terms (2) if (innov_var != nullptr) { @@ -130,37 +143,37 @@ void ComputeDragXInnovVarAndH(const matrix::Matrix& state, _innov_var = R + - _tmp46 * (-P(0, 4) * _tmp67 + P(1, 4) * _tmp70 + P(2, 4) * _tmp56 + P(21, 4) * _tmp63 + - P(22, 4) * _tmp57 + P(3, 4) * _tmp71 + P(4, 4) * _tmp46 + P(5, 4) * _tmp72) + - _tmp56 * (-P(0, 2) * _tmp67 + P(1, 2) * _tmp70 + P(2, 2) * _tmp56 + P(21, 2) * _tmp63 + - P(22, 2) * _tmp57 + P(3, 2) * _tmp71 + P(4, 2) * _tmp46 + P(5, 2) * _tmp72) + - _tmp57 * (-P(0, 22) * _tmp67 + P(1, 22) * _tmp70 + P(2, 22) * _tmp56 + P(21, 22) * _tmp63 + - P(22, 22) * _tmp57 + P(3, 22) * _tmp71 + P(4, 22) * _tmp46 + P(5, 22) * _tmp72) + - _tmp63 * (-P(0, 21) * _tmp67 + P(1, 21) * _tmp70 + P(2, 21) * _tmp56 + P(21, 21) * _tmp63 + - P(22, 21) * _tmp57 + P(3, 21) * _tmp71 + P(4, 21) * _tmp46 + P(5, 21) * _tmp72) - - _tmp67 * (-P(0, 0) * _tmp67 + P(1, 0) * _tmp70 + P(2, 0) * _tmp56 + P(21, 0) * _tmp63 + - P(22, 0) * _tmp57 + P(3, 0) * _tmp71 + P(4, 0) * _tmp46 + P(5, 0) * _tmp72) + - _tmp70 * (-P(0, 1) * _tmp67 + P(1, 1) * _tmp70 + P(2, 1) * _tmp56 + P(21, 1) * _tmp63 + - P(22, 1) * _tmp57 + P(3, 1) * _tmp71 + P(4, 1) * _tmp46 + P(5, 1) * _tmp72) + - _tmp71 * (-P(0, 3) * _tmp67 + P(1, 3) * _tmp70 + P(2, 3) * _tmp56 + P(21, 3) * _tmp63 + - P(22, 3) * _tmp57 + P(3, 3) * _tmp71 + P(4, 3) * _tmp46 + P(5, 3) * _tmp72) + - _tmp72 * (-P(0, 5) * _tmp67 + P(1, 5) * _tmp70 + P(2, 5) * _tmp56 + P(21, 5) * _tmp63 + - P(22, 5) * _tmp57 + P(3, 5) * _tmp71 + P(4, 5) * _tmp46 + P(5, 5) * _tmp72); + _tmp38 * (P(0, 22) * _tmp76 + P(1, 22) * _tmp75 + P(2, 22) * _tmp73 + P(21, 22) * _tmp45 + + P(22, 22) * _tmp38 + P(3, 22) * _tmp77 + P(4, 22) * _tmp39 + P(5, 22) * _tmp78) + + _tmp39 * (P(0, 4) * _tmp76 + P(1, 4) * _tmp75 + P(2, 4) * _tmp73 + P(21, 4) * _tmp45 + + P(22, 4) * _tmp38 + P(3, 4) * _tmp77 + P(4, 4) * _tmp39 + P(5, 4) * _tmp78) + + _tmp45 * (P(0, 21) * _tmp76 + P(1, 21) * _tmp75 + P(2, 21) * _tmp73 + P(21, 21) * _tmp45 + + P(22, 21) * _tmp38 + P(3, 21) * _tmp77 + P(4, 21) * _tmp39 + P(5, 21) * _tmp78) + + _tmp73 * (P(0, 2) * _tmp76 + P(1, 2) * _tmp75 + P(2, 2) * _tmp73 + P(21, 2) * _tmp45 + + P(22, 2) * _tmp38 + P(3, 2) * _tmp77 + P(4, 2) * _tmp39 + P(5, 2) * _tmp78) + + _tmp75 * (P(0, 1) * _tmp76 + P(1, 1) * _tmp75 + P(2, 1) * _tmp73 + P(21, 1) * _tmp45 + + P(22, 1) * _tmp38 + P(3, 1) * _tmp77 + P(4, 1) * _tmp39 + P(5, 1) * _tmp78) + + _tmp76 * (P(0, 0) * _tmp76 + P(1, 0) * _tmp75 + P(2, 0) * _tmp73 + P(21, 0) * _tmp45 + + P(22, 0) * _tmp38 + P(3, 0) * _tmp77 + P(4, 0) * _tmp39 + P(5, 0) * _tmp78) + + _tmp77 * (P(0, 3) * _tmp76 + P(1, 3) * _tmp75 + P(2, 3) * _tmp73 + P(21, 3) * _tmp45 + + P(22, 3) * _tmp38 + P(3, 3) * _tmp77 + P(4, 3) * _tmp39 + P(5, 3) * _tmp78) + + _tmp78 * (P(0, 5) * _tmp76 + P(1, 5) * _tmp75 + P(2, 5) * _tmp73 + P(21, 5) * _tmp45 + + P(22, 5) * _tmp38 + P(3, 5) * _tmp77 + P(4, 5) * _tmp39 + P(5, 5) * _tmp78); } if (Hx != nullptr) { - matrix::Matrix& _hx = (*Hx); + matrix::Matrix& _hx = (*Hx); _hx.setZero(); - _hx(0, 0) = -_tmp67; - _hx(1, 0) = _tmp70; - _hx(2, 0) = _tmp56; - _hx(3, 0) = _tmp71; - _hx(4, 0) = _tmp46; - _hx(5, 0) = _tmp72; - _hx(21, 0) = _tmp63; - _hx(22, 0) = _tmp57; + _hx(0, 0) = _tmp76; + _hx(1, 0) = _tmp75; + _hx(2, 0) = _tmp73; + _hx(3, 0) = _tmp77; + _hx(4, 0) = _tmp39; + _hx(5, 0) = _tmp78; + _hx(21, 0) = _tmp45; + _hx(22, 0) = _tmp38; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h index c504d0db636a..a7bf92840c16 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_h.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_drag_y_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * rho: Scalar * cd: Scalar * cm: Scalar @@ -26,105 +26,116 @@ namespace sym { * * Outputs: * innov_var: Scalar - * Hy: Matrix23_1 + * Hy: Matrix24_1 */ template -void ComputeDragYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar rho, +void ComputeDragYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar rho, const Scalar cd, const Scalar cm, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const Hy = nullptr) { - // Total ops: 317 + matrix::Matrix* const Hy = nullptr) { + // Total ops: 360 // Input arrays - // Intermediate terms (73) - const Scalar _tmp0 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = _tmp1 * state(0, 0); - const Scalar _tmp3 = _tmp0 + _tmp2; - const Scalar _tmp4 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp5 = -2 * _tmp4; - const Scalar _tmp6 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp7 = -2 * _tmp6; - const Scalar _tmp8 = _tmp5 + _tmp7 + 1; + // Intermediate terms (76) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(1, 0); + const Scalar _tmp3 = _tmp2 * state(2, 0); + const Scalar _tmp4 = -_tmp1 + _tmp3; + const Scalar _tmp5 = _tmp4 * cm; + const Scalar _tmp6 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp7 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = _tmp6 + _tmp7 + 1; const Scalar _tmp9 = -state(22, 0) + state(4, 0); - const Scalar _tmp10 = 2 * state(0, 0); - const Scalar _tmp11 = _tmp10 * state(3, 0); - const Scalar _tmp12 = _tmp1 * state(2, 0); - const Scalar _tmp13 = _tmp11 + _tmp12; - const Scalar _tmp14 = -state(23, 0) + state(5, 0); - const Scalar _tmp15 = _tmp10 * state(2, 0); - const Scalar _tmp16 = -_tmp15; - const Scalar _tmp17 = _tmp1 * state(3, 0); - const Scalar _tmp18 = _tmp16 + _tmp17; - const Scalar _tmp19 = _tmp13 * _tmp14 + _tmp18 * state(6, 0); - const Scalar _tmp20 = _tmp19 + _tmp8 * _tmp9; - const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp22 = 1 - 2 * _tmp21; - const Scalar _tmp23 = _tmp22 + _tmp7; - const Scalar _tmp24 = -_tmp2; - const Scalar _tmp25 = _tmp0 + _tmp24; - const Scalar _tmp26 = _tmp15 + _tmp17; - const Scalar _tmp27 = _tmp14 * _tmp25 + _tmp26 * _tmp9; - const Scalar _tmp28 = _tmp23 * state(6, 0) + _tmp27; - const Scalar _tmp29 = _tmp22 + _tmp5; - const Scalar _tmp30 = -_tmp11; - const Scalar _tmp31 = _tmp12 + _tmp30; - const Scalar _tmp32 = _tmp3 * state(6, 0) + _tmp31 * _tmp9; - const Scalar _tmp33 = _tmp14 * _tmp29 + _tmp32; - const Scalar _tmp34 = std::sqrt(Scalar(std::pow(_tmp20, Scalar(2)) + std::pow(_tmp28, Scalar(2)) + - std::pow(_tmp33, Scalar(2)) + epsilon)); - const Scalar _tmp35 = cd * rho; - const Scalar _tmp36 = Scalar(0.5) * _tmp34 * _tmp35; - const Scalar _tmp37 = 2 * _tmp20; - const Scalar _tmp38 = 2 * _tmp28; - const Scalar _tmp39 = 2 * _tmp33; - const Scalar _tmp40 = Scalar(0.25) * _tmp33 * _tmp35 / _tmp34; - const Scalar _tmp41 = - -_tmp3 * _tmp36 - _tmp3 * cm - _tmp40 * (_tmp18 * _tmp37 + _tmp23 * _tmp38 + _tmp3 * _tmp39); - const Scalar _tmp42 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp43 = -_tmp42; - const Scalar _tmp44 = -_tmp6; - const Scalar _tmp45 = -_tmp12; - const Scalar _tmp46 = -_tmp0; - const Scalar _tmp47 = -_tmp21; - const Scalar _tmp48 = _tmp4 + _tmp47; - const Scalar _tmp49 = _tmp42 + _tmp44; - const Scalar _tmp50 = _tmp27 + state(6, 0) * (_tmp48 + _tmp49); - const Scalar _tmp51 = - -_tmp36 * _tmp50 - - _tmp40 * (_tmp38 * (_tmp14 * (_tmp21 + _tmp4 + _tmp43 + _tmp44) + _tmp9 * (_tmp11 + _tmp45) + - state(6, 0) * (_tmp24 + _tmp46)) + - _tmp39 * _tmp50) - - _tmp50 * cm; - const Scalar _tmp52 = _tmp43 + _tmp6; - const Scalar _tmp53 = -_tmp17; - const Scalar _tmp54 = - _tmp14 * (_tmp30 + _tmp45) + _tmp9 * (_tmp48 + _tmp52) + state(6, 0) * (_tmp15 + _tmp53); - const Scalar _tmp55 = -_tmp4; + const Scalar _tmp10 = _tmp1 + _tmp3; + const Scalar _tmp11 = -state(23, 0) + state(5, 0); + const Scalar _tmp12 = _tmp0 * state(2, 0); + const Scalar _tmp13 = _tmp2 * state(3, 0); + const Scalar _tmp14 = -_tmp12 + _tmp13; + const Scalar _tmp15 = _tmp10 * _tmp11 + _tmp14 * state(6, 0) + _tmp8 * _tmp9; + const Scalar _tmp16 = 2 * state(3, 0); + const Scalar _tmp17 = _tmp16 * state(2, 0); + const Scalar _tmp18 = _tmp2 * state(0, 0); + const Scalar _tmp19 = _tmp17 - _tmp18; + const Scalar _tmp20 = _tmp12 + _tmp13; + const Scalar _tmp21 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp22 = _tmp21 + _tmp7; + const Scalar _tmp23 = _tmp11 * _tmp19 + _tmp20 * _tmp9 + _tmp22 * state(6, 0); + const Scalar _tmp24 = _tmp21 + _tmp6; + const Scalar _tmp25 = _tmp17 + _tmp18; + const Scalar _tmp26 = _tmp11 * _tmp24 + _tmp25 * state(6, 0) + _tmp4 * _tmp9; + const Scalar _tmp27 = std::sqrt(Scalar(std::pow(_tmp15, Scalar(2)) + std::pow(_tmp23, Scalar(2)) + + std::pow(_tmp26, Scalar(2)) + epsilon)); + const Scalar _tmp28 = cd * rho; + const Scalar _tmp29 = Scalar(0.5) * _tmp27 * _tmp28; + const Scalar _tmp30 = _tmp29 * _tmp4; + const Scalar _tmp31 = 2 * _tmp15; + const Scalar _tmp32 = _tmp31 * _tmp8; + const Scalar _tmp33 = 2 * _tmp23; + const Scalar _tmp34 = _tmp20 * _tmp33; + const Scalar _tmp35 = 2 * _tmp26; + const Scalar _tmp36 = _tmp35 * _tmp4; + const Scalar _tmp37 = Scalar(0.25) * _tmp26 * _tmp28 / _tmp27; + const Scalar _tmp38 = _tmp30 - _tmp37 * (-_tmp32 - _tmp34 - _tmp36) + _tmp5; + const Scalar _tmp39 = -_tmp30 - _tmp37 * (_tmp32 + _tmp34 + _tmp36) - _tmp5; + const Scalar _tmp40 = _tmp24 * cm; + const Scalar _tmp41 = _tmp10 * _tmp31; + const Scalar _tmp42 = _tmp19 * _tmp33; + const Scalar _tmp43 = _tmp24 * _tmp35; + const Scalar _tmp44 = _tmp24 * _tmp29; + const Scalar _tmp45 = -_tmp37 * (_tmp41 + _tmp42 + _tmp43) - _tmp40 - _tmp44; + const Scalar _tmp46 = _tmp16 * _tmp9; + const Scalar _tmp47 = _tmp0 * _tmp11; + const Scalar _tmp48 = state(1, 0) * state(6, 0); + const Scalar _tmp49 = 2 * state(2, 0); + const Scalar _tmp50 = _tmp11 * _tmp49; + const Scalar _tmp51 = _tmp16 * state(6, 0); + const Scalar _tmp52 = 4 * _tmp11; + const Scalar _tmp53 = _tmp49 * _tmp9; + const Scalar _tmp54 = _tmp0 * state(6, 0); + const Scalar _tmp55 = -_tmp52 * state(1, 0) + _tmp53 + _tmp54; const Scalar _tmp56 = - -_tmp36 * _tmp54 - - _tmp40 * (_tmp37 * (_tmp14 * (_tmp42 + _tmp47 + _tmp55 + _tmp6) + _tmp32) + _tmp39 * _tmp54) - - _tmp54 * cm; - const Scalar _tmp57 = _tmp29 * cm; - const Scalar _tmp58 = _tmp13 * _tmp37; - const Scalar _tmp59 = _tmp25 * _tmp38; - const Scalar _tmp60 = _tmp29 * _tmp39; - const Scalar _tmp61 = _tmp29 * _tmp36; - const Scalar _tmp62 = -_tmp40 * (-_tmp58 - _tmp59 - _tmp60) + _tmp57 + _tmp61; - const Scalar _tmp63 = _tmp21 + _tmp55; - const Scalar _tmp64 = _tmp40 * (_tmp37 * (_tmp14 * (_tmp2 + _tmp46) + _tmp9 * (_tmp16 + _tmp53) + - state(6, 0) * (_tmp52 + _tmp63)) + - _tmp38 * (_tmp19 + _tmp9 * (_tmp49 + _tmp63))); - const Scalar _tmp65 = -_tmp40 * (_tmp58 + _tmp59 + _tmp60) - _tmp57 - _tmp61; - const Scalar _tmp66 = _tmp31 * cm; - const Scalar _tmp67 = _tmp31 * _tmp36; - const Scalar _tmp68 = _tmp37 * _tmp8; - const Scalar _tmp69 = _tmp26 * _tmp38; - const Scalar _tmp70 = _tmp31 * _tmp39; - const Scalar _tmp71 = -_tmp40 * (_tmp68 + _tmp69 + _tmp70) - _tmp66 - _tmp67; - const Scalar _tmp72 = -_tmp40 * (-_tmp68 - _tmp69 - _tmp70) + _tmp66 + _tmp67; + -Scalar(1) / Scalar(2) * _tmp29 * _tmp55 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp50 + _tmp51) + _tmp33 * (_tmp46 - _tmp47 - 4 * _tmp48) + _tmp35 * _tmp55) - + Scalar(1) / Scalar(2) * _tmp55 * cm; + const Scalar _tmp57 = 2 * _tmp48; + const Scalar _tmp58 = -_tmp46 + _tmp57; + const Scalar _tmp59 = _tmp11 * _tmp2; + const Scalar _tmp60 = _tmp11 * _tmp16; + const Scalar _tmp61 = state(2, 0) * state(6, 0); + const Scalar _tmp62 = 2 * _tmp61; + const Scalar _tmp63 = + -Scalar(1) / Scalar(2) * _tmp29 * _tmp58 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp60 - _tmp62) + _tmp33 * (_tmp53 - _tmp59) + _tmp35 * _tmp58) - + Scalar(1) / Scalar(2) * _tmp58 * cm; + const Scalar _tmp64 = _tmp2 * _tmp9; + const Scalar _tmp65 = _tmp51 + _tmp64; + const Scalar _tmp66 = _tmp0 * _tmp9; + const Scalar _tmp67 = 4 * _tmp9; + const Scalar _tmp68 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp65 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (-_tmp54 + _tmp59 - _tmp67 * state(2, 0)) + + _tmp33 * (_tmp60 - 4 * _tmp61 + _tmp66) + _tmp35 * _tmp65) - + Scalar(1) / Scalar(2) * _tmp65 * cm; + const Scalar _tmp69 = -_tmp52 * state(3, 0) + _tmp62 - _tmp66; + const Scalar _tmp70 = -Scalar(1) / Scalar(2) * _tmp29 * _tmp69 - + Scalar(1) / Scalar(2) * _tmp37 * + (_tmp31 * (_tmp47 + _tmp57 - _tmp67 * state(3, 0)) + + _tmp33 * (_tmp50 + _tmp64) + _tmp35 * _tmp69) - + Scalar(1) / Scalar(2) * _tmp69 * cm; + const Scalar _tmp71 = + -_tmp56 * state(2, 0) - _tmp63 * state(3, 0) + _tmp68 * state(1, 0) + _tmp70 * state(0, 0); + const Scalar _tmp72 = + _tmp56 * state(3, 0) - _tmp63 * state(2, 0) + _tmp68 * state(0, 0) - _tmp70 * state(1, 0); + const Scalar _tmp73 = + _tmp56 * state(0, 0) - _tmp63 * state(1, 0) - _tmp68 * state(3, 0) + _tmp70 * state(2, 0); + const Scalar _tmp74 = -_tmp37 * (-_tmp41 - _tmp42 - _tmp43) + _tmp40 + _tmp44; + const Scalar _tmp75 = -_tmp25 * _tmp29 - _tmp25 * cm - + _tmp37 * (_tmp14 * _tmp31 + _tmp22 * _tmp33 + _tmp25 * _tmp35); // Output terms (2) if (innov_var != nullptr) { @@ -132,37 +143,37 @@ void ComputeDragYInnovVarAndH(const matrix::Matrix& state, _innov_var = R + - _tmp41 * (P(0, 5) * _tmp51 - P(1, 5) * _tmp64 + P(2, 5) * _tmp56 + P(21, 5) * _tmp72 + - P(22, 5) * _tmp62 + P(3, 5) * _tmp71 + P(4, 5) * _tmp65 + P(5, 5) * _tmp41) + - _tmp51 * (P(0, 0) * _tmp51 - P(1, 0) * _tmp64 + P(2, 0) * _tmp56 + P(21, 0) * _tmp72 + - P(22, 0) * _tmp62 + P(3, 0) * _tmp71 + P(4, 0) * _tmp65 + P(5, 0) * _tmp41) + - _tmp56 * (P(0, 2) * _tmp51 - P(1, 2) * _tmp64 + P(2, 2) * _tmp56 + P(21, 2) * _tmp72 + - P(22, 2) * _tmp62 + P(3, 2) * _tmp71 + P(4, 2) * _tmp65 + P(5, 2) * _tmp41) + - _tmp62 * (P(0, 22) * _tmp51 - P(1, 22) * _tmp64 + P(2, 22) * _tmp56 + P(21, 22) * _tmp72 + - P(22, 22) * _tmp62 + P(3, 22) * _tmp71 + P(4, 22) * _tmp65 + P(5, 22) * _tmp41) - - _tmp64 * (P(0, 1) * _tmp51 - P(1, 1) * _tmp64 + P(2, 1) * _tmp56 + P(21, 1) * _tmp72 + - P(22, 1) * _tmp62 + P(3, 1) * _tmp71 + P(4, 1) * _tmp65 + P(5, 1) * _tmp41) + - _tmp65 * (P(0, 4) * _tmp51 - P(1, 4) * _tmp64 + P(2, 4) * _tmp56 + P(21, 4) * _tmp72 + - P(22, 4) * _tmp62 + P(3, 4) * _tmp71 + P(4, 4) * _tmp65 + P(5, 4) * _tmp41) + - _tmp71 * (P(0, 3) * _tmp51 - P(1, 3) * _tmp64 + P(2, 3) * _tmp56 + P(21, 3) * _tmp72 + - P(22, 3) * _tmp62 + P(3, 3) * _tmp71 + P(4, 3) * _tmp65 + P(5, 3) * _tmp41) + - _tmp72 * (P(0, 21) * _tmp51 - P(1, 21) * _tmp64 + P(2, 21) * _tmp56 + P(21, 21) * _tmp72 + - P(22, 21) * _tmp62 + P(3, 21) * _tmp71 + P(4, 21) * _tmp65 + P(5, 21) * _tmp41); + _tmp38 * (P(0, 21) * _tmp73 + P(1, 21) * _tmp72 + P(2, 21) * _tmp71 + P(21, 21) * _tmp38 + + P(22, 21) * _tmp74 + P(3, 21) * _tmp39 + P(4, 21) * _tmp45 + P(5, 21) * _tmp75) + + _tmp39 * (P(0, 3) * _tmp73 + P(1, 3) * _tmp72 + P(2, 3) * _tmp71 + P(21, 3) * _tmp38 + + P(22, 3) * _tmp74 + P(3, 3) * _tmp39 + P(4, 3) * _tmp45 + P(5, 3) * _tmp75) + + _tmp45 * (P(0, 4) * _tmp73 + P(1, 4) * _tmp72 + P(2, 4) * _tmp71 + P(21, 4) * _tmp38 + + P(22, 4) * _tmp74 + P(3, 4) * _tmp39 + P(4, 4) * _tmp45 + P(5, 4) * _tmp75) + + _tmp71 * (P(0, 2) * _tmp73 + P(1, 2) * _tmp72 + P(2, 2) * _tmp71 + P(21, 2) * _tmp38 + + P(22, 2) * _tmp74 + P(3, 2) * _tmp39 + P(4, 2) * _tmp45 + P(5, 2) * _tmp75) + + _tmp72 * (P(0, 1) * _tmp73 + P(1, 1) * _tmp72 + P(2, 1) * _tmp71 + P(21, 1) * _tmp38 + + P(22, 1) * _tmp74 + P(3, 1) * _tmp39 + P(4, 1) * _tmp45 + P(5, 1) * _tmp75) + + _tmp73 * (P(0, 0) * _tmp73 + P(1, 0) * _tmp72 + P(2, 0) * _tmp71 + P(21, 0) * _tmp38 + + P(22, 0) * _tmp74 + P(3, 0) * _tmp39 + P(4, 0) * _tmp45 + P(5, 0) * _tmp75) + + _tmp74 * (P(0, 22) * _tmp73 + P(1, 22) * _tmp72 + P(2, 22) * _tmp71 + P(21, 22) * _tmp38 + + P(22, 22) * _tmp74 + P(3, 22) * _tmp39 + P(4, 22) * _tmp45 + P(5, 22) * _tmp75) + + _tmp75 * (P(0, 5) * _tmp73 + P(1, 5) * _tmp72 + P(2, 5) * _tmp71 + P(21, 5) * _tmp38 + + P(22, 5) * _tmp74 + P(3, 5) * _tmp39 + P(4, 5) * _tmp45 + P(5, 5) * _tmp75); } if (Hy != nullptr) { - matrix::Matrix& _hy = (*Hy); + matrix::Matrix& _hy = (*Hy); _hy.setZero(); - _hy(0, 0) = _tmp51; - _hy(1, 0) = -_tmp64; - _hy(2, 0) = _tmp56; - _hy(3, 0) = _tmp71; - _hy(4, 0) = _tmp65; - _hy(5, 0) = _tmp41; - _hy(21, 0) = _tmp72; - _hy(22, 0) = _tmp62; + _hy(0, 0) = _tmp73; + _hy(1, 0) = _tmp72; + _hy(2, 0) = _tmp71; + _hy(3, 0) = _tmp39; + _hy(4, 0) = _tmp45; + _hy(5, 0) = _tmp75; + _hy(21, 0) = _tmp38; + _hy(22, 0) = _tmp74; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h new file mode 100644 index 000000000000..72a2adf0f67a --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hx.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hx + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHx(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(5, 0); + const Scalar _tmp1 = 2 * state(6, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) - _tmp1 * state(2, 0); + const Scalar _tmp3 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0); + const Scalar _tmp5 = 4 * state(4, 0); + const Scalar _tmp6 = 2 * state(1, 0); + const Scalar _tmp7 = _tmp0 * state(0, 0) - _tmp5 * state(3, 0) + _tmp6 * state(6, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp7; + const Scalar _tmp9 = 2 * state(0, 0); + const Scalar _tmp10 = _tmp0 * state(1, 0) - _tmp5 * state(2, 0) - _tmp9 * state(6, 0); + const Scalar _tmp11 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp2; + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = -_tmp11 * state(3, 0) - _tmp2 * _tmp3 + _tmp4 * state(0, 0) + _tmp8 * state(2, 0); + _h(1, 0) = _tmp11 * state(0, 0) - _tmp12 * state(2, 0) - _tmp3 * _tmp7 + _tmp4 * state(3, 0); + _h(2, 0) = _tmp10 * _tmp3 - _tmp12 * state(3, 0) - _tmp4 * state(2, 0) + _tmp8 * state(0, 0); + _h(3, 0) = -2 * std::pow(state(2, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(4, 0) = _tmp6 * state(2, 0) + _tmp9 * state(3, 0); + _h(5, 0) = _tmp6 * state(3, 0) - _tmp9 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h new file mode 100644 index 000000000000..a4dd6f94d94c --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hy.h @@ -0,0 +1,67 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hy + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHy(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 64 + + // Input arrays + + // Intermediate terms (9) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = + -Scalar(1) / Scalar(2) * _tmp0 * state(3, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(6, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp3 * state(6, 0); + const Scalar _tmp5 = 4 * state(5, 0); + const Scalar _tmp6 = 2 * state(6, 0); + const Scalar _tmp7 = -Scalar(1) / Scalar(2) * _tmp0 * state(0, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(3, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(2, 0); + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp6 * state(0, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = + -_tmp2 * state(1, 0) - _tmp4 * state(3, 0) + _tmp7 * state(2, 0) + _tmp8 * state(0, 0); + _h(1, 0) = + -_tmp2 * state(2, 0) + _tmp4 * state(0, 0) - _tmp7 * state(1, 0) + _tmp8 * state(3, 0); + _h(2, 0) = + -_tmp2 * state(3, 0) + _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * state(2, 0); + _h(3, 0) = _tmp1 * state(2, 0) - _tmp3 * state(0, 0); + _h(4, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + _h(5, 0) = _tmp1 * state(0, 0) + _tmp3 * state(2, 0); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h new file mode 100644 index 000000000000..395bb85b4a2e --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_ev_body_vel_hz.h @@ -0,0 +1,63 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_ev_body_vel_hz + * + * Args: + * state: Matrix25_1 + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeEvBodyVelHz(const matrix::Matrix& state, + matrix::Matrix* const H = nullptr) { + // Total ops: 60 + + // Input arrays + + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(4, 0); + const Scalar _tmp1 = 2 * state(5, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(1, 0) + (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0); + const Scalar _tmp3 = _tmp0 * state(2, 0) - _tmp1 * state(1, 0); + const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp3; + const Scalar _tmp5 = 4 * state(6, 0); + const Scalar _tmp6 = _tmp0 * state(3, 0) - _tmp1 * state(0, 0) - _tmp5 * state(1, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0) - _tmp5 * state(2, 0); + const Scalar _tmp9 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp10 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp11 = 2 * state(2, 0); + const Scalar _tmp12 = 2 * state(1, 0); + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(0, 0) = _tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp7 * state(0, 0) - _tmp8 * _tmp9; + _h(1, 0) = _tmp10 * state(0, 0) - _tmp2 * state(1, 0) - _tmp4 * state(2, 0) + _tmp6 * _tmp9; + _h(2, 0) = _tmp10 * state(1, 0) + _tmp2 * state(0, 0) - _tmp3 * _tmp9 - _tmp7 * state(2, 0); + _h(3, 0) = _tmp11 * state(0, 0) + _tmp12 * state(3, 0); + _h(4, 0) = _tmp11 * state(3, 0) - _tmp12 * state(0, 0); + _h(5, 0) = -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h index 11dfaacfcc0f..75809345d968 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h @@ -16,105 +16,153 @@ namespace sym { * Symbolic function: compute_flow_xy_innov_var_and_hx * * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * distance: Scalar + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Matrix21 - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar distance, - const Scalar R, const Scalar epsilon, +void ComputeFlowXyInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, matrix::Matrix* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 196 + matrix::Matrix* const H = nullptr) { + // Total ops: 431 // Input arrays - // Intermediate terms (33) - const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = - Scalar(1.0) / - (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp4 = _tmp3 * (-2 * _tmp0 + _tmp2); - const Scalar _tmp5 = 2 * state(2, 0); - const Scalar _tmp6 = _tmp5 * state(0, 0); - const Scalar _tmp7 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp8 = _tmp5 * state(3, 0); - const Scalar _tmp9 = 2 * state(0, 0); - const Scalar _tmp10 = _tmp9 * state(1, 0); - const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -_tmp0; - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp3 * (state(4, 0) * (_tmp6 + _tmp7) + state(5, 0) * (-_tmp10 + _tmp8) + - state(6, 0) * (_tmp1 - _tmp11 + _tmp14)); - const Scalar _tmp16 = _tmp9 * state(3, 0); - const Scalar _tmp17 = -_tmp16; - const Scalar _tmp18 = _tmp5 * state(1, 0); - const Scalar _tmp19 = _tmp17 + _tmp18; - const Scalar _tmp20 = _tmp19 * _tmp3; - const Scalar _tmp21 = _tmp10 + _tmp8; - const Scalar _tmp22 = _tmp21 * _tmp3; - const Scalar _tmp23 = -_tmp7; - const Scalar _tmp24 = -_tmp12; - const Scalar _tmp25 = _tmp3 * (state(4, 0) * (_tmp1 + _tmp11 + _tmp13 + _tmp24) + - state(5, 0) * (_tmp17 - _tmp18) + state(6, 0) * (_tmp23 + _tmp6)); - const Scalar _tmp26 = _tmp3 * (-2 * _tmp11 + _tmp2); - const Scalar _tmp27 = -_tmp6; - const Scalar _tmp28 = -_tmp1 + _tmp11; - const Scalar _tmp29 = _tmp3 * (state(4, 0) * (_tmp23 + _tmp27) + state(5, 0) * (_tmp10 - _tmp8) + - state(6, 0) * (_tmp0 + _tmp24 + _tmp28)); - const Scalar _tmp30 = - _tmp3 * (_tmp19 * state(4, 0) + _tmp21 * state(6, 0) + state(5, 0) * (_tmp14 + _tmp28)); - const Scalar _tmp31 = _tmp3 * (_tmp16 + _tmp18); - const Scalar _tmp32 = _tmp3 * (_tmp27 + _tmp7); + // Intermediate terms (66) + const Scalar _tmp0 = state(2, 0) * state(4, 0); + const Scalar _tmp1 = state(1, 0) * state(5, 0); + const Scalar _tmp2 = 2 * state(6, 0); + const Scalar _tmp3 = _tmp2 * state(0, 0); + const Scalar _tmp4 = -2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp5 = 1 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp6 = _tmp4 + _tmp5; + const Scalar _tmp7 = state(24, 0) - state(9, 0); + const Scalar _tmp8 = + _tmp7 + epsilon * (2 * math::min(0, (((_tmp7) > 0) - ((_tmp7) < 0))) + 1); + const Scalar _tmp9 = Scalar(1.0) / (_tmp8); + const Scalar _tmp10 = _tmp6 * _tmp9; + const Scalar _tmp11 = 2 * state(0, 0) * state(3, 0); + const Scalar _tmp12 = 2 * state(1, 0); + const Scalar _tmp13 = _tmp12 * state(2, 0); + const Scalar _tmp14 = -_tmp11 + _tmp13; + const Scalar _tmp15 = 2 * state(2, 0); + const Scalar _tmp16 = _tmp12 * state(0, 0) + _tmp15 * state(3, 0); + const Scalar _tmp17 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp18 = _tmp17 + _tmp5; + const Scalar _tmp19 = _tmp14 * state(4, 0) + _tmp16 * state(6, 0) + _tmp18 * state(5, 0); + const Scalar _tmp20 = 4 * _tmp9; + const Scalar _tmp21 = _tmp19 * _tmp20; + const Scalar _tmp22 = _tmp10 * (2 * _tmp0 - 4 * _tmp1 + _tmp3) - _tmp21 * state(1, 0); + const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * _tmp22; + const Scalar _tmp24 = 2 * state(4, 0); + const Scalar _tmp25 = 4 * state(3, 0); + const Scalar _tmp26 = _tmp2 * state(2, 0); + const Scalar _tmp27 = -_tmp24 * state(0, 0) - _tmp25 * state(5, 0) + _tmp26; + const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10; + const Scalar _tmp29 = _tmp28 * state(0, 0); + const Scalar _tmp30 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp31 = _tmp2 * state(1, 0); + const Scalar _tmp32 = _tmp10 * (-_tmp24 * state(3, 0) + _tmp31); + const Scalar _tmp33 = _tmp2 * state(3, 0); + const Scalar _tmp34 = _tmp10 * (_tmp24 * state(1, 0) + _tmp33) - _tmp21 * state(2, 0); + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * _tmp34; + const Scalar _tmp36 = + -_tmp23 * state(2, 0) + _tmp27 * _tmp29 - _tmp30 * _tmp32 + _tmp35 * state(1, 0); + const Scalar _tmp37 = _tmp10 * _tmp14; + const Scalar _tmp38 = _tmp28 * state(2, 0); + const Scalar _tmp39 = (Scalar(1) / Scalar(2)) * _tmp32; + const Scalar _tmp40 = + _tmp23 * state(0, 0) + _tmp27 * _tmp38 - _tmp30 * _tmp34 - _tmp39 * state(1, 0); + const Scalar _tmp41 = _tmp28 * state(1, 0); + const Scalar _tmp42 = + _tmp22 * _tmp30 - _tmp27 * _tmp41 + _tmp35 * state(0, 0) - _tmp39 * state(2, 0); + const Scalar _tmp43 = _tmp10 * _tmp18; + const Scalar _tmp44 = _tmp6 / std::pow(_tmp8, Scalar(2)); + const Scalar _tmp45 = _tmp19 * _tmp44; + const Scalar _tmp46 = _tmp10 * _tmp16; + const Scalar _tmp47 = _tmp12 * state(3, 0) - _tmp15 * state(0, 0); + const Scalar _tmp48 = _tmp10 * _tmp47; + const Scalar _tmp49 = _tmp11 + _tmp13; + const Scalar _tmp50 = _tmp10 * _tmp49; + const Scalar _tmp51 = _tmp17 + _tmp4 + 1; + const Scalar _tmp52 = _tmp10 * _tmp51; + const Scalar _tmp53 = _tmp47 * state(6, 0) + _tmp49 * state(5, 0) + _tmp51 * state(4, 0); + const Scalar _tmp54 = _tmp20 * _tmp53; + const Scalar _tmp55 = 2 * state(5, 0); + const Scalar _tmp56 = -_tmp10 * (_tmp33 + _tmp55 * state(2, 0)) + _tmp54 * state(1, 0); + const Scalar _tmp57 = -_tmp10 * (-4 * _tmp0 + 2 * _tmp1 - _tmp3) + _tmp54 * state(2, 0); + const Scalar _tmp58 = (Scalar(1) / Scalar(2)) * _tmp57; + const Scalar _tmp59 = -_tmp25 * state(4, 0) + _tmp31 + _tmp55 * state(0, 0); + const Scalar _tmp60 = -_tmp26 + _tmp55 * state(3, 0); + const Scalar _tmp61 = _tmp30 * _tmp56 + _tmp38 * _tmp60 + _tmp41 * _tmp59 + _tmp58 * state(0, 0); + const Scalar _tmp62 = (Scalar(1) / Scalar(2)) * _tmp56; + const Scalar _tmp63 = -_tmp30 * _tmp57 - _tmp38 * _tmp59 + _tmp41 * _tmp60 + _tmp62 * state(0, 0); + const Scalar _tmp64 = + _tmp10 * _tmp30 * _tmp60 - _tmp29 * _tmp59 + _tmp58 * state(1, 0) - _tmp62 * state(2, 0); + const Scalar _tmp65 = _tmp44 * _tmp53; // Output terms (2) if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = R + - _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp25 + P(3, 0) * _tmp20 + - P(4, 0) * _tmp4 + P(5, 0) * _tmp22) + - _tmp20 * (P(0, 3) * _tmp15 + P(2, 3) * _tmp25 + P(3, 3) * _tmp20 + - P(4, 3) * _tmp4 + P(5, 3) * _tmp22) + - _tmp22 * (P(0, 5) * _tmp15 + P(2, 5) * _tmp25 + P(3, 5) * _tmp20 + - P(4, 5) * _tmp4 + P(5, 5) * _tmp22) + - _tmp25 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp25 + P(3, 2) * _tmp20 + - P(4, 2) * _tmp4 + P(5, 2) * _tmp22) + - _tmp4 * (P(0, 4) * _tmp15 + P(2, 4) * _tmp25 + P(3, 4) * _tmp20 + - P(4, 4) * _tmp4 + P(5, 4) * _tmp22); - _innov_var(1, 0) = R - - _tmp26 * (-P(1, 3) * _tmp29 - P(2, 3) * _tmp30 - P(3, 3) * _tmp26 - - P(4, 3) * _tmp31 - P(5, 3) * _tmp32) - - _tmp29 * (-P(1, 1) * _tmp29 - P(2, 1) * _tmp30 - P(3, 1) * _tmp26 - - P(4, 1) * _tmp31 - P(5, 1) * _tmp32) - - _tmp30 * (-P(1, 2) * _tmp29 - P(2, 2) * _tmp30 - P(3, 2) * _tmp26 - - P(4, 2) * _tmp31 - P(5, 2) * _tmp32) - - _tmp31 * (-P(1, 4) * _tmp29 - P(2, 4) * _tmp30 - P(3, 4) * _tmp26 - - P(4, 4) * _tmp31 - P(5, 4) * _tmp32) - - _tmp32 * (-P(1, 5) * _tmp29 - P(2, 5) * _tmp30 - P(3, 5) * _tmp26 - - P(4, 5) * _tmp31 - P(5, 5) * _tmp32); + _innov_var(0, 0) = + R + + _tmp36 * (P(0, 2) * _tmp40 + P(1, 2) * _tmp42 + P(2, 2) * _tmp36 - P(23, 2) * _tmp45 + + P(3, 2) * _tmp37 + P(4, 2) * _tmp43 + P(5, 2) * _tmp46 + P(8, 2) * _tmp45) + + _tmp37 * (P(0, 3) * _tmp40 + P(1, 3) * _tmp42 + P(2, 3) * _tmp36 - P(23, 3) * _tmp45 + + P(3, 3) * _tmp37 + P(4, 3) * _tmp43 + P(5, 3) * _tmp46 + P(8, 3) * _tmp45) + + _tmp40 * (P(0, 0) * _tmp40 + P(1, 0) * _tmp42 + P(2, 0) * _tmp36 - P(23, 0) * _tmp45 + + P(3, 0) * _tmp37 + P(4, 0) * _tmp43 + P(5, 0) * _tmp46 + P(8, 0) * _tmp45) + + _tmp42 * (P(0, 1) * _tmp40 + P(1, 1) * _tmp42 + P(2, 1) * _tmp36 - P(23, 1) * _tmp45 + + P(3, 1) * _tmp37 + P(4, 1) * _tmp43 + P(5, 1) * _tmp46 + P(8, 1) * _tmp45) + + _tmp43 * (P(0, 4) * _tmp40 + P(1, 4) * _tmp42 + P(2, 4) * _tmp36 - P(23, 4) * _tmp45 + + P(3, 4) * _tmp37 + P(4, 4) * _tmp43 + P(5, 4) * _tmp46 + P(8, 4) * _tmp45) - + _tmp45 * (P(0, 23) * _tmp40 + P(1, 23) * _tmp42 + P(2, 23) * _tmp36 - P(23, 23) * _tmp45 + + P(3, 23) * _tmp37 + P(4, 23) * _tmp43 + P(5, 23) * _tmp46 + P(8, 23) * _tmp45) + + _tmp45 * (P(0, 8) * _tmp40 + P(1, 8) * _tmp42 + P(2, 8) * _tmp36 - P(23, 8) * _tmp45 + + P(3, 8) * _tmp37 + P(4, 8) * _tmp43 + P(5, 8) * _tmp46 + P(8, 8) * _tmp45) + + _tmp46 * (P(0, 5) * _tmp40 + P(1, 5) * _tmp42 + P(2, 5) * _tmp36 - P(23, 5) * _tmp45 + + P(3, 5) * _tmp37 + P(4, 5) * _tmp43 + P(5, 5) * _tmp46 + P(8, 5) * _tmp45); + _innov_var(1, 0) = + R - + _tmp48 * (P(0, 5) * _tmp63 + P(1, 5) * _tmp61 + P(2, 5) * _tmp64 + P(23, 5) * _tmp65 - + P(3, 5) * _tmp52 - P(4, 5) * _tmp50 - P(5, 5) * _tmp48 - P(8, 5) * _tmp65) - + _tmp50 * (P(0, 4) * _tmp63 + P(1, 4) * _tmp61 + P(2, 4) * _tmp64 + P(23, 4) * _tmp65 - + P(3, 4) * _tmp52 - P(4, 4) * _tmp50 - P(5, 4) * _tmp48 - P(8, 4) * _tmp65) - + _tmp52 * (P(0, 3) * _tmp63 + P(1, 3) * _tmp61 + P(2, 3) * _tmp64 + P(23, 3) * _tmp65 - + P(3, 3) * _tmp52 - P(4, 3) * _tmp50 - P(5, 3) * _tmp48 - P(8, 3) * _tmp65) + + _tmp61 * (P(0, 1) * _tmp63 + P(1, 1) * _tmp61 + P(2, 1) * _tmp64 + P(23, 1) * _tmp65 - + P(3, 1) * _tmp52 - P(4, 1) * _tmp50 - P(5, 1) * _tmp48 - P(8, 1) * _tmp65) + + _tmp63 * (P(0, 0) * _tmp63 + P(1, 0) * _tmp61 + P(2, 0) * _tmp64 + P(23, 0) * _tmp65 - + P(3, 0) * _tmp52 - P(4, 0) * _tmp50 - P(5, 0) * _tmp48 - P(8, 0) * _tmp65) + + _tmp64 * (P(0, 2) * _tmp63 + P(1, 2) * _tmp61 + P(2, 2) * _tmp64 + P(23, 2) * _tmp65 - + P(3, 2) * _tmp52 - P(4, 2) * _tmp50 - P(5, 2) * _tmp48 - P(8, 2) * _tmp65) + + _tmp65 * (P(0, 23) * _tmp63 + P(1, 23) * _tmp61 + P(2, 23) * _tmp64 + P(23, 23) * _tmp65 - + P(3, 23) * _tmp52 - P(4, 23) * _tmp50 - P(5, 23) * _tmp48 - P(8, 23) * _tmp65) - + _tmp65 * (P(0, 8) * _tmp63 + P(1, 8) * _tmp61 + P(2, 8) * _tmp64 + P(23, 8) * _tmp65 - + P(3, 8) * _tmp52 - P(4, 8) * _tmp50 - P(5, 8) * _tmp48 - P(8, 8) * _tmp65); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp15; - _h(2, 0) = _tmp25; - _h(3, 0) = _tmp20; - _h(4, 0) = _tmp4; - _h(5, 0) = _tmp22; + _h(0, 0) = _tmp40; + _h(1, 0) = _tmp42; + _h(2, 0) = _tmp36; + _h(3, 0) = _tmp37; + _h(4, 0) = _tmp43; + _h(5, 0) = _tmp46; + _h(8, 0) = _tmp45; + _h(23, 0) = -_tmp45; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h index 3e00a2f60490..178f6d75e64f 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h @@ -16,79 +16,100 @@ namespace sym { * Symbolic function: compute_flow_y_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 - * distance: Scalar + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar distance, - const Scalar R, const Scalar epsilon, - Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 116 +void ComputeFlowYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, + const Scalar epsilon, Scalar* const innov_var = nullptr, + matrix::Matrix* const H = nullptr) { + // Total ops: 232 // Input arrays - // Intermediate terms (19) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp2 = - Scalar(1.0) / - (distance + epsilon * (2 * math::min(0, (((distance) > 0) - ((distance) < 0))) + 1)); - const Scalar _tmp3 = _tmp2 * (-2 * _tmp0 - 2 * _tmp1 + 1); - const Scalar _tmp4 = -2 * state(0, 0) * state(2, 0); - const Scalar _tmp5 = 2 * state(3, 0); - const Scalar _tmp6 = _tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp5 * state(2, 0); - const Scalar _tmp8 = 2 * state(1, 0); - const Scalar _tmp9 = _tmp8 * state(0, 0); - const Scalar _tmp10 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp11 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp12 = -_tmp0 + _tmp1; - const Scalar _tmp13 = _tmp2 * (state(4, 0) * (_tmp4 - _tmp6) + state(5, 0) * (-_tmp7 + _tmp9) + - state(6, 0) * (-_tmp10 + _tmp11 + _tmp12)); - const Scalar _tmp14 = _tmp5 * state(0, 0); - const Scalar _tmp15 = _tmp8 * state(2, 0); - const Scalar _tmp16 = - _tmp2 * (state(4, 0) * (-_tmp14 + _tmp15) + state(5, 0) * (_tmp10 - _tmp11 + _tmp12) + - state(6, 0) * (_tmp7 + _tmp9)); - const Scalar _tmp17 = _tmp2 * (_tmp14 + _tmp15); - const Scalar _tmp18 = _tmp2 * (_tmp4 + _tmp6); + // Intermediate terms (28) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -_tmp0 * state(2, 0) + _tmp1 * state(3, 0); + const Scalar _tmp3 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp4 = _tmp3 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp5 = state(24, 0) - state(9, 0); + const Scalar _tmp6 = + _tmp5 + epsilon * (2 * math::min(0, (((_tmp5) > 0) - ((_tmp5) < 0))) + 1); + const Scalar _tmp7 = Scalar(1.0) / (_tmp6); + const Scalar _tmp8 = _tmp4 * _tmp7; + const Scalar _tmp9 = _tmp2 * _tmp8; + const Scalar _tmp10 = _tmp0 * state(3, 0) + _tmp1 * state(2, 0); + const Scalar _tmp11 = _tmp10 * _tmp8; + const Scalar _tmp12 = _tmp3 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp13 = _tmp12 * _tmp8; + const Scalar _tmp14 = _tmp10 * state(5, 0) + _tmp12 * state(4, 0) + _tmp2 * state(6, 0); + const Scalar _tmp15 = 4 * _tmp14 * _tmp7; + const Scalar _tmp16 = 2 * state(5, 0); + const Scalar _tmp17 = 2 * state(6, 0); + const Scalar _tmp18 = + (Scalar(1) / Scalar(2)) * _tmp15 * state(1, 0) - + Scalar(1) / Scalar(2) * _tmp8 * (_tmp16 * state(2, 0) + _tmp17 * state(3, 0)); + const Scalar _tmp19 = 4 * state(4, 0); + const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * _tmp15 * state(2, 0) - + Scalar(1) / Scalar(2) * _tmp8 * + (_tmp16 * state(1, 0) - _tmp17 * state(0, 0) - _tmp19 * state(2, 0)); + const Scalar _tmp21 = (Scalar(1) / Scalar(2)) * _tmp8; + const Scalar _tmp22 = + _tmp21 * (_tmp16 * state(0, 0) + _tmp17 * state(1, 0) - _tmp19 * state(3, 0)); + const Scalar _tmp23 = _tmp21 * (_tmp16 * state(3, 0) - _tmp17 * state(2, 0)); + const Scalar _tmp24 = + _tmp18 * state(3, 0) + _tmp20 * state(0, 0) + _tmp22 * state(1, 0) + _tmp23 * state(2, 0); + const Scalar _tmp25 = + _tmp18 * state(0, 0) - _tmp20 * state(3, 0) - _tmp22 * state(2, 0) + _tmp23 * state(1, 0); + const Scalar _tmp26 = + -_tmp18 * state(2, 0) + _tmp20 * state(1, 0) - _tmp22 * state(0, 0) + _tmp23 * state(3, 0); + const Scalar _tmp27 = _tmp14 * _tmp4 / std::pow(_tmp6, Scalar(2)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R - - _tmp13 * (-P(1, 1) * _tmp13 - P(2, 1) * _tmp16 - P(3, 1) * _tmp3 - - P(4, 1) * _tmp17 - P(5, 1) * _tmp18) - - _tmp16 * (-P(1, 2) * _tmp13 - P(2, 2) * _tmp16 - P(3, 2) * _tmp3 - - P(4, 2) * _tmp17 - P(5, 2) * _tmp18) - - _tmp17 * (-P(1, 4) * _tmp13 - P(2, 4) * _tmp16 - P(3, 4) * _tmp3 - - P(4, 4) * _tmp17 - P(5, 4) * _tmp18) - - _tmp18 * (-P(1, 5) * _tmp13 - P(2, 5) * _tmp16 - P(3, 5) * _tmp3 - - P(4, 5) * _tmp17 - P(5, 5) * _tmp18) - - _tmp3 * (-P(1, 3) * _tmp13 - P(2, 3) * _tmp16 - P(3, 3) * _tmp3 - - P(4, 3) * _tmp17 - P(5, 3) * _tmp18); + _innov_var = + R - + _tmp11 * (P(0, 4) * _tmp25 + P(1, 4) * _tmp24 + P(2, 4) * _tmp26 + P(23, 4) * _tmp27 - + P(3, 4) * _tmp13 - P(4, 4) * _tmp11 - P(5, 4) * _tmp9 - P(8, 4) * _tmp27) - + _tmp13 * (P(0, 3) * _tmp25 + P(1, 3) * _tmp24 + P(2, 3) * _tmp26 + P(23, 3) * _tmp27 - + P(3, 3) * _tmp13 - P(4, 3) * _tmp11 - P(5, 3) * _tmp9 - P(8, 3) * _tmp27) + + _tmp24 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp24 + P(2, 1) * _tmp26 + P(23, 1) * _tmp27 - + P(3, 1) * _tmp13 - P(4, 1) * _tmp11 - P(5, 1) * _tmp9 - P(8, 1) * _tmp27) + + _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp24 + P(2, 0) * _tmp26 + P(23, 0) * _tmp27 - + P(3, 0) * _tmp13 - P(4, 0) * _tmp11 - P(5, 0) * _tmp9 - P(8, 0) * _tmp27) + + _tmp26 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp24 + P(2, 2) * _tmp26 + P(23, 2) * _tmp27 - + P(3, 2) * _tmp13 - P(4, 2) * _tmp11 - P(5, 2) * _tmp9 - P(8, 2) * _tmp27) + + _tmp27 * (P(0, 23) * _tmp25 + P(1, 23) * _tmp24 + P(2, 23) * _tmp26 + P(23, 23) * _tmp27 - + P(3, 23) * _tmp13 - P(4, 23) * _tmp11 - P(5, 23) * _tmp9 - P(8, 23) * _tmp27) - + _tmp27 * (P(0, 8) * _tmp25 + P(1, 8) * _tmp24 + P(2, 8) * _tmp26 + P(23, 8) * _tmp27 - + P(3, 8) * _tmp13 - P(4, 8) * _tmp11 - P(5, 8) * _tmp9 - P(8, 8) * _tmp27) - + _tmp9 * (P(0, 5) * _tmp25 + P(1, 5) * _tmp24 + P(2, 5) * _tmp26 + P(23, 5) * _tmp27 - + P(3, 5) * _tmp13 - P(4, 5) * _tmp11 - P(5, 5) * _tmp9 - P(8, 5) * _tmp27); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(1, 0) = -_tmp13; - _h(2, 0) = -_tmp16; - _h(3, 0) = -_tmp3; - _h(4, 0) = -_tmp17; - _h(5, 0) = -_tmp18; + _h(0, 0) = _tmp25; + _h(1, 0) = _tmp24; + _h(2, 0) = _tmp26; + _h(3, 0) = -_tmp13; + _h(4, 0) = -_tmp11; + _h(5, 0) = -_tmp9; + _h(8, 0) = -_tmp27; + _h(23, 0) = _tmp27; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h index d1be17dc3556..028d7aba5587 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gnss_yaw_pred_innov_var_and_h.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_gnss_yaw_pred_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * antenna_yaw_offset: Scalar * R: Scalar * epsilon: Scalar @@ -25,76 +25,81 @@ namespace sym { * Outputs: * meas_pred: Scalar * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, +void ComputeGnssYawPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar antenna_yaw_offset, const Scalar R, const Scalar epsilon, Scalar* const meas_pred = nullptr, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 95 + matrix::Matrix* const H = nullptr) { + // Total ops: 114 // Input arrays - // Intermediate terms (28) - const Scalar _tmp0 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = std::sin(antenna_yaw_offset); - const Scalar _tmp4 = 2 * state(0, 0); - const Scalar _tmp5 = _tmp4 * state(3, 0); - const Scalar _tmp6 = 2 * state(2, 0); - const Scalar _tmp7 = _tmp6 * state(1, 0); - const Scalar _tmp8 = std::cos(antenna_yaw_offset); - const Scalar _tmp9 = _tmp3 * (-2 * _tmp0 + _tmp2) + _tmp8 * (_tmp5 + _tmp7); - const Scalar _tmp10 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp11 = -_tmp5; - const Scalar _tmp12 = _tmp11 + _tmp7; - const Scalar _tmp13 = _tmp12 * _tmp3 + _tmp8 * (-2 * _tmp10 + _tmp2); - const Scalar _tmp14 = _tmp13 + epsilon * ((((_tmp13) > 0) - ((_tmp13) < 0)) + Scalar(0.5)); - const Scalar _tmp15 = _tmp6 * state(0, 0); - const Scalar _tmp16 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp17 = std::pow(_tmp14, Scalar(2)); - const Scalar _tmp18 = _tmp9 / _tmp17; - const Scalar _tmp19 = _tmp6 * state(3, 0); - const Scalar _tmp20 = _tmp4 * state(1, 0); - const Scalar _tmp21 = Scalar(1.0) / (_tmp14); - const Scalar _tmp22 = _tmp17 / (_tmp17 + std::pow(_tmp9, Scalar(2))); - const Scalar _tmp23 = - _tmp22 * (-_tmp18 * _tmp8 * (-_tmp15 - _tmp16) + _tmp21 * _tmp8 * (-_tmp19 + _tmp20)); - const Scalar _tmp24 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp25 = -_tmp0 + _tmp10; - const Scalar _tmp26 = - _tmp22 * (-_tmp18 * (_tmp12 * _tmp8 + _tmp3 * (_tmp1 - _tmp24 + _tmp25)) + - _tmp21 * (_tmp3 * (_tmp11 - _tmp7) + _tmp8 * (-_tmp1 + _tmp24 + _tmp25))); + // Intermediate terms (29) + const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = std::sin(antenna_yaw_offset); + const Scalar _tmp2 = 2 * state(0, 0) * state(3, 0); + const Scalar _tmp3 = 2 * state(1, 0) * state(2, 0); + const Scalar _tmp4 = std::cos(antenna_yaw_offset); + const Scalar _tmp5 = + _tmp1 * (_tmp0 - 2 * std::pow(state(1, 0), Scalar(2))) + _tmp4 * (_tmp2 + _tmp3); + const Scalar _tmp6 = + _tmp1 * (-_tmp2 + _tmp3) + _tmp4 * (_tmp0 - 2 * std::pow(state(2, 0), Scalar(2))); + const Scalar _tmp7 = _tmp6 + epsilon * ((((_tmp6) > 0) - ((_tmp6) < 0)) + Scalar(0.5)); + const Scalar _tmp8 = 2 * _tmp1; + const Scalar _tmp9 = std::pow(_tmp7, Scalar(2)); + const Scalar _tmp10 = _tmp5 / _tmp9; + const Scalar _tmp11 = _tmp10 * _tmp8; + const Scalar _tmp12 = 4 * _tmp1; + const Scalar _tmp13 = 2 * _tmp4; + const Scalar _tmp14 = Scalar(1.0) / (_tmp7); + const Scalar _tmp15 = + -_tmp11 * state(2, 0) + _tmp14 * (-_tmp12 * state(1, 0) + _tmp13 * state(2, 0)); + const Scalar _tmp16 = (Scalar(1) / Scalar(2)) * _tmp9 / (std::pow(_tmp5, Scalar(2)) + _tmp9); + const Scalar _tmp17 = _tmp15 * _tmp16; + const Scalar _tmp18 = _tmp13 * _tmp14; + const Scalar _tmp19 = _tmp11 * state(3, 0) + _tmp18 * state(3, 0); + const Scalar _tmp20 = _tmp16 * _tmp19; + const Scalar _tmp21 = 4 * _tmp4; + const Scalar _tmp22 = -_tmp10 * (-_tmp21 * state(3, 0) - _tmp8 * state(0, 0)) + + _tmp14 * (-_tmp12 * state(3, 0) + _tmp13 * state(0, 0)); + const Scalar _tmp23 = _tmp16 * state(2, 0); + const Scalar _tmp24 = + _tmp16 * (-_tmp10 * (-_tmp21 * state(2, 0) + _tmp8 * state(1, 0)) + _tmp18 * state(1, 0)); + const Scalar _tmp25 = + _tmp17 * state(0, 0) - _tmp20 * state(1, 0) + _tmp22 * _tmp23 - _tmp24 * state(3, 0); + const Scalar _tmp26 = _tmp16 * _tmp22; const Scalar _tmp27 = - _tmp22 * (-_tmp18 * _tmp3 * (_tmp15 + _tmp16) + _tmp21 * _tmp3 * (_tmp19 - _tmp20)); + _tmp17 * state(3, 0) - _tmp19 * _tmp23 + _tmp24 * state(0, 0) - _tmp26 * state(1, 0); + const Scalar _tmp28 = + -_tmp15 * _tmp23 - _tmp20 * state(3, 0) + _tmp24 * state(1, 0) + _tmp26 * state(0, 0); // Output terms (3) if (meas_pred != nullptr) { Scalar& _meas_pred = (*meas_pred); - _meas_pred = std::atan2(_tmp9, _tmp14); + _meas_pred = std::atan2(_tmp5, _tmp7); } if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp23 * (P(0, 1) * _tmp27 + P(1, 1) * _tmp23 + P(2, 1) * _tmp26) + - _tmp26 * (P(0, 2) * _tmp27 + P(1, 2) * _tmp23 + P(2, 2) * _tmp26) + - _tmp27 * (P(0, 0) * _tmp27 + P(1, 0) * _tmp23 + P(2, 0) * _tmp26); + _innov_var = R + _tmp25 * (P(0, 0) * _tmp25 + P(1, 0) * _tmp27 + P(2, 0) * _tmp28) + + _tmp27 * (P(0, 1) * _tmp25 + P(1, 1) * _tmp27 + P(2, 1) * _tmp28) + + _tmp28 * (P(0, 2) * _tmp25 + P(1, 2) * _tmp27 + P(2, 2) * _tmp28); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp27; - _h(1, 0) = _tmp23; - _h(2, 0) = _tmp26; + _h(0, 0) = _tmp25; + _h(1, 0) = _tmp27; + _h(2, 0) = _tmp28; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h index f3f074e82db7..a8fc12fe93bd 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_xyz_innov_var_and_hx.h @@ -16,60 +16,57 @@ namespace sym { * Symbolic function: compute_gravity_xyz_innov_var_and_hx * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * * Outputs: * innov_var: Matrix31 - * Hx: Matrix23_1 + * Hx: Matrix24_1 */ template -void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeGravityXyzInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, matrix::Matrix* const innov_var = nullptr, - matrix::Matrix* const Hx = nullptr) { - // Total ops: 51 + matrix::Matrix* const Hx = nullptr) { + // Total ops: 53 // Input arrays - // Intermediate terms (16) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp3 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp4 = _tmp0 + _tmp1 - _tmp2 - _tmp3; - const Scalar _tmp5 = 2 * state(2, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(0, 0); - const Scalar _tmp9 = -_tmp6 - _tmp8; - const Scalar _tmp10 = -_tmp0 - _tmp1 + _tmp2 + _tmp3; - const Scalar _tmp11 = _tmp5 * state(0, 0); - const Scalar _tmp12 = _tmp7 * state(3, 0); - const Scalar _tmp13 = -_tmp11 + _tmp12; - const Scalar _tmp14 = _tmp6 + _tmp8; - const Scalar _tmp15 = _tmp11 - _tmp12; + // Intermediate terms (13) + const Scalar _tmp0 = 2 * state(0, 0); + const Scalar _tmp1 = -_tmp0 * state(3, 0); + const Scalar _tmp2 = 2 * state(2, 0); + const Scalar _tmp3 = _tmp2 * state(1, 0); + const Scalar _tmp4 = _tmp1 - _tmp3; + const Scalar _tmp5 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp6 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp7 = std::pow(state(1, 0), Scalar(2)) - std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = -_tmp5 + _tmp6 + _tmp7; + const Scalar _tmp9 = _tmp1 + _tmp3; + const Scalar _tmp10 = _tmp5 - _tmp6 + _tmp7; + const Scalar _tmp11 = _tmp0 * state(1, 0) - _tmp2 * state(3, 0); + const Scalar _tmp12 = _tmp2 * state(0, 0) + 2 * state(1, 0) * state(3, 0); // Output terms (2) if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = R + _tmp4 * (P(1, 1) * _tmp4 + P(2, 1) * _tmp9) + - _tmp9 * (P(1, 2) * _tmp4 + P(2, 2) * _tmp9); - _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(2, 0) * _tmp13) + - _tmp13 * (P(0, 2) * _tmp10 + P(2, 2) * _tmp13); - _innov_var(2, 0) = R + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp15) + - _tmp15 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp15); + _innov_var(0, 0) = R + _tmp4 * (P(0, 0) * _tmp4 + P(1, 0) * _tmp8) + + _tmp8 * (P(0, 1) * _tmp4 + P(1, 1) * _tmp8); + _innov_var(1, 0) = R + _tmp10 * (P(0, 0) * _tmp10 + P(1, 0) * _tmp9) + + _tmp9 * (P(0, 1) * _tmp10 + P(1, 1) * _tmp9); + _innov_var(2, 0) = R + _tmp11 * (P(0, 0) * _tmp11 + P(1, 0) * _tmp12) + + _tmp12 * (P(0, 1) * _tmp11 + P(1, 1) * _tmp12); } if (Hx != nullptr) { - matrix::Matrix& _hx = (*Hx); + matrix::Matrix& _hx = (*Hx); _hx.setZero(); - _hx(1, 0) = _tmp4; - _hx(2, 0) = _tmp9; + _hx(0, 0) = _tmp4; + _hx(1, 0) = _tmp8; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h index 2c3a11b682e5..b562373162f2 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_y_innov_var_and_h.h @@ -16,43 +16,43 @@ namespace sym { * Symbolic function: compute_gravity_y_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * * Outputs: * innov_var: Scalar - * Hy: Matrix23_1 + * Hy: Matrix24_1 */ template -void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeGravityYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, Scalar* const innov_var = nullptr, - matrix::Matrix* const Hy = nullptr) { + matrix::Matrix* const Hy = nullptr) { // Total ops: 22 // Input arrays // Intermediate terms (2) - const Scalar _tmp0 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) + - std::pow(state(2, 0), Scalar(2)) - std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = -2 * state(0, 0) * state(2, 0) + 2 * state(1, 0) * state(3, 0); + const Scalar _tmp0 = -2 * state(0, 0) * state(3, 0) + 2 * state(1, 0) * state(2, 0); + const Scalar _tmp1 = -std::pow(state(0, 0), Scalar(2)) + std::pow(state(1, 0), Scalar(2)) - + std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp0 * (P(0, 0) * _tmp0 + P(2, 0) * _tmp1) + - _tmp1 * (P(0, 2) * _tmp0 + P(2, 2) * _tmp1); + _innov_var = R + _tmp0 * (P(0, 1) * _tmp1 + P(1, 1) * _tmp0) + + _tmp1 * (P(0, 0) * _tmp1 + P(1, 0) * _tmp0); } if (Hy != nullptr) { - matrix::Matrix& _hy = (*Hy); + matrix::Matrix& _hy = (*Hy); _hy.setZero(); - _hy(0, 0) = _tmp0; - _hy(2, 0) = _tmp1; + _hy(0, 0) = _tmp1; + _hy(1, 0) = _tmp0; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h index 43d48e128321..e5495af10c77 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_gravity_z_innov_var_and_h.h @@ -16,19 +16,19 @@ namespace sym { * Symbolic function: compute_gravity_z_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * * Outputs: * innov_var: Scalar - * Hz: Matrix23_1 + * Hz: Matrix24_1 */ template -void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, Scalar* const innov_var = nullptr, - matrix::Matrix* const Hz = nullptr) { + matrix::Matrix* const Hz = nullptr) { // Total ops: 18 // Input arrays @@ -36,8 +36,8 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, // Intermediate terms (4) const Scalar _tmp0 = 2 * state(2, 0); const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); - const Scalar _tmp3 = _tmp0 * state(0, 0) - _tmp1 * state(3, 0); + const Scalar _tmp2 = -_tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = _tmp0 * state(0, 0) + _tmp1 * state(3, 0); // Output terms (2) if (innov_var != nullptr) { @@ -48,7 +48,7 @@ void ComputeGravityZInnovVarAndH(const matrix::Matrix& state, } if (Hz != nullptr) { - matrix::Matrix& _hz = (*Hz); + matrix::Matrix& _hz = (*Hz); _hz.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_h.h new file mode 100644 index 000000000000..2f66cebf47b7 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_h.h @@ -0,0 +1,43 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_hagl_h + * + * Args: + * + * Outputs: + * H: Matrix24_1 + */ +template +void ComputeHaglH(matrix::Matrix* const H = nullptr) { + // Total ops: 0 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + if (H != nullptr) { + matrix::Matrix& _h = (*H); + + _h.setZero(); + + _h(8, 0) = -1; + _h(23, 0) = 1; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_innov_var.h new file mode 100644 index 000000000000..fd50c97cf5d4 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_hagl_innov_var.h @@ -0,0 +1,43 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_hagl_innov_var + * + * Args: + * P: Matrix24_24 + * R: Scalar + * + * Outputs: + * innov_var: Scalar + */ +template +void ComputeHaglInnovVar(const matrix::Matrix& P, const Scalar R, + Scalar* const innov_var = nullptr) { + // Total ops: 4 + + // Input arrays + + // Intermediate terms (0) + + // Output terms (1) + if (innov_var != nullptr) { + Scalar& _innov_var = (*innov_var); + + _innov_var = P(23, 23) - P(23, 8) - P(8, 23) + P(8, 8) + R; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h index 009892f6d9d1..803bf45e875e 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_declination_pred_innov_var_and_h.h @@ -16,22 +16,22 @@ namespace sym { * Symbolic function: compute_mag_declination_pred_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * * Outputs: * pred: Scalar * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const pred = nullptr, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { + matrix::Matrix* const H = nullptr) { // Total ops: 22 // Input arrays @@ -59,7 +59,7 @@ void ComputeMagDeclinationPredInnovVarAndH(const matrix::Matrix& } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h index dde9a8cd9ee9..4ff8368c4964 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_mag_innov_innov_var_and_hx * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * meas: Matrix31 * R: Scalar * epsilon: Scalar @@ -25,136 +25,176 @@ namespace sym { * Outputs: * innov: Matrix31 * innov_var: Matrix31 - * Hx: Matrix23_1 + * Hx: Matrix24_1 */ template -void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, - const matrix::Matrix& P, +void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, + const matrix::Matrix& P, const matrix::Matrix& meas, const Scalar R, const Scalar epsilon, matrix::Matrix* const innov = nullptr, matrix::Matrix* const innov_var = nullptr, - matrix::Matrix* const Hx = nullptr) { - // Total ops: 314 + matrix::Matrix* const Hx = nullptr) { + // Total ops: 461 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (47) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = -2 * _tmp0; - const Scalar _tmp2 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp3 = -2 * _tmp2; - const Scalar _tmp4 = _tmp1 + _tmp3 + 1; - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = _tmp5 * state(2, 0); - const Scalar _tmp11 = -_tmp10; - const Scalar _tmp12 = _tmp7 * state(3, 0); - const Scalar _tmp13 = _tmp11 + _tmp12; - const Scalar _tmp14 = _tmp13 * state(18, 0) + _tmp9 * state(17, 0); - const Scalar _tmp15 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp16 = 1 - 2 * _tmp15; - const Scalar _tmp17 = _tmp1 + _tmp16; - const Scalar _tmp18 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp19 = _tmp7 * state(0, 0); - const Scalar _tmp20 = _tmp18 + _tmp19; - const Scalar _tmp21 = -_tmp6; - const Scalar _tmp22 = _tmp21 + _tmp8; - const Scalar _tmp23 = _tmp20 * state(18, 0) + _tmp22 * state(16, 0); - const Scalar _tmp24 = _tmp16 + _tmp3; - const Scalar _tmp25 = -_tmp19; - const Scalar _tmp26 = _tmp18 + _tmp25; - const Scalar _tmp27 = _tmp10 + _tmp12; - const Scalar _tmp28 = _tmp26 * state(17, 0) + _tmp27 * state(16, 0); - const Scalar _tmp29 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp30 = -_tmp29; - const Scalar _tmp31 = _tmp2 + _tmp30; - const Scalar _tmp32 = -_tmp0; - const Scalar _tmp33 = _tmp15 + _tmp32; - const Scalar _tmp34 = -_tmp18; - const Scalar _tmp35 = -_tmp12; - const Scalar _tmp36 = state(16, 0) * (_tmp11 + _tmp35) + state(17, 0) * (_tmp19 + _tmp34) + - state(18, 0) * (_tmp31 + _tmp33); - const Scalar _tmp37 = -_tmp15; - const Scalar _tmp38 = _tmp23 + state(17, 0) * (_tmp2 + _tmp29 + _tmp32 + _tmp37); - const Scalar _tmp39 = _tmp0 + _tmp37; - const Scalar _tmp40 = -_tmp8; - const Scalar _tmp41 = state(16, 0) * (_tmp31 + _tmp39) + state(17, 0) * (_tmp21 + _tmp40) + - state(18, 0) * (_tmp10 + _tmp35); - const Scalar _tmp42 = -_tmp2; - const Scalar _tmp43 = _tmp29 + _tmp42; - const Scalar _tmp44 = _tmp28 + state(18, 0) * (_tmp39 + _tmp43); - const Scalar _tmp45 = _tmp14 + state(16, 0) * (_tmp33 + _tmp43); - const Scalar _tmp46 = state(16, 0) * (_tmp40 + _tmp6) + - state(17, 0) * (_tmp0 + _tmp15 + _tmp30 + _tmp42) + - state(18, 0) * (_tmp25 + _tmp34); + // Intermediate terms (68) + const Scalar _tmp0 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp2 = _tmp0 + _tmp1; + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = _tmp3 * state(0, 0); + const Scalar _tmp5 = 2 * state(2, 0); + const Scalar _tmp6 = _tmp5 * state(1, 0); + const Scalar _tmp7 = _tmp4 + _tmp6; + const Scalar _tmp8 = _tmp5 * state(0, 0); + const Scalar _tmp9 = 2 * state(1, 0); + const Scalar _tmp10 = _tmp9 * state(3, 0); + const Scalar _tmp11 = _tmp10 - _tmp8; + const Scalar _tmp12 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp13 = _tmp0 + _tmp12 + 1; + const Scalar _tmp14 = _tmp5 * state(3, 0); + const Scalar _tmp15 = _tmp9 * state(0, 0); + const Scalar _tmp16 = _tmp14 + _tmp15; + const Scalar _tmp17 = -_tmp4 + _tmp6; + const Scalar _tmp18 = _tmp1 + _tmp12; + const Scalar _tmp19 = _tmp14 - _tmp15; + const Scalar _tmp20 = _tmp10 + _tmp8; + const Scalar _tmp21 = _tmp3 * state(18, 0); + const Scalar _tmp22 = _tmp5 * state(17, 0); + const Scalar _tmp23 = _tmp21 + _tmp22; + const Scalar _tmp24 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp25 = 2 * state(17, 0); + const Scalar _tmp26 = _tmp25 * state(3, 0); + const Scalar _tmp27 = _tmp5 * state(18, 0); + const Scalar _tmp28 = _tmp26 - _tmp27; + const Scalar _tmp29 = (Scalar(1) / Scalar(2)) * _tmp28; + const Scalar _tmp30 = 4 * state(2, 0); + const Scalar _tmp31 = _tmp9 * state(17, 0); + const Scalar _tmp32 = 2 * state(0, 0); + const Scalar _tmp33 = _tmp32 * state(18, 0); + const Scalar _tmp34 = -_tmp30 * state(16, 0) + _tmp31 - _tmp33; + const Scalar _tmp35 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp36 = _tmp9 * state(18, 0); + const Scalar _tmp37 = _tmp25 * state(0, 0); + const Scalar _tmp38 = (Scalar(1) / Scalar(2)) * _tmp36 + (Scalar(1) / Scalar(2)) * _tmp37 - + 2 * state(16, 0) * state(3, 0); + const Scalar _tmp39 = + -_tmp23 * _tmp24 - _tmp29 * state(3, 0) + _tmp34 * _tmp35 + _tmp38 * state(0, 0); + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * _tmp23; + const Scalar _tmp41 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp42 = + -_tmp24 * _tmp28 + _tmp34 * _tmp41 - _tmp38 * state(1, 0) + _tmp40 * state(3, 0); + const Scalar _tmp43 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp44 = + -_tmp29 * state(1, 0) - _tmp34 * _tmp43 + _tmp38 * state(2, 0) + _tmp40 * state(0, 0); + const Scalar _tmp45 = _tmp3 * state(16, 0); + const Scalar _tmp46 = _tmp36 - _tmp45; + const Scalar _tmp47 = (Scalar(1) / Scalar(2)) * _tmp46; + const Scalar _tmp48 = _tmp9 * state(16, 0); + const Scalar _tmp49 = (Scalar(1) / Scalar(2)) * _tmp21 + (Scalar(1) / Scalar(2)) * _tmp48; + const Scalar _tmp50 = _tmp5 * state(16, 0); + const Scalar _tmp51 = 4 * state(17, 0); + const Scalar _tmp52 = _tmp33 + _tmp50 - _tmp51 * state(1, 0); + const Scalar _tmp53 = _tmp32 * state(16, 0); + const Scalar _tmp54 = _tmp27 - _tmp51 * state(3, 0) - _tmp53; + const Scalar _tmp55 = + _tmp24 * _tmp54 + _tmp41 * _tmp52 - _tmp47 * state(1, 0) - _tmp49 * state(3, 0); + const Scalar _tmp56 = + -_tmp24 * _tmp52 + _tmp41 * _tmp54 - _tmp47 * state(3, 0) + _tmp49 * state(1, 0); + const Scalar _tmp57 = -_tmp24 * _tmp46 - _tmp35 * _tmp54 + _tmp43 * _tmp52 + _tmp49 * state(0, 0); + const Scalar _tmp58 = -_tmp31 + _tmp50; + const Scalar _tmp59 = (Scalar(1) / Scalar(2)) * _tmp58; + const Scalar _tmp60 = _tmp22 + _tmp48; + const Scalar _tmp61 = (Scalar(1) / Scalar(2)) * _tmp60; + const Scalar _tmp62 = _tmp26 - _tmp30 * state(18, 0) + _tmp53; + const Scalar _tmp63 = -Scalar(1) / Scalar(2) * _tmp37 + (Scalar(1) / Scalar(2)) * _tmp45 - + 2 * state(1, 0) * state(18, 0); + const Scalar _tmp64 = + _tmp35 * _tmp62 - _tmp59 * state(3, 0) + _tmp61 * state(0, 0) - _tmp63 * state(2, 0); + const Scalar _tmp65 = (Scalar(1) / Scalar(2)) * _tmp62; + const Scalar _tmp66 = + _tmp24 * _tmp60 - _tmp59 * state(1, 0) + _tmp63 * state(0, 0) - _tmp65 * state(3, 0); + const Scalar _tmp67 = + -_tmp24 * _tmp58 - _tmp61 * state(1, 0) + _tmp63 * state(3, 0) + _tmp65 * state(0, 0); // Output terms (3) if (innov != nullptr) { matrix::Matrix& _innov = (*innov); - _innov(0, 0) = _tmp14 + _tmp4 * state(16, 0) - meas(0, 0) + state(19, 0); - _innov(1, 0) = _tmp17 * state(17, 0) + _tmp23 - meas(1, 0) + state(20, 0); - _innov(2, 0) = _tmp24 * state(18, 0) + _tmp28 - meas(2, 0) + state(21, 0); + _innov(0, 0) = _tmp11 * state(18, 0) + _tmp2 * state(16, 0) + _tmp7 * state(17, 0) - + meas(0, 0) + state(19, 0); + _innov(1, 0) = _tmp13 * state(17, 0) + _tmp16 * state(18, 0) + _tmp17 * state(16, 0) - + meas(1, 0) + state(20, 0); + _innov(2, 0) = _tmp18 * state(18, 0) + _tmp19 * state(17, 0) + _tmp20 * state(16, 0) - + meas(2, 0) + state(21, 0); } if (innov_var != nullptr) { matrix::Matrix& _innov_var = (*innov_var); - _innov_var(0, 0) = P(1, 18) * _tmp36 + P(15, 18) * _tmp4 + P(16, 18) * _tmp9 + - P(17, 18) * _tmp13 + P(18, 18) + P(2, 18) * _tmp38 + R + - _tmp13 * (P(1, 17) * _tmp36 + P(15, 17) * _tmp4 + P(16, 17) * _tmp9 + - P(17, 17) * _tmp13 + P(18, 17) + P(2, 17) * _tmp38) + - _tmp36 * (P(1, 1) * _tmp36 + P(15, 1) * _tmp4 + P(16, 1) * _tmp9 + - P(17, 1) * _tmp13 + P(18, 1) + P(2, 1) * _tmp38) + - _tmp38 * (P(1, 2) * _tmp36 + P(15, 2) * _tmp4 + P(16, 2) * _tmp9 + - P(17, 2) * _tmp13 + P(18, 2) + P(2, 2) * _tmp38) + - _tmp4 * (P(1, 15) * _tmp36 + P(15, 15) * _tmp4 + P(16, 15) * _tmp9 + - P(17, 15) * _tmp13 + P(18, 15) + P(2, 15) * _tmp38) + - _tmp9 * (P(1, 16) * _tmp36 + P(15, 16) * _tmp4 + P(16, 16) * _tmp9 + - P(17, 16) * _tmp13 + P(18, 16) + P(2, 16) * _tmp38); - _innov_var(1, 0) = P(0, 19) * _tmp44 + P(15, 19) * _tmp22 + P(16, 19) * _tmp17 + - P(17, 19) * _tmp20 + P(19, 19) + P(2, 19) * _tmp41 + R + - _tmp17 * (P(0, 16) * _tmp44 + P(15, 16) * _tmp22 + P(16, 16) * _tmp17 + - P(17, 16) * _tmp20 + P(19, 16) + P(2, 16) * _tmp41) + - _tmp20 * (P(0, 17) * _tmp44 + P(15, 17) * _tmp22 + P(16, 17) * _tmp17 + - P(17, 17) * _tmp20 + P(19, 17) + P(2, 17) * _tmp41) + - _tmp22 * (P(0, 15) * _tmp44 + P(15, 15) * _tmp22 + P(16, 15) * _tmp17 + - P(17, 15) * _tmp20 + P(19, 15) + P(2, 15) * _tmp41) + - _tmp41 * (P(0, 2) * _tmp44 + P(15, 2) * _tmp22 + P(16, 2) * _tmp17 + - P(17, 2) * _tmp20 + P(19, 2) + P(2, 2) * _tmp41) + - _tmp44 * (P(0, 0) * _tmp44 + P(15, 0) * _tmp22 + P(16, 0) * _tmp17 + - P(17, 0) * _tmp20 + P(19, 0) + P(2, 0) * _tmp41); - _innov_var(2, 0) = P(0, 20) * _tmp46 + P(1, 20) * _tmp45 + P(15, 20) * _tmp27 + - P(16, 20) * _tmp26 + P(17, 20) * _tmp24 + P(20, 20) + R + - _tmp24 * (P(0, 17) * _tmp46 + P(1, 17) * _tmp45 + P(15, 17) * _tmp27 + - P(16, 17) * _tmp26 + P(17, 17) * _tmp24 + P(20, 17)) + - _tmp26 * (P(0, 16) * _tmp46 + P(1, 16) * _tmp45 + P(15, 16) * _tmp27 + - P(16, 16) * _tmp26 + P(17, 16) * _tmp24 + P(20, 16)) + - _tmp27 * (P(0, 15) * _tmp46 + P(1, 15) * _tmp45 + P(15, 15) * _tmp27 + - P(16, 15) * _tmp26 + P(17, 15) * _tmp24 + P(20, 15)) + - _tmp45 * (P(0, 1) * _tmp46 + P(1, 1) * _tmp45 + P(15, 1) * _tmp27 + - P(16, 1) * _tmp26 + P(17, 1) * _tmp24 + P(20, 1)) + - _tmp46 * (P(0, 0) * _tmp46 + P(1, 0) * _tmp45 + P(15, 0) * _tmp27 + - P(16, 0) * _tmp26 + P(17, 0) * _tmp24 + P(20, 0)); + _innov_var(0, 0) = + P(0, 18) * _tmp44 + P(1, 18) * _tmp42 + P(15, 18) * _tmp2 + P(16, 18) * _tmp7 + + P(17, 18) * _tmp11 + P(18, 18) + P(2, 18) * _tmp39 + R + + _tmp11 * (P(0, 17) * _tmp44 + P(1, 17) * _tmp42 + P(15, 17) * _tmp2 + P(16, 17) * _tmp7 + + P(17, 17) * _tmp11 + P(18, 17) + P(2, 17) * _tmp39) + + _tmp2 * (P(0, 15) * _tmp44 + P(1, 15) * _tmp42 + P(15, 15) * _tmp2 + P(16, 15) * _tmp7 + + P(17, 15) * _tmp11 + P(18, 15) + P(2, 15) * _tmp39) + + _tmp39 * (P(0, 2) * _tmp44 + P(1, 2) * _tmp42 + P(15, 2) * _tmp2 + P(16, 2) * _tmp7 + + P(17, 2) * _tmp11 + P(18, 2) + P(2, 2) * _tmp39) + + _tmp42 * (P(0, 1) * _tmp44 + P(1, 1) * _tmp42 + P(15, 1) * _tmp2 + P(16, 1) * _tmp7 + + P(17, 1) * _tmp11 + P(18, 1) + P(2, 1) * _tmp39) + + _tmp44 * (P(0, 0) * _tmp44 + P(1, 0) * _tmp42 + P(15, 0) * _tmp2 + P(16, 0) * _tmp7 + + P(17, 0) * _tmp11 + P(18, 0) + P(2, 0) * _tmp39) + + _tmp7 * (P(0, 16) * _tmp44 + P(1, 16) * _tmp42 + P(15, 16) * _tmp2 + P(16, 16) * _tmp7 + + P(17, 16) * _tmp11 + P(18, 16) + P(2, 16) * _tmp39); + _innov_var(1, 0) = + P(0, 19) * _tmp55 + P(1, 19) * _tmp57 + P(15, 19) * _tmp17 + P(16, 19) * _tmp13 + + P(17, 19) * _tmp16 + P(19, 19) + P(2, 19) * _tmp56 + R + + _tmp13 * (P(0, 16) * _tmp55 + P(1, 16) * _tmp57 + P(15, 16) * _tmp17 + P(16, 16) * _tmp13 + + P(17, 16) * _tmp16 + P(19, 16) + P(2, 16) * _tmp56) + + _tmp16 * (P(0, 17) * _tmp55 + P(1, 17) * _tmp57 + P(15, 17) * _tmp17 + P(16, 17) * _tmp13 + + P(17, 17) * _tmp16 + P(19, 17) + P(2, 17) * _tmp56) + + _tmp17 * (P(0, 15) * _tmp55 + P(1, 15) * _tmp57 + P(15, 15) * _tmp17 + P(16, 15) * _tmp13 + + P(17, 15) * _tmp16 + P(19, 15) + P(2, 15) * _tmp56) + + _tmp55 * (P(0, 0) * _tmp55 + P(1, 0) * _tmp57 + P(15, 0) * _tmp17 + P(16, 0) * _tmp13 + + P(17, 0) * _tmp16 + P(19, 0) + P(2, 0) * _tmp56) + + _tmp56 * (P(0, 2) * _tmp55 + P(1, 2) * _tmp57 + P(15, 2) * _tmp17 + P(16, 2) * _tmp13 + + P(17, 2) * _tmp16 + P(19, 2) + P(2, 2) * _tmp56) + + _tmp57 * (P(0, 1) * _tmp55 + P(1, 1) * _tmp57 + P(15, 1) * _tmp17 + P(16, 1) * _tmp13 + + P(17, 1) * _tmp16 + P(19, 1) + P(2, 1) * _tmp56); + _innov_var(2, 0) = + P(0, 20) * _tmp66 + P(1, 20) * _tmp67 + P(15, 20) * _tmp20 + P(16, 20) * _tmp19 + + P(17, 20) * _tmp18 + P(2, 20) * _tmp64 + P(20, 20) + R + + _tmp18 * (P(0, 17) * _tmp66 + P(1, 17) * _tmp67 + P(15, 17) * _tmp20 + P(16, 17) * _tmp19 + + P(17, 17) * _tmp18 + P(2, 17) * _tmp64 + P(20, 17)) + + _tmp19 * (P(0, 16) * _tmp66 + P(1, 16) * _tmp67 + P(15, 16) * _tmp20 + P(16, 16) * _tmp19 + + P(17, 16) * _tmp18 + P(2, 16) * _tmp64 + P(20, 16)) + + _tmp20 * (P(0, 15) * _tmp66 + P(1, 15) * _tmp67 + P(15, 15) * _tmp20 + P(16, 15) * _tmp19 + + P(17, 15) * _tmp18 + P(2, 15) * _tmp64 + P(20, 15)) + + _tmp64 * (P(0, 2) * _tmp66 + P(1, 2) * _tmp67 + P(15, 2) * _tmp20 + P(16, 2) * _tmp19 + + P(17, 2) * _tmp18 + P(2, 2) * _tmp64 + P(20, 2)) + + _tmp66 * (P(0, 0) * _tmp66 + P(1, 0) * _tmp67 + P(15, 0) * _tmp20 + P(16, 0) * _tmp19 + + P(17, 0) * _tmp18 + P(2, 0) * _tmp64 + P(20, 0)) + + _tmp67 * (P(0, 1) * _tmp66 + P(1, 1) * _tmp67 + P(15, 1) * _tmp20 + P(16, 1) * _tmp19 + + P(17, 1) * _tmp18 + P(2, 1) * _tmp64 + P(20, 1)); } if (Hx != nullptr) { - matrix::Matrix& _hx = (*Hx); + matrix::Matrix& _hx = (*Hx); _hx.setZero(); - _hx(1, 0) = _tmp36; - _hx(2, 0) = _tmp38; - _hx(15, 0) = _tmp4; - _hx(16, 0) = _tmp9; - _hx(17, 0) = _tmp13; + _hx(0, 0) = _tmp44; + _hx(1, 0) = _tmp42; + _hx(2, 0) = _tmp39; + _hx(15, 0) = _tmp2; + _hx(16, 0) = _tmp7; + _hx(17, 0) = _tmp11; _hx(18, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h index 9f22c4f1fac7..697cd92a8243 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_y_innov_var_and_h.h @@ -16,21 +16,21 @@ namespace sym { * Symbolic function: compute_mag_y_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeMagYInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeMagYInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 110 + matrix::Matrix* const H = nullptr) { + // Total ops: 159 // Unused inputs (void)epsilon; @@ -38,55 +38,63 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, // Input arrays // Intermediate terms (18) - const Scalar _tmp0 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; - const Scalar _tmp3 = 2 * state(2, 0); - const Scalar _tmp4 = _tmp3 * state(3, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp4 + _tmp6; - const Scalar _tmp8 = -_tmp5 * state(3, 0); - const Scalar _tmp9 = _tmp3 * state(1, 0); - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp13 = _tmp0 - _tmp1; - const Scalar _tmp14 = _tmp3 * state(0, 0); - const Scalar _tmp15 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) + - state(17, 0) * (_tmp8 - _tmp9) + state(18, 0) * (_tmp14 - _tmp15); - const Scalar _tmp17 = state(16, 0) * (_tmp14 + _tmp15) + state(17, 0) * (_tmp4 - _tmp6) + - state(18, 0) * (_tmp11 - _tmp12 + _tmp13); + const Scalar _tmp0 = 2 * state(2, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); + const Scalar _tmp3 = 2 * state(3, 0); + const Scalar _tmp4 = _tmp0 * state(1, 0) - _tmp3 * state(0, 0); + const Scalar _tmp5 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(3, 0), Scalar(2)) + 1; + const Scalar _tmp6 = _tmp1 * state(18, 0) - _tmp3 * state(16, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp6; + const Scalar _tmp8 = (Scalar(1) / Scalar(2)) * _tmp1 * state(16, 0) + + (Scalar(1) / Scalar(2)) * _tmp3 * state(18, 0); + const Scalar _tmp9 = 4 * state(17, 0); + const Scalar _tmp10 = 2 * state(0, 0); + const Scalar _tmp11 = _tmp0 * state(16, 0) + _tmp10 * state(18, 0) - _tmp9 * state(1, 0); + const Scalar _tmp12 = (Scalar(1) / Scalar(2)) * _tmp11; + const Scalar _tmp13 = (Scalar(1) / Scalar(2)) * _tmp0 * state(18, 0) - + Scalar(1) / Scalar(2) * _tmp10 * state(16, 0) - + Scalar(1) / Scalar(2) * _tmp9 * state(3, 0); + const Scalar _tmp14 = + _tmp12 * state(0, 0) + _tmp13 * state(2, 0) - _tmp7 * state(1, 0) - _tmp8 * state(3, 0); + const Scalar _tmp15 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp16 = + -_tmp11 * _tmp15 + _tmp13 * state(0, 0) - _tmp7 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp17 = + _tmp12 * state(3, 0) - _tmp13 * state(1, 0) - _tmp15 * _tmp6 + _tmp8 * state(0, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = P(0, 19) * _tmp17 + P(15, 19) * _tmp10 + P(16, 19) * _tmp2 + P(17, 19) * _tmp7 + - P(19, 19) + P(2, 19) * _tmp16 + R + - _tmp10 * (P(0, 15) * _tmp17 + P(15, 15) * _tmp10 + P(16, 15) * _tmp2 + - P(17, 15) * _tmp7 + P(19, 15) + P(2, 15) * _tmp16) + - _tmp16 * (P(0, 2) * _tmp17 + P(15, 2) * _tmp10 + P(16, 2) * _tmp2 + - P(17, 2) * _tmp7 + P(19, 2) + P(2, 2) * _tmp16) + - _tmp17 * (P(0, 0) * _tmp17 + P(15, 0) * _tmp10 + P(16, 0) * _tmp2 + - P(17, 0) * _tmp7 + P(19, 0) + P(2, 0) * _tmp16) + - _tmp2 * (P(0, 16) * _tmp17 + P(15, 16) * _tmp10 + P(16, 16) * _tmp2 + - P(17, 16) * _tmp7 + P(19, 16) + P(2, 16) * _tmp16) + - _tmp7 * (P(0, 17) * _tmp17 + P(15, 17) * _tmp10 + P(16, 17) * _tmp2 + - P(17, 17) * _tmp7 + P(19, 17) + P(2, 17) * _tmp16); + _innov_var = P(0, 19) * _tmp14 + P(1, 19) * _tmp17 + P(15, 19) * _tmp4 + P(16, 19) * _tmp5 + + P(17, 19) * _tmp2 + P(19, 19) + P(2, 19) * _tmp16 + R + + _tmp14 * (P(0, 0) * _tmp14 + P(1, 0) * _tmp17 + P(15, 0) * _tmp4 + + P(16, 0) * _tmp5 + P(17, 0) * _tmp2 + P(19, 0) + P(2, 0) * _tmp16) + + _tmp16 * (P(0, 2) * _tmp14 + P(1, 2) * _tmp17 + P(15, 2) * _tmp4 + + P(16, 2) * _tmp5 + P(17, 2) * _tmp2 + P(19, 2) + P(2, 2) * _tmp16) + + _tmp17 * (P(0, 1) * _tmp14 + P(1, 1) * _tmp17 + P(15, 1) * _tmp4 + + P(16, 1) * _tmp5 + P(17, 1) * _tmp2 + P(19, 1) + P(2, 1) * _tmp16) + + _tmp2 * (P(0, 17) * _tmp14 + P(1, 17) * _tmp17 + P(15, 17) * _tmp4 + + P(16, 17) * _tmp5 + P(17, 17) * _tmp2 + P(19, 17) + P(2, 17) * _tmp16) + + _tmp4 * (P(0, 15) * _tmp14 + P(1, 15) * _tmp17 + P(15, 15) * _tmp4 + + P(16, 15) * _tmp5 + P(17, 15) * _tmp2 + P(19, 15) + P(2, 15) * _tmp16) + + _tmp5 * (P(0, 16) * _tmp14 + P(1, 16) * _tmp17 + P(15, 16) * _tmp4 + + P(16, 16) * _tmp5 + P(17, 16) * _tmp2 + P(19, 16) + P(2, 16) * _tmp16); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp17; + _h(0, 0) = _tmp14; + _h(1, 0) = _tmp17; _h(2, 0) = _tmp16; - _h(15, 0) = _tmp10; - _h(16, 0) = _tmp2; - _h(17, 0) = _tmp7; + _h(15, 0) = _tmp4; + _h(16, 0) = _tmp5; + _h(17, 0) = _tmp2; _h(19, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h index 0563f1df9e22..1432222d4756 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_mag_z_innov_var_and_h.h @@ -16,77 +16,85 @@ namespace sym { * Symbolic function: compute_mag_z_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeMagZInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeMagZInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 110 + matrix::Matrix* const H = nullptr) { + // Total ops: 161 // Unused inputs (void)epsilon; // Input arrays - // Intermediate terms (18) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp2 = -2 * _tmp0 - 2 * _tmp1 + 1; - const Scalar _tmp3 = 2 * state(2, 0); - const Scalar _tmp4 = _tmp3 * state(3, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = -_tmp5 * state(1, 0); - const Scalar _tmp7 = _tmp4 + _tmp6; - const Scalar _tmp8 = _tmp3 * state(0, 0); - const Scalar _tmp9 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp10 = _tmp8 + _tmp9; - const Scalar _tmp11 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp12 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp13 = -_tmp0 + _tmp1; - const Scalar _tmp14 = _tmp5 * state(3, 0); - const Scalar _tmp15 = _tmp3 * state(1, 0); - const Scalar _tmp16 = state(16, 0) * (-_tmp11 + _tmp12 + _tmp13) + - state(17, 0) * (_tmp14 + _tmp15) + state(18, 0) * (-_tmp8 + _tmp9); - const Scalar _tmp17 = state(16, 0) * (_tmp14 - _tmp15) + - state(17, 0) * (_tmp11 - _tmp12 + _tmp13) + state(18, 0) * (-_tmp4 + _tmp6); + // Intermediate terms (15) + const Scalar _tmp0 = 2 * state(16, 0); + const Scalar _tmp1 = 2 * state(17, 0); + const Scalar _tmp2 = + (Scalar(1) / Scalar(2)) * _tmp0 * state(2, 0) - Scalar(1) / Scalar(2) * _tmp1 * state(1, 0); + const Scalar _tmp3 = 2 * state(1, 0); + const Scalar _tmp4 = (Scalar(1) / Scalar(2)) * _tmp1 * state(2, 0) + + (Scalar(1) / Scalar(2)) * _tmp3 * state(16, 0); + const Scalar _tmp5 = 4 * state(18, 0); + const Scalar _tmp6 = (Scalar(1) / Scalar(2)) * _tmp0 * state(0, 0) + + (Scalar(1) / Scalar(2)) * _tmp1 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(2, 0); + const Scalar _tmp7 = (Scalar(1) / Scalar(2)) * _tmp0 * state(3, 0) - + Scalar(1) / Scalar(2) * _tmp1 * state(0, 0) - + Scalar(1) / Scalar(2) * _tmp5 * state(1, 0); + const Scalar _tmp8 = + -_tmp2 * state(3, 0) + _tmp4 * state(0, 0) + _tmp6 * state(1, 0) - _tmp7 * state(2, 0); + const Scalar _tmp9 = + -_tmp2 * state(1, 0) + _tmp4 * state(2, 0) - _tmp6 * state(3, 0) + _tmp7 * state(0, 0); + const Scalar _tmp10 = + -_tmp2 * state(2, 0) - _tmp4 * state(1, 0) + _tmp6 * state(0, 0) + _tmp7 * state(3, 0); + const Scalar _tmp11 = + -2 * std::pow(state(1, 0), Scalar(2)) - 2 * std::pow(state(2, 0), Scalar(2)) + 1; + const Scalar _tmp12 = 2 * state(2, 0); + const Scalar _tmp13 = _tmp12 * state(3, 0) - _tmp3 * state(0, 0); + const Scalar _tmp14 = _tmp12 * state(0, 0) + _tmp3 * state(3, 0); // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = P(0, 20) * _tmp17 + P(1, 20) * _tmp16 + P(15, 20) * _tmp10 + P(16, 20) * _tmp7 + - P(17, 20) * _tmp2 + P(20, 20) + R + - _tmp10 * (P(0, 15) * _tmp17 + P(1, 15) * _tmp16 + P(15, 15) * _tmp10 + - P(16, 15) * _tmp7 + P(17, 15) * _tmp2 + P(20, 15)) + - _tmp16 * (P(0, 1) * _tmp17 + P(1, 1) * _tmp16 + P(15, 1) * _tmp10 + - P(16, 1) * _tmp7 + P(17, 1) * _tmp2 + P(20, 1)) + - _tmp17 * (P(0, 0) * _tmp17 + P(1, 0) * _tmp16 + P(15, 0) * _tmp10 + - P(16, 0) * _tmp7 + P(17, 0) * _tmp2 + P(20, 0)) + - _tmp2 * (P(0, 17) * _tmp17 + P(1, 17) * _tmp16 + P(15, 17) * _tmp10 + - P(16, 17) * _tmp7 + P(17, 17) * _tmp2 + P(20, 17)) + - _tmp7 * (P(0, 16) * _tmp17 + P(1, 16) * _tmp16 + P(15, 16) * _tmp10 + - P(16, 16) * _tmp7 + P(17, 16) * _tmp2 + P(20, 16)); + _innov_var = P(0, 20) * _tmp9 + P(1, 20) * _tmp10 + P(15, 20) * _tmp14 + P(16, 20) * _tmp13 + + P(17, 20) * _tmp11 + P(2, 20) * _tmp8 + P(20, 20) + R + + _tmp10 * (P(0, 1) * _tmp9 + P(1, 1) * _tmp10 + P(15, 1) * _tmp14 + + P(16, 1) * _tmp13 + P(17, 1) * _tmp11 + P(2, 1) * _tmp8 + P(20, 1)) + + _tmp11 * (P(0, 17) * _tmp9 + P(1, 17) * _tmp10 + P(15, 17) * _tmp14 + + P(16, 17) * _tmp13 + P(17, 17) * _tmp11 + P(2, 17) * _tmp8 + P(20, 17)) + + _tmp13 * (P(0, 16) * _tmp9 + P(1, 16) * _tmp10 + P(15, 16) * _tmp14 + + P(16, 16) * _tmp13 + P(17, 16) * _tmp11 + P(2, 16) * _tmp8 + P(20, 16)) + + _tmp14 * (P(0, 15) * _tmp9 + P(1, 15) * _tmp10 + P(15, 15) * _tmp14 + + P(16, 15) * _tmp13 + P(17, 15) * _tmp11 + P(2, 15) * _tmp8 + P(20, 15)) + + _tmp8 * (P(0, 2) * _tmp9 + P(1, 2) * _tmp10 + P(15, 2) * _tmp14 + + P(16, 2) * _tmp13 + P(17, 2) * _tmp11 + P(2, 2) * _tmp8 + P(20, 2)) + + _tmp9 * (P(0, 0) * _tmp9 + P(1, 0) * _tmp10 + P(15, 0) * _tmp14 + + P(16, 0) * _tmp13 + P(17, 0) * _tmp11 + P(2, 0) * _tmp8 + P(20, 0)); } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp17; - _h(1, 0) = _tmp16; - _h(15, 0) = _tmp10; - _h(16, 0) = _tmp7; - _h(17, 0) = _tmp2; + _h(0, 0) = _tmp9; + _h(1, 0) = _tmp10; + _h(2, 0) = _tmp8; + _h(15, 0) = _tmp14; + _h(16, 0) = _tmp13; + _h(17, 0) = _tmp11; _h(20, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h index 30acca8d22cc..002be9bbe8e9 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_h_and_k.h @@ -16,166 +16,172 @@ namespace sym { * Symbolic function: compute_sideslip_h_and_k * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * innov_var: Scalar * epsilon: Scalar * * Outputs: - * H: Matrix23_1 - * K: Matrix23_1 + * H: Matrix24_1 + * K: Matrix24_1 */ template -void ComputeSideslipHAndK(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar innov_var, - const Scalar epsilon, matrix::Matrix* const H = nullptr, - matrix::Matrix* const K = nullptr) { - // Total ops: 469 +void ComputeSideslipHAndK(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar innov_var, + const Scalar epsilon, matrix::Matrix* const H = nullptr, + matrix::Matrix* const K = nullptr) { + // Total ops: 513 // Input arrays - // Intermediate terms (46) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = -2 * _tmp0 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(2, 0); - const Scalar _tmp8 = _tmp7 * state(1, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = _tmp7 * state(0, 0); - const Scalar _tmp12 = -_tmp11; - const Scalar _tmp13 = 2 * state(1, 0) * state(3, 0); - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4; - const Scalar _tmp16 = - _tmp15 + epsilon * (2 * math::min(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1); - const Scalar _tmp17 = Scalar(1.0) / (_tmp16); - const Scalar _tmp18 = _tmp7 * state(3, 0); - const Scalar _tmp19 = _tmp5 * state(1, 0); - const Scalar _tmp20 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp21 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp22 = -_tmp21; - const Scalar _tmp23 = _tmp20 + _tmp22; - const Scalar _tmp24 = _tmp17 * (_tmp10 * (_tmp18 - _tmp19) + _tmp4 * (_tmp11 + _tmp13) + - state(6, 0) * (-_tmp0 + _tmp1 + _tmp23)); - const Scalar _tmp25 = -_tmp13; - const Scalar _tmp26 = -_tmp20; - const Scalar _tmp27 = _tmp0 - _tmp1; - const Scalar _tmp28 = _tmp2 - 2 * _tmp21; - const Scalar _tmp29 = -_tmp6; - const Scalar _tmp30 = _tmp29 + _tmp8; - const Scalar _tmp31 = _tmp18 + _tmp19; - const Scalar _tmp32 = _tmp30 * _tmp4 + _tmp31 * state(6, 0); - const Scalar _tmp33 = (_tmp10 * _tmp28 + _tmp32) / std::pow(_tmp16, Scalar(2)); - const Scalar _tmp34 = _tmp33 * (_tmp10 * (-_tmp18 + _tmp19) + _tmp4 * (_tmp12 + _tmp25) + - state(6, 0) * (_tmp21 + _tmp26 + _tmp27)); - const Scalar _tmp35 = - _tmp17 * (_tmp10 * (_tmp29 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp22 + _tmp26) + - state(6, 0) * (_tmp11 + _tmp25)) - - _tmp33 * (_tmp10 * (_tmp23 + _tmp27) + _tmp32); - const Scalar _tmp36 = _tmp3 * _tmp33; - const Scalar _tmp37 = _tmp17 * _tmp30; + // Intermediate terms (43) + const Scalar _tmp0 = -state(23, 0) + state(5, 0); + const Scalar _tmp1 = 2 * state(1, 0); + const Scalar _tmp2 = -state(22, 0) + state(4, 0); + const Scalar _tmp3 = 4 * _tmp2; + const Scalar _tmp4 = 2 * state(6, 0); + const Scalar _tmp5 = _tmp4 * state(0, 0); + const Scalar _tmp6 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp7 = _tmp6 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp8 = 2 * state(0, 0); + const Scalar _tmp9 = _tmp8 * state(3, 0); + const Scalar _tmp10 = 2 * state(2, 0); + const Scalar _tmp11 = _tmp10 * state(1, 0); + const Scalar _tmp12 = _tmp11 + _tmp9; + const Scalar _tmp13 = _tmp1 * state(3, 0) - _tmp10 * state(0, 0); + const Scalar _tmp14 = _tmp0 * _tmp12 + _tmp13 * state(6, 0) + _tmp2 * _tmp7; + const Scalar _tmp15 = + _tmp14 + epsilon * (2 * math::min(0, (((_tmp14) > 0) - ((_tmp14) < 0))) + 1); + const Scalar _tmp16 = _tmp6 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp17 = _tmp11 - _tmp9; + const Scalar _tmp18 = _tmp10 * state(3, 0) + _tmp8 * state(1, 0); + const Scalar _tmp19 = + (_tmp0 * _tmp16 + _tmp17 * _tmp2 + _tmp18 * state(6, 0)) / std::pow(_tmp15, Scalar(2)); + const Scalar _tmp20 = _tmp4 * state(3, 0); + const Scalar _tmp21 = Scalar(1.0) / (_tmp15); + const Scalar _tmp22 = + -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp1 - _tmp3 * state(2, 0) - _tmp5) + + (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp1 * _tmp2 + _tmp20); + const Scalar _tmp23 = 4 * _tmp0; + const Scalar _tmp24 = + -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp10 + _tmp20) + + (Scalar(1) / Scalar(2)) * _tmp21 * (_tmp10 * _tmp2 - _tmp23 * state(1, 0) + _tmp5); + const Scalar _tmp25 = 2 * state(3, 0); + const Scalar _tmp26 = _tmp4 * state(2, 0); + const Scalar _tmp27 = _tmp4 * state(1, 0); + const Scalar _tmp28 = -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp25 - _tmp26) + + (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp25 + _tmp27); + const Scalar _tmp29 = + -Scalar(1) / Scalar(2) * _tmp19 * (_tmp0 * _tmp8 + _tmp27 - _tmp3 * state(3, 0)) + + (Scalar(1) / Scalar(2)) * _tmp21 * (-_tmp2 * _tmp8 - _tmp23 * state(3, 0) + _tmp26); + const Scalar _tmp30 = + -_tmp22 * state(3, 0) + _tmp24 * state(0, 0) - _tmp28 * state(1, 0) + _tmp29 * state(2, 0); + const Scalar _tmp31 = + _tmp22 * state(0, 0) + _tmp24 * state(3, 0) - _tmp28 * state(2, 0) - _tmp29 * state(1, 0); + const Scalar _tmp32 = + _tmp22 * state(1, 0) - _tmp24 * state(2, 0) - _tmp28 * state(3, 0) + _tmp29 * state(0, 0); + const Scalar _tmp33 = _tmp19 * _tmp7; + const Scalar _tmp34 = _tmp17 * _tmp21; + const Scalar _tmp35 = -_tmp33 + _tmp34; + const Scalar _tmp36 = _tmp12 * _tmp19; + const Scalar _tmp37 = _tmp16 * _tmp21; const Scalar _tmp38 = -_tmp36 + _tmp37; - const Scalar _tmp39 = _tmp33 * _tmp9; - const Scalar _tmp40 = _tmp17 * _tmp28; - const Scalar _tmp41 = -_tmp39 + _tmp40; - const Scalar _tmp42 = -_tmp14 * _tmp33 + _tmp17 * _tmp31; - const Scalar _tmp43 = _tmp36 - _tmp37; - const Scalar _tmp44 = _tmp39 - _tmp40; - const Scalar _tmp45 = Scalar(1.0) / (math::max(epsilon, innov_var)); + const Scalar _tmp39 = -_tmp13 * _tmp19 + _tmp18 * _tmp21; + const Scalar _tmp40 = _tmp33 - _tmp34; + const Scalar _tmp41 = _tmp36 - _tmp37; + const Scalar _tmp42 = Scalar(1.0) / (math::max(epsilon, innov_var)); // Output terms (2) if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp24; - _h(1, 0) = -_tmp34; - _h(2, 0) = _tmp35; - _h(3, 0) = _tmp38; - _h(4, 0) = _tmp41; - _h(5, 0) = _tmp42; - _h(21, 0) = _tmp43; - _h(22, 0) = _tmp44; + _h(0, 0) = _tmp30; + _h(1, 0) = _tmp31; + _h(2, 0) = _tmp32; + _h(3, 0) = _tmp35; + _h(4, 0) = _tmp38; + _h(5, 0) = _tmp39; + _h(21, 0) = _tmp40; + _h(22, 0) = _tmp41; } if (K != nullptr) { - matrix::Matrix& _k = (*K); + matrix::Matrix& _k = (*K); _k(0, 0) = - _tmp45 * (P(0, 0) * _tmp24 - P(0, 1) * _tmp34 + P(0, 2) * _tmp35 + P(0, 21) * _tmp43 + - P(0, 22) * _tmp44 + P(0, 3) * _tmp38 + P(0, 4) * _tmp41 + P(0, 5) * _tmp42); + _tmp42 * (P(0, 0) * _tmp30 + P(0, 1) * _tmp31 + P(0, 2) * _tmp32 + P(0, 21) * _tmp40 + + P(0, 22) * _tmp41 + P(0, 3) * _tmp35 + P(0, 4) * _tmp38 + P(0, 5) * _tmp39); _k(1, 0) = - _tmp45 * (P(1, 0) * _tmp24 - P(1, 1) * _tmp34 + P(1, 2) * _tmp35 + P(1, 21) * _tmp43 + - P(1, 22) * _tmp44 + P(1, 3) * _tmp38 + P(1, 4) * _tmp41 + P(1, 5) * _tmp42); + _tmp42 * (P(1, 0) * _tmp30 + P(1, 1) * _tmp31 + P(1, 2) * _tmp32 + P(1, 21) * _tmp40 + + P(1, 22) * _tmp41 + P(1, 3) * _tmp35 + P(1, 4) * _tmp38 + P(1, 5) * _tmp39); _k(2, 0) = - _tmp45 * (P(2, 0) * _tmp24 - P(2, 1) * _tmp34 + P(2, 2) * _tmp35 + P(2, 21) * _tmp43 + - P(2, 22) * _tmp44 + P(2, 3) * _tmp38 + P(2, 4) * _tmp41 + P(2, 5) * _tmp42); + _tmp42 * (P(2, 0) * _tmp30 + P(2, 1) * _tmp31 + P(2, 2) * _tmp32 + P(2, 21) * _tmp40 + + P(2, 22) * _tmp41 + P(2, 3) * _tmp35 + P(2, 4) * _tmp38 + P(2, 5) * _tmp39); _k(3, 0) = - _tmp45 * (P(3, 0) * _tmp24 - P(3, 1) * _tmp34 + P(3, 2) * _tmp35 + P(3, 21) * _tmp43 + - P(3, 22) * _tmp44 + P(3, 3) * _tmp38 + P(3, 4) * _tmp41 + P(3, 5) * _tmp42); + _tmp42 * (P(3, 0) * _tmp30 + P(3, 1) * _tmp31 + P(3, 2) * _tmp32 + P(3, 21) * _tmp40 + + P(3, 22) * _tmp41 + P(3, 3) * _tmp35 + P(3, 4) * _tmp38 + P(3, 5) * _tmp39); _k(4, 0) = - _tmp45 * (P(4, 0) * _tmp24 - P(4, 1) * _tmp34 + P(4, 2) * _tmp35 + P(4, 21) * _tmp43 + - P(4, 22) * _tmp44 + P(4, 3) * _tmp38 + P(4, 4) * _tmp41 + P(4, 5) * _tmp42); + _tmp42 * (P(4, 0) * _tmp30 + P(4, 1) * _tmp31 + P(4, 2) * _tmp32 + P(4, 21) * _tmp40 + + P(4, 22) * _tmp41 + P(4, 3) * _tmp35 + P(4, 4) * _tmp38 + P(4, 5) * _tmp39); _k(5, 0) = - _tmp45 * (P(5, 0) * _tmp24 - P(5, 1) * _tmp34 + P(5, 2) * _tmp35 + P(5, 21) * _tmp43 + - P(5, 22) * _tmp44 + P(5, 3) * _tmp38 + P(5, 4) * _tmp41 + P(5, 5) * _tmp42); + _tmp42 * (P(5, 0) * _tmp30 + P(5, 1) * _tmp31 + P(5, 2) * _tmp32 + P(5, 21) * _tmp40 + + P(5, 22) * _tmp41 + P(5, 3) * _tmp35 + P(5, 4) * _tmp38 + P(5, 5) * _tmp39); _k(6, 0) = - _tmp45 * (P(6, 0) * _tmp24 - P(6, 1) * _tmp34 + P(6, 2) * _tmp35 + P(6, 21) * _tmp43 + - P(6, 22) * _tmp44 + P(6, 3) * _tmp38 + P(6, 4) * _tmp41 + P(6, 5) * _tmp42); + _tmp42 * (P(6, 0) * _tmp30 + P(6, 1) * _tmp31 + P(6, 2) * _tmp32 + P(6, 21) * _tmp40 + + P(6, 22) * _tmp41 + P(6, 3) * _tmp35 + P(6, 4) * _tmp38 + P(6, 5) * _tmp39); _k(7, 0) = - _tmp45 * (P(7, 0) * _tmp24 - P(7, 1) * _tmp34 + P(7, 2) * _tmp35 + P(7, 21) * _tmp43 + - P(7, 22) * _tmp44 + P(7, 3) * _tmp38 + P(7, 4) * _tmp41 + P(7, 5) * _tmp42); + _tmp42 * (P(7, 0) * _tmp30 + P(7, 1) * _tmp31 + P(7, 2) * _tmp32 + P(7, 21) * _tmp40 + + P(7, 22) * _tmp41 + P(7, 3) * _tmp35 + P(7, 4) * _tmp38 + P(7, 5) * _tmp39); _k(8, 0) = - _tmp45 * (P(8, 0) * _tmp24 - P(8, 1) * _tmp34 + P(8, 2) * _tmp35 + P(8, 21) * _tmp43 + - P(8, 22) * _tmp44 + P(8, 3) * _tmp38 + P(8, 4) * _tmp41 + P(8, 5) * _tmp42); + _tmp42 * (P(8, 0) * _tmp30 + P(8, 1) * _tmp31 + P(8, 2) * _tmp32 + P(8, 21) * _tmp40 + + P(8, 22) * _tmp41 + P(8, 3) * _tmp35 + P(8, 4) * _tmp38 + P(8, 5) * _tmp39); _k(9, 0) = - _tmp45 * (P(9, 0) * _tmp24 - P(9, 1) * _tmp34 + P(9, 2) * _tmp35 + P(9, 21) * _tmp43 + - P(9, 22) * _tmp44 + P(9, 3) * _tmp38 + P(9, 4) * _tmp41 + P(9, 5) * _tmp42); + _tmp42 * (P(9, 0) * _tmp30 + P(9, 1) * _tmp31 + P(9, 2) * _tmp32 + P(9, 21) * _tmp40 + + P(9, 22) * _tmp41 + P(9, 3) * _tmp35 + P(9, 4) * _tmp38 + P(9, 5) * _tmp39); _k(10, 0) = - _tmp45 * (P(10, 0) * _tmp24 - P(10, 1) * _tmp34 + P(10, 2) * _tmp35 + P(10, 21) * _tmp43 + - P(10, 22) * _tmp44 + P(10, 3) * _tmp38 + P(10, 4) * _tmp41 + P(10, 5) * _tmp42); + _tmp42 * (P(10, 0) * _tmp30 + P(10, 1) * _tmp31 + P(10, 2) * _tmp32 + P(10, 21) * _tmp40 + + P(10, 22) * _tmp41 + P(10, 3) * _tmp35 + P(10, 4) * _tmp38 + P(10, 5) * _tmp39); _k(11, 0) = - _tmp45 * (P(11, 0) * _tmp24 - P(11, 1) * _tmp34 + P(11, 2) * _tmp35 + P(11, 21) * _tmp43 + - P(11, 22) * _tmp44 + P(11, 3) * _tmp38 + P(11, 4) * _tmp41 + P(11, 5) * _tmp42); + _tmp42 * (P(11, 0) * _tmp30 + P(11, 1) * _tmp31 + P(11, 2) * _tmp32 + P(11, 21) * _tmp40 + + P(11, 22) * _tmp41 + P(11, 3) * _tmp35 + P(11, 4) * _tmp38 + P(11, 5) * _tmp39); _k(12, 0) = - _tmp45 * (P(12, 0) * _tmp24 - P(12, 1) * _tmp34 + P(12, 2) * _tmp35 + P(12, 21) * _tmp43 + - P(12, 22) * _tmp44 + P(12, 3) * _tmp38 + P(12, 4) * _tmp41 + P(12, 5) * _tmp42); + _tmp42 * (P(12, 0) * _tmp30 + P(12, 1) * _tmp31 + P(12, 2) * _tmp32 + P(12, 21) * _tmp40 + + P(12, 22) * _tmp41 + P(12, 3) * _tmp35 + P(12, 4) * _tmp38 + P(12, 5) * _tmp39); _k(13, 0) = - _tmp45 * (P(13, 0) * _tmp24 - P(13, 1) * _tmp34 + P(13, 2) * _tmp35 + P(13, 21) * _tmp43 + - P(13, 22) * _tmp44 + P(13, 3) * _tmp38 + P(13, 4) * _tmp41 + P(13, 5) * _tmp42); + _tmp42 * (P(13, 0) * _tmp30 + P(13, 1) * _tmp31 + P(13, 2) * _tmp32 + P(13, 21) * _tmp40 + + P(13, 22) * _tmp41 + P(13, 3) * _tmp35 + P(13, 4) * _tmp38 + P(13, 5) * _tmp39); _k(14, 0) = - _tmp45 * (P(14, 0) * _tmp24 - P(14, 1) * _tmp34 + P(14, 2) * _tmp35 + P(14, 21) * _tmp43 + - P(14, 22) * _tmp44 + P(14, 3) * _tmp38 + P(14, 4) * _tmp41 + P(14, 5) * _tmp42); + _tmp42 * (P(14, 0) * _tmp30 + P(14, 1) * _tmp31 + P(14, 2) * _tmp32 + P(14, 21) * _tmp40 + + P(14, 22) * _tmp41 + P(14, 3) * _tmp35 + P(14, 4) * _tmp38 + P(14, 5) * _tmp39); _k(15, 0) = - _tmp45 * (P(15, 0) * _tmp24 - P(15, 1) * _tmp34 + P(15, 2) * _tmp35 + P(15, 21) * _tmp43 + - P(15, 22) * _tmp44 + P(15, 3) * _tmp38 + P(15, 4) * _tmp41 + P(15, 5) * _tmp42); + _tmp42 * (P(15, 0) * _tmp30 + P(15, 1) * _tmp31 + P(15, 2) * _tmp32 + P(15, 21) * _tmp40 + + P(15, 22) * _tmp41 + P(15, 3) * _tmp35 + P(15, 4) * _tmp38 + P(15, 5) * _tmp39); _k(16, 0) = - _tmp45 * (P(16, 0) * _tmp24 - P(16, 1) * _tmp34 + P(16, 2) * _tmp35 + P(16, 21) * _tmp43 + - P(16, 22) * _tmp44 + P(16, 3) * _tmp38 + P(16, 4) * _tmp41 + P(16, 5) * _tmp42); + _tmp42 * (P(16, 0) * _tmp30 + P(16, 1) * _tmp31 + P(16, 2) * _tmp32 + P(16, 21) * _tmp40 + + P(16, 22) * _tmp41 + P(16, 3) * _tmp35 + P(16, 4) * _tmp38 + P(16, 5) * _tmp39); _k(17, 0) = - _tmp45 * (P(17, 0) * _tmp24 - P(17, 1) * _tmp34 + P(17, 2) * _tmp35 + P(17, 21) * _tmp43 + - P(17, 22) * _tmp44 + P(17, 3) * _tmp38 + P(17, 4) * _tmp41 + P(17, 5) * _tmp42); + _tmp42 * (P(17, 0) * _tmp30 + P(17, 1) * _tmp31 + P(17, 2) * _tmp32 + P(17, 21) * _tmp40 + + P(17, 22) * _tmp41 + P(17, 3) * _tmp35 + P(17, 4) * _tmp38 + P(17, 5) * _tmp39); _k(18, 0) = - _tmp45 * (P(18, 0) * _tmp24 - P(18, 1) * _tmp34 + P(18, 2) * _tmp35 + P(18, 21) * _tmp43 + - P(18, 22) * _tmp44 + P(18, 3) * _tmp38 + P(18, 4) * _tmp41 + P(18, 5) * _tmp42); + _tmp42 * (P(18, 0) * _tmp30 + P(18, 1) * _tmp31 + P(18, 2) * _tmp32 + P(18, 21) * _tmp40 + + P(18, 22) * _tmp41 + P(18, 3) * _tmp35 + P(18, 4) * _tmp38 + P(18, 5) * _tmp39); _k(19, 0) = - _tmp45 * (P(19, 0) * _tmp24 - P(19, 1) * _tmp34 + P(19, 2) * _tmp35 + P(19, 21) * _tmp43 + - P(19, 22) * _tmp44 + P(19, 3) * _tmp38 + P(19, 4) * _tmp41 + P(19, 5) * _tmp42); + _tmp42 * (P(19, 0) * _tmp30 + P(19, 1) * _tmp31 + P(19, 2) * _tmp32 + P(19, 21) * _tmp40 + + P(19, 22) * _tmp41 + P(19, 3) * _tmp35 + P(19, 4) * _tmp38 + P(19, 5) * _tmp39); _k(20, 0) = - _tmp45 * (P(20, 0) * _tmp24 - P(20, 1) * _tmp34 + P(20, 2) * _tmp35 + P(20, 21) * _tmp43 + - P(20, 22) * _tmp44 + P(20, 3) * _tmp38 + P(20, 4) * _tmp41 + P(20, 5) * _tmp42); + _tmp42 * (P(20, 0) * _tmp30 + P(20, 1) * _tmp31 + P(20, 2) * _tmp32 + P(20, 21) * _tmp40 + + P(20, 22) * _tmp41 + P(20, 3) * _tmp35 + P(20, 4) * _tmp38 + P(20, 5) * _tmp39); _k(21, 0) = - _tmp45 * (P(21, 0) * _tmp24 - P(21, 1) * _tmp34 + P(21, 2) * _tmp35 + P(21, 21) * _tmp43 + - P(21, 22) * _tmp44 + P(21, 3) * _tmp38 + P(21, 4) * _tmp41 + P(21, 5) * _tmp42); + _tmp42 * (P(21, 0) * _tmp30 + P(21, 1) * _tmp31 + P(21, 2) * _tmp32 + P(21, 21) * _tmp40 + + P(21, 22) * _tmp41 + P(21, 3) * _tmp35 + P(21, 4) * _tmp38 + P(21, 5) * _tmp39); _k(22, 0) = - _tmp45 * (P(22, 0) * _tmp24 - P(22, 1) * _tmp34 + P(22, 2) * _tmp35 + P(22, 21) * _tmp43 + - P(22, 22) * _tmp44 + P(22, 3) * _tmp38 + P(22, 4) * _tmp41 + P(22, 5) * _tmp42); + _tmp42 * (P(22, 0) * _tmp30 + P(22, 1) * _tmp31 + P(22, 2) * _tmp32 + P(22, 21) * _tmp40 + + P(22, 22) * _tmp41 + P(22, 3) * _tmp35 + P(22, 4) * _tmp38 + P(22, 5) * _tmp39); + _k(23, 0) = + _tmp42 * (P(23, 0) * _tmp30 + P(23, 1) * _tmp31 + P(23, 2) * _tmp32 + P(23, 21) * _tmp40 + + P(23, 22) * _tmp41 + P(23, 3) * _tmp35 + P(23, 4) * _tmp38 + P(23, 5) * _tmp39); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h index 48dd3e284f34..6918bddfc366 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_sideslip_innov_and_innov_var.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: compute_sideslip_innov_and_innov_var * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * epsilon: Scalar * @@ -26,73 +26,74 @@ namespace sym { * innov_var: Scalar */ template -void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, const Scalar epsilon, Scalar* const innov = nullptr, Scalar* const innov_var = nullptr) { - // Total ops: 235 + // Total ops: 265 // Input arrays - // Intermediate terms (46) - const Scalar _tmp0 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp1 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp2 = 1 - 2 * _tmp1; - const Scalar _tmp3 = -2 * _tmp0 + _tmp2; - const Scalar _tmp4 = -state(22, 0) + state(4, 0); - const Scalar _tmp5 = 2 * state(0, 0); - const Scalar _tmp6 = _tmp5 * state(3, 0); - const Scalar _tmp7 = 2 * state(1, 0); - const Scalar _tmp8 = _tmp7 * state(2, 0); - const Scalar _tmp9 = _tmp6 + _tmp8; - const Scalar _tmp10 = -state(23, 0) + state(5, 0); - const Scalar _tmp11 = _tmp5 * state(2, 0); - const Scalar _tmp12 = -_tmp11; - const Scalar _tmp13 = _tmp7 * state(3, 0); - const Scalar _tmp14 = _tmp12 + _tmp13; - const Scalar _tmp15 = _tmp10 * _tmp9 + _tmp14 * state(6, 0) + _tmp3 * _tmp4; - const Scalar _tmp16 = - _tmp15 + epsilon * (2 * math::min(0, (((_tmp15) > 0) - ((_tmp15) < 0))) + 1); - const Scalar _tmp17 = Scalar(1.0) / (_tmp16); - const Scalar _tmp18 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp19 = -2 * _tmp18 + _tmp2; - const Scalar _tmp20 = -_tmp6; - const Scalar _tmp21 = _tmp20 + _tmp8; - const Scalar _tmp22 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp23 = _tmp5 * state(1, 0); - const Scalar _tmp24 = _tmp22 + _tmp23; - const Scalar _tmp25 = _tmp21 * _tmp4 + _tmp24 * state(6, 0); - const Scalar _tmp26 = _tmp10 * _tmp19 + _tmp25; - const Scalar _tmp27 = _tmp26 / std::pow(_tmp16, Scalar(2)); - const Scalar _tmp28 = -_tmp14 * _tmp27 + _tmp17 * _tmp24; - const Scalar _tmp29 = _tmp27 * _tmp3; - const Scalar _tmp30 = _tmp17 * _tmp21; - const Scalar _tmp31 = -_tmp29 + _tmp30; - const Scalar _tmp32 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp33 = -_tmp32; - const Scalar _tmp34 = -_tmp18; - const Scalar _tmp35 = -_tmp13; - const Scalar _tmp36 = _tmp0 - _tmp1; - const Scalar _tmp37 = _tmp32 + _tmp34; + // Intermediate terms (42) + const Scalar _tmp0 = 1 - 2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp1 = _tmp0 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp2 = -state(22, 0) + state(4, 0); + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp3 * state(3, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = _tmp4 + _tmp6; + const Scalar _tmp8 = -state(23, 0) + state(5, 0); + const Scalar _tmp9 = 2 * state(2, 0); + const Scalar _tmp10 = _tmp5 * state(3, 0) - _tmp9 * state(0, 0); + const Scalar _tmp11 = _tmp1 * _tmp2 + _tmp10 * state(6, 0) + _tmp7 * _tmp8; + const Scalar _tmp12 = + _tmp11 + epsilon * (2 * math::min(0, (((_tmp11) > 0) - ((_tmp11) < 0))) + 1); + const Scalar _tmp13 = Scalar(1.0) / (_tmp12); + const Scalar _tmp14 = _tmp0 - 2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp15 = -_tmp4 + _tmp6; + const Scalar _tmp16 = _tmp5 * state(0, 0) + _tmp9 * state(3, 0); + const Scalar _tmp17 = _tmp14 * _tmp8 + _tmp15 * _tmp2 + _tmp16 * state(6, 0); + const Scalar _tmp18 = _tmp17 / std::pow(_tmp12, Scalar(2)); + const Scalar _tmp19 = _tmp18 * _tmp7; + const Scalar _tmp20 = _tmp13 * _tmp14; + const Scalar _tmp21 = _tmp19 - _tmp20; + const Scalar _tmp22 = -_tmp10 * _tmp18 + _tmp13 * _tmp16; + const Scalar _tmp23 = _tmp1 * _tmp18; + const Scalar _tmp24 = _tmp13 * _tmp15; + const Scalar _tmp25 = -_tmp23 + _tmp24; + const Scalar _tmp26 = 2 * state(6, 0); + const Scalar _tmp27 = _tmp26 * state(0, 0); + const Scalar _tmp28 = _tmp26 * state(3, 0); + const Scalar _tmp29 = + (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp5 + _tmp28) - + Scalar(1) / Scalar(2) * _tmp18 * (-4 * _tmp2 * state(2, 0) - _tmp27 + _tmp5 * _tmp8); + const Scalar _tmp30 = + (Scalar(1) / Scalar(2)) * _tmp13 * (_tmp2 * _tmp9 + _tmp27 - 4 * _tmp8 * state(1, 0)) - + Scalar(1) / Scalar(2) * _tmp18 * (_tmp28 + _tmp8 * _tmp9); + const Scalar _tmp31 = 2 * state(3, 0); + const Scalar _tmp32 = _tmp26 * state(2, 0); + const Scalar _tmp33 = _tmp26 * state(1, 0); + const Scalar _tmp34 = (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp31 + _tmp33) - + Scalar(1) / Scalar(2) * _tmp18 * (_tmp31 * _tmp8 - _tmp32); + const Scalar _tmp35 = 4 * state(3, 0); + const Scalar _tmp36 = + (Scalar(1) / Scalar(2)) * _tmp13 * (-_tmp2 * _tmp3 + _tmp32 - _tmp35 * _tmp8) - + Scalar(1) / Scalar(2) * _tmp18 * (-_tmp2 * _tmp35 + _tmp3 * _tmp8 + _tmp33); + const Scalar _tmp37 = + _tmp29 * state(1, 0) - _tmp30 * state(2, 0) - _tmp34 * state(3, 0) + _tmp36 * state(0, 0); const Scalar _tmp38 = - _tmp17 * (_tmp10 * (_tmp20 - _tmp8) + _tmp4 * (_tmp0 + _tmp1 + _tmp33 + _tmp34) + - state(6, 0) * (_tmp11 + _tmp35)) - - _tmp27 * (_tmp10 * (_tmp36 + _tmp37) + _tmp25); - const Scalar _tmp39 = _tmp29 - _tmp30; - const Scalar _tmp40 = _tmp27 * _tmp9; - const Scalar _tmp41 = _tmp17 * _tmp19; - const Scalar _tmp42 = -_tmp40 + _tmp41; - const Scalar _tmp43 = _tmp27 * (_tmp10 * (-_tmp22 + _tmp23) + _tmp4 * (_tmp12 + _tmp35) + - state(6, 0) * (_tmp18 + _tmp33 + _tmp36)); - const Scalar _tmp44 = _tmp40 - _tmp41; - const Scalar _tmp45 = _tmp17 * (_tmp10 * (_tmp22 - _tmp23) + _tmp4 * (_tmp11 + _tmp13) + - state(6, 0) * (-_tmp0 + _tmp1 + _tmp37)); + -_tmp29 * state(3, 0) + _tmp30 * state(0, 0) - _tmp34 * state(1, 0) + _tmp36 * state(2, 0); + const Scalar _tmp39 = + _tmp29 * state(0, 0) + _tmp30 * state(3, 0) - _tmp34 * state(2, 0) - _tmp36 * state(1, 0); + const Scalar _tmp40 = _tmp23 - _tmp24; + const Scalar _tmp41 = -_tmp19 + _tmp20; // Output terms (2) if (innov != nullptr) { Scalar& _innov = (*innov); - _innov = _tmp17 * _tmp26; + _innov = _tmp13 * _tmp17; } if (innov_var != nullptr) { @@ -100,22 +101,22 @@ void ComputeSideslipInnovAndInnovVar(const matrix::Matrix& state, _innov_var = R + - _tmp28 * (P(0, 5) * _tmp45 - P(1, 5) * _tmp43 + P(2, 5) * _tmp38 + P(21, 5) * _tmp39 + - P(22, 5) * _tmp44 + P(3, 5) * _tmp31 + P(4, 5) * _tmp42 + P(5, 5) * _tmp28) + - _tmp31 * (P(0, 3) * _tmp45 - P(1, 3) * _tmp43 + P(2, 3) * _tmp38 + P(21, 3) * _tmp39 + - P(22, 3) * _tmp44 + P(3, 3) * _tmp31 + P(4, 3) * _tmp42 + P(5, 3) * _tmp28) + - _tmp38 * (P(0, 2) * _tmp45 - P(1, 2) * _tmp43 + P(2, 2) * _tmp38 + P(21, 2) * _tmp39 + - P(22, 2) * _tmp44 + P(3, 2) * _tmp31 + P(4, 2) * _tmp42 + P(5, 2) * _tmp28) + - _tmp39 * (P(0, 21) * _tmp45 - P(1, 21) * _tmp43 + P(2, 21) * _tmp38 + P(21, 21) * _tmp39 + - P(22, 21) * _tmp44 + P(3, 21) * _tmp31 + P(4, 21) * _tmp42 + P(5, 21) * _tmp28) + - _tmp42 * (P(0, 4) * _tmp45 - P(1, 4) * _tmp43 + P(2, 4) * _tmp38 + P(21, 4) * _tmp39 + - P(22, 4) * _tmp44 + P(3, 4) * _tmp31 + P(4, 4) * _tmp42 + P(5, 4) * _tmp28) - - _tmp43 * (P(0, 1) * _tmp45 - P(1, 1) * _tmp43 + P(2, 1) * _tmp38 + P(21, 1) * _tmp39 + - P(22, 1) * _tmp44 + P(3, 1) * _tmp31 + P(4, 1) * _tmp42 + P(5, 1) * _tmp28) + - _tmp44 * (P(0, 22) * _tmp45 - P(1, 22) * _tmp43 + P(2, 22) * _tmp38 + P(21, 22) * _tmp39 + - P(22, 22) * _tmp44 + P(3, 22) * _tmp31 + P(4, 22) * _tmp42 + P(5, 22) * _tmp28) + - _tmp45 * (P(0, 0) * _tmp45 - P(1, 0) * _tmp43 + P(2, 0) * _tmp38 + P(21, 0) * _tmp39 + - P(22, 0) * _tmp44 + P(3, 0) * _tmp31 + P(4, 0) * _tmp42 + P(5, 0) * _tmp28); + _tmp21 * (P(0, 22) * _tmp38 + P(1, 22) * _tmp39 + P(2, 22) * _tmp37 + P(21, 22) * _tmp40 + + P(22, 22) * _tmp21 + P(3, 22) * _tmp25 + P(4, 22) * _tmp41 + P(5, 22) * _tmp22) + + _tmp22 * (P(0, 5) * _tmp38 + P(1, 5) * _tmp39 + P(2, 5) * _tmp37 + P(21, 5) * _tmp40 + + P(22, 5) * _tmp21 + P(3, 5) * _tmp25 + P(4, 5) * _tmp41 + P(5, 5) * _tmp22) + + _tmp25 * (P(0, 3) * _tmp38 + P(1, 3) * _tmp39 + P(2, 3) * _tmp37 + P(21, 3) * _tmp40 + + P(22, 3) * _tmp21 + P(3, 3) * _tmp25 + P(4, 3) * _tmp41 + P(5, 3) * _tmp22) + + _tmp37 * (P(0, 2) * _tmp38 + P(1, 2) * _tmp39 + P(2, 2) * _tmp37 + P(21, 2) * _tmp40 + + P(22, 2) * _tmp21 + P(3, 2) * _tmp25 + P(4, 2) * _tmp41 + P(5, 2) * _tmp22) + + _tmp38 * (P(0, 0) * _tmp38 + P(1, 0) * _tmp39 + P(2, 0) * _tmp37 + P(21, 0) * _tmp40 + + P(22, 0) * _tmp21 + P(3, 0) * _tmp25 + P(4, 0) * _tmp41 + P(5, 0) * _tmp22) + + _tmp39 * (P(0, 1) * _tmp38 + P(1, 1) * _tmp39 + P(2, 1) * _tmp37 + P(21, 1) * _tmp40 + + P(22, 1) * _tmp21 + P(3, 1) * _tmp25 + P(4, 1) * _tmp41 + P(5, 1) * _tmp22) + + _tmp40 * (P(0, 21) * _tmp38 + P(1, 21) * _tmp39 + P(2, 21) * _tmp37 + P(21, 21) * _tmp40 + + P(22, 21) * _tmp21 + P(3, 21) * _tmp25 + P(4, 21) * _tmp41 + P(5, 21) * _tmp22) + + _tmp41 * (P(0, 4) * _tmp38 + P(1, 4) * _tmp39 + P(2, 4) * _tmp37 + P(21, 4) * _tmp40 + + P(22, 4) * _tmp21 + P(3, 4) * _tmp25 + P(4, 4) * _tmp41 + P(5, 4) * _tmp22); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h new file mode 100644 index 000000000000..d0af79dc7aec --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_wind_init_and_cov_from_wind_speed_and_direction.h @@ -0,0 +1,61 @@ +// ----------------------------------------------------------------------------- +// This file was autogenerated by symforce from template: +// function/FUNCTION.h.jinja +// Do NOT modify by hand. +// ----------------------------------------------------------------------------- + +#pragma once + +#include + +namespace sym { + +/** + * This function was autogenerated from a symbolic function. Do not modify by hand. + * + * Symbolic function: compute_wind_init_and_cov_from_wind_speed_and_direction + * + * Args: + * wind_speed: Scalar + * wind_direction: Scalar + * wind_speed_var: Scalar + * wind_direction_var: Scalar + * + * Outputs: + * wind: Matrix21 + * P_wind: Matrix21 + */ +template +void ComputeWindInitAndCovFromWindSpeedAndDirection( + const Scalar wind_speed, const Scalar wind_direction, const Scalar wind_speed_var, + const Scalar wind_direction_var, matrix::Matrix* const wind = nullptr, + matrix::Matrix* const P_wind = nullptr) { + // Total ops: 14 + + // Input arrays + + // Intermediate terms (5) + const Scalar _tmp0 = std::cos(wind_direction); + const Scalar _tmp1 = std::sin(wind_direction); + const Scalar _tmp2 = std::pow(_tmp0, Scalar(2)); + const Scalar _tmp3 = std::pow(_tmp1, Scalar(2)); + const Scalar _tmp4 = wind_direction_var * std::pow(wind_speed, Scalar(2)); + + // Output terms (2) + if (wind != nullptr) { + matrix::Matrix& _wind = (*wind); + + _wind(0, 0) = _tmp0 * wind_speed; + _wind(1, 0) = _tmp1 * wind_speed; + } + + if (P_wind != nullptr) { + matrix::Matrix& _p_wind = (*P_wind); + + _p_wind(0, 0) = _tmp2 * wind_speed_var + _tmp3 * _tmp4; + _p_wind(1, 0) = _tmp2 * _tmp4 + _tmp3 * wind_speed_var; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h index 443fb0b03c5e..41f21774336d 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_innov_var_and_h.h @@ -16,48 +16,41 @@ namespace sym { * Symbolic function: compute_yaw_innov_var_and_h * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * R: Scalar * * Outputs: * innov_var: Scalar - * H: Matrix23_1 + * H: Matrix24_1 */ template -void ComputeYawInnovVarAndH(const matrix::Matrix& state, - const matrix::Matrix& P, const Scalar R, +void ComputeYawInnovVarAndH(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar R, Scalar* const innov_var = nullptr, - matrix::Matrix* const H = nullptr) { - // Total ops: 36 + matrix::Matrix* const H = nullptr) { + // Total ops: 1 + + // Unused inputs + (void)state; // Input arrays - // Intermediate terms (5) - const Scalar _tmp0 = 2 * state(2, 0); - const Scalar _tmp1 = 2 * state(1, 0); - const Scalar _tmp2 = -_tmp0 * state(0, 0) + _tmp1 * state(3, 0); - const Scalar _tmp3 = _tmp0 * state(3, 0) + _tmp1 * state(0, 0); - const Scalar _tmp4 = std::pow(state(0, 0), Scalar(2)) - std::pow(state(1, 0), Scalar(2)) - - std::pow(state(2, 0), Scalar(2)) + std::pow(state(3, 0), Scalar(2)); + // Intermediate terms (0) // Output terms (2) if (innov_var != nullptr) { Scalar& _innov_var = (*innov_var); - _innov_var = R + _tmp2 * (P(0, 0) * _tmp2 + P(1, 0) * _tmp3 + P(2, 0) * _tmp4) + - _tmp3 * (P(0, 1) * _tmp2 + P(1, 1) * _tmp3 + P(2, 1) * _tmp4) + - _tmp4 * (P(0, 2) * _tmp2 + P(1, 2) * _tmp3 + P(2, 2) * _tmp4); + _innov_var = P(2, 2) + R; } if (H != nullptr) { - matrix::Matrix& _h = (*H); + matrix::Matrix& _h = (*H); _h.setZero(); - _h(0, 0) = _tmp2; - _h(1, 0) = _tmp3; - _h(2, 0) = _tmp4; + _h(2, 0) = 1; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h index 338ee277c039..edf246a6bf60 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/predict_covariance.h @@ -16,8 +16,8 @@ namespace sym { * Symbolic function: predict_covariance * * Args: - * state: Matrix24_1 - * P: Matrix23_23 + * state: Matrix25_1 + * P: Matrix24_24 * accel: Matrix31 * accel_var: Matrix31 * gyro: Matrix31 @@ -25,516 +25,374 @@ namespace sym { * dt: Scalar * * Outputs: - * res: Matrix23_23 + * res: Matrix24_24 */ template -matrix::Matrix PredictCovariance(const matrix::Matrix& state, - const matrix::Matrix& P, +matrix::Matrix PredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, const matrix::Matrix& accel, const matrix::Matrix& accel_var, const matrix::Matrix& gyro, const Scalar gyro_var, const Scalar dt) { - // Total ops: 1793 + // Total ops: 1810 + + // Unused inputs + (void)gyro; // Input arrays - // Intermediate terms (134) - const Scalar _tmp0 = dt * gyro(1, 0); - const Scalar _tmp1 = dt * state(11, 0); - const Scalar _tmp2 = -_tmp0 + _tmp1; - const Scalar _tmp3 = dt * gyro(2, 0); - const Scalar _tmp4 = dt * state(12, 0); - const Scalar _tmp5 = _tmp3 - _tmp4; - const Scalar _tmp6 = P(0, 2) + P(1, 2) * _tmp5 + P(2, 2) * _tmp2 - P(9, 2) * dt; - const Scalar _tmp7 = P(0, 1) + P(1, 1) * _tmp5 + P(2, 1) * _tmp2 - P(9, 1) * dt; - const Scalar _tmp8 = P(0, 9) + P(1, 9) * _tmp5 + P(2, 9) * _tmp2 - P(9, 9) * dt; - const Scalar _tmp9 = std::pow(dt, Scalar(2)); - const Scalar _tmp10 = _tmp9 * gyro_var; - const Scalar _tmp11 = P(0, 0) + P(1, 0) * _tmp5 + P(2, 0) * _tmp2 - P(9, 0) * dt; - const Scalar _tmp12 = P(0, 10) + P(1, 10) * _tmp5 + P(2, 10) * _tmp2 - P(9, 10) * dt; - const Scalar _tmp13 = -_tmp3 + _tmp4; - const Scalar _tmp14 = dt * gyro(0, 0); - const Scalar _tmp15 = dt * state(10, 0); - const Scalar _tmp16 = _tmp14 - _tmp15; - const Scalar _tmp17 = P(0, 10) * _tmp13 + P(1, 10) - P(10, 10) * dt + P(2, 10) * _tmp16; - const Scalar _tmp18 = P(0, 0) * _tmp13 + P(1, 0) - P(10, 0) * dt + P(2, 0) * _tmp16; - const Scalar _tmp19 = P(0, 2) * _tmp13 + P(1, 2) - P(10, 2) * dt + P(2, 2) * _tmp16; - const Scalar _tmp20 = P(0, 1) * _tmp13 + P(1, 1) - P(10, 1) * dt + P(2, 1) * _tmp16; - const Scalar _tmp21 = P(0, 11) + P(1, 11) * _tmp5 + P(2, 11) * _tmp2 - P(9, 11) * dt; - const Scalar _tmp22 = _tmp0 - _tmp1; - const Scalar _tmp23 = -_tmp14 + _tmp15; - const Scalar _tmp24 = P(0, 11) * _tmp13 + P(1, 11) - P(10, 11) * dt + P(2, 11) * _tmp16; - const Scalar _tmp25 = P(0, 11) * _tmp22 + P(1, 11) * _tmp23 - P(11, 11) * dt + P(2, 11); - const Scalar _tmp26 = P(0, 1) * _tmp22 + P(1, 1) * _tmp23 - P(11, 1) * dt + P(2, 1); - const Scalar _tmp27 = P(0, 0) * _tmp22 + P(1, 0) * _tmp23 - P(11, 0) * dt + P(2, 0); - const Scalar _tmp28 = P(0, 2) * _tmp22 + P(1, 2) * _tmp23 - P(11, 2) * dt + P(2, 2); - const Scalar _tmp29 = 2 * state(0, 0); - const Scalar _tmp30 = _tmp29 * state(3, 0); - const Scalar _tmp31 = -_tmp30; - const Scalar _tmp32 = 2 * state(1, 0); - const Scalar _tmp33 = _tmp32 * state(2, 0); + // Intermediate terms (144) + const Scalar _tmp0 = 2 * state(1, 0); + const Scalar _tmp1 = _tmp0 * state(3, 0); + const Scalar _tmp2 = -_tmp1 * dt; + const Scalar _tmp3 = 2 * state(0, 0); + const Scalar _tmp4 = _tmp3 * state(2, 0); + const Scalar _tmp5 = _tmp4 * dt; + const Scalar _tmp6 = _tmp2 - _tmp5; + const Scalar _tmp7 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp8 = _tmp7 * dt; + const Scalar _tmp9 = std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp10 = -_tmp9 * dt; + const Scalar _tmp11 = std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp12 = _tmp11 * dt; + const Scalar _tmp13 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp14 = _tmp13 * dt; + const Scalar _tmp15 = _tmp10 + _tmp12 - _tmp14 + _tmp8; + const Scalar _tmp16 = _tmp13 + _tmp7; + const Scalar _tmp17 = _tmp11 + _tmp9; + const Scalar _tmp18 = _tmp16 + _tmp17; + const Scalar _tmp19 = _tmp0 * state(2, 0); + const Scalar _tmp20 = -_tmp19 * dt; + const Scalar _tmp21 = _tmp3 * state(3, 0); + const Scalar _tmp22 = _tmp21 * dt; + const Scalar _tmp23 = _tmp20 + _tmp22; + const Scalar _tmp24 = + P(0, 11) * _tmp18 + P(10, 11) * _tmp23 + P(11, 11) * _tmp6 + P(9, 11) * _tmp15; + const Scalar _tmp25 = + P(0, 10) * _tmp18 + P(10, 10) * _tmp23 + P(11, 10) * _tmp6 + P(9, 10) * _tmp15; + const Scalar _tmp26 = P(0, 0) * _tmp18 + P(10, 0) * _tmp23 + P(11, 0) * _tmp6 + P(9, 0) * _tmp15; + const Scalar _tmp27 = P(0, 9) * _tmp18 + P(10, 9) * _tmp23 + P(11, 9) * _tmp6 + P(9, 9) * _tmp15; + const Scalar _tmp28 = _tmp10 + _tmp14; + const Scalar _tmp29 = -_tmp12 + _tmp28 + _tmp8; + const Scalar _tmp30 = _tmp0 * state(0, 0); + const Scalar _tmp31 = _tmp30 * dt; + const Scalar _tmp32 = 2 * state(2, 0) * state(3, 0); + const Scalar _tmp33 = -_tmp32 * dt; const Scalar _tmp34 = _tmp31 + _tmp33; - const Scalar _tmp35 = accel(0, 0) - state(13, 0); - const Scalar _tmp36 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp37 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp38 = -_tmp37; - const Scalar _tmp39 = _tmp36 + _tmp38; - const Scalar _tmp40 = std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp41 = -_tmp40; - const Scalar _tmp42 = std::pow(state(3, 0), Scalar(2)); - const Scalar _tmp43 = _tmp41 + _tmp42; - const Scalar _tmp44 = accel(1, 0) - state(14, 0); - const Scalar _tmp45 = dt * (_tmp34 * _tmp35 + _tmp44 * (_tmp39 + _tmp43)); - const Scalar _tmp46 = P(0, 12) + P(1, 12) * _tmp5 + P(2, 12) * _tmp2 - P(9, 12) * dt; - const Scalar _tmp47 = -2 * _tmp42; - const Scalar _tmp48 = -2 * _tmp36; - const Scalar _tmp49 = _tmp47 + _tmp48 + 1; - const Scalar _tmp50 = _tmp49 * dt; - const Scalar _tmp51 = _tmp29 * state(2, 0); - const Scalar _tmp52 = -_tmp51; - const Scalar _tmp53 = _tmp32 * state(3, 0); - const Scalar _tmp54 = -_tmp53; - const Scalar _tmp55 = -_tmp42; - const Scalar _tmp56 = _tmp40 + _tmp55; - const Scalar _tmp57 = -_tmp36; - const Scalar _tmp58 = _tmp37 + _tmp57; - const Scalar _tmp59 = accel(2, 0) - state(15, 0); - const Scalar _tmp60 = dt * (_tmp35 * (_tmp52 + _tmp54) + _tmp59 * (_tmp56 + _tmp58)); - const Scalar _tmp61 = _tmp51 + _tmp53; - const Scalar _tmp62 = -_tmp33; - const Scalar _tmp63 = dt * (_tmp44 * _tmp61 + _tmp59 * (_tmp30 + _tmp62)); - const Scalar _tmp64 = P(0, 13) + P(1, 13) * _tmp5 + P(2, 13) * _tmp2 - P(9, 13) * dt; - const Scalar _tmp65 = _tmp34 * dt; - const Scalar _tmp66 = P(0, 14) + P(1, 14) * _tmp5 + P(2, 14) * _tmp2 - P(9, 14) * dt; - const Scalar _tmp67 = _tmp61 * dt; - const Scalar _tmp68 = P(0, 3) + P(1, 3) * _tmp5 + P(2, 3) * _tmp2 - P(9, 3) * dt; - const Scalar _tmp69 = P(0, 13) * _tmp13 + P(1, 13) - P(10, 13) * dt + P(2, 13) * _tmp16; - const Scalar _tmp70 = P(0, 14) * _tmp13 + P(1, 14) - P(10, 14) * dt + P(2, 14) * _tmp16; - const Scalar _tmp71 = P(0, 12) * _tmp13 + P(1, 12) - P(10, 12) * dt + P(2, 12) * _tmp16; - const Scalar _tmp72 = P(0, 3) * _tmp13 + P(1, 3) - P(10, 3) * dt + P(2, 3) * _tmp16; - const Scalar _tmp73 = P(0, 14) * _tmp22 + P(1, 14) * _tmp23 - P(11, 14) * dt + P(2, 14); - const Scalar _tmp74 = P(0, 13) * _tmp22 + P(1, 13) * _tmp23 - P(11, 13) * dt + P(2, 13); - const Scalar _tmp75 = P(0, 12) * _tmp22 + P(1, 12) * _tmp23 - P(11, 12) * dt + P(2, 12); - const Scalar _tmp76 = P(0, 3) * _tmp22 + P(1, 3) * _tmp23 - P(11, 3) * dt + P(2, 3); - const Scalar _tmp77 = P(0, 1) * _tmp63 + P(1, 1) * _tmp60 - P(12, 1) * _tmp50 - - P(13, 1) * _tmp65 - P(14, 1) * _tmp67 + P(2, 1) * _tmp45 + P(3, 1); - const Scalar _tmp78 = P(0, 14) * _tmp63 + P(1, 14) * _tmp60 - P(12, 14) * _tmp50 - - P(13, 14) * _tmp65 - P(14, 14) * _tmp67 + P(2, 14) * _tmp45 + P(3, 14); - const Scalar _tmp79 = P(0, 13) * _tmp63 + P(1, 13) * _tmp60 - P(12, 13) * _tmp50 - - P(13, 13) * _tmp65 - P(14, 13) * _tmp67 + P(2, 13) * _tmp45 + P(3, 13); - const Scalar _tmp80 = P(0, 0) * _tmp63 + P(1, 0) * _tmp60 - P(12, 0) * _tmp50 - - P(13, 0) * _tmp65 - P(14, 0) * _tmp67 + P(2, 0) * _tmp45 + P(3, 0); - const Scalar _tmp81 = _tmp9 * accel_var(0, 0); - const Scalar _tmp82 = P(0, 2) * _tmp63 + P(1, 2) * _tmp60 - P(12, 2) * _tmp50 - - P(13, 2) * _tmp65 - P(14, 2) * _tmp67 + P(2, 2) * _tmp45 + P(3, 2); - const Scalar _tmp83 = _tmp9 * accel_var(1, 0); - const Scalar _tmp84 = _tmp9 * accel_var(2, 0); - const Scalar _tmp85 = P(0, 12) * _tmp63 + P(1, 12) * _tmp60 - P(12, 12) * _tmp50 - - P(13, 12) * _tmp65 - P(14, 12) * _tmp67 + P(2, 12) * _tmp45 + P(3, 12); - const Scalar _tmp86 = P(0, 3) * _tmp63 + P(1, 3) * _tmp60 - P(12, 3) * _tmp50 - - P(13, 3) * _tmp65 - P(14, 3) * _tmp67 + P(2, 3) * _tmp45 + P(3, 3); - const Scalar _tmp87 = 2 * state(2, 0) * state(3, 0); - const Scalar _tmp88 = _tmp29 * state(1, 0); - const Scalar _tmp89 = -_tmp88; - const Scalar _tmp90 = _tmp87 + _tmp89; - const Scalar _tmp91 = dt * (_tmp44 * _tmp90 + _tmp59 * (_tmp43 + _tmp58)); - const Scalar _tmp92 = dt * (_tmp35 * (_tmp39 + _tmp56) + _tmp44 * (_tmp31 + _tmp62)); - const Scalar _tmp93 = 1 - 2 * _tmp37; - const Scalar _tmp94 = _tmp47 + _tmp93; - const Scalar _tmp95 = _tmp94 * dt; - const Scalar _tmp96 = _tmp90 * dt; - const Scalar _tmp97 = _tmp30 + _tmp33; - const Scalar _tmp98 = -_tmp87; - const Scalar _tmp99 = dt * (_tmp35 * (_tmp88 + _tmp98) + _tmp59 * _tmp97); - const Scalar _tmp100 = _tmp97 * dt; - const Scalar _tmp101 = P(0, 4) + P(1, 4) * _tmp5 + P(2, 4) * _tmp2 - P(9, 4) * dt; - const Scalar _tmp102 = P(0, 4) * _tmp13 + P(1, 4) - P(10, 4) * dt + P(2, 4) * _tmp16; - const Scalar _tmp103 = _tmp74 * dt; - const Scalar _tmp104 = P(0, 4) * _tmp22 + P(1, 4) * _tmp23 - P(11, 4) * dt + P(2, 4); - const Scalar _tmp105 = _tmp81 * _tmp97; - const Scalar _tmp106 = _tmp84 * _tmp90; - const Scalar _tmp107 = P(0, 4) * _tmp63 + P(1, 4) * _tmp60 - P(12, 4) * _tmp50 - - P(13, 4) * _tmp65 - P(14, 4) * _tmp67 + P(2, 4) * _tmp45 + P(3, 4); - const Scalar _tmp108 = P(0, 14) * _tmp91 + P(1, 14) * _tmp99 - P(12, 14) * _tmp100 - - P(13, 14) * _tmp95 - P(14, 14) * _tmp96 + P(2, 14) * _tmp92 + P(4, 14); - const Scalar _tmp109 = P(0, 0) * _tmp91 + P(1, 0) * _tmp99 - P(12, 0) * _tmp100 - - P(13, 0) * _tmp95 - P(14, 0) * _tmp96 + P(2, 0) * _tmp92 + P(4, 0); - const Scalar _tmp110 = P(0, 13) * _tmp91 + P(1, 13) * _tmp99 - P(12, 13) * _tmp100 - - P(13, 13) * _tmp95 - P(14, 13) * _tmp96 + P(2, 13) * _tmp92 + P(4, 13); - const Scalar _tmp111 = P(0, 12) * _tmp91 + P(1, 12) * _tmp99 - P(12, 12) * _tmp100 - - P(13, 12) * _tmp95 - P(14, 12) * _tmp96 + P(2, 12) * _tmp92 + P(4, 12); - const Scalar _tmp112 = P(0, 1) * _tmp91 + P(1, 1) * _tmp99 - P(12, 1) * _tmp100 - - P(13, 1) * _tmp95 - P(14, 1) * _tmp96 + P(2, 1) * _tmp92 + P(4, 1); - const Scalar _tmp113 = P(0, 2) * _tmp91 + P(1, 2) * _tmp99 - P(12, 2) * _tmp100 - - P(13, 2) * _tmp95 - P(14, 2) * _tmp96 + P(2, 2) * _tmp92 + P(4, 2); - const Scalar _tmp114 = P(0, 4) * _tmp91 + P(1, 4) * _tmp99 - P(12, 4) * _tmp100 - - P(13, 4) * _tmp95 - P(14, 4) * _tmp96 + P(2, 4) * _tmp92 + P(4, 4); + const Scalar _tmp35 = P(0, 1) * _tmp18 + P(10, 1) * _tmp23 + P(11, 1) * _tmp6 + P(9, 1) * _tmp15; + const Scalar _tmp36 = _tmp20 - _tmp22; + const Scalar _tmp37 = _tmp23 * gyro_var; + const Scalar _tmp38 = _tmp36 * gyro_var; + const Scalar _tmp39 = _tmp6 * gyro_var; + const Scalar _tmp40 = + P(1, 10) * _tmp18 + P(10, 10) * _tmp29 + P(11, 10) * _tmp34 + P(9, 10) * _tmp36; + const Scalar _tmp41 = + P(1, 11) * _tmp18 + P(10, 11) * _tmp29 + P(11, 11) * _tmp34 + P(9, 11) * _tmp36; + const Scalar _tmp42 = P(1, 9) * _tmp18 + P(10, 9) * _tmp29 + P(11, 9) * _tmp34 + P(9, 9) * _tmp36; + const Scalar _tmp43 = P(1, 1) * _tmp18 + P(10, 1) * _tmp29 + P(11, 1) * _tmp34 + P(9, 1) * _tmp36; + const Scalar _tmp44 = _tmp12 + _tmp28 - _tmp8; + const Scalar _tmp45 = -_tmp31 + _tmp33; + const Scalar _tmp46 = P(0, 2) * _tmp18 + P(10, 2) * _tmp23 + P(11, 2) * _tmp6 + P(9, 2) * _tmp15; + const Scalar _tmp47 = _tmp2 + _tmp5; + const Scalar _tmp48 = P(1, 2) * _tmp18 + P(10, 2) * _tmp29 + P(11, 2) * _tmp34 + P(9, 2) * _tmp36; + const Scalar _tmp49 = + P(10, 10) * _tmp45 + P(11, 10) * _tmp44 + P(2, 10) * _tmp18 + P(9, 10) * _tmp47; + const Scalar _tmp50 = + P(10, 11) * _tmp45 + P(11, 11) * _tmp44 + P(2, 11) * _tmp18 + P(9, 11) * _tmp47; + const Scalar _tmp51 = P(10, 2) * _tmp45 + P(11, 2) * _tmp44 + P(2, 2) * _tmp18 + P(9, 2) * _tmp47; + const Scalar _tmp52 = P(10, 9) * _tmp45 + P(11, 9) * _tmp44 + P(2, 9) * _tmp18 + P(9, 9) * _tmp47; + const Scalar _tmp53 = + P(0, 12) * _tmp18 + P(10, 12) * _tmp23 + P(11, 12) * _tmp6 + P(9, 12) * _tmp15; + const Scalar _tmp54 = -2 * _tmp7; + const Scalar _tmp55 = -2 * _tmp11; + const Scalar _tmp56 = _tmp54 + _tmp55 + 1; + const Scalar _tmp57 = _tmp56 * dt; + const Scalar _tmp58 = + P(0, 13) * _tmp18 + P(10, 13) * _tmp23 + P(11, 13) * _tmp6 + P(9, 13) * _tmp15; + const Scalar _tmp59 = -_tmp21; + const Scalar _tmp60 = _tmp19 + _tmp59; + const Scalar _tmp61 = _tmp60 * dt; + const Scalar _tmp62 = + P(0, 14) * _tmp18 + P(10, 14) * _tmp23 + P(11, 14) * _tmp6 + P(9, 14) * _tmp15; + const Scalar _tmp63 = _tmp1 + _tmp4; + const Scalar _tmp64 = _tmp63 * dt; + const Scalar _tmp65 = -_tmp4; + const Scalar _tmp66 = _tmp1 + _tmp65; + const Scalar _tmp67 = accel(0, 0) - state(13, 0); + const Scalar _tmp68 = -_tmp13; + const Scalar _tmp69 = _tmp68 + _tmp7; + const Scalar _tmp70 = -_tmp11; + const Scalar _tmp71 = _tmp70 + _tmp9; + const Scalar _tmp72 = accel(2, 0) - state(15, 0); + const Scalar _tmp73 = _tmp30 + _tmp32; + const Scalar _tmp74 = accel(1, 0) - state(14, 0); + const Scalar _tmp75 = dt * (_tmp66 * _tmp67 + _tmp72 * (_tmp69 + _tmp71) + _tmp73 * _tmp74); + const Scalar _tmp76 = -_tmp19; + const Scalar _tmp77 = -_tmp9; + const Scalar _tmp78 = -_tmp32; + const Scalar _tmp79 = dt * (_tmp67 * (_tmp59 + _tmp76) + _tmp72 * (_tmp30 + _tmp78) + + _tmp74 * (_tmp16 + _tmp70 + _tmp77)); + const Scalar _tmp80 = P(0, 3) * _tmp18 + P(10, 3) * _tmp23 + P(11, 3) * _tmp6 + P(9, 3) * _tmp15; + const Scalar _tmp81 = + P(1, 12) * _tmp18 + P(10, 12) * _tmp29 + P(11, 12) * _tmp34 + P(9, 12) * _tmp36; + const Scalar _tmp82 = + P(1, 13) * _tmp18 + P(10, 13) * _tmp29 + P(11, 13) * _tmp34 + P(9, 13) * _tmp36; + const Scalar _tmp83 = + P(1, 14) * _tmp18 + P(10, 14) * _tmp29 + P(11, 14) * _tmp34 + P(9, 14) * _tmp36; + const Scalar _tmp84 = P(1, 3) * _tmp18 + P(10, 3) * _tmp29 + P(11, 3) * _tmp34 + P(9, 3) * _tmp36; + const Scalar _tmp85 = + P(10, 12) * _tmp45 + P(11, 12) * _tmp44 + P(2, 12) * _tmp18 + P(9, 12) * _tmp47; + const Scalar _tmp86 = P(10, 1) * _tmp45 + P(11, 1) * _tmp44 + P(2, 1) * _tmp18 + P(9, 1) * _tmp47; + const Scalar _tmp87 = + P(10, 13) * _tmp45 + P(11, 13) * _tmp44 + P(2, 13) * _tmp18 + P(9, 13) * _tmp47; + const Scalar _tmp88 = + P(10, 14) * _tmp45 + P(11, 14) * _tmp44 + P(2, 14) * _tmp18 + P(9, 14) * _tmp47; + const Scalar _tmp89 = P(10, 3) * _tmp45 + P(11, 3) * _tmp44 + P(2, 3) * _tmp18 + P(9, 3) * _tmp47; + const Scalar _tmp90 = P(1, 12) * _tmp75 - P(12, 12) * _tmp57 - P(13, 12) * _tmp61 - + P(14, 12) * _tmp64 + P(2, 12) * _tmp79 + P(3, 12); + const Scalar _tmp91 = P(1, 1) * _tmp75 - P(12, 1) * _tmp57 - P(13, 1) * _tmp61 - + P(14, 1) * _tmp64 + P(2, 1) * _tmp79 + P(3, 1); + const Scalar _tmp92 = P(1, 2) * _tmp75 - P(12, 2) * _tmp57 - P(13, 2) * _tmp61 - + P(14, 2) * _tmp64 + P(2, 2) * _tmp79 + P(3, 2); + const Scalar _tmp93 = P(1, 13) * _tmp75 - P(12, 13) * _tmp57 - P(13, 13) * _tmp61 - + P(14, 13) * _tmp64 + P(2, 13) * _tmp79 + P(3, 13); + const Scalar _tmp94 = P(1, 14) * _tmp75 - P(12, 14) * _tmp57 - P(13, 14) * _tmp61 - + P(14, 14) * _tmp64 + P(2, 14) * _tmp79 + P(3, 14); + const Scalar _tmp95 = std::pow(dt, Scalar(2)); + const Scalar _tmp96 = _tmp95 * accel_var(0, 0); + const Scalar _tmp97 = _tmp95 * accel_var(1, 0); + const Scalar _tmp98 = _tmp95 * accel_var(2, 0); + const Scalar _tmp99 = P(1, 3) * _tmp75 - P(12, 3) * _tmp57 - P(13, 3) * _tmp61 - + P(14, 3) * _tmp64 + P(2, 3) * _tmp79 + P(3, 3); + const Scalar _tmp100 = 1 - 2 * _tmp13; + const Scalar _tmp101 = _tmp100 + _tmp54; + const Scalar _tmp102 = _tmp101 * dt; + const Scalar _tmp103 = -_tmp7; + const Scalar _tmp104 = _tmp103 + _tmp13; + const Scalar _tmp105 = dt * (_tmp60 * _tmp74 + _tmp63 * _tmp72 + _tmp67 * (_tmp104 + _tmp71)); + const Scalar _tmp106 = -_tmp30; + const Scalar _tmp107 = _tmp106 + _tmp32; + const Scalar _tmp108 = _tmp107 * dt; + const Scalar _tmp109 = _tmp19 + _tmp21; + const Scalar _tmp110 = _tmp109 * dt; + const Scalar _tmp111 = -_tmp1; + const Scalar _tmp112 = _tmp11 + _tmp77; + const Scalar _tmp113 = dt * (_tmp67 * (_tmp111 + _tmp4) + _tmp72 * (_tmp104 + _tmp112) + + _tmp74 * (_tmp106 + _tmp78)); + const Scalar _tmp114 = P(0, 4) * _tmp18 + P(10, 4) * _tmp23 + P(11, 4) * _tmp6 + P(9, 4) * _tmp15; const Scalar _tmp115 = - dt * (_tmp44 * (_tmp38 + _tmp40 + _tmp42 + _tmp57) + _tmp59 * (_tmp89 + _tmp98)); - const Scalar _tmp116 = _tmp48 + _tmp93; - const Scalar _tmp117 = _tmp116 * dt; - const Scalar _tmp118 = _tmp87 + _tmp88; - const Scalar _tmp119 = _tmp118 * dt; - const Scalar _tmp120 = dt * (_tmp118 * _tmp35 + _tmp44 * (_tmp51 + _tmp54)); - const Scalar _tmp121 = _tmp52 + _tmp53; - const Scalar _tmp122 = dt * (_tmp121 * _tmp59 + _tmp35 * (_tmp36 + _tmp37 + _tmp41 + _tmp55)); - const Scalar _tmp123 = _tmp121 * dt; - const Scalar _tmp124 = P(0, 5) + P(1, 5) * _tmp5 + P(2, 5) * _tmp2 - P(9, 5) * dt; - const Scalar _tmp125 = P(0, 5) * _tmp13 + P(1, 5) - P(10, 5) * dt + P(2, 5) * _tmp16; - const Scalar _tmp126 = P(0, 5) * _tmp22 + P(1, 5) * _tmp23 - P(11, 5) * dt + P(2, 5); - const Scalar _tmp127 = _tmp118 * _tmp83; - const Scalar _tmp128 = P(0, 5) * _tmp63 + P(1, 5) * _tmp60 - P(12, 5) * _tmp50 - - P(13, 5) * _tmp65 - P(14, 5) * _tmp67 + P(2, 5) * _tmp45 + P(3, 5); - const Scalar _tmp129 = P(0, 5) * _tmp91 + P(1, 5) * _tmp99 - P(12, 5) * _tmp100 - - P(13, 5) * _tmp95 - P(14, 5) * _tmp96 + P(2, 5) * _tmp92 + P(4, 5); - const Scalar _tmp130 = P(0, 13) * _tmp115 + P(1, 13) * _tmp122 - P(12, 13) * _tmp123 - - P(13, 13) * _tmp119 - P(14, 13) * _tmp117 + P(2, 13) * _tmp120 + P(5, 13); - const Scalar _tmp131 = P(0, 14) * _tmp115 + P(1, 14) * _tmp122 - P(12, 14) * _tmp123 - - P(13, 14) * _tmp119 - P(14, 14) * _tmp117 + P(2, 14) * _tmp120 + P(5, 14); - const Scalar _tmp132 = P(0, 12) * _tmp115 + P(1, 12) * _tmp122 - P(12, 12) * _tmp123 - - P(13, 12) * _tmp119 - P(14, 12) * _tmp117 + P(2, 12) * _tmp120 + P(5, 12); - const Scalar _tmp133 = P(0, 5) * _tmp115 + P(1, 5) * _tmp122 - P(12, 5) * _tmp123 - - P(13, 5) * _tmp119 - P(14, 5) * _tmp117 + P(2, 5) * _tmp120 + P(5, 5); + P(1, 0) * _tmp18 + P(10, 0) * _tmp29 + P(11, 0) * _tmp34 + P(9, 0) * _tmp36; + const Scalar _tmp116 = + P(1, 4) * _tmp18 + P(10, 4) * _tmp29 + P(11, 4) * _tmp34 + P(9, 4) * _tmp36; + const Scalar _tmp117 = + P(10, 0) * _tmp45 + P(11, 0) * _tmp44 + P(2, 0) * _tmp18 + P(9, 0) * _tmp47; + const Scalar _tmp118 = + P(10, 4) * _tmp45 + P(11, 4) * _tmp44 + P(2, 4) * _tmp18 + P(9, 4) * _tmp47; + const Scalar _tmp119 = _tmp56 * _tmp96; + const Scalar _tmp120 = _tmp60 * _tmp97; + const Scalar _tmp121 = P(1, 0) * _tmp75 - P(12, 0) * _tmp57 - P(13, 0) * _tmp61 - + P(14, 0) * _tmp64 + P(2, 0) * _tmp79 + P(3, 0); + const Scalar _tmp122 = _tmp63 * _tmp98; + const Scalar _tmp123 = P(1, 4) * _tmp75 - P(12, 4) * _tmp57 - P(13, 4) * _tmp61 - + P(14, 4) * _tmp64 + P(2, 4) * _tmp79 + P(3, 4); + const Scalar _tmp124 = P(0, 0) * _tmp113 - P(12, 0) * _tmp110 - P(13, 0) * _tmp102 - + P(14, 0) * _tmp108 + P(2, 0) * _tmp105 + P(4, 0); + const Scalar _tmp125 = P(0, 13) * _tmp113 - P(12, 13) * _tmp110 - P(13, 13) * _tmp102 - + P(14, 13) * _tmp108 + P(2, 13) * _tmp105 + P(4, 13); + const Scalar _tmp126 = P(0, 14) * _tmp113 - P(12, 14) * _tmp110 - P(13, 14) * _tmp102 - + P(14, 14) * _tmp108 + P(2, 14) * _tmp105 + P(4, 14); + const Scalar _tmp127 = P(0, 12) * _tmp113 - P(12, 12) * _tmp110 - P(13, 12) * _tmp102 - + P(14, 12) * _tmp108 + P(2, 12) * _tmp105 + P(4, 12); + const Scalar _tmp128 = P(0, 4) * _tmp113 - P(12, 4) * _tmp110 - P(13, 4) * _tmp102 - + P(14, 4) * _tmp108 + P(2, 4) * _tmp105 + P(4, 4); + const Scalar _tmp129 = _tmp100 + _tmp55; + const Scalar _tmp130 = _tmp129 * dt; + const Scalar _tmp131 = + dt * (_tmp67 * (_tmp112 + _tmp69) + _tmp72 * (_tmp111 + _tmp65) + _tmp74 * (_tmp21 + _tmp76)); + const Scalar _tmp132 = _tmp73 * dt; + const Scalar _tmp133 = _tmp66 * dt; + const Scalar _tmp134 = + dt * (_tmp107 * _tmp72 + _tmp109 * _tmp67 + _tmp74 * (_tmp103 + _tmp17 + _tmp68)); + const Scalar _tmp135 = P(0, 5) * _tmp18 + P(10, 5) * _tmp23 + P(11, 5) * _tmp6 + P(9, 5) * _tmp15; + const Scalar _tmp136 = + P(1, 5) * _tmp18 + P(10, 5) * _tmp29 + P(11, 5) * _tmp34 + P(9, 5) * _tmp36; + const Scalar _tmp137 = + P(10, 5) * _tmp45 + P(11, 5) * _tmp44 + P(2, 5) * _tmp18 + P(9, 5) * _tmp47; + const Scalar _tmp138 = P(1, 5) * _tmp75 - P(12, 5) * _tmp57 - P(13, 5) * _tmp61 - + P(14, 5) * _tmp64 + P(2, 5) * _tmp79 + P(3, 5); + const Scalar _tmp139 = P(0, 5) * _tmp113 - P(12, 5) * _tmp110 - P(13, 5) * _tmp102 - + P(14, 5) * _tmp108 + P(2, 5) * _tmp105 + P(4, 5); + const Scalar _tmp140 = P(0, 14) * _tmp134 + P(1, 14) * _tmp131 - P(12, 14) * _tmp133 - + P(13, 14) * _tmp132 - P(14, 14) * _tmp130 + P(5, 14); + const Scalar _tmp141 = P(0, 13) * _tmp134 + P(1, 13) * _tmp131 - P(12, 13) * _tmp133 - + P(13, 13) * _tmp132 - P(14, 13) * _tmp130 + P(5, 13); + const Scalar _tmp142 = P(0, 12) * _tmp134 + P(1, 12) * _tmp131 - P(12, 12) * _tmp133 - + P(13, 12) * _tmp132 - P(14, 12) * _tmp130 + P(5, 12); + const Scalar _tmp143 = P(0, 5) * _tmp134 + P(1, 5) * _tmp131 - P(12, 5) * _tmp133 - + P(13, 5) * _tmp132 - P(14, 5) * _tmp130 + P(5, 5); // Output terms (1) - matrix::Matrix _res; + matrix::Matrix _res; + + _res.setZero(); - _res(0, 0) = _tmp10 + _tmp11 + _tmp2 * _tmp6 + _tmp5 * _tmp7 - _tmp8 * dt; - _res(1, 0) = 0; - _res(2, 0) = 0; - _res(3, 0) = 0; - _res(4, 0) = 0; - _res(5, 0) = 0; - _res(6, 0) = 0; - _res(7, 0) = 0; - _res(8, 0) = 0; - _res(9, 0) = 0; - _res(10, 0) = 0; - _res(11, 0) = 0; - _res(12, 0) = 0; - _res(13, 0) = 0; - _res(14, 0) = 0; - _res(15, 0) = 0; - _res(16, 0) = 0; - _res(17, 0) = 0; - _res(18, 0) = 0; - _res(19, 0) = 0; - _res(20, 0) = 0; - _res(21, 0) = 0; - _res(22, 0) = 0; - _res(0, 1) = _tmp11 * _tmp13 - _tmp12 * dt + _tmp16 * _tmp6 + _tmp7; - _res(1, 1) = _tmp10 + _tmp13 * _tmp18 + _tmp16 * _tmp19 - _tmp17 * dt + _tmp20; - _res(2, 1) = 0; - _res(3, 1) = 0; - _res(4, 1) = 0; - _res(5, 1) = 0; - _res(6, 1) = 0; - _res(7, 1) = 0; - _res(8, 1) = 0; - _res(9, 1) = 0; - _res(10, 1) = 0; - _res(11, 1) = 0; - _res(12, 1) = 0; - _res(13, 1) = 0; - _res(14, 1) = 0; - _res(15, 1) = 0; - _res(16, 1) = 0; - _res(17, 1) = 0; - _res(18, 1) = 0; - _res(19, 1) = 0; - _res(20, 1) = 0; - _res(21, 1) = 0; - _res(22, 1) = 0; - _res(0, 2) = _tmp11 * _tmp22 - _tmp21 * dt + _tmp23 * _tmp7 + _tmp6; - _res(1, 2) = _tmp18 * _tmp22 + _tmp19 + _tmp20 * _tmp23 - _tmp24 * dt; - _res(2, 2) = _tmp10 + _tmp22 * _tmp27 + _tmp23 * _tmp26 - _tmp25 * dt + _tmp28; - _res(3, 2) = 0; - _res(4, 2) = 0; - _res(5, 2) = 0; - _res(6, 2) = 0; - _res(7, 2) = 0; - _res(8, 2) = 0; - _res(9, 2) = 0; - _res(10, 2) = 0; - _res(11, 2) = 0; - _res(12, 2) = 0; - _res(13, 2) = 0; - _res(14, 2) = 0; - _res(15, 2) = 0; - _res(16, 2) = 0; - _res(17, 2) = 0; - _res(18, 2) = 0; - _res(19, 2) = 0; - _res(20, 2) = 0; - _res(21, 2) = 0; - _res(22, 2) = 0; - _res(0, 3) = _tmp11 * _tmp63 + _tmp45 * _tmp6 - _tmp46 * _tmp50 + _tmp60 * _tmp7 - - _tmp64 * _tmp65 - _tmp66 * _tmp67 + _tmp68; - _res(1, 3) = _tmp18 * _tmp63 + _tmp19 * _tmp45 + _tmp20 * _tmp60 - _tmp50 * _tmp71 - - _tmp65 * _tmp69 - _tmp67 * _tmp70 + _tmp72; - _res(2, 3) = _tmp26 * _tmp60 + _tmp27 * _tmp63 + _tmp28 * _tmp45 - _tmp50 * _tmp75 - - _tmp65 * _tmp74 - _tmp67 * _tmp73 + _tmp76; - _res(3, 3) = std::pow(_tmp34, Scalar(2)) * _tmp83 + _tmp45 * _tmp82 + - std::pow(_tmp49, Scalar(2)) * _tmp81 - _tmp50 * _tmp85 + _tmp60 * _tmp77 + - std::pow(_tmp61, Scalar(2)) * _tmp84 + _tmp63 * _tmp80 - _tmp65 * _tmp79 - - _tmp67 * _tmp78 + _tmp86; - _res(4, 3) = 0; - _res(5, 3) = 0; - _res(6, 3) = 0; - _res(7, 3) = 0; - _res(8, 3) = 0; - _res(9, 3) = 0; - _res(10, 3) = 0; - _res(11, 3) = 0; - _res(12, 3) = 0; - _res(13, 3) = 0; - _res(14, 3) = 0; - _res(15, 3) = 0; - _res(16, 3) = 0; - _res(17, 3) = 0; - _res(18, 3) = 0; - _res(19, 3) = 0; - _res(20, 3) = 0; - _res(21, 3) = 0; - _res(22, 3) = 0; - _res(0, 4) = -_tmp100 * _tmp46 + _tmp101 + _tmp11 * _tmp91 + _tmp6 * _tmp92 - _tmp64 * _tmp95 - - _tmp66 * _tmp96 + _tmp7 * _tmp99; - _res(1, 4) = -_tmp100 * _tmp71 + _tmp102 + _tmp18 * _tmp91 + _tmp19 * _tmp92 + _tmp20 * _tmp99 - - _tmp69 * _tmp95 - _tmp70 * _tmp96; - _res(2, 4) = -_tmp100 * _tmp75 - _tmp103 * _tmp94 + _tmp104 + _tmp26 * _tmp99 + _tmp27 * _tmp91 + - _tmp28 * _tmp92 - _tmp73 * _tmp96; - _res(3, 4) = -_tmp100 * _tmp85 + _tmp105 * _tmp49 + _tmp106 * _tmp61 + _tmp107 + - _tmp34 * _tmp83 * _tmp94 + _tmp77 * _tmp99 - _tmp78 * _tmp96 - _tmp79 * _tmp95 + - _tmp80 * _tmp91 + _tmp82 * _tmp92; - _res(4, 4) = -_tmp100 * _tmp111 - _tmp108 * _tmp96 + _tmp109 * _tmp91 - _tmp110 * _tmp95 + - _tmp112 * _tmp99 + _tmp113 * _tmp92 + _tmp114 + - _tmp81 * std::pow(_tmp97, Scalar(2)) + _tmp83 * std::pow(_tmp94, Scalar(2)) + - _tmp84 * std::pow(_tmp90, Scalar(2)); - _res(5, 4) = 0; - _res(6, 4) = 0; - _res(7, 4) = 0; - _res(8, 4) = 0; - _res(9, 4) = 0; - _res(10, 4) = 0; - _res(11, 4) = 0; - _res(12, 4) = 0; - _res(13, 4) = 0; - _res(14, 4) = 0; - _res(15, 4) = 0; - _res(16, 4) = 0; - _res(17, 4) = 0; - _res(18, 4) = 0; - _res(19, 4) = 0; - _res(20, 4) = 0; - _res(21, 4) = 0; - _res(22, 4) = 0; - _res(0, 5) = _tmp11 * _tmp115 - _tmp117 * _tmp66 - _tmp119 * _tmp64 + _tmp120 * _tmp6 + - _tmp122 * _tmp7 - _tmp123 * _tmp46 + _tmp124; - _res(1, 5) = _tmp115 * _tmp18 - _tmp117 * _tmp70 - _tmp119 * _tmp69 + _tmp120 * _tmp19 + - _tmp122 * _tmp20 - _tmp123 * _tmp71 + _tmp125; - _res(2, 5) = -_tmp103 * _tmp118 + _tmp115 * _tmp27 - _tmp117 * _tmp73 + _tmp120 * _tmp28 + - _tmp122 * _tmp26 - _tmp123 * _tmp75 + _tmp126; - _res(3, 5) = _tmp115 * _tmp80 + _tmp116 * _tmp61 * _tmp84 - _tmp117 * _tmp78 - _tmp119 * _tmp79 + - _tmp120 * _tmp82 + _tmp121 * _tmp49 * _tmp81 + _tmp122 * _tmp77 - _tmp123 * _tmp85 + - _tmp127 * _tmp34 + _tmp128; - _res(4, 5) = _tmp105 * _tmp121 + _tmp106 * _tmp116 - _tmp108 * _tmp117 + _tmp109 * _tmp115 - - _tmp110 * _tmp119 - _tmp111 * _tmp123 + _tmp112 * _tmp122 + _tmp113 * _tmp120 + - _tmp127 * _tmp94 + _tmp129; - _res(5, 5) = _tmp115 * (P(0, 0) * _tmp115 + P(1, 0) * _tmp122 - P(12, 0) * _tmp123 - - P(13, 0) * _tmp119 - P(14, 0) * _tmp117 + P(2, 0) * _tmp120 + P(5, 0)) + - std::pow(_tmp116, Scalar(2)) * _tmp84 - _tmp117 * _tmp131 + - std::pow(_tmp118, Scalar(2)) * _tmp83 - _tmp119 * _tmp130 + - _tmp120 * (P(0, 2) * _tmp115 + P(1, 2) * _tmp122 - P(12, 2) * _tmp123 - - P(13, 2) * _tmp119 - P(14, 2) * _tmp117 + P(2, 2) * _tmp120 + P(5, 2)) + - std::pow(_tmp121, Scalar(2)) * _tmp81 + - _tmp122 * (P(0, 1) * _tmp115 + P(1, 1) * _tmp122 - P(12, 1) * _tmp123 - - P(13, 1) * _tmp119 - P(14, 1) * _tmp117 + P(2, 1) * _tmp120 + P(5, 1)) - - _tmp123 * _tmp132 + _tmp133; - _res(6, 5) = 0; - _res(7, 5) = 0; - _res(8, 5) = 0; - _res(9, 5) = 0; - _res(10, 5) = 0; - _res(11, 5) = 0; - _res(12, 5) = 0; - _res(13, 5) = 0; - _res(14, 5) = 0; - _res(15, 5) = 0; - _res(16, 5) = 0; - _res(17, 5) = 0; - _res(18, 5) = 0; - _res(19, 5) = 0; - _res(20, 5) = 0; - _res(21, 5) = 0; - _res(22, 5) = 0; - _res(0, 6) = P(0, 6) + P(1, 6) * _tmp5 + P(2, 6) * _tmp2 - P(9, 6) * dt + _tmp68 * dt; - _res(1, 6) = P(0, 6) * _tmp13 + P(1, 6) - P(10, 6) * dt + P(2, 6) * _tmp16 + _tmp72 * dt; - _res(2, 6) = P(0, 6) * _tmp22 + P(1, 6) * _tmp23 - P(11, 6) * dt + P(2, 6) + _tmp76 * dt; - _res(3, 6) = P(0, 6) * _tmp63 + P(1, 6) * _tmp60 - P(12, 6) * _tmp50 - P(13, 6) * _tmp65 - - P(14, 6) * _tmp67 + P(2, 6) * _tmp45 + P(3, 6) + _tmp86 * dt; - _res(4, 6) = P(0, 6) * _tmp91 + P(1, 6) * _tmp99 - P(12, 6) * _tmp100 - P(13, 6) * _tmp95 - - P(14, 6) * _tmp96 + P(2, 6) * _tmp92 + P(4, 6) + - dt * (P(0, 3) * _tmp91 + P(1, 3) * _tmp99 - P(12, 3) * _tmp100 - P(13, 3) * _tmp95 - - P(14, 3) * _tmp96 + P(2, 3) * _tmp92 + P(4, 3)); - _res(5, 6) = P(0, 6) * _tmp115 + P(1, 6) * _tmp122 - P(12, 6) * _tmp123 - P(13, 6) * _tmp119 - - P(14, 6) * _tmp117 + P(2, 6) * _tmp120 + P(5, 6) + - dt * (P(0, 3) * _tmp115 + P(1, 3) * _tmp122 - P(12, 3) * _tmp123 - - P(13, 3) * _tmp119 - P(14, 3) * _tmp117 + P(2, 3) * _tmp120 + P(5, 3)); + _res(0, 0) = std::pow(_tmp15, Scalar(2)) * gyro_var + _tmp15 * _tmp27 + _tmp18 * _tmp26 + + std::pow(_tmp23, Scalar(2)) * gyro_var + _tmp23 * _tmp25 + _tmp24 * _tmp6 + + std::pow(_tmp6, Scalar(2)) * gyro_var; + _res(0, 1) = _tmp15 * _tmp38 + _tmp18 * _tmp35 + _tmp24 * _tmp34 + _tmp25 * _tmp29 + + _tmp27 * _tmp36 + _tmp29 * _tmp37 + _tmp34 * _tmp39; + _res(1, 1) = _tmp18 * _tmp43 + std::pow(_tmp29, Scalar(2)) * gyro_var + _tmp29 * _tmp40 + + std::pow(_tmp34, Scalar(2)) * gyro_var + _tmp34 * _tmp41 + + std::pow(_tmp36, Scalar(2)) * gyro_var + _tmp36 * _tmp42; + _res(0, 2) = _tmp15 * _tmp47 * gyro_var + _tmp18 * _tmp46 + _tmp24 * _tmp44 + _tmp25 * _tmp45 + + _tmp27 * _tmp47 + _tmp37 * _tmp45 + _tmp39 * _tmp44; + _res(1, 2) = _tmp18 * _tmp48 + _tmp29 * _tmp45 * gyro_var + _tmp34 * _tmp44 * gyro_var + + _tmp38 * _tmp47 + _tmp40 * _tmp45 + _tmp41 * _tmp44 + _tmp42 * _tmp47; + _res(2, 2) = _tmp18 * _tmp51 + std::pow(_tmp44, Scalar(2)) * gyro_var + _tmp44 * _tmp50 + + std::pow(_tmp45, Scalar(2)) * gyro_var + _tmp45 * _tmp49 + + std::pow(_tmp47, Scalar(2)) * gyro_var + _tmp47 * _tmp52; + _res(0, 3) = _tmp35 * _tmp75 + _tmp46 * _tmp79 - _tmp53 * _tmp57 - _tmp58 * _tmp61 - + _tmp62 * _tmp64 + _tmp80; + _res(1, 3) = _tmp43 * _tmp75 + _tmp48 * _tmp79 - _tmp57 * _tmp81 - _tmp61 * _tmp82 - + _tmp64 * _tmp83 + _tmp84; + _res(2, 3) = _tmp51 * _tmp79 - _tmp57 * _tmp85 - _tmp61 * _tmp87 - _tmp64 * _tmp88 + + _tmp75 * _tmp86 + _tmp89; + _res(3, 3) = std::pow(_tmp56, Scalar(2)) * _tmp96 - _tmp57 * _tmp90 + + std::pow(_tmp60, Scalar(2)) * _tmp97 - _tmp61 * _tmp93 + + std::pow(_tmp63, Scalar(2)) * _tmp98 - _tmp64 * _tmp94 + _tmp75 * _tmp91 + + _tmp79 * _tmp92 + _tmp99; + _res(0, 4) = -_tmp102 * _tmp58 + _tmp105 * _tmp46 - _tmp108 * _tmp62 - _tmp110 * _tmp53 + + _tmp113 * _tmp26 + _tmp114; + _res(1, 4) = -_tmp102 * _tmp82 + _tmp105 * _tmp48 - _tmp108 * _tmp83 - _tmp110 * _tmp81 + + _tmp113 * _tmp115 + _tmp116; + _res(2, 4) = -_tmp102 * _tmp87 + _tmp105 * _tmp51 - _tmp108 * _tmp88 - _tmp110 * _tmp85 + + _tmp113 * _tmp117 + _tmp118; + _res(3, 4) = _tmp101 * _tmp120 - _tmp102 * _tmp93 + _tmp105 * _tmp92 + _tmp107 * _tmp122 - + _tmp108 * _tmp94 + _tmp109 * _tmp119 - _tmp110 * _tmp90 + _tmp113 * _tmp121 + + _tmp123; + _res(4, 4) = std::pow(_tmp101, Scalar(2)) * _tmp97 - _tmp102 * _tmp125 + + _tmp105 * (P(0, 2) * _tmp113 - P(12, 2) * _tmp110 - P(13, 2) * _tmp102 - + P(14, 2) * _tmp108 + P(2, 2) * _tmp105 + P(4, 2)) + + std::pow(_tmp107, Scalar(2)) * _tmp98 - _tmp108 * _tmp126 + + std::pow(_tmp109, Scalar(2)) * _tmp96 - _tmp110 * _tmp127 + _tmp113 * _tmp124 + + _tmp128; + _res(0, 5) = -_tmp130 * _tmp62 + _tmp131 * _tmp35 - _tmp132 * _tmp58 - _tmp133 * _tmp53 + + _tmp134 * _tmp26 + _tmp135; + _res(1, 5) = _tmp115 * _tmp134 - _tmp130 * _tmp83 + _tmp131 * _tmp43 - _tmp132 * _tmp82 - + _tmp133 * _tmp81 + _tmp136; + _res(2, 5) = _tmp117 * _tmp134 - _tmp130 * _tmp88 + _tmp131 * _tmp86 - _tmp132 * _tmp87 - + _tmp133 * _tmp85 + _tmp137; + _res(3, 5) = _tmp119 * _tmp66 + _tmp120 * _tmp73 + _tmp121 * _tmp134 + _tmp122 * _tmp129 - + _tmp130 * _tmp94 + _tmp131 * _tmp91 - _tmp132 * _tmp93 - _tmp133 * _tmp90 + _tmp138; + _res(4, 5) = _tmp101 * _tmp73 * _tmp97 + _tmp107 * _tmp129 * _tmp98 + _tmp109 * _tmp66 * _tmp96 + + _tmp124 * _tmp134 - _tmp125 * _tmp132 - _tmp126 * _tmp130 - _tmp127 * _tmp133 + + _tmp131 * (P(0, 1) * _tmp113 - P(12, 1) * _tmp110 - P(13, 1) * _tmp102 - + P(14, 1) * _tmp108 + P(2, 1) * _tmp105 + P(4, 1)) + + _tmp139; + _res(5, 5) = std::pow(_tmp129, Scalar(2)) * _tmp98 - _tmp130 * _tmp140 + + _tmp131 * (P(0, 1) * _tmp134 + P(1, 1) * _tmp131 - P(12, 1) * _tmp133 - + P(13, 1) * _tmp132 - P(14, 1) * _tmp130 + P(5, 1)) - + _tmp132 * _tmp141 - _tmp133 * _tmp142 + + _tmp134 * (P(0, 0) * _tmp134 + P(1, 0) * _tmp131 - P(12, 0) * _tmp133 - + P(13, 0) * _tmp132 - P(14, 0) * _tmp130 + P(5, 0)) + + _tmp143 + std::pow(_tmp66, Scalar(2)) * _tmp96 + + std::pow(_tmp73, Scalar(2)) * _tmp97; + _res(0, 6) = + P(0, 6) * _tmp18 + P(10, 6) * _tmp23 + P(11, 6) * _tmp6 + P(9, 6) * _tmp15 + _tmp80 * dt; + _res(1, 6) = + P(1, 6) * _tmp18 + P(10, 6) * _tmp29 + P(11, 6) * _tmp34 + P(9, 6) * _tmp36 + _tmp84 * dt; + _res(2, 6) = + P(10, 6) * _tmp45 + P(11, 6) * _tmp44 + P(2, 6) * _tmp18 + P(9, 6) * _tmp47 + _tmp89 * dt; + _res(3, 6) = P(1, 6) * _tmp75 - P(12, 6) * _tmp57 - P(13, 6) * _tmp61 - P(14, 6) * _tmp64 + + P(2, 6) * _tmp79 + P(3, 6) + _tmp99 * dt; + _res(4, 6) = P(0, 6) * _tmp113 - P(12, 6) * _tmp110 - P(13, 6) * _tmp102 - P(14, 6) * _tmp108 + + P(2, 6) * _tmp105 + P(4, 6) + + dt * (P(0, 3) * _tmp113 - P(12, 3) * _tmp110 - P(13, 3) * _tmp102 - + P(14, 3) * _tmp108 + P(2, 3) * _tmp105 + P(4, 3)); + _res(5, 6) = P(0, 6) * _tmp134 + P(1, 6) * _tmp131 - P(12, 6) * _tmp133 - P(13, 6) * _tmp132 - + P(14, 6) * _tmp130 + P(5, 6) + + dt * (P(0, 3) * _tmp134 + P(1, 3) * _tmp131 - P(12, 3) * _tmp133 - + P(13, 3) * _tmp132 - P(14, 3) * _tmp130 + P(5, 3)); _res(6, 6) = P(3, 6) * dt + P(6, 6) + dt * (P(3, 3) * dt + P(6, 3)); - _res(7, 6) = 0; - _res(8, 6) = 0; - _res(9, 6) = 0; - _res(10, 6) = 0; - _res(11, 6) = 0; - _res(12, 6) = 0; - _res(13, 6) = 0; - _res(14, 6) = 0; - _res(15, 6) = 0; - _res(16, 6) = 0; - _res(17, 6) = 0; - _res(18, 6) = 0; - _res(19, 6) = 0; - _res(20, 6) = 0; - _res(21, 6) = 0; - _res(22, 6) = 0; - _res(0, 7) = P(0, 7) + P(1, 7) * _tmp5 + P(2, 7) * _tmp2 - P(9, 7) * dt + _tmp101 * dt; - _res(1, 7) = P(0, 7) * _tmp13 + P(1, 7) - P(10, 7) * dt + P(2, 7) * _tmp16 + _tmp102 * dt; - _res(2, 7) = P(0, 7) * _tmp22 + P(1, 7) * _tmp23 - P(11, 7) * dt + P(2, 7) + _tmp104 * dt; - _res(3, 7) = P(0, 7) * _tmp63 + P(1, 7) * _tmp60 - P(12, 7) * _tmp50 - P(13, 7) * _tmp65 - - P(14, 7) * _tmp67 + P(2, 7) * _tmp45 + P(3, 7) + _tmp107 * dt; - _res(4, 7) = P(0, 7) * _tmp91 + P(1, 7) * _tmp99 - P(12, 7) * _tmp100 - P(13, 7) * _tmp95 - - P(14, 7) * _tmp96 + P(2, 7) * _tmp92 + P(4, 7) + _tmp114 * dt; - _res(5, 7) = P(0, 7) * _tmp115 + P(1, 7) * _tmp122 - P(12, 7) * _tmp123 - P(13, 7) * _tmp119 - - P(14, 7) * _tmp117 + P(2, 7) * _tmp120 + P(5, 7) + - dt * (P(0, 4) * _tmp115 + P(1, 4) * _tmp122 - P(12, 4) * _tmp123 - - P(13, 4) * _tmp119 - P(14, 4) * _tmp117 + P(2, 4) * _tmp120 + P(5, 4)); + _res(0, 7) = + P(0, 7) * _tmp18 + P(10, 7) * _tmp23 + P(11, 7) * _tmp6 + P(9, 7) * _tmp15 + _tmp114 * dt; + _res(1, 7) = + P(1, 7) * _tmp18 + P(10, 7) * _tmp29 + P(11, 7) * _tmp34 + P(9, 7) * _tmp36 + _tmp116 * dt; + _res(2, 7) = + P(10, 7) * _tmp45 + P(11, 7) * _tmp44 + P(2, 7) * _tmp18 + P(9, 7) * _tmp47 + _tmp118 * dt; + _res(3, 7) = P(1, 7) * _tmp75 - P(12, 7) * _tmp57 - P(13, 7) * _tmp61 - P(14, 7) * _tmp64 + + P(2, 7) * _tmp79 + P(3, 7) + _tmp123 * dt; + _res(4, 7) = P(0, 7) * _tmp113 - P(12, 7) * _tmp110 - P(13, 7) * _tmp102 - P(14, 7) * _tmp108 + + P(2, 7) * _tmp105 + P(4, 7) + _tmp128 * dt; + _res(5, 7) = P(0, 7) * _tmp134 + P(1, 7) * _tmp131 - P(12, 7) * _tmp133 - P(13, 7) * _tmp132 - + P(14, 7) * _tmp130 + P(5, 7) + + dt * (P(0, 4) * _tmp134 + P(1, 4) * _tmp131 - P(12, 4) * _tmp133 - + P(13, 4) * _tmp132 - P(14, 4) * _tmp130 + P(5, 4)); _res(6, 7) = P(3, 7) * dt + P(6, 7) + dt * (P(3, 4) * dt + P(6, 4)); _res(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); - _res(8, 7) = 0; - _res(9, 7) = 0; - _res(10, 7) = 0; - _res(11, 7) = 0; - _res(12, 7) = 0; - _res(13, 7) = 0; - _res(14, 7) = 0; - _res(15, 7) = 0; - _res(16, 7) = 0; - _res(17, 7) = 0; - _res(18, 7) = 0; - _res(19, 7) = 0; - _res(20, 7) = 0; - _res(21, 7) = 0; - _res(22, 7) = 0; - _res(0, 8) = P(0, 8) + P(1, 8) * _tmp5 + P(2, 8) * _tmp2 - P(9, 8) * dt + _tmp124 * dt; - _res(1, 8) = P(0, 8) * _tmp13 + P(1, 8) - P(10, 8) * dt + P(2, 8) * _tmp16 + _tmp125 * dt; - _res(2, 8) = P(0, 8) * _tmp22 + P(1, 8) * _tmp23 - P(11, 8) * dt + P(2, 8) + _tmp126 * dt; - _res(3, 8) = P(0, 8) * _tmp63 + P(1, 8) * _tmp60 - P(12, 8) * _tmp50 - P(13, 8) * _tmp65 - - P(14, 8) * _tmp67 + P(2, 8) * _tmp45 + P(3, 8) + _tmp128 * dt; - _res(4, 8) = P(0, 8) * _tmp91 + P(1, 8) * _tmp99 - P(12, 8) * _tmp100 - P(13, 8) * _tmp95 - - P(14, 8) * _tmp96 + P(2, 8) * _tmp92 + P(4, 8) + _tmp129 * dt; - _res(5, 8) = P(0, 8) * _tmp115 + P(1, 8) * _tmp122 - P(12, 8) * _tmp123 - P(13, 8) * _tmp119 - - P(14, 8) * _tmp117 + P(2, 8) * _tmp120 + P(5, 8) + _tmp133 * dt; + _res(0, 8) = + P(0, 8) * _tmp18 + P(10, 8) * _tmp23 + P(11, 8) * _tmp6 + P(9, 8) * _tmp15 + _tmp135 * dt; + _res(1, 8) = + P(1, 8) * _tmp18 + P(10, 8) * _tmp29 + P(11, 8) * _tmp34 + P(9, 8) * _tmp36 + _tmp136 * dt; + _res(2, 8) = + P(10, 8) * _tmp45 + P(11, 8) * _tmp44 + P(2, 8) * _tmp18 + P(9, 8) * _tmp47 + _tmp137 * dt; + _res(3, 8) = P(1, 8) * _tmp75 - P(12, 8) * _tmp57 - P(13, 8) * _tmp61 - P(14, 8) * _tmp64 + + P(2, 8) * _tmp79 + P(3, 8) + _tmp138 * dt; + _res(4, 8) = P(0, 8) * _tmp113 - P(12, 8) * _tmp110 - P(13, 8) * _tmp102 - P(14, 8) * _tmp108 + + P(2, 8) * _tmp105 + P(4, 8) + _tmp139 * dt; + _res(5, 8) = P(0, 8) * _tmp134 + P(1, 8) * _tmp131 - P(12, 8) * _tmp133 - P(13, 8) * _tmp132 - + P(14, 8) * _tmp130 + P(5, 8) + _tmp143 * dt; _res(6, 8) = P(3, 8) * dt + P(6, 8) + dt * (P(3, 5) * dt + P(6, 5)); _res(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); _res(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); - _res(9, 8) = 0; - _res(10, 8) = 0; - _res(11, 8) = 0; - _res(12, 8) = 0; - _res(13, 8) = 0; - _res(14, 8) = 0; - _res(15, 8) = 0; - _res(16, 8) = 0; - _res(17, 8) = 0; - _res(18, 8) = 0; - _res(19, 8) = 0; - _res(20, 8) = 0; - _res(21, 8) = 0; - _res(22, 8) = 0; - _res(0, 9) = _tmp8; - _res(1, 9) = P(0, 9) * _tmp13 + P(1, 9) - P(10, 9) * dt + P(2, 9) * _tmp16; - _res(2, 9) = P(0, 9) * _tmp22 + P(1, 9) * _tmp23 - P(11, 9) * dt + P(2, 9); - _res(3, 9) = P(0, 9) * _tmp63 + P(1, 9) * _tmp60 - P(12, 9) * _tmp50 - P(13, 9) * _tmp65 - - P(14, 9) * _tmp67 + P(2, 9) * _tmp45 + P(3, 9); - _res(4, 9) = P(0, 9) * _tmp91 + P(1, 9) * _tmp99 - P(12, 9) * _tmp100 - P(13, 9) * _tmp95 - - P(14, 9) * _tmp96 + P(2, 9) * _tmp92 + P(4, 9); - _res(5, 9) = P(0, 9) * _tmp115 + P(1, 9) * _tmp122 - P(12, 9) * _tmp123 - P(13, 9) * _tmp119 - - P(14, 9) * _tmp117 + P(2, 9) * _tmp120 + P(5, 9); + _res(0, 9) = _tmp27; + _res(1, 9) = _tmp42; + _res(2, 9) = _tmp52; + _res(3, 9) = P(1, 9) * _tmp75 - P(12, 9) * _tmp57 - P(13, 9) * _tmp61 - P(14, 9) * _tmp64 + + P(2, 9) * _tmp79 + P(3, 9); + _res(4, 9) = P(0, 9) * _tmp113 - P(12, 9) * _tmp110 - P(13, 9) * _tmp102 - P(14, 9) * _tmp108 + + P(2, 9) * _tmp105 + P(4, 9); + _res(5, 9) = P(0, 9) * _tmp134 + P(1, 9) * _tmp131 - P(12, 9) * _tmp133 - P(13, 9) * _tmp132 - + P(14, 9) * _tmp130 + P(5, 9); _res(6, 9) = P(3, 9) * dt + P(6, 9); _res(7, 9) = P(4, 9) * dt + P(7, 9); _res(8, 9) = P(5, 9) * dt + P(8, 9); _res(9, 9) = P(9, 9); - _res(10, 9) = 0; - _res(11, 9) = 0; - _res(12, 9) = 0; - _res(13, 9) = 0; - _res(14, 9) = 0; - _res(15, 9) = 0; - _res(16, 9) = 0; - _res(17, 9) = 0; - _res(18, 9) = 0; - _res(19, 9) = 0; - _res(20, 9) = 0; - _res(21, 9) = 0; - _res(22, 9) = 0; - _res(0, 10) = _tmp12; - _res(1, 10) = _tmp17; - _res(2, 10) = P(0, 10) * _tmp22 + P(1, 10) * _tmp23 - P(11, 10) * dt + P(2, 10); - _res(3, 10) = P(0, 10) * _tmp63 + P(1, 10) * _tmp60 - P(12, 10) * _tmp50 - P(13, 10) * _tmp65 - - P(14, 10) * _tmp67 + P(2, 10) * _tmp45 + P(3, 10); - _res(4, 10) = P(0, 10) * _tmp91 + P(1, 10) * _tmp99 - P(12, 10) * _tmp100 - P(13, 10) * _tmp95 - - P(14, 10) * _tmp96 + P(2, 10) * _tmp92 + P(4, 10); - _res(5, 10) = P(0, 10) * _tmp115 + P(1, 10) * _tmp122 - P(12, 10) * _tmp123 - - P(13, 10) * _tmp119 - P(14, 10) * _tmp117 + P(2, 10) * _tmp120 + P(5, 10); + _res(0, 10) = _tmp25; + _res(1, 10) = _tmp40; + _res(2, 10) = _tmp49; + _res(3, 10) = P(1, 10) * _tmp75 - P(12, 10) * _tmp57 - P(13, 10) * _tmp61 - P(14, 10) * _tmp64 + + P(2, 10) * _tmp79 + P(3, 10); + _res(4, 10) = P(0, 10) * _tmp113 - P(12, 10) * _tmp110 - P(13, 10) * _tmp102 - + P(14, 10) * _tmp108 + P(2, 10) * _tmp105 + P(4, 10); + _res(5, 10) = P(0, 10) * _tmp134 + P(1, 10) * _tmp131 - P(12, 10) * _tmp133 - + P(13, 10) * _tmp132 - P(14, 10) * _tmp130 + P(5, 10); _res(6, 10) = P(3, 10) * dt + P(6, 10); _res(7, 10) = P(4, 10) * dt + P(7, 10); _res(8, 10) = P(5, 10) * dt + P(8, 10); _res(9, 10) = P(9, 10); _res(10, 10) = P(10, 10); - _res(11, 10) = 0; - _res(12, 10) = 0; - _res(13, 10) = 0; - _res(14, 10) = 0; - _res(15, 10) = 0; - _res(16, 10) = 0; - _res(17, 10) = 0; - _res(18, 10) = 0; - _res(19, 10) = 0; - _res(20, 10) = 0; - _res(21, 10) = 0; - _res(22, 10) = 0; - _res(0, 11) = _tmp21; - _res(1, 11) = _tmp24; - _res(2, 11) = _tmp25; - _res(3, 11) = P(0, 11) * _tmp63 + P(1, 11) * _tmp60 - P(12, 11) * _tmp50 - P(13, 11) * _tmp65 - - P(14, 11) * _tmp67 + P(2, 11) * _tmp45 + P(3, 11); - _res(4, 11) = P(0, 11) * _tmp91 + P(1, 11) * _tmp99 - P(12, 11) * _tmp100 - P(13, 11) * _tmp95 - - P(14, 11) * _tmp96 + P(2, 11) * _tmp92 + P(4, 11); - _res(5, 11) = P(0, 11) * _tmp115 + P(1, 11) * _tmp122 - P(12, 11) * _tmp123 - - P(13, 11) * _tmp119 - P(14, 11) * _tmp117 + P(2, 11) * _tmp120 + P(5, 11); + _res(0, 11) = _tmp24; + _res(1, 11) = _tmp41; + _res(2, 11) = _tmp50; + _res(3, 11) = P(1, 11) * _tmp75 - P(12, 11) * _tmp57 - P(13, 11) * _tmp61 - P(14, 11) * _tmp64 + + P(2, 11) * _tmp79 + P(3, 11); + _res(4, 11) = P(0, 11) * _tmp113 - P(12, 11) * _tmp110 - P(13, 11) * _tmp102 - + P(14, 11) * _tmp108 + P(2, 11) * _tmp105 + P(4, 11); + _res(5, 11) = P(0, 11) * _tmp134 + P(1, 11) * _tmp131 - P(12, 11) * _tmp133 - + P(13, 11) * _tmp132 - P(14, 11) * _tmp130 + P(5, 11); _res(6, 11) = P(3, 11) * dt + P(6, 11); _res(7, 11) = P(4, 11) * dt + P(7, 11); _res(8, 11) = P(5, 11) * dt + P(8, 11); _res(9, 11) = P(9, 11); _res(10, 11) = P(10, 11); _res(11, 11) = P(11, 11); - _res(12, 11) = 0; - _res(13, 11) = 0; - _res(14, 11) = 0; - _res(15, 11) = 0; - _res(16, 11) = 0; - _res(17, 11) = 0; - _res(18, 11) = 0; - _res(19, 11) = 0; - _res(20, 11) = 0; - _res(21, 11) = 0; - _res(22, 11) = 0; - _res(0, 12) = _tmp46; - _res(1, 12) = _tmp71; - _res(2, 12) = _tmp75; - _res(3, 12) = _tmp85; - _res(4, 12) = _tmp111; - _res(5, 12) = _tmp132; + _res(0, 12) = _tmp53; + _res(1, 12) = _tmp81; + _res(2, 12) = _tmp85; + _res(3, 12) = _tmp90; + _res(4, 12) = _tmp127; + _res(5, 12) = _tmp142; _res(6, 12) = P(3, 12) * dt + P(6, 12); _res(7, 12) = P(4, 12) * dt + P(7, 12); _res(8, 12) = P(5, 12) * dt + P(8, 12); @@ -542,22 +400,12 @@ matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix PredictCovariance(const matrix::Matrix mag_I{}; matrix::Vector3 mag_B{}; matrix::Vector2 wind_vel{}; + float terrain{}; - matrix::Vector Data() const { - matrix::Vector state; + matrix::Vector Data() const { + matrix::Vector state; state.slice<4, 1>(0, 0) = quat_nominal; state.slice<3, 1>(4, 0) = vel; state.slice<3, 1>(7, 0) = pos; @@ -29,15 +30,16 @@ struct StateSample { state.slice<3, 1>(16, 0) = mag_I; state.slice<3, 1>(19, 0) = mag_B; state.slice<2, 1>(22, 0) = wind_vel; + state.slice<1, 1>(24, 0) = terrain; return state; }; - const matrix::Vector& vector() const { - return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); + const matrix::Vector& vector() const { + return *reinterpret_cast*>(const_cast(reinterpret_cast(&quat_nominal))); }; }; -static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); +static_assert(sizeof(matrix::Vector) == sizeof(StateSample), "state vector doesn't match StateSample size"); struct IdxDof { unsigned idx; unsigned dof; }; namespace State { @@ -49,7 +51,8 @@ namespace State { static constexpr IdxDof mag_I{15, 3}; static constexpr IdxDof mag_B{18, 3}; static constexpr IdxDof wind_vel{21, 2}; - static constexpr uint8_t size{23}; + static constexpr IdxDof terrain{23, 1}; + static constexpr uint8_t size{24}; }; } #endif // !EKF_STATE_H diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h deleted file mode 100644 index 6b82a6534f74..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h +++ /dev/null @@ -1,66 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: terr_est_compute_flow_xy_innov_var_and_hx - * - * Args: - * terrain_vpos: Scalar - * P: Scalar - * q_att: Matrix41 - * v: Matrix31 - * pos_z: Scalar - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Matrix21 - * H: Scalar - */ -template -void TerrEstComputeFlowXyInnovVarAndHx(const Scalar terrain_vpos, const Scalar P, - const matrix::Matrix& q_att, - const matrix::Matrix& v, const Scalar pos_z, - const Scalar R, const Scalar epsilon, - matrix::Matrix* const innov_var = nullptr, - Scalar* const H = nullptr) { - // Total ops: 27 - - // Input arrays - - // Intermediate terms (4) - const Scalar _tmp0 = - -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; - const Scalar _tmp1 = pos_z - terrain_vpos; - const Scalar _tmp2 = - -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); - const Scalar _tmp3 = P * std::pow(_tmp0, Scalar(2)) / std::pow(_tmp2, Scalar(4)); - - // Output terms (2) - if (innov_var != nullptr) { - matrix::Matrix& _innov_var = (*innov_var); - - _innov_var(0, 0) = R + _tmp3 * std::pow(v(1, 0), Scalar(2)); - _innov_var(1, 0) = R + _tmp3 * std::pow(v(0, 0), Scalar(2)); - } - - if (H != nullptr) { - Scalar& _h = (*H); - - _h = _tmp0 * v(1, 0) / std::pow(_tmp2, Scalar(2)); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h deleted file mode 100644 index 461db323cfdd..000000000000 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h +++ /dev/null @@ -1,65 +0,0 @@ -// ----------------------------------------------------------------------------- -// This file was autogenerated by symforce from template: -// function/FUNCTION.h.jinja -// Do NOT modify by hand. -// ----------------------------------------------------------------------------- - -#pragma once - -#include - -namespace sym { - -/** - * This function was autogenerated from a symbolic function. Do not modify by hand. - * - * Symbolic function: terr_est_compute_flow_y_innov_var_and_h - * - * Args: - * terrain_vpos: Scalar - * P: Scalar - * q_att: Matrix41 - * v: Matrix31 - * pos_z: Scalar - * R: Scalar - * epsilon: Scalar - * - * Outputs: - * innov_var: Scalar - * H: Scalar - */ -template -void TerrEstComputeFlowYInnovVarAndH(const Scalar terrain_vpos, const Scalar P, - const matrix::Matrix& q_att, - const matrix::Matrix& v, const Scalar pos_z, - const Scalar R, const Scalar epsilon, - Scalar* const innov_var = nullptr, Scalar* const H = nullptr) { - // Total ops: 25 - - // Input arrays - - // Intermediate terms (3) - const Scalar _tmp0 = - -2 * std::pow(q_att(1, 0), Scalar(2)) - 2 * std::pow(q_att(2, 0), Scalar(2)) + 1; - const Scalar _tmp1 = pos_z - terrain_vpos; - const Scalar _tmp2 = - -_tmp1 + epsilon * (2 * math::min(0, -(((_tmp1) > 0) - ((_tmp1) < 0))) + 1); - - // Output terms (2) - if (innov_var != nullptr) { - Scalar& _innov_var = (*innov_var); - - _innov_var = - P * std::pow(_tmp0, Scalar(2)) * std::pow(v(0, 0), Scalar(2)) / std::pow(_tmp2, Scalar(4)) + - R; - } - - if (H != nullptr) { - Scalar& _h = (*H); - - _h = -_tmp0 * v(0, 0) / std::pow(_tmp2, Scalar(2)); - } -} // NOLINT(readability/fn_size) - -// NOLINTNEXTLINE(readability/fn_size) -} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/utils/__init__.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/__init__.py new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py similarity index 98% rename from src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py rename to src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py index b08ade69a47c..faf431da7167 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/utils/derivation_utils.py @@ -56,7 +56,7 @@ def generate_px4_function(function_name, output_names): codegen = Codegen.function( function_name, output_names=output_names, - config=CppConfig()) + config=CppConfig(zero_initialization_sparsity_threshold=1)) metadata = codegen.generate_function( output_dir="generated", skip_directory_nesting=True) diff --git a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py index 01fe5e758ca9..f30c7cf4a7f1 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/baro_static_pressure_compensation/baro_static_pressure_compensation_tuning.py @@ -150,7 +150,7 @@ def baroCorrection(x, v_body): return correction -def run(logfile): +def run(logfile, w_hpf): (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile) # x[0]: pcoef_xn / g @@ -162,8 +162,10 @@ def run(logfile): # Remove low ferquency part of the signal as we're only interested in the short-term errors baro_error -= baro_error[0] - sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos') - baro_error = sosfilt(sos, baro_error) + + if (w_hpf > 0): + sos = butter(4, w_hpf, 'hp', fs=1/(t[1]-t[0]), output='sos') + baro_error = sosfilt(sos, baro_error) J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function @@ -224,8 +226,10 @@ def run(logfile): # Provide parameter file path and name parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + parser.add_argument('--hpf', help='Cuttoff frequency of high-pass filter on baro error (Hz)', type=float, default=-1) args = parser.parse_args() logfile = os.path.abspath(args.logfile) # Convert to absolute path + w_hpf = 2 * np.pi * args.hpf - run(logfile) + run(logfile, w_hpf) diff --git a/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py new file mode 100644 index 000000000000..457c1eb8e9bd --- /dev/null +++ b/src/modules/ekf2/EKF/python/tuning_tools/gyro_integration.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +""" + Copyright (c) 2024 PX4 Development Team + 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. +""" + +import matplotlib.pylab as plt +from pyulog import ULog +from pyulog.px4 import PX4ULog +import numpy as np +import sym +import symforce.symbolic as sf + +def getAllData(logfile): + log = ULog(logfile) + + gyro = np.matrix([getData(log, 'sensor_combined', 'gyro_rad[0]'), + getData(log, 'sensor_combined', 'gyro_rad[1]'), + getData(log, 'sensor_combined', 'gyro_rad[2]')]) + t_gyro = getTimestampsSeconds(log, 'sensor_combined') + + t_gyro -= t_gyro[0] + + q = np.matrix([getData(log, 'vehicle_attitude', 'q[0]'), + getData(log, 'vehicle_attitude', 'q[1]'), + getData(log, 'vehicle_attitude', 'q[2]'), + getData(log, 'vehicle_attitude', 'q[3]')]) + t_q = getTimestampsSeconds(log, 'vehicle_attitude') + t_q -= t_q[0] + + return (t_gyro, gyro, t_q, q) + +def getData(log, topic_name, variable_name, instance=0): + variable_data = np.array([]) + for elem in log.data_list: + if elem.name == topic_name: + if instance == elem.multi_id: + variable_data = elem.data[variable_name] + break + + return variable_data + +def us2s(time_us): + return time_us * 1e-6 + +def getTimestampsSeconds(log, topic_name, instance=0): + return us2s(getData(log, topic_name, 'timestamp', instance)) + +def integrateAngularRate(t, angular_rate, rot_init=sym.Rot3()): + R = rot_init + roll = [] + pitch = [] + yaw = [] + t_prev = 0 + + for i in range(len(t)): + dt = t[i] - t_prev + R = R * sym.Rot3.from_tangent(angular_rate[:, i] * dt) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, att[0]) + pitch = np.append(pitch, att[1]) + roll = np.append(roll, att[2]) + + t_prev = t[i] + + return (roll, pitch, yaw) + +def quat2RollPitchYaw(t, q): + roll = [] + pitch = [] + yaw = [] + + for i in range(len(t)): + vect = sf.V3(float(q[1, i]), float(q[2, i]), float(q[3, i])) + quat = sf.Quaternion(w=q[0, i], xyz=vect) + R = sf.Rot3(quat) + att = R.to_yaw_pitch_roll() + yaw = np.append(yaw, float(att[0].evalf())) + pitch = np.append(pitch, float(att[1].evalf())) + roll = np.append(roll, float(att[2].evalf())) + + return (roll, pitch, yaw) + + +def run(logfile): + (t, gyro, t_q, q) = getAllData(logfile) + + (roll, pitch, yaw) = quat2RollPitchYaw(t_q, q) + (roll_raw, pitch_raw, yaw_raw) = integrateAngularRate(t, gyro, rot_init=sym.Rot3.from_yaw_pitch_roll(yaw[0], pitch[0], roll[0])) + + # Plot data + plt.figure(1) + plt.suptitle(logfile.split('/')[-1]) + + ax1 = plt.subplot(3, 1, 1) + ax1.plot(t_q, np.rad2deg(roll), '-') + ax1.plot(t, np.rad2deg(roll_raw), '--') + ax1.set_ylabel("roll (deg)") + ax1.legend(["estimated", "integrated"]) + ax1.grid() + + ax2 = plt.subplot(3, 1, 2, sharex=ax1) + ax2.plot(t_q, np.rad2deg(pitch)) + ax2.plot(t, np.rad2deg(pitch_raw), '--') + ax2.set_ylabel("pitch (deg)") + ax2.legend(["estimated", "integrated"]) + ax2.grid() + + ax3 = plt.subplot(3, 1, 3, sharex=ax1) + ax3.plot(t_q, np.rad2deg(yaw)) + ax3.plot(t, np.rad2deg(yaw_raw), '--') + ax3.set_xlabel("time (s)") + ax3.set_ylabel("yaw (deg)") + ax3.legend(["estimated", "integrated"]) + ax3.grid() + plt.show() + +if __name__ == '__main__': + import os + import argparse + + script_path = os.path.split(os.path.realpath(__file__))[0] + + parser = argparse.ArgumentParser( + description='Integrate angular velocity to attitude and compare it with attitude estimate') + + parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + args = parser.parse_args() + + logfile = os.path.abspath(args.logfile) + + run(logfile) diff --git a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py index 61b746fb3886..c0047a00f3ed 100644 --- a/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py +++ b/src/modules/ekf2/EKF/python/tuning_tools/mc_wind_estimator/mc_wind_estimator_tuning.py @@ -45,14 +45,20 @@ import quaternion from scipy import optimize -def getAllData(logfile): +def getAllData(logfile, use_gnss): log = ULog(logfile) - v_local = np.matrix([getData(log, 'vehicle_local_position', 'vx'), - getData(log, 'vehicle_local_position', 'vy'), - getData(log, 'vehicle_local_position', 'vz')]) + if use_gnss: + v_local = np.array([getData(log, 'vehicle_gps_position', 'vel_n_m_s'), + getData(log, 'vehicle_gps_position', 'vel_e_m_s'), + getData(log, 'vehicle_gps_position', 'vel_d_m_s')]) + t_v_local = ms2s(getData(log, 'vehicle_gps_position', 'timestamp')) - t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) + else: + v_local = np.array([getData(log, 'vehicle_local_position', 'vx'), + getData(log, 'vehicle_local_position', 'vy'), + getData(log, 'vehicle_local_position', 'vz')]) + t_v_local = ms2s(getData(log, 'vehicle_local_position', 'timestamp')) accel = np.matrix([getData(log, 'sensor_combined', 'accelerometer_m_s2[0]'), getData(log, 'sensor_combined', 'accelerometer_m_s2[1]'), @@ -126,8 +132,8 @@ def getData(log, topic_name, variable_name, instance=0): def ms2s(time_ms): return time_ms * 1e-6 -def run(logfile): - (t, v_body, a_body) = getAllData(logfile) +def run(logfile, use_gnss): + (t, v_body, a_body) = getAllData(logfile, use_gnss) rho = 1.15 # air densitiy rho15 = 1.225 # air density at 15 degC @@ -197,8 +203,10 @@ def run(logfile): # Provide parameter file path and name parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str) + parser.add_argument('--gnss', help='Use GNSS velocity instead of local velocity estimate', + action='store_true') args = parser.parse_args() logfile = os.path.abspath(args.logfile) # Convert to absolute path - run(logfile) + run(logfile, args.gnss) 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..42c95282314d 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 @@ -1,6 +1,6 @@ matplotlib==3.5.1 numpy==1.22.2 pyulog==0.9.0 -quaternion==3.5.2.post4 +numpy-quaternion==2023.0.4 scipy==1.8.0 sympy==1.10.1 diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp deleted file mode 100644 index 8fede499e5a2..000000000000 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ /dev/null @@ -1,257 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2022 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 range_height_control.cpp - * Control functions for ekf range finder height fusion - */ - -#include "ekf.h" - -void Ekf::controlRangeHeightFusion() -{ - static constexpr const char *HGT_SRC_NAME = "RNG"; - - bool rng_data_ready = false; - - if (_range_buffer) { - // Get range data from buffer and check validity - rng_data_ready = _range_buffer->pop_first_older_than(_time_delayed_us, _range_sensor.getSampleAddress()); - _range_sensor.setDataReadiness(rng_data_ready); - - // update range sensor angle parameters in case they have changed - _range_sensor.setPitchOffset(_params.rng_sens_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - - _range_sensor.runChecks(_time_delayed_us, _R_to_earth); - - if (_range_sensor.isDataHealthy()) { - // correct the range data for position offset relative to the IMU - const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - - if (_control_status.flags.in_air) { - const bool horizontal_motion = _control_status.flags.fixed_wing - || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f)); - - const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - const float var = sq(_params.range_noise) + dist_dependant_var; - - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - _rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us); - } - - } else { - // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements - // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air - && _range_sensor.isRegularlySendingData() - && _range_sensor.isDataReady()) { - - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); // bypass the checks - } - } - - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); - - } else { - return; - } - - auto &aid_src = _aid_src_rng_hgt; - HeightBiasEstimator &bias_est = _rng_hgt_b_est; - - bias_est.predict(_dt_ekf_avg); - - if (rng_data_ready && _range_sensor.getSampleAddress()) { - - const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); - const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); - - const float innov_gate = math::max(_params.range_innov_gate, 1.f); - - const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); - - // vertical position innovation - baro measurement has opposite sign to earth z axis - updateVerticalPositionAidSrcStatus(_range_sensor.getSampleAddress()->time_us, - -(measurement - bias_est.getBias()), - measurement_var + bias_est.getBiasVar(), - innov_gate, - aid_src); - - // update the bias estimator before updating the main filter but after - // using its current state to compute the vertical position innovation - if (measurement_valid && _range_sensor.isDataHealthy()) { - bias_est.setMaxStateNoise(sqrtf(measurement_var)); - bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); - } - - // determine if we should use height aiding - const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); - const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) || do_conditional_range_aid) - && measurement_valid - && _range_sensor.isDataHealthy(); - - const bool starting_conditions_passing = continuing_conditions_passing - && isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL) - && _range_sensor.isRegularlySendingData(); - - if (_control_status.flags.rng_hgt) { - if (continuing_conditions_passing) { - - fuseVerticalPosition(aid_src); - - const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max); - - if (isHeightResetRequired()) { - // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); - - _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias())); - bias_est.setBias(_state.pos(2) + measurement); - - // reset vertical velocity - resetVerticalVelocityToZero(); - - aid_src.time_last_fuse = _time_delayed_us; - - } else if (is_fusion_failing) { - // Some other height source is still working - ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME); - stopRngHgtFusion(); - _control_status.flags.rng_fault = true; - _range_sensor.setFaulty(); - } - - } else { - ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME); - stopRngHgtFusion(); - } - - } else { - if (starting_conditions_passing) { - if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL))) { - // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. - ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); - _height_sensor_ref = HeightSensor::RANGE; - bias_est.setBias(_state.pos(2) + measurement); - - } else if ((_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast(RngCtrl::CONDITIONAL))) { - // Range finder is the primary height source, the ground is now the datum used - // to compute the local vertical position - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); - _height_sensor_ref = HeightSensor::RANGE; - - _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-measurement, measurement_var); - bias_est.reset(); - - } else { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + measurement); - } - - aid_src.time_last_fuse = _time_delayed_us; - bias_est.setFusionActive(); - _control_status.flags.rng_hgt = true; - } - } - - } else if (_control_status.flags.rng_hgt - && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) { - // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME); - stopRngHgtFusion(); - } -} - -bool Ekf::isConditionalRangeAidSuitable() -{ -#if defined(CONFIG_EKF2_TERRAIN) - if (_control_status.flags.in_air - && _range_sensor.isHealthy() - && isTerrainEstimateValid()) { - // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching - // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice - float range_hagl_max = _params.max_hagl_for_range_aid; - float max_vel_xy = _params.max_vel_for_range_aid; - - const float hagl_innov = _aid_src_terrain_range_finder.innovation; - const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; - - const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var)); - - bool is_hagl_stable = (hagl_test_ratio < 1.f); - - if (!_control_status.flags.rng_hgt) { - range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; - max_vel_xy = 0.7f * _params.max_vel_for_range_aid; - is_hagl_stable = (hagl_test_ratio < 0.01f); - } - - const float range_hagl = _terrain_vpos - _state.pos(2); - - const bool is_in_range = (range_hagl < range_hagl_max); - - bool is_below_max_speed = true; - - if (isHorizontalAidingActive()) { - is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy); - } - - return is_in_range && is_hagl_stable && is_below_max_speed; - } -#endif // CONFIG_EKF2_TERRAIN - - return false; -} - -void Ekf::stopRngHgtFusion() -{ - if (_control_status.flags.rng_hgt) { - - if (_height_sensor_ref == HeightSensor::RANGE) { - _height_sensor_ref = HeightSensor::UNKNOWN; - } - - _rng_hgt_b_est.setFusionInactive(); - resetEstimatorAidStatus(_aid_src_rng_hgt); - - _control_status.flags.rng_hgt = false; - } -} diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp new file mode 100644 index 000000000000..61356b1a41af --- /dev/null +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2015-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 terrain_control.cpp + */ + +#include "ekf.h" +#include "ekf_derivation/generated/compute_hagl_innov_var.h" + +#include + +void Ekf::initTerrain() +{ + // assume a ground clearance + _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + + // use the ground clearance value as our uncertainty + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); +} + +void Ekf::controlTerrainFakeFusion() +{ + // If we are on ground, store the local position and time to use as a reference + if (!_control_status.flags.in_air) { + _last_on_ground_posD = _state.pos(2); + _control_status.flags.rng_fault = false; + + } else if (!_control_status_prev.flags.in_air) { + // Let the estimator run freely before arming for bench testing purposes, but reset on takeoff + // because when using optical flow measurements, it is safer to start with a small distance to ground + // as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior. + initTerrain(); + } + + if (!_control_status.flags.in_air + && !_control_status.flags.rng_terrain + && !_control_status.flags.opt_flow_terrain) { + + bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6); + + if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { + initTerrain(); + } + } +} + +bool Ekf::isTerrainEstimateValid() const +{ + bool valid = false; + + if (_time_last_terrain_fuse != 0) { + // Assume being valid when the uncertainty is small compared to the height above ground + float hagl_var = INFINITY; + sym::ComputeHaglInnovVar(P, 0.f, &hagl_var); + + if (hagl_var < fmaxf(sq(0.1f * getHagl()), 0.2f)) { + valid = true; + } + } + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + // Assume that the terrain estimate is always valid when direct observations are fused + if (_control_status.flags.rng_terrain && isRecent(_aid_src_rng_hgt.time_last_fuse, _params.hgt_fusion_timeout_max)) { + valid = true; + } + +#endif // CONFIG_EKF2_RANGE_FINDER + + return valid; +} diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp deleted file mode 100644 index 7e450f125314..000000000000 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ /dev/null @@ -1,473 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-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 terrain_estimator.cpp - * Function for fusing rangefinder and optical flow measurements - * to estimate terrain vertical position - */ - -#include "ekf.h" -#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h" -#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h" - -#include - -void Ekf::initHagl() -{ - // assume a ground clearance - _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; - - // use the ground clearance value as our uncertainty - _terrain_var = sq(_params.rng_gnd_clearance); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - _aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us; -#endif // CONFIG_EKF2_OPTICAL_FLOW -} - -void Ekf::runTerrainEstimator(const imuSample &imu_delayed) -{ - // If we are on ground, store the local position and time to use as a reference - if (!_control_status.flags.in_air) { - _last_on_ground_posD = _state.pos(2); - _control_status.flags.rng_fault = false; - - } else if (!_control_status_prev.flags.in_air) { - // Let the estimator run freely before arming for bench testing purposes, but reset on takeoff - // because when using optical flow measurements, it is safer to start with a small distance to ground - // as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior. - initHagl(); - } - - predictHagl(imu_delayed); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - controlHaglRngFusion(); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - controlHaglFlowFusion(); -#endif // CONFIG_EKF2_OPTICAL_FLOW - - controlHaglFakeFusion(); - - // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) - if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) { - _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); - } -} - -void Ekf::predictHagl(const imuSample &imu_delayed) -{ - // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle - - // process noise due to errors in vehicle height estimate - _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); - - // process noise due to terrain gradient - _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) - * (sq(_state.vel(0)) + sq(_state.vel(1))); - - // limit the variance to prevent it becoming badly conditioned - _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); -} - -#if defined(CONFIG_EKF2_RANGE_FINDER) -void Ekf::controlHaglRngFusion() -{ - if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) - || _control_status.flags.rng_fault) { - - stopHaglRngFusion(); - return; - } - - if (_range_sensor.isDataReady()) { - updateHaglRng(_aid_src_terrain_range_finder); - } - - if (_range_sensor.isDataHealthy()) { - - const bool continuing_conditions_passing = _rng_consistency_check.isKinematicallyConsistent(); - //&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height - - const bool starting_conditions_passing = continuing_conditions_passing - && _range_sensor.isRegularlySendingData() - && (_rng_consistency_check.getTestRatio() < 1.f); - - _time_last_healthy_rng_data = _time_delayed_us; - - if (_hagl_sensor_status.flags.range_finder) { - if (continuing_conditions_passing) { - fuseHaglRng(_aid_src_terrain_range_finder); - - // We have been rejecting range data for too long - const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); - const bool is_fusion_failing = isTimedOut(_aid_src_terrain_range_finder.time_last_fuse, timeout); - - if (is_fusion_failing) { - if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) { - // Data seems good, attempt a reset - resetHaglRng(); - - } else if (starting_conditions_passing) { - // The sensor can probably not detect the ground properly - // declare the sensor faulty and stop the fusion - _control_status.flags.rng_fault = true; - _range_sensor.setFaulty(true); - stopHaglRngFusion(); - - } else { - // This could be a temporary issue, stop the fusion without declaring the sensor faulty - stopHaglRngFusion(); - } - } - - } else { - stopHaglRngFusion(); - } - - } else { - if (starting_conditions_passing) { - _hagl_sensor_status.flags.range_finder = true; - - // Reset the state to the measurement only if the test ratio is large, - // otherwise let it converge through the fusion - if (_hagl_sensor_status.flags.flow && (_aid_src_terrain_range_finder.test_ratio < 0.2f)) { - fuseHaglRng(_aid_src_terrain_range_finder); - - } else { - resetHaglRng(); - } - } - } - - } else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) { - // No data anymore. Stop until it comes back. - stopHaglRngFusion(); - } -} - -float Ekf::getRngVar() const -{ - return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()); -} - -void Ekf::resetHaglRng() -{ - _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); - _terrain_var = getRngVar(); - _terrain_vpos_reset_counter++; - - _aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us; -} - -void Ekf::stopHaglRngFusion() -{ - if (_hagl_sensor_status.flags.range_finder) { - ECL_INFO("stopping HAGL range fusion"); - - // reset flags - resetEstimatorAidStatus(_aid_src_terrain_range_finder); - - _innov_check_fail_status.flags.reject_hagl = false; - - _hagl_sensor_status.flags.range_finder = false; - } -} - -void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const -{ - // get a height above ground measurement from the range finder assuming a flat earth - const float meas_hagl = _range_sensor.getDistBottom(); - - // predict the hagl from the vehicle position and terrain height - const float pred_hagl = _terrain_vpos - _state.pos(2); - - // calculate the innovation - const float hagl_innov = pred_hagl - meas_hagl; - - // calculate the observation variance adding the variance of the vehicles own height uncertainty - const float obs_variance = getRngVar(); - - // calculate the innovation variance - limiting it to prevent a badly conditioned fusion - const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - - // perform an innovation consistency check and only fuse data if it passes - const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f); - - - aid_src.timestamp_sample = _time_delayed_us; // TODO - - aid_src.observation = meas_hagl; - aid_src.observation_variance = obs_variance; - - aid_src.innovation = hagl_innov; - aid_src.innovation_variance = hagl_innov_var; - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.fused = false; -} - -void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src) -{ - if (!aid_src.innovation_rejected) { - // calculate the Kalman gain - const float gain = _terrain_var / aid_src.innovation_variance; - - // correct the state - _terrain_vpos -= gain * aid_src.innovation; - - // correct the variance - _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); - - // record last successful fusion event - _innov_check_fail_status.flags.reject_hagl = false; - - aid_src.time_last_fuse = _time_delayed_us; - aid_src.fused = true; - - } else { - _innov_check_fail_status.flags.reject_hagl = true; - aid_src.fused = false; - } -} -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) -void Ekf::controlHaglFlowFusion() -{ - if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)) { - stopHaglFlowFusion(); - return; - } - - if (_flow_data_ready) { - updateOptFlow(_aid_src_terrain_optical_flow); - - const bool continuing_conditions_passing = _control_status.flags.in_air - && !_control_status.flags.opt_flow - && _control_status.flags.gps - && !_hagl_sensor_status.flags.range_finder; - - const bool starting_conditions_passing = continuing_conditions_passing; - - if (_hagl_sensor_status.flags.flow) { - if (continuing_conditions_passing) { - - // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon - fuseFlowForTerrain(_aid_src_terrain_optical_flow); - - // TODO: do something when failing continuously the innovation check - /* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */ - - /* if (is_fusion_failing) { */ - /* resetHaglFlow(); */ - /* } */ - - } else { - stopHaglFlowFusion(); - } - - } else { - if (starting_conditions_passing) { - _hagl_sensor_status.flags.flow = true; - // TODO: do a reset instead of trying to fuse the data? - fuseFlowForTerrain(_aid_src_terrain_optical_flow); - } - } - - _flow_data_ready = false; - - } else if (_hagl_sensor_status.flags.flow && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { - // No data anymore. Stop until it comes back. - stopHaglFlowFusion(); - } -} - -void Ekf::stopHaglFlowFusion() -{ - if (_hagl_sensor_status.flags.flow) { - ECL_INFO("stopping HAGL flow fusion"); - - _hagl_sensor_status.flags.flow = false; - resetEstimatorAidStatus(_aid_src_terrain_optical_flow); - } -} - -void Ekf::resetHaglFlow() -{ - // TODO: use the flow data - _terrain_vpos = fmaxf(0.0f, _state.pos(2)); - _terrain_var = 100.0f; - _terrain_vpos_reset_counter++; - - _aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us; -} - -void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) -{ - flow.fused = true; - - const float R_LOS = flow.observation_variance[0]; - - // calculate the height above the ground of the optical flow camera. Since earth frame is NED - // a positive offset in earth frame leads to a smaller height above the ground. - float range = predictFlowRange(); - - const float state = _terrain_vpos; // linearize both axes using the same state value - Vector2f innov_var; - float H; - sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(flow.innovation_variance); - - if ((flow.innovation_variance[0] < R_LOS) - || (flow.innovation_variance[1] < R_LOS)) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - _terrain_var = 100.0f; - return; - } - - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f)); - - _innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f); - - // if either axis fails we abort the fusion - if (flow.innovation_rejected) { - return; - } - - // fuse observation axes sequentially - for (uint8_t index = 0; index <= 1; index++) { - if (index == 0) { - // everything was already computed above - - } else if (index == 1) { - // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H); - - // recalculate the innovation using the updated state - const Vector2f vel_body = predictFlowVelBody(); - range = predictFlowRange(); - flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1]; - - if (flow.innovation_variance[1] < R_LOS) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - _terrain_var = 100.0f; - return; - } - } - - float Kfusion = _terrain_var * H / flow.innovation_variance[index]; - - _terrain_vpos += Kfusion * flow.innovation[0]; - // constrain terrain to minimum allowed value and predict height above ground - _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f)); - } - - _fault_status.flags.bad_optflow_X = false; - _fault_status.flags.bad_optflow_Y = false; - - flow.time_last_fuse = _time_delayed_us; - flow.fused = true; -} -#endif // CONFIG_EKF2_OPTICAL_FLOW - -void Ekf::controlHaglFakeFusion() -{ - if (!_control_status.flags.in_air - && !_hagl_sensor_status.flags.range_finder - && !_hagl_sensor_status.flags.flow) { - - bool recent_terrain_aiding = false; - -#if defined(CONFIG_EKF2_RANGE_FINDER) - recent_terrain_aiding |= isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)1e6); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - recent_terrain_aiding |= isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)1e6); -#endif // CONFIG_EKF2_OPTICAL_FLOW - - if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { - initHagl(); - } - } -} - -bool Ekf::isTerrainEstimateValid() const -{ - bool valid = false; - -#if defined(CONFIG_EKF2_RANGE_FINDER) - - // we have been fusing range finder measurements in the last 5 seconds - if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) { - if (_hagl_sensor_status.flags.range_finder || !_control_status.flags.in_air) { - valid = true; - } - } - -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - - // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds - // this can only be the case if the main filter does not fuse optical flow - if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) { - valid = true; - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - - return valid; -} - -void Ekf::terrainHandleVerticalPositionReset(const float delta_z) { - _terrain_vpos += delta_z; -} diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index fbb079c1fdee..349a40092fd4 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -33,58 +33,14 @@ #include "ekf.h" -void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, - const float innov_gate, estimator_aid_source2d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 2; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - aid_src.timestamp_sample = time_us; -} - -void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, - const float innov_gate, estimator_aid_source3d_s &aid_src) const -{ - resetEstimatorAidStatus(aid_src); - - for (int i = 0; i < 3; i++) { - aid_src.observation[i] = obs(i); - aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; - - aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - const int state_index = State::vel.idx + i; - aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i]; - } - - setEstimatorAidStatusTestRatio(aid_src, innov_gate); - - // vz special case if there is bad vertical acceleration data, then don't reject measurement, - // but limit innovation to prevent spikes that could destabilise the filter - if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) { - const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]); - aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit); - aid_src.innovation_rejected = false; - } - - aid_src.timestamp_sample = time_us; -} - -void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) +bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) { // vx, vy if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -94,15 +50,20 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } -void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) +bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { // vx, vy, vz if (!aid_src.innovation_rejected - && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], State::vel.idx + 0) - && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], State::vel.idx + 1) - && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], State::vel.idx + 2) + && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0], + State::vel.idx + 0) + && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1], + State::vel.idx + 1) + && fuseDirectStateMeasurement(aid_src.innovation[2], aid_src.innovation_variance[2], aid_src.observation_variance[2], + State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -113,6 +74,8 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) } else { aid_src.fused = false; } + + return aid_src.fused; } void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var) diff --git a/src/modules/ekf2/EKF/wind.cpp b/src/modules/ekf2/EKF/wind.cpp new file mode 100644 index 000000000000..ca3e8abc8b0b --- /dev/null +++ b/src/modules/ekf2/EKF/wind.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 wind.cpp + * Helper functions for wind states + */ + +#include "ekf.h" +#include + +void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction, float wind_speed_accuracy, + float wind_direction_accuracy) +{ + if (!_control_status.flags.in_air) { + + const float wind_speed_constrained = math::max(wind_speed, 0.0f); + const float wind_direction_var = sq(wind_direction_accuracy); + const float wind_speed_var = sq(wind_speed_accuracy); + + Vector2f wind; + Vector2f wind_var; + + sym::ComputeWindInitAndCovFromWindSpeedAndDirection(wind_speed_constrained, wind_direction, wind_speed_var, + wind_direction_var, &wind, &wind_var); + + ECL_INFO("reset wind states to external observation"); + _information_events.flags.reset_wind_to_ext_obs = true; + _external_wind_init = true; + + resetWindTo(wind, wind_var); + + } +} + +void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var) +{ + _state.wind_vel = wind; + + if (PX4_ISFINITE(wind_var(0))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx, + math::min(sq(_params.initial_wind_uncertainty), wind_var(0))); + } + + if (PX4_ISFINITE(wind_var(1))) { + P.uncorrelateCovarianceSetVariance<1>(State::wind_vel.idx + 1, + math::min(sq(_params.initial_wind_uncertainty), wind_var(1))); + } +} + +void Ekf::resetWindCov() +{ + // start with a small initial uncertainty to improve the initial estimate + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); +} + +void Ekf::resetWindToZero() +{ + ECL_INFO("reset wind to zero"); + _state.wind_vel.setZero(); + resetWindCov(); +} diff --git a/src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt b/src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt new file mode 100644 index 000000000000..1f12a2ba0338 --- /dev/null +++ b/src/modules/ekf2/EKF/yaw_estimator/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +add_library(yaw_estimator + EKFGSF_yaw.cpp + EKFGSF_yaw.h +) + +add_dependencies(yaw_estimator prebuild_targets) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp similarity index 94% rename from src/modules/ekf2/EKF/EKFGSF_yaw.cpp rename to src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp index 697e027e6d85..7aad6620cee5 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.cpp +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 @@ -32,10 +32,24 @@ ****************************************************************************/ #include "EKFGSF_yaw.h" + #include -#include "python/ekf_derivation/generated/yaw_est_predict_covariance.h" -#include "python/ekf_derivation/generated/yaw_est_compute_measurement_update.h" +#include // CONSTANTS_ONE_G + +#include "derivation/generated/yaw_est_predict_covariance.h" +#include "derivation/generated/yaw_est_compute_measurement_update.h" + +using matrix::AxisAnglef; +using matrix::Dcmf; +using matrix::Eulerf; +using matrix::Matrix3f; +using matrix::Quatf; +using matrix::Vector2f; +using matrix::Vector3f; +using matrix::wrap_pi; +using math::Utilities::getEulerYaw; +using math::Utilities::updateYawInRotMat; EKFGSF_yaw::EKFGSF_yaw() { @@ -49,13 +63,14 @@ void EKFGSF_yaw::reset() _gsf_yaw_variance = INFINITY; } -void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air) +void EKFGSF_yaw::predict(const matrix::Vector3f &delta_ang, const float delta_ang_dt, const matrix::Vector3f &delta_vel, + const float delta_vel_dt, bool in_air) { - const Vector3f accel = imu_sample.delta_vel / imu_sample.delta_vel_dt; + const Vector3f accel = delta_vel / delta_vel_dt; - if (imu_sample.delta_vel_dt > 0.001f) { + if (delta_vel_dt > 0.001f) { // to reduce effect of vibration, filter using an LPF whose time constant is 1/10 of the AHRS tilt correction time constant - const float filter_coef = fminf(10.f * imu_sample.delta_vel_dt * _tilt_gain, 1.f); + const float filter_coef = fminf(10.f * delta_vel_dt * _tilt_gain, 1.f); _ahrs_accel = _ahrs_accel * (1.f - filter_coef) + accel * filter_coef; } else { @@ -76,7 +91,7 @@ void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air) && (accel_lpf_norm_sq > sq(lower_accel_limit)) && (accel_lpf_norm_sq < sq(upper_accel_limit)); if (ok_to_align) { - ahrsAlignTilt(imu_sample.delta_vel); + ahrsAlignTilt(delta_vel); _ahrs_ekf_gsf_tilt_aligned = true; } else { @@ -85,8 +100,7 @@ void EKFGSF_yaw::predict(const imuSample &imu_sample, bool in_air) } for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) { - predictEKF(model_index, imu_sample.delta_ang, imu_sample.delta_ang_dt, - imu_sample.delta_vel, imu_sample.delta_vel_dt, in_air); + predictEKF(model_index, delta_ang, delta_ang_dt, delta_vel, delta_vel_dt, in_air); } } @@ -287,8 +301,8 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index, const Vector3f &delta_ang // Use fixed values for delta angle process noise variances const float d_ang_var = sq(_gyro_noise * delta_ang_dt); - sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, - Vector2f(dvx, dvy), d_vel_var, daz, d_ang_var, &_ekf_gsf[model_index].P); + _ekf_gsf[model_index].P = sym::YawEstPredictCovariance(_ekf_gsf[model_index].X, _ekf_gsf[model_index].P, Vector2f(dvx, + dvy), d_vel_var, daz, d_ang_var); // covariance matrix is symmetrical, so copy upper half to lower half _ekf_gsf[model_index].P(1, 0) = _ekf_gsf[model_index].P(0, 1); diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h similarity index 78% rename from src/modules/ekf2/EKF/EKFGSF_yaw.h rename to src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h index 7add1dedef7a..ec84ed06f925 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/yaw_estimator/EKFGSF_yaw.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2024 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 @@ -34,40 +34,28 @@ #ifndef EKF_EKFGSF_YAW_H #define EKF_EKFGSF_YAW_H -#include -#include -#include - -#include "common.h" - -using matrix::AxisAnglef; -using matrix::Dcmf; -using matrix::Eulerf; -using matrix::Matrix3f; -using matrix::Quatf; -using matrix::Vector2f; -using matrix::Vector3f; -using matrix::wrap_pi; +#include +#include static constexpr uint8_t N_MODELS_EKFGSF = 5; -using namespace estimator; - class EKFGSF_yaw { public: EKFGSF_yaw(); // Update Filter States - this should be called whenever new IMU data is available - void predict(const imuSample &imu_sample, bool in_air = false); + void predict(const matrix::Vector3f &delta_ang, const float delta_ang_dt, + const matrix::Vector3f &delta_vel, const float delta_vel_dt, + bool in_air = false); - void fuseVelocity(const Vector2f &vel_NE, // NE velocity measurement (m/s) - float vel_accuracy, // 1-sigma accuracy of velocity measurement (m/s) - bool in_air); + // vel_NE: NE velocity measurement (m/s) + // vel_accuracy: 1-sigma accuracy of velocity measurement (m/s) + void fuseVelocity(const matrix::Vector2f &vel_NE, float vel_accuracy, bool in_air); void setTrueAirspeed(float true_airspeed) { _true_airspeed = true_airspeed; } - void setGyroBias(const Vector3f &imu_gyro_bias, const bool force = false) + void setGyroBias(const matrix::Vector3f &imu_gyro_bias, const bool force = false) { // Initialise to gyro bias estimate from main filter because there could be a large // uncorrected rate gyro bias error about the gravity vector @@ -106,31 +94,31 @@ class EKFGSF_yaw float _true_airspeed{NAN}; // true airspeed used for centripetal accel compensation (m/s) struct { - Dcmf R{matrix::eye()}; // matrix that rotates a vector from body to earth frame - Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation + matrix::Dcmf R{matrix::eye()}; // matrix that rotates a vector from body to earth frame + matrix::Vector3f gyro_bias{}; // gyro bias learned and used by the quaternion calculation } _ahrs_ekf_gsf[N_MODELS_EKFGSF] {}; bool _ahrs_ekf_gsf_tilt_aligned{false}; // true the initial tilt alignment has been calculated - Vector3f _ahrs_accel{0.f, 0.f, 0.f}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s) + matrix::Vector3f _ahrs_accel{0.f, 0.f, 0.f}; // low pass filtered body frame specific force vector used by AHRS calculation (m/s/s) // calculate the gain from gravity vector misalingment to tilt correction to be used by all AHRS filters float ahrsCalcAccelGain() const; // update specified AHRS rotation matrix using IMU and optionally true airspeed data - void ahrsPredict(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt); + void ahrsPredict(const uint8_t model_index, const matrix::Vector3f &delta_ang, const float delta_ang_dt); // align all AHRS roll and pitch orientations using IMU delta velocity vector - void ahrsAlignTilt(const Vector3f &delta_vel); + void ahrsAlignTilt(const matrix::Vector3f &delta_vel); // align all AHRS yaw orientations to initial values void ahrsAlignYaw(); // Efficient propagation of a delta angle in body frame applied to the body to earth frame rotation matrix - Matrix3f ahrsPredictRotMat(const Matrix3f &R, const Vector3f &g); + matrix::Matrix3f ahrsPredictRotMat(const matrix::Matrix3f &R, const matrix::Vector3f &g); // Declarations used by a bank of N_MODELS_EKFGSF EKFs - struct _ekf_gsf_struct { + struct { matrix::Vector3f X{}; // Vel North (m/s), Vel East (m/s), yaw (rad)s matrix::SquareMatrix P{}; // covariance matrix matrix::SquareMatrix S_inverse{}; // inverse of the innovation covariance matrix @@ -141,15 +129,15 @@ class EKFGSF_yaw bool _ekf_gsf_vel_fuse_started{}; // true when the EKF's have started fusing velocity data and the prediction and update processing is active // initialise states and covariance data for the GSF and EKF filters - void initialiseEKFGSF(const Vector2f &vel_NE, const float vel_accuracy); + void initialiseEKFGSF(const matrix::Vector2f &vel_NE, const float vel_accuracy); // predict state and covariance for the specified EKF using inertial data - void predictEKF(const uint8_t model_index, const Vector3f &delta_ang, const float delta_ang_dt, - const Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false); + void predictEKF(const uint8_t model_index, const matrix::Vector3f &delta_ang, const float delta_ang_dt, + const matrix::Vector3f &delta_vel, const float delta_vel_dt, bool in_air = false); // update state and covariance for the specified EKF using a NE velocity measurement // return false if update failed - bool updateEKF(const uint8_t model_index, const Vector2f &vel_NE, const float vel_accuracy); + bool updateEKF(const uint8_t model_index, const matrix::Vector2f &vel_NE, const float vel_accuracy); inline float sq(float x) const { return x * x; }; diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py similarity index 90% rename from src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py rename to src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py index e45fed5b25ce..8f9e767931cb 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py +++ b/src/modules/ekf2/EKF/yaw_estimator/derivation/derivation_yaw_estimator.py @@ -1,7 +1,7 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ - Copyright (c) 2022-2023 PX4 Development Team + Copyright (c) 2022-2024 PX4 Development Team Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: @@ -38,7 +38,12 @@ import symforce.symbolic as sf from symforce.values import Values -from derivation_utils import * + +# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils +import os, sys +derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils" +sys.path.append(derivation_utils_dir) +import derivation_utils State = Values( vel = sf.V2(), @@ -148,7 +153,7 @@ def yaw_est_compute_measurement_update( S = H * P * H.T + R S_det = S[0, 0] * S[1, 1] - S[1, 0] * S[0, 1] - S_det_inv = add_epsilon_sign(1 / S_det, S_det, epsilon) + S_det_inv = derivation_utils.add_epsilon_sign(1 / S_det, S_det, epsilon) # Compute inverse using simple formula for 2x2 matrix and using protected division S_inv = sf.M22([[S[1, 1], -S[0, 1]], [-S[1, 0], S[0, 0]]]) * S_det_inv @@ -166,5 +171,5 @@ def yaw_est_compute_measurement_update( return (S_inv, S_det_inv, K, P_new) print("Derive yaw estimator equations...") -generate_px4_function(yaw_est_predict_covariance, output_names=["P_new"]) -generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"]) +derivation_utils.generate_px4_function(yaw_est_predict_covariance, output_names=None) +derivation_utils.generate_px4_function(yaw_est_compute_measurement_update, output_names=["S_inv", "S_det_inv", "K", "P_new"]) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h similarity index 97% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h rename to src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h index 4f0048b7acad..a7c412def368 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h +++ b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_compute_measurement_update.h @@ -86,12 +86,11 @@ void YawEstComputeMeasurementUpdate(const matrix::Matrix& P, const if (P_new != nullptr) { matrix::Matrix& _p_new = (*P_new); + _p_new.setZero(); + _p_new(0, 0) = -P(0, 0) * _tmp9 + P(0, 0) - P(1, 0) * _tmp12; - _p_new(1, 0) = 0; - _p_new(2, 0) = 0; _p_new(0, 1) = -P(0, 1) * _tmp9 + P(0, 1) - P(1, 1) * _tmp12; _p_new(1, 1) = -P(0, 1) * _tmp10 - P(1, 1) * _tmp13 + P(1, 1); - _p_new(2, 1) = 0; _p_new(0, 2) = -P(0, 2) * _tmp9 + P(0, 2) - P(1, 2) * _tmp12; _p_new(1, 2) = -P(0, 2) * _tmp10 - P(1, 2) * _tmp13 + P(1, 2); _p_new(2, 2) = -P(0, 2) * _tmp11 - P(1, 2) * _tmp14 + P(2, 2); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h similarity index 59% rename from src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h rename to src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h index 5d2c4327a55a..d3fefb61d848 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h +++ b/src/modules/ekf2/EKF/yaw_estimator/derivation/generated/yaw_est_predict_covariance.h @@ -24,14 +24,14 @@ namespace sym { * d_ang_var: Scalar * * Outputs: - * P_new: Matrix33 + * res: Matrix33 */ template -void YawEstPredictCovariance(const matrix::Matrix& state, - const matrix::Matrix& P, - const matrix::Matrix& d_vel, const Scalar d_vel_var, - const Scalar d_ang, const Scalar d_ang_var, - matrix::Matrix* const P_new = nullptr) { +matrix::Matrix YawEstPredictCovariance(const matrix::Matrix& state, + const matrix::Matrix& P, + const matrix::Matrix& d_vel, + const Scalar d_vel_var, const Scalar d_ang, + const Scalar d_ang_var) { // Total ops: 39 // Input arrays @@ -48,19 +48,18 @@ void YawEstPredictCovariance(const matrix::Matrix& state, const Scalar _tmp7 = std::pow(d_ang, Scalar(2)) + 1; // Output terms (1) - if (P_new != nullptr) { - matrix::Matrix& _p_new = (*P_new); + matrix::Matrix _res; - _p_new(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4; - _p_new(1, 0) = 0; - _p_new(2, 0) = 0; - _p_new(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5; - _p_new(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6; - _p_new(2, 1) = 0; - _p_new(0, 2) = _tmp3 * _tmp7; - _p_new(1, 2) = _tmp6 * _tmp7; - _p_new(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var; - } + _res.setZero(); + + _res(0, 0) = P(0, 0) + P(2, 0) * _tmp2 + _tmp2 * _tmp3 + _tmp4; + _res(0, 1) = P(0, 1) + P(2, 1) * _tmp2 + _tmp3 * _tmp5; + _res(1, 1) = P(1, 1) + P(2, 1) * _tmp5 + _tmp4 + _tmp5 * _tmp6; + _res(0, 2) = _tmp3 * _tmp7; + _res(1, 2) = _tmp6 * _tmp7; + _res(2, 2) = P(2, 2) * std::pow(_tmp7, Scalar(2)) + d_ang_var; + + return _res; } // NOLINT(readability/fn_size) // NOLINTNEXTLINE(readability/fn_size) diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 80ce35210b14..f87dcb8e41c2 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -37,23 +37,8 @@ #include -// update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status) -{ - VectorState H_YAW; - sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW); - - return fuseYaw(aid_src_status, H_YAW); -} - bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW) { - // define the innovation gate size - float gate_sigma = math::max(_params.heading_innov_gate, 1.f); - - // innovation test ratio - setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma); - // check if the innovation variance calculation is badly conditioned if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) { // the innovation variance contribution from the state covariances is not negative, no fault @@ -83,7 +68,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H Kfusion(row) *= heading_innov_var_inv; } - // set the magnetometer unhealthy if the test fails + // set the heading unhealthy if the test fails if (aid_src_status.innovation_rejected) { _innov_check_fail_status.flags.reject_yaw = true; @@ -91,13 +76,14 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias if (!_control_status.flags.in_air - && isTimedOut(_time_last_in_air, (uint64_t)5e6) - && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) - ) { + && isTimedOut(_time_last_in_air, (uint64_t)5e6) + && isTimedOut(aid_src_status.time_last_fuse, (uint64_t)1e6) + ) { // constrain the innovation to the maximum set by the gate // we need to delay this forced fusion to avoid starting it // immediately after touchdown, when the drone is still armed - float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); + const float gate_sigma = math::max(_params.heading_innov_gate, 1.f); + const float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance)); aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit); // also reset the yaw gyro variance to converge faster and avoid @@ -122,13 +108,10 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H _fault_status.flags.bad_hdg = false; return true; - - } else { - _fault_status.flags.bad_hdg = true; } // otherwise - aid_src_status.fused = false; + _fault_status.flags.bad_hdg = true; return false; } @@ -136,3 +119,52 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec { sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW); } + +void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) +{ + // save a copy of the quaternion state for later use in calculating the amount of reset change + const Quatf quat_before_reset = _state.quat_nominal; + + // update the yaw angle variance + if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) { + P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance); + } + + // update transformation matrix from body to world frame using the current estimate + // update the rotation matrix using the new yaw value + _R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal)); + + // calculate the amount that the quaternion has changed by + const Quatf quat_after_reset(_R_to_earth); + const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized()); + + // update quaternion states + _state.quat_nominal = quat_after_reset; + + // add the reset amount to the output observer buffered data + _output_predictor.resetQuaternion(q_error); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + // update EV attitude error filter + if (_ev_q_error_initialized) { + const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized(); + _ev_q_error_filt.reset(ev_q_error_updated); + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + // record the state change + if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) { + _state_reset_status.quat_change = q_error; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.quat_change = q_error * _state_reset_status.quat_change; + _state_reset_status.quat_change.normalize(); + } + + _state_reset_status.reset_count.quat++; + + _time_last_heading_fuse = _time_delayed_us; +} diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c75093711ea9..af2eb6615f40 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -64,6 +64,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #endif // CONFIG_EKF2_WIND _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), + _param_ekf2_delay_max(_params->delay_max_ms), _param_ekf2_imu_ctrl(_params->imu_ctrl), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), @@ -146,7 +147,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_min_rng(_params->rng_gnd_clearance), #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_TERRAIN) - _param_ekf2_terr_mask(_params->terrain_fusion_mode), _param_ekf2_terr_noise(_params->terrain_p_noise), _param_ekf2_terr_grad(_params->terrain_gradient), #endif // CONFIG_EKF2_TERRAIN @@ -181,6 +181,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_OPTICAL_FLOW) _param_ekf2_of_ctrl(_params->flow_ctrl), + _param_ekf2_of_gyr_src(_params->flow_gyro_src), _param_ekf2_of_delay(_params->flow_delay_ms), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), @@ -213,18 +214,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_abl_tau(_params->acc_bias_learn_tc), _param_ekf2_gyr_b_lim(_params->gyro_bias_lim) { - // advertise expected minimal topic set immediately to ensure logging - _attitude_pub.advertise(); - _local_position_pub.advertise(); - - _estimator_event_flags_pub.advertise(); - _estimator_innovation_test_ratios_pub.advertise(); - _estimator_innovation_variances_pub.advertise(); - _estimator_innovations_pub.advertise(); - _estimator_sensor_bias_pub.advertise(); - _estimator_states_pub.advertise(); - _estimator_status_flags_pub.advertise(); - _estimator_status_pub.advertise(); + AdvertiseTopics(); } EKF2::~EKF2() @@ -233,123 +223,164 @@ EKF2::~EKF2() perf_free(_msg_missed_imu_perf); } -#if defined(CONFIG_EKF2_MULTI_INSTANCE) -bool EKF2::multi_init(int imu, int mag) +void EKF2::AdvertiseTopics() { - // advertise all topics to ensure consistent uORB instance numbering - _ekf2_timestamps_pub.advertise(); + // advertise expected minimal topic set immediately for logging + _attitude_pub.advertise(); + _local_position_pub.advertise(); _estimator_event_flags_pub.advertise(); - _estimator_innovation_test_ratios_pub.advertise(); - _estimator_innovation_variances_pub.advertise(); - _estimator_innovations_pub.advertise(); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _estimator_optical_flow_vel_pub.advertise(); -#endif // CONFIG_EKF2_OPTICAL_FLOW _estimator_sensor_bias_pub.advertise(); - _estimator_states_pub.advertise(); - _estimator_status_flags_pub.advertise(); _estimator_status_pub.advertise(); + _estimator_status_flags_pub.advertise(); + + if (_multi_mode) { + // only force advertise these in multi mode to ensure consistent uORB instance numbering + _global_position_pub.advertise(); + _odometry_pub.advertise(); + +#if defined(CONFIG_EKF2_WIND) + _wind_pub.advertise(); +#endif // CONFIG_EKF2_WIND + } #if defined(CONFIG_EKF2_GNSS) - _yaw_est_pub.advertise(); + + if (_param_ekf2_gps_ctrl.get()) { + _estimator_gps_status_pub.advertise(); + _yaw_est_pub.advertise(); + } + #endif // CONFIG_EKF2_GNSS + // verbose logging + if (_param_ekf2_log_verbose.get()) { + _estimator_innovation_test_ratios_pub.advertise(); + _estimator_innovation_variances_pub.advertise(); + _estimator_innovations_pub.advertise(); + _estimator_states_pub.advertise(); + +#if defined(CONFIG_EKF2_AIRSPEED) + + if (_param_ekf2_arsp_thr.get() > 0.f) { + _estimator_aid_src_airspeed_pub.advertise(); + } + +#endif // CONFIG_EKF2_AIRSPEED + #if defined(CONFIG_EKF2_BAROMETER) - // baro advertise - if (_param_ekf2_baro_ctrl.get()) { - _estimator_aid_src_baro_hgt_pub.advertise(); - _estimator_baro_bias_pub.advertise(); - } + if (_param_ekf2_baro_ctrl.get()) { + _estimator_aid_src_baro_hgt_pub.advertise(); + _estimator_baro_bias_pub.advertise(); + } #endif // CONFIG_EKF2_BAROMETER +#if defined(CONFIG_EKF2_DRAG_FUSION) + + if (_param_ekf2_drag_ctrl.get()) { + _estimator_aid_src_drag_pub.advertise(); + } + +#endif // CONFIG_EKF2_DRAG_FUSION + #if defined(CONFIG_EKF2_EXTERNAL_VISION) - // EV advertise - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { - _estimator_aid_src_ev_hgt_pub.advertise(); - _estimator_ev_pos_bias_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VPOS)) { + _estimator_aid_src_ev_hgt_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::HPOS)) { - _estimator_aid_src_ev_pos_pub.advertise(); - _estimator_ev_pos_bias_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::HPOS)) { + _estimator_aid_src_ev_pos_pub.advertise(); + _estimator_ev_pos_bias_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VEL)) { - _estimator_aid_src_ev_vel_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::VEL)) { + _estimator_aid_src_ev_vel_pub.advertise(); + } - if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::YAW)) { - _estimator_aid_src_ev_yaw_pub.advertise(); - } + if (_param_ekf2_ev_ctrl.get() & static_cast(EvCtrl::YAW)) { + _estimator_aid_src_ev_yaw_pub.advertise(); + } #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - // GNSS advertise - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { - _estimator_aid_src_gnss_hgt_pub.advertise(); - _estimator_gnss_hgt_bias_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get()) { + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VPOS)) { + _estimator_aid_src_gnss_hgt_pub.advertise(); + _estimator_gnss_hgt_bias_pub.advertise(); + } - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::HPOS)) { - _estimator_aid_src_gnss_pos_pub.advertise(); - _estimator_gps_status_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::HPOS)) { + _estimator_aid_src_gnss_pos_pub.advertise(); + } - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VEL)) { - _estimator_aid_src_gnss_vel_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::VEL)) { + _estimator_aid_src_gnss_vel_pub.advertise(); + } # if defined(CONFIG_EKF2_GNSS_YAW) - if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { - _estimator_aid_src_gnss_yaw_pub.advertise(); - } + if (_param_ekf2_gps_ctrl.get() & static_cast(GnssCtrl::YAW)) { + _estimator_aid_src_gnss_yaw_pub.advertise(); + } # endif // CONFIG_EKF2_GNSS_YAW + } + #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_GRAVITY_FUSION) - if (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GravityVector)) { - _estimator_aid_src_gravity_pub.advertise(); - } + if (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GravityVector)) { + _estimator_aid_src_gravity_pub.advertise(); + } #endif // CONFIG_EKF2_GRAVITY_FUSION +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { + _estimator_aid_src_mag_pub.advertise(); + } + +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_param_ekf2_of_ctrl.get()) { + _estimator_optical_flow_vel_pub.advertise(); + _estimator_aid_src_optical_flow_pub.advertise(); + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + #if defined(CONFIG_EKF2_RANGE_FINDER) - // RNG advertise - if (_param_ekf2_rng_ctrl.get()) { - _estimator_aid_src_rng_hgt_pub.advertise(); - _estimator_rng_hgt_bias_pub.advertise(); - } + // RNG advertise + if (_param_ekf2_rng_ctrl.get()) { + _estimator_aid_src_rng_hgt_pub.advertise(); + } #endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // mag advertise - if (_param_ekf2_mag_type.get() != MagFuseType::NONE) { - _estimator_aid_src_mag_heading_pub.advertise(); - _estimator_aid_src_mag_pub.advertise(); - } +#if defined(CONFIG_EKF2_SIDESLIP) -#endif // CONFIG_EKF2_MAGNETOMETER + if (_param_ekf2_fuse_beta.get()) { + _estimator_aid_src_sideslip_pub.advertise(); + } - _attitude_pub.advertise(); - _local_position_pub.advertise(); - _global_position_pub.advertise(); - _odometry_pub.advertise(); +#endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_WIND) - _wind_pub.advertise(); -#endif // CONFIG_EKF2_WIND + } // end verbose logging +} +#if defined(CONFIG_EKF2_MULTI_INSTANCE) +bool EKF2::multi_init(int imu, int mag) +{ bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -418,6 +449,9 @@ void EKF2::Run() VerifyParams(); + // force advertise topics immediately for logging (EKF2_LOG_VERBOSE, per aid source control) + AdvertiseTopics(); + #if defined(CONFIG_EKF2_GNSS) _ekf.set_min_required_gps_health_time(_param_ekf2_req_gps_h.get() * 1_s); #endif // CONFIG_EKF2_GNSS @@ -439,46 +473,6 @@ void EKF2::Run() #endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_BAROMETER) - - // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output - if (_params->baro_ctrl == 1) { - float sens_baro_rate = 0.f; - - if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) { - if (sens_baro_rate > 0) { - float interval_ms = roundf(1000.f / sens_baro_rate); - - if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { - PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); - _params->sensor_interval_max_ms = interval_ms; - } - } - } - } - -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_MAGNETOMETER) - - // if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output - if (_params->mag_fusion_type != MagFuseType::NONE) { - float sens_mag_rate = 0.f; - - if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) { - if (sens_mag_rate > 0) { - float interval_ms = roundf(1000.f / sens_mag_rate); - - if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) { - PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms); - _params->sensor_interval_max_ms = interval_ms; - } - } - } - } - -#endif // CONFIG_EKF2_MAGNETOMETER - _ekf.updateParameters(); } @@ -504,6 +498,12 @@ void EKF2::Run() vehicle_command_s vehicle_command; if (_vehicle_command_sub.update(&vehicle_command)) { + + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_command.command; + command_ack.target_system = vehicle_command.source_system; + command_ack.target_component = vehicle_command.source_component; + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN) { double latitude = vehicle_command.param5; double longitude = vehicle_command.param6; @@ -516,15 +516,24 @@ void EKF2::Run() PX4_INFO("%d - New NED origin (LLA): %3.10f, %3.10f, %4.3f\n", _instance, latitude, longitude, static_cast(altitude)); + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + } else { PX4_ERR("%d - Failed to set new NED origin (LLA): %3.10f, %3.10f, %4.3f\n", _instance, latitude, longitude, static_cast(altitude)); + + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; } - } - if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { - if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning) && - PX4_ISFINITE(vehicle_command.param2) && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6)) { + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + + } else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE) { + + if ((_ekf.control_status_flags().wind_dead_reckoning || _ekf.control_status_flags().inertial_dead_reckoning + || (!_ekf.control_status_flags().in_air && !_ekf.control_status_flags().gps)) && PX4_ISFINITE(vehicle_command.param2) + && PX4_ISFINITE(vehicle_command.param5) && PX4_ISFINITE(vehicle_command.param6) + ) { const float measurement_delay_seconds = math::constrain(vehicle_command.param2, 0.0f, kMaxDelaySecondsExternalPosMeasurement); @@ -537,9 +546,32 @@ void EKF2::Run() } // TODO add check for lat and long validity - _ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, - accuracy, timestamp_observation); + if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, + accuracy, timestamp_observation) + ) { + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED; + } + + } else { + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; // TODO: expand } + + command_ack.timestamp = hrt_absolute_time(); + _vehicle_command_ack_pub.publish(command_ack); + } + + if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) { +#if defined(CONFIG_EKF2_WIND) + // wind direction is given as azimuth where wind blows FROM + // PX4 backend expects direction where wind blows TO + const float wind_direction_rad = wrap_pi(math::radians(vehicle_command.param3) + M_PI_F); + const float wind_direction_accuracy_rad = math::radians(vehicle_command.param4); + _ekf.resetWindToExternalObservation(vehicle_command.param1, wind_direction_rad, vehicle_command.param2, + wind_direction_accuracy_rad); +#endif // CONFIG_EKF2_WIND } } } @@ -757,28 +789,31 @@ void EKF2::Run() // publish status/logging messages PublishEventFlags(now); - PublishInnovations(now); - PublishInnovationTestRatios(now); - PublishInnovationVariances(now); - PublishStates(now); PublishStatus(now); PublishStatusFlags(now); - PublishAidSourceStatus(now); + + if (_param_ekf2_log_verbose.get()) { + PublishAidSourceStatus(now); + PublishInnovations(now); + PublishInnovationTestRatios(now); + PublishInnovationVariances(now); + PublishStates(now); #if defined(CONFIG_EKF2_BAROMETER) - PublishBaroBias(now); + PublishBaroBias(now); #endif // CONFIG_EKF2_BAROMETER -#if defined(CONFIG_EKF2_RANGE_FINDER) - PublishRngHgtBias(now); -#endif // CONFIG_EKF2_RANGE_FINDER - #if defined(CONFIG_EKF2_EXTERNAL_VISION) - PublishEvPosBias(now); + PublishEvPosBias(now); #endif // CONFIG_EKF2_EXTERNAL_VISION #if defined(CONFIG_EKF2_GNSS) - PublishGnssHgtBias(now); + PublishGnssHgtBias(now); +#endif // CONFIG_EKF2_GNSS + + } + +#if defined(CONFIG_EKF2_GNSS) PublishGpsStatus(now); PublishYawEstimatorStatus(now); #endif // CONFIG_EKF2_GNSS @@ -810,6 +845,7 @@ void EKF2::VerifyParams() if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO) && (_param_ekf2_mag_type.get() != MagFuseType::HEADING) && (_param_ekf2_mag_type.get() != MagFuseType::NONE) + && (_param_ekf2_mag_type.get() != MagFuseType::INIT) ) { mavlink_log_critical(&_mavlink_log_pub, "EKF2_MAG_TYPE invalid, resetting to default"); @@ -824,6 +860,82 @@ void EKF2::VerifyParams() } #endif // CONFIG_EKF2_MAGNETOMETER + + float delay_max = _param_ekf2_delay_max.get(); + +#if defined(CONFIG_EKF2_AUXVEL) + + if (_param_ekf2_avel_delay.get() > delay_max) { + delay_max = _param_ekf2_avel_delay.get(); + } + +#endif // CONFIG_EKF2_AUXVEL + +#if defined(CONFIG_EKF2_BAROMETER) + + if (_param_ekf2_baro_delay.get() > delay_max) { + delay_max = _param_ekf2_baro_delay.get(); + } + +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_AIRSPEED) + + if (_param_ekf2_asp_delay.get() > delay_max) { + delay_max = _param_ekf2_asp_delay.get(); + } + +#endif // CONFIG_EKF2_AIRSPEED + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (_param_ekf2_mag_delay.get() > delay_max) { + delay_max = _param_ekf2_mag_delay.get(); + } + +#endif // CONFIG_EKF2_MAGNETOMETER + +#if defined(CONFIG_EKF2_RANGE_FINDER) + + if (_param_ekf2_rng_delay.get() > delay_max) { + delay_max = _param_ekf2_rng_delay.get(); + } + +#endif // CONFIG_EKF2_RANGE_FINDER + +#if defined(CONFIG_EKF2_GNSS) + + if (_param_ekf2_gps_delay.get() > delay_max) { + delay_max = _param_ekf2_gps_delay.get(); + } + +#endif // CONFIG_EKF2_GNSS + +#if defined(CONFIG_EKF2_OPTICAL_FLOW) + + if (_param_ekf2_of_delay.get() > delay_max) { + delay_max = _param_ekf2_of_delay.get(); + } + +#endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_param_ekf2_ev_delay.get() > delay_max) { + delay_max = _param_ekf2_ev_delay.get(); + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + + if (delay_max > _param_ekf2_delay_max.get()) { + /* EVENT + * @description EKF2_DELAY_MAX({1}ms) is too small compared to the maximum sensor delay ({2}) + */ + events::send(events::ID("nf_delay_max_too_small"), events::Log::Warning, + "EKF2_DELAY_MAX increased to {2}ms, please reboot", _param_ekf2_delay_max.get(), + delay_max); + _param_ekf2_delay_max.commit_no_notification(delay_max); + } } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -874,9 +986,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_MAGNETOMETER) - // mag heading - PublishAidSourceStatus(_ekf.aid_src_mag_heading(), _status_mag_heading_pub_last, _estimator_aid_src_mag_heading_pub); - // mag 3d PublishAidSourceStatus(_ekf.aid_src_mag(), _status_mag_pub_last, _estimator_aid_src_mag_pub); #endif // CONFIG_EKF2_MAGNETOMETER @@ -895,21 +1004,6 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // optical flow PublishAidSourceStatus(_ekf.aid_src_optical_flow(), _status_optical_flow_pub_last, _estimator_aid_src_optical_flow_pub); #endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_TERRAIN) -# if defined(CONFIG_EKF2_RANGE_FINDER) - // range finder - PublishAidSourceStatus(_ekf.aid_src_terrain_range_finder(), _status_terrain_range_finder_pub_last, - _estimator_aid_src_terrain_range_finder_pub); -#endif // CONFIG_EKF2_RANGE_FINDER - -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - // optical flow - PublishAidSourceStatus(_ekf.aid_src_terrain_optical_flow(), _status_terrain_optical_flow_pub_last, - _estimator_aid_src_terrain_optical_flow_pub); -# endif // CONFIG_EKF2_OPTICAL_FLOW - -#endif // CONFIG_EKF2_TERRAIN } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -963,21 +1057,6 @@ void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) } #endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_RANGE_FINDER) -void EKF2::PublishRngHgtBias(const hrt_abstime ×tamp) -{ - if (_ekf.get_rng_sample_delayed().time_us != 0) { - const BiasEstimator::status &status = _ekf.getRngHgtBiasEstimatorStatus(); - - if (fabsf(status.bias - _last_rng_hgt_bias_published) > 0.001f) { - _estimator_rng_hgt_bias_pub.publish(fillEstimatorBiasMsg(status, _ekf.get_rng_sample_delayed().time_us, timestamp)); - - _last_rng_hgt_bias_published = status.bias; - } - } -} -#endif // CONFIG_EKF2_RANGE_FINDER - #if defined(CONFIG_EKF2_EXTERNAL_VISION) void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) { @@ -1040,16 +1119,7 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) _filter_information_event_changes++; } - // warning events - uint32_t warning_events = _ekf.warning_event_status().value; - bool warning_event_updated = false; - - if (warning_events != 0) { - warning_event_updated = true; - _filter_warning_event_changes++; - } - - if (information_event_updated || warning_event_updated) { + if (information_event_updated) { estimator_event_flags_s event_flags{}; event_flags.timestamp_sample = _ekf.time_delayed_us(); @@ -1072,27 +1142,12 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) event_flags.reset_hgt_to_rng = _ekf.information_event_flags().reset_hgt_to_rng; event_flags.reset_hgt_to_ev = _ekf.information_event_flags().reset_hgt_to_ev; - event_flags.warning_event_changes = _filter_warning_event_changes; - event_flags.gps_quality_poor = _ekf.warning_event_flags().gps_quality_poor; - event_flags.gps_fusion_timout = _ekf.warning_event_flags().gps_fusion_timout; - event_flags.gps_data_stopped = _ekf.warning_event_flags().gps_data_stopped; - event_flags.gps_data_stopped_using_alternate = _ekf.warning_event_flags().gps_data_stopped_using_alternate; - event_flags.height_sensor_timeout = _ekf.warning_event_flags().height_sensor_timeout; - event_flags.stopping_navigation = _ekf.warning_event_flags().stopping_mag_use; - event_flags.invalid_accel_bias_cov_reset = _ekf.warning_event_flags().invalid_accel_bias_cov_reset; - event_flags.bad_yaw_using_gps_course = _ekf.warning_event_flags().bad_yaw_using_gps_course; - event_flags.stopping_mag_use = _ekf.warning_event_flags().stopping_mag_use; - event_flags.vision_data_stopped = _ekf.warning_event_flags().vision_data_stopped; - event_flags.emergency_yaw_reset_mag_stopped = _ekf.warning_event_flags().emergency_yaw_reset_mag_stopped; - event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped; - event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_event_flags_pub.update(event_flags); _last_event_flags_publish = event_flags.timestamp; _ekf.clear_information_events(); - _ekf.clear_warning_events(); } else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) { // continue publishing periodically @@ -1148,6 +1203,9 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = true; } + float delta_hagl = 0.f; + _ekf.get_hagl_reset(&delta_hagl, &global_pos.terrain_reset_counter); + global_pos.delta_terrain = -delta_z; #endif // CONFIG_EKF2_TERRAIN global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning @@ -1186,6 +1244,7 @@ void EKF2::PublishGpsStatus(const hrt_abstime ×tamp) estimator_gps_status.check_fail_max_vert_drift = _ekf.gps_check_fail_status_flags().vdrift; estimator_gps_status.check_fail_max_horz_spd_err = _ekf.gps_check_fail_status_flags().hspeed; estimator_gps_status.check_fail_max_vert_spd_err = _ekf.gps_check_fail_status_flags().vspeed; + estimator_gps_status.check_fail_spoofed_gps = _ekf.gps_check_fail_status_flags().spoofed; estimator_gps_status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_gps_status_pub.publish(estimator_gps_status); @@ -1239,10 +1298,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) // Optical flow innovations.flow[0] = _ekf.aid_src_optical_flow().innovation[0]; innovations.flow[1] = _ekf.aid_src_optical_flow().innovation[1]; -# if defined(CONFIG_EKF2_TERRAIN) - innovations.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation[0]; - innovations.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation[1]; -# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW // heading @@ -1280,7 +1335,7 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) // hagl - innovations.hagl = _ekf.aid_src_terrain_range_finder().innovation; + innovations.hagl = _ekf.aid_src_rng_hgt().innovation; #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -1290,43 +1345,6 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_innovations_pub.publish(innovations); - - // calculate noise filtered velocity innovations which are used for pre-flight checking - if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) { - // fully reset on takeoff or landing - _preflt_checker.reset(); - } - - if (!_ekf.control_status_flags().in_air) { - // TODO: move to run before publications - _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); -#endif // CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW) - // set dist bottom to scale flow innovation - const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2); - _preflt_checker.setDistBottom(dist_bottom); -#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos); - _preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel); - _preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt); -#endif // CONFIG_EKF2_EXTERNAL_VISION -#if defined(CONFIG_EKF2_BAROMETER) - _preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt); -#endif // CONFIG_EKF2_BAROMETER - _preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt); -#if defined(CONFIG_EKF2_RANGE_FINDER) - _preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt); -#endif // CONFIG_EKF2_RANGE_FINDER - - _preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing); - - _preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations); - } } void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) @@ -1373,10 +1391,6 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) // Optical flow test_ratios.flow[0] = _ekf.aid_src_optical_flow().test_ratio[0]; test_ratios.flow[1] = _ekf.aid_src_optical_flow().test_ratio[1]; -# if defined(CONFIG_EKF2_TERRAIN) - test_ratios.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().test_ratio[0]; - test_ratios.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().test_ratio[1]; -# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW // heading @@ -1414,7 +1428,7 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) // hagl - test_ratios.hagl = _ekf.aid_src_terrain_range_finder().test_ratio; + test_ratios.hagl = _ekf.aid_src_rng_hgt().test_ratio; #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -1470,10 +1484,6 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) // Optical flow variances.flow[0] = _ekf.aid_src_optical_flow().innovation_variance[0]; variances.flow[1] = _ekf.aid_src_optical_flow().innovation_variance[1]; -# if defined(CONFIG_EKF2_TERRAIN) - variances.terr_flow[0] = _ekf.aid_src_terrain_optical_flow().innovation_variance[0]; - variances.terr_flow[1] = _ekf.aid_src_terrain_optical_flow().innovation_variance[1]; -# endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_OPTICAL_FLOW // heading @@ -1511,7 +1521,7 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_RANGE_FINDER) // hagl - variances.hagl = _ekf.aid_src_terrain_range_finder().innovation_variance; + variances.hagl = _ekf.aid_src_rng_hgt().innovation_variance; #endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -1587,9 +1597,21 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_TERRAIN) // Distance to bottom surface (ground) in meters, must be positive - lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); - lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); + lpos.dist_bottom = math::max(_ekf.getHagl(), 0.f); + lpos.dist_bottom_var = _ekf.getTerrainVariance(); + _ekf.get_hagl_reset(&lpos.delta_dist_bottom, &lpos.dist_bottom_reset_counter); + + lpos.dist_bottom_sensor_bitfield = vehicle_local_position_s::DIST_BOTTOM_SENSOR_NONE; + + if (_ekf.control_status_flags().rng_terrain) { + lpos.dist_bottom_sensor_bitfield |= vehicle_local_position_s::DIST_BOTTOM_SENSOR_RANGE; + } + + if (_ekf.control_status_flags().opt_flow_terrain) { + lpos.dist_bottom_sensor_bitfield |= vehicle_local_position_s::DIST_BOTTOM_SENSOR_FLOW; + } + #endif // CONFIG_EKF2_TERRAIN _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); @@ -1658,7 +1680,7 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa _ekf.getPositionVariance().copyTo(odom.position_variance); // orientation covariance - _ekf.getQuaternionVariance().copyTo(odom.orientation_variance); + _ekf.getRotVarBody().copyTo(odom.orientation_variance); odom.reset_counter = _ekf.get_quat_reset_count() + _ekf.get_velNE_reset_count() + _ekf.get_velD_reset_count() @@ -1769,18 +1791,32 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.control_mode_flags = _ekf.control_status().value; status.filter_fault_flags = _ekf.fault_status().value; - uint16_t innov_check_flags_temp = 0; - _ekf.get_innovation_test_status(innov_check_flags_temp, status.mag_test_ratio, - status.vel_test_ratio, status.pos_test_ratio, - status.hgt_test_ratio, status.tas_test_ratio, - status.hagl_test_ratio, status.beta_test_ratio); + // vel_test_ratio + float vel_xy_test_ratio = _ekf.getHorizontalVelocityInnovationTestRatio(); + float vel_z_test_ratio = _ekf.getVerticalVelocityInnovationTestRatio(); + + if (PX4_ISFINITE(vel_xy_test_ratio) && PX4_ISFINITE(vel_z_test_ratio)) { + status.vel_test_ratio = math::max(vel_xy_test_ratio, vel_z_test_ratio); + + } else if (PX4_ISFINITE(vel_xy_test_ratio)) { + status.vel_test_ratio = vel_xy_test_ratio; + + } else if (PX4_ISFINITE(vel_z_test_ratio)) { + status.vel_test_ratio = vel_z_test_ratio; - // Bit mismatch between ecl and Firmware, combine the 2 first bits to preserve msg definition - // TODO: legacy use only, those flags are also in estimator_status_flags - status.innovation_check_flags = (innov_check_flags_temp >> 1) | (innov_check_flags_temp & 0x1); + } else { + status.vel_test_ratio = NAN; + } + + status.hdg_test_ratio = _ekf.getHeadingInnovationTestRatio(); + status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio(); + status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio(); + status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio(); + status.hagl_test_ratio = _ekf.getHeightAboveGroundInnovationTestRatio(); + status.beta_test_ratio = _ekf.getSyntheticSideslipInnovationTestRatio(); _ekf.get_ekf_lpos_accuracy(&status.pos_horiz_accuracy, &status.pos_vert_accuracy); - _ekf.get_ekf_soln_status(&status.solution_status_flags); + status.solution_status_flags = _ekf.get_ekf_soln_status(); // reset counters status.reset_count_vel_ne = _ekf.state_reset_status().reset_count.velNE; @@ -1791,10 +1827,13 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.time_slip = _last_time_slip_us * 1e-6f; - status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); - status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); - status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); - status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); + static constexpr float kMinTestRatioPreflight = 0.5f; + status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.hdg_test_ratio); + status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio); + status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio); + status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio); + status.pre_flt_fail_innov_vel_vert = (kMinTestRatioPreflight < vel_z_test_ratio); + status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; @@ -1867,12 +1906,12 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_fuse_aspd = _ekf.control_status_flags().fuse_aspd; status_flags.cs_gnd_effect = _ekf.control_status_flags().gnd_effect; status_flags.cs_rng_stuck = _ekf.control_status_flags().rng_stuck; - status_flags.cs_gps_yaw = _ekf.control_status_flags().gps_yaw; + status_flags.cs_gnss_yaw = _ekf.control_status_flags().gnss_yaw; status_flags.cs_mag_aligned_in_flight = _ekf.control_status_flags().mag_aligned_in_flight; status_flags.cs_ev_vel = _ekf.control_status_flags().ev_vel; status_flags.cs_synthetic_mag_z = _ekf.control_status_flags().synthetic_mag_z; status_flags.cs_vehicle_at_rest = _ekf.control_status_flags().vehicle_at_rest; - status_flags.cs_gps_yaw_fault = _ekf.control_status_flags().gps_yaw_fault; + status_flags.cs_gnss_yaw_fault = _ekf.control_status_flags().gnss_yaw_fault; status_flags.cs_rng_fault = _ekf.control_status_flags().rng_fault; status_flags.cs_inertial_dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning; status_flags.cs_wind_dead_reckoning = _ekf.control_status_flags().wind_dead_reckoning; @@ -1884,6 +1923,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_ev_yaw_fault = _ekf.control_status_flags().ev_yaw_fault; status_flags.cs_mag_heading_consistent = _ekf.control_status_flags().mag_heading_consistent; status_flags.cs_aux_gpos = _ekf.control_status_flags().aux_gpos; + status_flags.cs_rng_terrain = _ekf.control_status_flags().rng_terrain; + status_flags.cs_opt_flow_terrain = _ekf.control_status_flags().opt_flow_terrain; status_flags.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -1990,7 +2031,7 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime ×tamp) _ekf.getFlowGyro().copyTo(flow_vel.gyro_rate); _ekf.getFlowGyroBias().copyTo(flow_vel.gyro_bias); - _ekf.getRefBodyRate().copyTo(flow_vel.ref_gyro); + _ekf.getFlowRefBodyRate().copyTo(flow_vel.ref_gyro); flow_vel.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); @@ -2035,14 +2076,19 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps) if (_airspeed_validated_sub.update(&airspeed_validated)) { if (PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > 0.f) && (airspeed_validated.selected_airspeed_index > 0) ) { + float cas2tas = 1.f; + + if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) + && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + cas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s; + } + airspeedSample airspeed_sample { .time_us = airspeed_validated.timestamp, .true_airspeed = airspeed_validated.true_airspeed_m_s, - .eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, + .eas2tas = cas2tas, }; _ekf.setAirspeedData(airspeed_sample); } @@ -2331,12 +2377,14 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) new_optical_flow = true; } +#if defined(CONFIG_EKF2_RANGE_FINDER) + // use optical_flow distance as range sample if distance_sensor unavailable if (PX4_ISFINITE(optical_flow.distance_m) && (ekf2_timestamps.timestamp > _last_range_sensor_update + 1_s)) { int8_t quality = static_cast(optical_flow.quality) / static_cast(UINT8_MAX) * 100.f; - rangeSample range_sample { + estimator::sensor::rangeSample range_sample { .time_us = optical_flow.timestamp_sample, .rng = optical_flow.distance_m, .quality = quality, @@ -2347,6 +2395,8 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.set_rangefinder_limits(optical_flow.min_ground_distance, optical_flow.max_ground_distance); } +#endif // CONFIG_EKF2_RANGE_FINDER + ekf2_timestamps.optical_flow_timestamp_rel = (int16_t)((int64_t)optical_flow.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100); } @@ -2390,6 +2440,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) .yaw = vehicle_gps_position.heading, //TODO: move to different message .yaw_acc = vehicle_gps_position.heading_accuracy, .yaw_offset = vehicle_gps_position.heading_offset, + .spoofed = vehicle_gps_position.spoofing_state == sensor_gps_s::SPOOFING_STATE_MULTIPLE, }; _ekf.setGpsData(gnss_sample); @@ -2475,7 +2526,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) { // EKF range sample if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) { - rangeSample range_sample { + estimator::sensor::rangeSample range_sample { .time_us = distance_sensor.timestamp, .rng = distance_sensor.current_distance, .quality = distance_sensor.signal_quality, @@ -2604,8 +2655,7 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) && (_ekf.fault_status().value == 0) && !_ekf.fault_status_flags().bad_acc_bias && !_ekf.fault_status_flags().bad_acc_clipping - && !_ekf.fault_status_flags().bad_acc_vertical - && !_ekf.warning_event_flags().invalid_accel_bias_cov_reset; + && !_ekf.fault_status_flags().bad_acc_vertical; const bool learning_valid = bias_valid && !_ekf.accel_bias_inhibited(); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 1d54315b8cc8..ef7280e88c18 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -42,7 +42,6 @@ #define EKF2_HPP #include "EKF/ekf.h" -#include "Utility/PreFlightChecker.hpp" #include "EKF2Selector.hpp" @@ -78,6 +77,7 @@ #include #include #include +#include #include #include #include @@ -162,6 +162,7 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem void Run() override; + void AdvertiseTopics(); void VerifyParams(); void PublishAidSourceStatus(const hrt_abstime ×tamp); @@ -307,11 +308,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem Vector3f _last_mag_bias_published{}; hrt_abstime _status_mag_pub_last{0}; - hrt_abstime _status_mag_heading_pub_last{0}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; - uORB::PublicationMulti _estimator_aid_src_mag_heading_pub{ORB_ID(estimator_aid_src_mag_heading)}; uORB::PublicationMulti _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)}; #endif // CONFIG_EKF2_MAGNETOMETER @@ -339,19 +338,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem hrt_abstime _status_aux_vel_pub_last{0}; #endif // CONFIG_EKF2_AUXVEL -#if defined(CONFIG_EKF2_TERRAIN) - -# if defined(CONFIG_EKF2_RANGE_FINDER) - uORB::PublicationMulti _estimator_aid_src_terrain_range_finder_pub {ORB_ID(estimator_aid_src_terrain_range_finder)}; - hrt_abstime _status_terrain_range_finder_pub_last{0}; -# endif // CONFIG_EKF2_RANGE_FINDER - -# if defined(CONFIG_EKF2_OPTICAL_FLOW) - uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub {ORB_ID(estimator_aid_src_terrain_optical_flow)}; - hrt_abstime _status_terrain_optical_flow_pub_last{0}; -# endif // CONFIG_EKF2_OPTICAL_FLOW -#endif // CONFIG_EKF2_TERRAIN - #if defined(CONFIG_EKF2_OPTICAL_FLOW) uORB::Subscription _vehicle_optical_flow_sub {ORB_ID(vehicle_optical_flow)}; uORB::PublicationMulti _estimator_optical_flow_vel_pub{ORB_ID(estimator_optical_flow_vel)}; @@ -401,17 +387,17 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Publication _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)}; + uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)}; #if defined(CONFIG_EKF2_RANGE_FINDER) hrt_abstime _status_rng_hgt_pub_last {0}; - float _last_rng_hgt_bias_published{}; - uORB::PublicationMulti _estimator_rng_hgt_bias_pub {ORB_ID(estimator_rng_hgt_bias)}; uORB::PublicationMulti _estimator_aid_src_rng_hgt_pub{ORB_ID(estimator_aid_src_rng_hgt)}; uORB::SubscriptionMultiArray _distance_sensor_subs{ORB_ID::distance_sensor}; @@ -431,7 +417,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uint32_t _filter_control_status_changes{0}; uint32_t _filter_fault_status_changes{0}; uint32_t _innov_check_fail_status_changes{0}; - uint32_t _filter_warning_event_changes{0}; uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; @@ -492,14 +477,14 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; #endif // CONFIG_EKF2_GRAVITY_FUSION - PreFlightChecker _preflt_checker; - Ekf _ekf; parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) DEFINE_PARAMETERS( + (ParamBool) _param_ekf2_log_verbose, (ParamExtInt) _param_ekf2_predict_us, + (ParamExtFloat) _param_ekf2_delay_max, (ParamExtInt) _param_ekf2_imu_ctrl, #if defined(CONFIG_EKF2_AUXVEL) @@ -620,7 +605,6 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem (ParamExtFloat) _param_ekf2_min_rng, #endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_TERRAIN) - (ParamExtInt) _param_ekf2_terr_mask, (ParamExtFloat) _param_ekf2_terr_noise, (ParamExtFloat) _param_ekf2_terr_grad, #endif // CONFIG_EKF2_TERRAIN @@ -672,6 +656,8 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem // optical flow fusion (ParamExtInt) _param_ekf2_of_ctrl, ///< optical flow fusion selection + (ParamExtInt) + _param_ekf2_of_gyr_src, (ParamExtFloat) _param_ekf2_of_delay, ///< optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow integration interval (ParamExtFloat) diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index 99fb863eed96..f5f67e2cded9 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/src/modules/ekf2/EKF2Selector.cpp @@ -479,18 +479,32 @@ void EKF2Selector::PublishVehicleLocalPosition() _delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading); } + // HAGL (dist_bottom) reset + if (!instance_change + && (local_position.dist_bottom_reset_counter == _local_position_last.dist_bottom_reset_counter + 1)) { + ++_hagl_reset_counter; + _delta_hagl_reset = local_position.delta_dist_bottom; + + } else if (instance_change + || (local_position.dist_bottom_reset_counter != _local_position_last.dist_bottom_reset_counter)) { + ++_hagl_reset_counter; + _delta_hagl_reset = local_position.dist_bottom - _local_position_last.dist_bottom; + } + } else { _xy_reset_counter = local_position.xy_reset_counter; _z_reset_counter = local_position.z_reset_counter; _vxy_reset_counter = local_position.vxy_reset_counter; _vz_reset_counter = local_position.vz_reset_counter; _heading_reset_counter = local_position.heading_reset_counter; + _hagl_reset_counter = local_position.dist_bottom_reset_counter; _delta_xy_reset = Vector2f{local_position.delta_xy}; _delta_z_reset = local_position.delta_z; _delta_vxy_reset = Vector2f{local_position.delta_vxy}; _delta_vz_reset = local_position.delta_vz; _delta_heading_reset = local_position.delta_heading; + _delta_hagl_reset = local_position.delta_dist_bottom; } bool publish = true; @@ -513,6 +527,7 @@ void EKF2Selector::PublishVehicleLocalPosition() local_position.vxy_reset_counter = _vxy_reset_counter; local_position.vz_reset_counter = _vz_reset_counter; local_position.heading_reset_counter = _heading_reset_counter; + local_position.dist_bottom_reset_counter = _hagl_reset_counter; _delta_xy_reset.copyTo(local_position.delta_xy); local_position.delta_z = _delta_z_reset; @@ -612,6 +627,16 @@ void EKF2Selector::PublishVehicleGlobalPosition() _delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt; } + // terrain reset + if (!instance_change && (global_position.terrain_reset_counter == _global_position_last.terrain_reset_counter + 1)) { + ++_terrain_reset_counter; + _delta_terrain_reset = global_position.delta_terrain; + + } else if (instance_change || (global_position.terrain_reset_counter != _global_position_last.terrain_reset_counter)) { + ++_terrain_reset_counter; + _delta_terrain_reset = global_position.delta_terrain - _global_position_last.delta_terrain; + } + } else { _lat_lon_reset_counter = global_position.lat_lon_reset_counter; _alt_reset_counter = global_position.alt_reset_counter; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 6507233bd3e8..535af97e6d38 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -202,11 +202,14 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem matrix::Vector2f _delta_vxy_reset{}; float _delta_vz_reset{0.f}; float _delta_heading_reset{0}; + float _delta_hagl_reset{0.f}; + uint8_t _xy_reset_counter{0}; uint8_t _z_reset_counter{0}; uint8_t _vxy_reset_counter{0}; uint8_t _vz_reset_counter{0}; uint8_t _heading_reset_counter{0}; + uint8_t _hagl_reset_counter{0}; // vehicle_odometry vehicle_odometry_s _odometry_last{}; @@ -217,8 +220,11 @@ class EKF2Selector : public ModuleParams, public px4::ScheduledWorkItem double _delta_lat_reset{0}; double _delta_lon_reset{0}; float _delta_alt_reset{0.f}; + float _delta_terrain_reset{0.f}; + uint8_t _lat_lon_reset_counter{0}; uint8_t _alt_reset_counter{0}; + uint8_t _terrain_reset_counter{0}; // wind estimate wind_s _wind_last{}; diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index a1899344154f..93f96a656c1e 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -110,7 +110,6 @@ depends on MODULES_EKF2 bool "optical flow fusion support" default y select EKF2_TERRAIN - depends on EKF2_RANGE_FINDER ---help--- EKF2 optical flow fusion support. diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp deleted file mode 100644 index 1ac4b2dd2629..000000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ /dev/null @@ -1,180 +0,0 @@ -/**************************************************************************** - * - * 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 PreFlightCheckHelper.cpp - * Class handling the EKF2 innovation pre flight checks - */ - -#include "PreFlightChecker.hpp" - -void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov) -{ - const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv); - - _has_heading_failed = preFlightCheckHeadingFailed(innov, alpha); - _has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha); - _has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha); - _has_height_failed = preFlightCheckHeightFailed(innov, alpha); -} - -bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha) -{ - const float heading_test_limit = selectHeadingTestLimit(); - const float heading_innov_spike_lim = 2.0f * heading_test_limit; - - const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim); - - return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim); -} - -float PreFlightChecker::selectHeadingTestLimit() -{ - // Select the max allowed heading innovaton depending on whether we are not aiding navigation using - // observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion). - const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding; - - return (is_ne_aiding && !_can_observe_heading_in_flight) - ? _nav_heading_innov_test_lim // more restrictive test limit - : _heading_innov_test_lim; // less restrictive test limit -} - -bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha) -{ - bool has_failed = false; - - if (_is_using_gps_aiding || _is_using_ev_vel_aiding) { - const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])), - fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1]))); - Vector2f vel_ne_innov_lpf; - vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim); - vel_ne_innov_lpf(1) = _filter_vel_e_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim); - has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim); - } - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - - if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom; - Vector2f flow_innov_lpf; - flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); - flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); - has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim); - } - -#endif // CONFIG_EKF2_OPTICAL_FLOW - return has_failed; -} - -bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha) -{ - const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution - const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim); - return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim); -} - -bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha) -{ - bool has_failed = false; - - if (_is_using_baro_hgt_aiding) { - const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - if (_is_using_gps_hgt_aiding) { - const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - -#if defined(CONFIG_EKF2_RANGE_FINDER) - - if (_is_using_rng_hgt_aiding) { - const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - -#endif // CONFIG_EKF2_RANGE_FINDER - - if (_is_using_ev_hgt_aiding) { - const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim); - has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim); - } - - return has_failed; -} - -bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit, - const float spike_limit) -{ - return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit; -} - -bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit, - const float spike_limit) -{ - return innov_lpf.norm_squared() > sq(test_limit) - || innov.norm_squared() > sq(spike_limit); -} - -void PreFlightChecker::reset() -{ - _is_using_gps_aiding = false; - _is_using_ev_pos_aiding = false; - _is_using_ev_vel_aiding = false; - _is_using_baro_hgt_aiding = false; - _is_using_gps_hgt_aiding = false; - _is_using_ev_hgt_aiding = false; - _has_heading_failed = false; - _has_horiz_vel_failed = false; - _has_vert_vel_failed = false; - _has_height_failed = false; - _filter_vel_n_innov.reset(); - _filter_vel_e_innov.reset(); - _filter_vel_d_innov.reset(); - _filter_baro_hgt_innov.reset(); - _filter_gps_hgt_innov.reset(); - _filter_ev_hgt_innov.reset(); - _filter_heading_innov.reset(); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - _is_using_rng_hgt_aiding = false; - _filter_rng_hgt_innov.reset(); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - _is_using_flow_aiding = false; - _filter_flow_x_innov.reset(); - _filter_flow_y_innov.reset(); -#endif // CONFIG_EKF2_OPTICAL_FLOW -} diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp deleted file mode 100644 index a4fe6ab482a1..000000000000 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ /dev/null @@ -1,211 +0,0 @@ -/**************************************************************************** - * - * 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 PreFlightChecker.hpp - * Class handling the EKF2 innovation pre flight checks - * - * First call the update(...) function and then get the results - * using the hasXxxFailed() getters - */ - -#ifndef EKF2_PREFLIGHTCHECKER_HPP -#define EKF2_PREFLIGHTCHECKER_HPP - -#include -#include - -#include - -#include "InnovationLpf.hpp" - -using matrix::Vector2f; - -class PreFlightChecker -{ -public: - PreFlightChecker() = default; - ~PreFlightChecker() = default; - - /* - * Reset all the internal states of the class to their default value - */ - void reset(); - - /* - * Update the internal states - * @param dt the sampling time - * @param innov the ekf2_innovation_s struct containing the current innovations - */ - void update(float dt, const estimator_innovations_s &innov); - - /* - * If set to true, the checker will use a less conservative heading innovation check - */ - void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; } - - void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } - void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; } -#endif // CONFIG_EKF2_OPTICAL_FLOW - void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } - void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } - - void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; } - void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; } -#if defined(CONFIG_EKF2_RANGE_FINDER) - void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; } -#endif // CONFIG_EKF2_RANGE_FINDER - void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; } - - bool hasHeadingFailed() const { return _has_heading_failed; } - bool hasHorizVelFailed() const { return _has_horiz_vel_failed; } - bool hasVertVelFailed() const { return _has_vert_vel_failed; } - bool hasHeightFailed() const { return _has_height_failed; } - - /* - * Overall state of the pre fligh checks - * @return true if any of the check failed - */ - bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); } - - /* - * Horizontal checks overall result - * @return true if one of the horizontal checks failed - */ - bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; } - - /* - * Vertical checks overall result - * @return true if one of the vertical checks failed - */ - bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; } - - /* - * Check if the innovation fails the test - * To pass the test, the following conditions should be true: - * innov_lpf <= test_limit - * innov <= spike_limit - * @param innov_lpf the low-pass filtered innovation - * @param innov the current unfiltered innovation - * @param test_limit the magnitude test limit for innov_lpf - * @param spike_limit the magnitude test limit for innov - * @return true if the check failed the test, false otherwise - */ - static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit); - - /* - * Check if the a innovation of a 2D vector fails the test - * To pass the test, the following conditions should be true: - * innov_lpf <= test_limit - * innov <= spike_limit - * @param innov_lpf the low-pass filtered innovation - * @param innov the current unfiltered innovation - * @param test_limit the magnitude test limit for innov_lpf - * @param spike_limit the magnitude test limit for innov - * @return true if the check failed the test, false otherwise - */ - static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit); - - static constexpr float sq(float var) { return var * var; } - -private: - bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha); - float selectHeadingTestLimit(); - - bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha); - bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha); - bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha); - - bool _has_heading_failed{}; - bool _has_horiz_vel_failed{}; - bool _has_vert_vel_failed{}; - bool _has_height_failed{}; - - bool _can_observe_heading_in_flight{}; - bool _is_using_gps_aiding{}; - bool _is_using_ev_pos_aiding{}; - bool _is_using_ev_vel_aiding{}; - - bool _is_using_baro_hgt_aiding{}; - bool _is_using_gps_hgt_aiding{}; - bool _is_using_ev_hgt_aiding{}; - - // Low-pass filters for innovation pre-flight checks - InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec) - InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec) - InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec) - InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad) - - // Preflight low pass filter height innovation (m) - InnovationLpf _filter_baro_hgt_innov; - InnovationLpf _filter_gps_hgt_innov; - InnovationLpf _filter_ev_hgt_innov; - -#if defined(CONFIG_EKF2_RANGE_FINDER) - bool _is_using_rng_hgt_aiding {}; - InnovationLpf _filter_rng_hgt_innov; -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - bool _is_using_flow_aiding {}; - float _flow_dist_bottom {}; - InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) - InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) - - // Maximum permissible flow innovation to pass pre-flight checks - static constexpr float _flow_innov_test_lim = 0.5f; - - // Preflight flow innovation spike limit (rad) - static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim; -#endif // CONFIG_EKF2_OPTICAL_FLOW - - // Preflight low pass filter time constant inverse (1/sec) - static constexpr float _innov_lpf_tau_inv = 0.2f; - // Maximum permissible velocity innovation to pass pre-flight checks (m/sec) - static constexpr float _vel_innov_test_lim = 0.5f; - // Maximum permissible height innovation to pass pre-flight checks (m) - static constexpr float _hgt_innov_test_lim = 1.5f; - // Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad) - static constexpr float _nav_heading_innov_test_lim = 0.25f; - // Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad) - static constexpr float _heading_innov_test_lim = 0.52f; - - // Preflight velocity innovation spike limit (m/sec) - static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim; - // Preflight position innovation spike limit (m) - static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim; - -}; -#endif // !EKF2_PREFLIGHTCHECKER_HPP diff --git a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp b/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp deleted file mode 100644 index 16e1beddf5e8..000000000000 --- a/src/modules/ekf2/Utility/PreFlightCheckerTest.cpp +++ /dev/null @@ -1,97 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * Test code for PreFlightChecker class - * Run this test only using make tests TESTFILTER=PreFlightChecker - */ - -#include - -#include "PreFlightChecker.hpp" - -class PreFlightCheckerTest : public ::testing::Test -{ -}; - -TEST_F(PreFlightCheckerTest, testInnovFailed) -{ - const float test_limit = 1.0; ///< is the limit for innovation_lpf - const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf - const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5}; - const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1}; - const bool expected_result[9] = {false, false, true, false, true, true, true, true, true}; - - for (int i = 0; i < 9; i++) { - EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), - expected_result[i]); - } - - // Smaller test limit, all the checks should fail except the first - EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0)); - - for (int i = 1; i < 9; i++) { - EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); - } - - // Larger test limit, none of the checks should fail - for (int i = 0; i < 9; i++) { - EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0)); - } -} - -TEST_F(PreFlightCheckerTest, testInnov2dFailed) -{ - const float test_limit = 1.0; - const float spike_limit = 2.0; - Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}}; - Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}}; - const bool expected_result[4] = {false, true, true, true}; - - for (int i = 0; i < 4; i++) { - EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit), - expected_result[i]); - } - - // Smaller test limit, all the checks should fail except the first - EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0)); - - for (int i = 1; i < 4; i++) { - EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0)); - } - - // Larger test limit, none of the checks should fail - for (int i = 0; i < 4; i++) { - EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84)); - } -} diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c deleted file mode 100644 index 396ba9651559..000000000000 --- a/src/modules/ekf2/ekf2_params.c +++ /dev/null @@ -1,1574 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015-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 ekf2_params.c - * Parameter definition for ekf2. - * - * @author Roman Bast - * - */ - -/** - * EKF prediction period - * - * EKF prediction period in microseconds. This should ideally be an integer multiple of the IMU time delta. - * Actual filter update will be an integer multiple of IMU update. - * - * @group EKF2 - * @min 1000 - * @max 20000 - * @unit us - */ -PARAM_DEFINE_INT32(EKF2_PREDICT_US, 10000); - -/** - * IMU control - * - * @group EKF2 - * @min 0 - * @max 7 - * @bit 0 Gyro Bias - * @bit 1 Accel Bias - * @bit 2 Gravity vector fusion - */ -PARAM_DEFINE_INT32(EKF2_IMU_CTRL, 7); - -/** - * Magnetometer measurement delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_DELAY, 0); - -/** - * Barometer measurement delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_BARO_DELAY, 0); - -/** - * GPS measurement delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_DELAY, 110); - -/** - * Optical flow measurement delay relative to IMU measurements - * - * Assumes measurement is timestamped at trailing edge of integration period - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_DELAY, 20); - -/** - * Range finder measurement delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5); - -/** - * Airspeed measurement delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 100); - -/** - * Vision Position Estimator delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 0); - -/** - * Auxiliary Velocity Estimate (e.g from a landing target) delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5); - -/** - * Integer bitmask controlling GPS checks. - * - * Set bits to 1 to enable checks. Checks enabled by the following bit positions - * 0 : Minimum required sat count set by EKF2_REQ_NSATS - * 1 : Maximum allowed PDOP set by EKF2_REQ_PDOP - * 2 : Maximum allowed horizontal position error set by EKF2_REQ_EPH - * 3 : Maximum allowed vertical position error set by EKF2_REQ_EPV - * 4 : Maximum allowed speed error set by EKF2_REQ_SACC - * 5 : Maximum allowed horizontal position rate set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. - * 6 : Maximum allowed vertical position rate set by EKF2_REQ_VDRIFT. This check will only run when the vehicle is on ground and stationary. - * 7 : Maximum allowed horizontal speed set by EKF2_REQ_HDRIFT. This check will only run when the vehicle is on ground and stationary. - * 8 : Maximum allowed vertical velocity discrepancy set by EKF2_REQ_VDRIFT - * - * @group EKF2 - * @min 0 - * @max 511 - * @bit 0 Min sat count (EKF2_REQ_NSATS) - * @bit 1 Max PDOP (EKF2_REQ_PDOP) - * @bit 2 Max horizontal position error (EKF2_REQ_EPH) - * @bit 3 Max vertical position error (EKF2_REQ_EPV) - * @bit 4 Max speed error (EKF2_REQ_SACC) - * @bit 5 Max horizontal position rate (EKF2_REQ_HDRIFT) - * @bit 6 Max vertical position rate (EKF2_REQ_VDRIFT) - * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT) - * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT) - */ -PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245); - -/** - * Required EPH to use GPS. - * - * @group EKF2 - * @min 2 - * @max 100 - * @unit m - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_EPH, 3.0f); - -/** - * Required EPV to use GPS. - * - * @group EKF2 - * @min 2 - * @max 100 - * @unit m - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_EPV, 5.0f); - -/** - * Required speed accuracy to use GPS. - * - * @group EKF2 - * @min 0.5 - * @max 5.0 - * @unit m/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_SACC, 0.5f); - -/** - * Required satellite count to use GPS. - * - * @group EKF2 - * @min 4 - * @max 12 - */ -PARAM_DEFINE_INT32(EKF2_REQ_NSATS, 6); - -/** - * Maximum PDOP to use GPS. - * - * @group EKF2 - * @min 1.5 - * @max 5.0 - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_PDOP, 2.5f); - -/** - * Maximum horizontal drift speed to use GPS. - * - * @group EKF2 - * @min 0.1 - * @max 1.0 - * @unit m/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_HDRIFT, 0.1f); - -/** - * Maximum vertical drift speed to use GPS. - * - * @group EKF2 - * @min 0.1 - * @max 1.5 - * @decimal 2 - * @unit m/s - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.2f); - -/** - * Rate gyro noise for covariance prediction. - * - * @group EKF2 - * @min 0.0001 - * @max 0.1 - * @unit rad/s - * @decimal 4 - */ -PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.5e-2f); - -/** - * Accelerometer noise for covariance prediction. - * - * @group EKF2 - * @min 0.01 - * @max 1.0 - * @unit m/s^2 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 3.5e-1f); - -/** - * Process noise for IMU rate gyro bias prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.01 - * @unit rad/s^2 - * @decimal 6 - */ -PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 1.0e-3f); - -/** - * Process noise for IMU accelerometer bias prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.01 - * @unit m/s^3 - * @decimal 6 - */ -PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-3f); - -/** - * Process noise for body magnetic field prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.1 - * @unit gauss/s - * @decimal 6 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f); - -/** - * Process noise for earth magnetic field prediction. - * - * @group EKF2 - * @min 0.0 - * @max 0.1 - * @unit gauss/s - * @decimal 6 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f); - -/** - * Process noise spectral density for wind velocity prediction. - * - * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second. - * - * @group EKF2 - * @min 0.0 - * @max 1.0 - * @unit m/s^2/sqrt(Hz) - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_WIND_NSD, 5.0e-2f); - -/** - * Measurement noise for GNSS velocity. - * - * @group EKF2 - * @min 0.01 - * @max 5.0 - * @unit m/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.3f); - -/** - * Measurement noise for GNSS position. - * - * @group EKF2 - * @min 0.01 - * @max 10.0 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.5f); - -/** - * Measurement noise for non-aiding position hold. - * - * @group EKF2 - * @min 0.5 - * @max 50.0 - * @unit m - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_NOAID_NOISE, 10.0f); - -/** - * Measurement noise for barometric altitude. - * - * @group EKF2 - * @min 0.01 - * @max 15.0 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.5f); - -/** - * Measurement noise for magnetic heading fusion. - * - * @group EKF2 - * @min 0.01 - * @max 1.0 - * @unit rad - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.3f); - -/** - * Measurement noise for magnetometer 3-axis fusion. - * - * @group EKF2 - * @min 0.001 - * @max 1.0 - * @unit gauss - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_NOISE, 5.0e-2f); - -/** - * Measurement noise for airspeed fusion. - * - * @group EKF2 - * @min 0.5 - * @max 5.0 - * @unit m/s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_EAS_NOISE, 1.4f); - -/** - * Gate size for synthetic sideslip fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f); - -/** - * Noise for synthetic sideslip fusion. - * - * @group EKF2 - * @min 0.1 - * @max 1.0 - * @unit m/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f); - -/** - * Gate size for heading fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 2.6f); - -/** - * Gate size for magnetometer XYZ component fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_GATE, 3.0f); - -/** - * Integer bitmask controlling handling of magnetic declination. - * - * Set bits in the following positions to enable functions. - * 0 : Set to true to use the declination from the geo_lookup library when the GPS position becomes available, set to false to always use the EKF2_MAG_DECL value. - * 1 : Set to true to save the EKF2_MAG_DECL parameter to the value returned by the EKF when the vehicle disarms. - * 2 : Set to true to always use the declination as an observation when 3-axis magnetometer fusion is being used. - * - * @group EKF2 - * @min 0 - * @max 7 - * @bit 0 use geo_lookup declination - * @bit 1 save EKF2_MAG_DECL on disarm - * @bit 2 use declination as an observation - * @reboot_required true - */ -PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); - -/** - * Type of magnetometer fusion - * - * Integer controlling the type of magnetometer fusion used - magnetic heading or 3-component vector. - * The fusion of magnetometer data as a three component vector enables vehicle body fixed hard iron errors to be learned, but requires a stable earth field. - * If set to 'Automatic' magnetic heading fusion is used when on-ground and 3-axis magnetic field fusion in-flight with fallback to magnetic heading fusion if there is insufficient motion to make yaw or magnetic field states observable. - * If set to 'Magnetic heading' magnetic heading fusion is used at all times. - * If set to 'None' the magnetometer will not be used under any circumstance. If no external source of yaw is available, it is possible to use post-takeoff horizontal movement combined with GPS velocity measurements to align the yaw angle with the timer required (depending on the amount of movement and GPS data quality). - * @group EKF2 - * @value 0 Automatic - * @value 1 Magnetic heading - * @value 5 None - * @reboot_required true - */ -PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); - -/** - * Horizontal acceleration threshold used for heading observability check - * - * The heading is assumed to be observable when the body acceleration is greater than this parameter when a global position/velocity aiding source is active. - * - * @group EKF2 - * @min 0.0 - * @max 5.0 - * @unit m/s^2 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_ACCLIM, 0.5f); - -/** - * Gate size for barometric and GPS height fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f); - -/** - * Baro deadzone range for height fusion - * - * Sets the value of deadzone applied to negative baro innovations. - * Deadzone is enabled when EKF2_GND_EFF_DZ > 0. - * - * @group EKF2 - * @min 0.0 - * @max 10.0 - * @unit m - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GND_EFF_DZ, 4.0f); - -/** - * Height above ground level for ground effect zone - * - * Sets the maximum distance to the ground level where negative baro innovations are expected. - * - * @group EKF2 - * @min 0.0 - * @max 5.0 - * @unit m - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GND_MAX_HGT, 0.5f); - -/** - * Gate size for GNSS position fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f); - -/** - * Gate size for GNSS velocity fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f); - -/** - * Gate size for TAS fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 5.0f); - -/** - * Determines the reference source of height data used by the EKF. - * - * When multiple height sources are enabled at the same time, the height estimate will - * always converge towards the reference height source selected by this parameter. - * - * The range sensor and vision options should only be used when for operation over a flat surface as the local NED origin will move up and down with ground level. - * - * @group EKF2 - * @value 0 Barometric pressure - * @value 1 GPS - * @value 2 Range sensor - * @value 3 Vision - * @reboot_required true - */ -PARAM_DEFINE_INT32(EKF2_HGT_REF, 1); - -/** - * Barometric sensor height aiding - * - * If this parameter is enabled then the estimator will make use of the barometric height measurements to estimate its height in addition to other - * height sources (if activated). - * - * @group EKF2 - * @boolean - */ -PARAM_DEFINE_INT32(EKF2_BARO_CTRL, 1); - -/** - * External vision (EV) sensor aiding - * - * Set bits in the following positions to enable: - * 0 : Horizontal position fusion - * 1 : Vertical position fusion - * 2 : 3D velocity fusion - * 3 : Yaw - * - * @group EKF2 - * @min 0 - * @max 15 - * @bit 0 Horizontal position - * @bit 1 Vertical position - * @bit 2 3D velocity - * @bit 3 Yaw - */ -PARAM_DEFINE_INT32(EKF2_EV_CTRL, 15); - -/** - * GNSS sensor aiding - * - * Set bits in the following positions to enable: - * 0 : Longitude and latitude fusion - * 1 : Altitude fusion - * 2 : 3D velocity fusion - * 3 : Dual antenna heading fusion - * - * @group EKF2 - * @min 0 - * @max 15 - * @bit 0 Lon/lat - * @bit 1 Altitude - * @bit 2 3D velocity - * @bit 3 Dual antenna heading - */ -PARAM_DEFINE_INT32(EKF2_GPS_CTRL, 7); - -/** - * Range sensor height aiding - * - * WARNING: Range finder measurements are less reliable and can experience unexpected errors. - * For these reasons, if accurate control of height relative to ground is required, it is recommended to use the MPC_ALT_MODE parameter instead, - * unless baro errors are severe enough to cause problems with landing and takeoff. - * - * To en-/disable range finder for terrain height estimation, use EKF2_TERR_MASK instead. - * - * If this parameter is enabled then the estimator will make use of the range finder measurements to estimate its height in addition to other - * height sources (if activated). Range sensor aiding can be enabled (i.e.: always use) or set in "conditional" mode. - * - * Conditional mode: This enables the range finder to be used during low speed (< EKF2_RNG_A_VMAX) and low altitude (< EKF2_RNG_A_HMAX) - * operation, eg takeoff and landing, where baro interference from rotor wash is excessive and can corrupt EKF state - * estimates. It is intended to be used where a vertical takeoff and landing is performed, and horizontal flight does - * not occur until above EKF2_RNG_A_HMAX. - * - * @group EKF2 - * @value 0 Disable range fusion - * @value 1 Enabled (conditional mode) - * @value 2 Enabled - */ -PARAM_DEFINE_INT32(EKF2_RNG_CTRL, 1); - -/** - * Integer bitmask controlling fusion sources of the terrain estimator - * - * Set bits in the following positions to enable: - * 0 : Set to true to use range finder data if available - * 1 : Set to true to use optical flow data if available - * - * @group EKF2 - * @min 0 - * @max 3 - * @bit 0 use range finder - * @bit 1 use optical flow - */ -PARAM_DEFINE_INT32(EKF2_TERR_MASK, 3); - -/** - * Maximum lapsed time from last fusion of measurements that constrain velocity drift before the EKF will report the horizontal nav solution as invalid. - * - * @group EKF2 - * @group EKF2 - * @min 500000 - * @max 10000000 - * @unit us - */ -PARAM_DEFINE_INT32(EKF2_NOAID_TOUT, 5000000); - -/** - * Measurement noise for range finder fusion - * - * @group EKF2 - * @min 0.01 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_NOISE, 0.1f); - -/** - * Range finder range dependent noise scaler. - * - * Specifies the increase in range finder noise with range. - * - * @group EKF2 - * @min 0.0 - * @max 0.2 - * @unit m/m - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_SFE, 0.05f); - -/** - * Gate size for range finder fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); - -/** - * Expected range finder reading when on ground. - * - * If the vehicle is on ground, is not moving as determined by the motion test and the range finder is returning invalid or no data, then an assumed range value of EKF2_MIN_RNG will be used by the terrain estimator so that a terrain height estimate is available at the start of flight in situations where the range finder may be inside its minimum measurements distance when on ground. - * - * @group EKF2 - * @min 0.01 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); - -/** - * External vision (EV) noise mode - * - * If set to 0 (default) the measurement noise is taken from the vision message and the EV noise parameters are used as a lower bound. - * If set to 1 the observation noise is set from the parameters directly, - * - * @value 0 EV reported variance (parameter lower bound) - * @value 1 EV noise parameters - * @group EKF2 - */ -PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); - -/** - * External vision (EV) minimum quality (optional) - * - * External vision will only be started and fused if the quality metric is above this threshold. - * The quality metric is a completely optional field provided by some VIO systems. - * - * @group EKF2 - * @min 0 - * @max 100 - * @decimal 1 - */ -PARAM_DEFINE_INT32(EKF2_EV_QMIN, 0); - -/** - * Measurement noise for vision position observations used to lower bound or replace the uncertainty included in the message - * - * @group EKF2 - * @min 0.01 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f); - -/** - * Measurement noise for vision velocity observations used to lower bound or replace the uncertainty included in the message - * - * @group EKF2 - * @min 0.01 - * @unit m/s - * @decimal 2 -*/ -PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); - -/** - * Measurement noise for vision angle observations used to lower bound or replace the uncertainty included in the message - * - * @group EKF2 - * @min 0.05 - * @unit rad - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.1f); - -/** - * Accelerometer measurement noise for gravity based observations. - * - * @group EKF2 - * @min 0.1 - * @max 10.0 - * @unit g0 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_GRAV_NOISE, 1.0f); - -/** - * Optical flow aiding - * - * Enable optical flow fusion. - * - * @group EKF2 - * @boolean - */ -PARAM_DEFINE_INT32(EKF2_OF_CTRL, 0); - -/** - * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum - * - * @group EKF2 - * @min 0.05 - * @unit rad/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); - -/** - * Measurement noise for the optical flow sensor. - * - * (when it's reported quality metric is at the minimum set by EKF2_OF_QMIN). - * The following condition must be met: EKF2_OF_N_MAXN >= EKF2_OF_N_MIN - * - * @group EKF2 - * @min 0.05 - * @unit rad/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); - -/** - * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN. - * - * @group EKF2 - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); - -/** - * Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND. - * - * @group EKF2 - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0); - -/** - * Gate size for optical flow fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_GATE, 3.0f); - -/** - * Terrain altitude process noise - accounts for instability in vehicle height estimate - * - * @group EKF2 - * @min 0.5 - * @unit m/s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_TERR_NOISE, 5.0f); - -/** - * Magnitude of terrain gradient - * - * @group EKF2 - * @min 0.0 - * @unit m/m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_TERR_GRAD, 0.5f); - -/** - * X position of IMU in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_IMU_POS_X, 0.0f); - -/** - * Y position of IMU in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Y, 0.0f); - -/** - * Z position of IMU in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_IMU_POS_Z, 0.0f); - -/** - * X position of GPS antenna in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_X, 0.0f); - -/** - * Y position of GPS antenna in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Y, 0.0f); - -/** - * Z position of GPS antenna in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_POS_Z, 0.0f); - -/** - * X position of range finder origin in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_POS_X, 0.0f); - -/** - * Y position of range finder origin in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Y, 0.0f); - -/** - * Z position of range finder origin in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_POS_Z, 0.0f); - -/** - * X position of optical flow focal point in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_POS_X, 0.0f); - -/** - * Y position of optical flow focal point in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f); - -/** - * Z position of optical flow focal point in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f); - -/** -* X position of VI sensor focal point in body frame (forward axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f); - -/** - * Y position of VI sensor focal point in body frame (right axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f); - -/** - * Z position of VI sensor focal point in body frame (down axis with origin relative to vehicle centre of gravity) - * - * @group EKF2 - * @unit m - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); - -/** -* Airspeed fusion threshold. -* -* Airspeed data is fused for wind estimation if above this threshold. -* Set to 0 to disable airspeed fusion. -* For reliable wind estimation both sideslip (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. -* Only applies to fixed-wing vehicles (or VTOLs in fixed-wing mode). -* -* @group EKF2 -* @min 0.0 -* @unit m/s -* @decimal 1 -*/ -PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); - -/** -* Enable synthetic sideslip fusion. -* -* For reliable wind estimation both sideslip and airspeed fusion (see EKF2_ARSP_THR) should be enabled. -* Only applies to fixed-wing vehicles (or VTOLs in fixed-wing mode). -* Note: side slip fusion is currently not supported for tailsitters. -* -* @group EKF2 -* @boolean -*/ -PARAM_DEFINE_INT32(EKF2_FUSE_BETA, 0); - -/** - - * Time constant of the velocity output prediction and smoothing filter - * - * @group EKF2 - * @max 1.0 - * @unit s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.25f); - -/** - * Time constant of the position output prediction and smoothing filter. Controls how tightly the output track the EKF states. - * - * @group EKF2 - * @min 0.1 - * @max 1.0 - * @unit s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f); - -/** - * 1-sigma IMU gyro switch-on bias - * - * @group EKF2 - * @min 0.0 - * @max 0.2 - * @unit rad/s - * @reboot_required true - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f); - -/** - * 1-sigma IMU accelerometer switch-on bias - * - * @group EKF2 - * @min 0.0 - * @max 0.5 - * @unit m/s^2 - * @reboot_required true - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f); - -/** - * 1-sigma tilt angle uncertainty after gravity vector alignment - * - * @group EKF2 - * @min 0.0 - * @max 0.5 - * @unit rad - * @reboot_required true - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_ANGERR_INIT, 0.1f); - -/** - * Range sensor pitch offset. - * - * @group EKF2 - * @min -0.75 - * @max 0.75 - * @unit rad - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f); - -/** - * Maximum horizontal velocity allowed for conditional range aid mode. - * - * If the vehicle horizontal speed exceeds this value then the estimator will not fuse range measurements - * to estimate its height. This only applies when conditional range aid mode is activated (EKF2_RNG_CTRL = 1). - * - * @group EKF2 - * @min 0.1 - * @max 2 - * @unit m/s - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_A_VMAX, 1.0f); - -/** - * Maximum absolute altitude (height above ground level) allowed for conditional range aid mode. - * - * If the vehicle absolute altitude exceeds this value then the estimator will not fuse range measurements - * to estimate its height. This only applies when conditional range aid mode is activated (EKF2_RNG_CTRL = 1). - * - * @group EKF2 - * @min 1.0 - * @max 10.0 - * @unit m - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); - -/** - * Gate size used for innovation consistency checks for range aid fusion - * - * A lower value means HAGL needs to be more stable in order to use range finder for height estimation - * in range aid mode - * - * @group EKF2 - * @unit SD - * @min 0.1 - * @max 5.0 - */ -PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); - -/** - * Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) - * - * - * @group EKF2 - * @unit s - * @min 0.1 - * @max 5 -*/ -PARAM_DEFINE_FLOAT(EKF2_RNG_QLTY_T, 1.0f); - -/** - * Gate size used for range finder kinematic consistency check - * - * To be used, the time derivative of the distance sensor measurements projected on the vertical axis - * needs to be statistically consistent with the estimated vertical velocity of the drone. - * - * Decrease this value to make the filter more robust against range finder faulty data (stuck, reflections, ...). - * - * Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE) before tuning this gate. - * - * @group EKF2 - * @unit SD - * @min 0.1 - * @max 5.0 -*/ -PARAM_DEFINE_FLOAT(EKF2_RNG_K_GATE, 1.0f); - -/** - * Gate size for vision velocity estimate fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 -*/ -PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f); - -/** - * Gate size for vision position fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 -*/ -PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f); - -/** - * Multirotor wind estimation selection - * - * Activate wind speed estimation using specific-force measurements and - * a drag model defined by EKF2_BCOEF_[XY] and EKF2_MCOEF. - * - * Only use on vehicles that have their thrust aligned with the Z axis and - * no thrust in the XY plane. - * - * @group EKF2 - * @boolean - */ -PARAM_DEFINE_INT32(EKF2_DRAG_CTRL, 0); - -/** - * Specific drag force observation noise variance used by the multi-rotor specific drag force model. - * - * Increasing this makes the multi-rotor wind estimates adjust more slowly. - * - * @group EKF2 - * @min 0.5 - * @max 10.0 - * @unit (m/s^2)^2 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_DRAG_NOISE, 2.5f); - -/** - * X-axis ballistic coefficient used for multi-rotor wind estimation. - * - * This parameter controls the prediction of drag produced by bluff body drag along the forward/reverse axis when flying a multi-copter which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. - * Set this parameter to zero to turn off the bluff body drag model for this axis. - * - * @group EKF2 - * @min 0.0 - * @max 200.0 - * @unit kg/m^2 - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_BCOEF_X, 100.0f); - -/** - * Y-axis ballistic coefficient used for multi-rotor wind estimation. - * - * This parameter controls the prediction of drag produced by bluff body drag along the right/left axis when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed squared. The predicted drag from the rotors is specified separately by the EKF2_MCOEF parameter. - * Set this parameter to zero to turn off the bluff body drag model for this axis. - * - * @group EKF2 - * @min 0.0 - * @max 200.0 - * @unit kg/m^2 - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_BCOEF_Y, 100.0f); - -/** - * Propeller momentum drag coefficient used for multi-rotor wind estimation. - * - * This parameter controls the prediction of drag produced by the propellers when flying a multi-copter, which enables estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect scales with speed not speed squared and is produced because some of the air velocity normal to the propeller axis of rotation is lost when passing through the rotor disc. This changes the momentum of the flow which creates a drag reaction force. When comparing un-ducted propellers of the same diameter, the effect is roughly proportional to the area of the propeller blades when viewed side on and changes with propeller selection. Momentum drag is significantly higher for ducted rotors. To account for the drag produced by the body which scales with speed squared, see documentation for the EKF2_BCOEF_X and EKF2_BCOEF_Y parameters. - * Set this parameter to zero to turn off the momentum drag model for both axis. - * - * @group EKF2 - * @min 0 - * @max 1.0 - * @unit 1/s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MCOEF, 0.15f); - - -/** - * Upper limit on airspeed along individual axes used to correct baro for position error effects - * - * @group EKF2 - * @min 5.0 - * @max 50.0 - * @unit m/s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_ASPD_MAX, 20.0f); - -/** - * Static pressure position error coefficient for the positive X axis - * - * This is the ratio of static pressure error to dynamic pressure generated by a positive wind relative velocity along the X body axis. - * If the baro height estimate rises during forward flight, then this will be a negative number. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_PCOEF_XP, 0.0f); - -/** - * Static pressure position error coefficient for the negative X axis. - * - * This is the ratio of static pressure error to dynamic pressure generated by a negative wind relative velocity along the X body axis. - * If the baro height estimate rises during backwards flight, then this will be a negative number. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_PCOEF_XN, 0.0f); - -/** - * Pressure position error coefficient for the positive Y axis. - * - * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the positive Y (RH) body axis. - * If the baro height estimate rises during sideways flight to the right, then this will be a negative number. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_PCOEF_YP, 0.0f); - -/** - * Pressure position error coefficient for the negative Y axis. - * - * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the negative Y (LH) body axis. - * If the baro height estimate rises during sideways flight to the left, then this will be a negative number. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_PCOEF_YN, 0.0f); - -/** - * Static pressure position error coefficient for the Z axis. - * - * This is the ratio of static pressure error to dynamic pressure generated by a wind relative velocity along the Z body axis. - * - * @group EKF2 - * @min -0.5 - * @max 0.5 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_PCOEF_Z, 0.0f); - -/** - * Accelerometer bias learning limit. - * - * The ekf accel bias states will be limited to within a range equivalent to +- of this value. - * - * @group EKF2 - * @min 0.0 - * @max 0.8 - * @unit m/s^2 - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_ABL_LIM, 0.4f); - -/** - * Maximum IMU accel magnitude that allows IMU bias learning. - * - * If the magnitude of the IMU accelerometer vector exceeds this value, the EKF accel bias state estimation will be inhibited. - * This reduces the adverse effect of high manoeuvre accelerations and IMU nonlinerity and scale factor errors on the accel bias estimates. - * - * @group EKF2 - * @min 20.0 - * @max 200.0 - * @unit m/s^2 - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_ABL_ACCLIM, 25.0f); - -/** - * Maximum IMU gyro angular rate magnitude that allows IMU bias learning. - * - * If the magnitude of the IMU angular rate vector exceeds this value, the EKF accel bias state estimation will be inhibited. - * This reduces the adverse effect of rapid rotation rates and associated errors on the accel bias estimates. - * - * @group EKF2 - * @min 2.0 - * @max 20.0 - * @unit rad/s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f); - -/** - * Time constant used by acceleration and angular rate magnitude checks used to inhibit accel bias learning. - * - * The vector magnitude of angular rate and acceleration used to check if learning should be inhibited has a peak hold filter applied to it with an exponential decay. - * This parameter controls the time constant of the decay. - * - * @group EKF2 - * @min 0.1 - * @max 1.0 - * @unit s - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f); - -/** - * Gyro bias learning limit. - * - * The ekf gyro bias states will be limited to within a range equivalent to +- of this value. - * - * @group EKF2 - * @min 0.0 - * @max 0.4 - * @unit rad/s - * @decimal 3 - */ -PARAM_DEFINE_FLOAT(EKF2_GYR_B_LIM, 0.15f); - -/** - * Required GPS health time on startup - * - * Minimum continuous period without GPS failure required to mark a healthy GPS status. - * It can be reduced to speed up initialization, but it's recommended to keep this unchanged for a vehicle. - * - * @group EKF2 - * @min 0.1 - * @decimal 1 - * @unit s - * @reboot_required true - */ -PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); - -/** - * Magnetic field strength test selection - * - * Bitmask to set which check is used to decide whether the magnetometer data is valid. - * - * If GNSS data is received, the magnetic field is compared to a World - * Magnetic Model (WMM), otherwise an average value is used. - * This check is useful to reject occasional hard iron disturbance. - * - * Set bits to 1 to enable checks. Checks enabled by the following bit positions - * 0 : Magnetic field strength. Set tolerance using EKF2_MAG_CHK_STR - * 1 : Magnetic field inclination. Set tolerance using EKF2_MAG_CHK_INC - * 2 : Wait for GNSS to find the theoretical strength and inclination using the WMM - * - * @group EKF2 - * @min 0 - * @max 7 - * @bit 0 Strength (EKF2_MAG_CHK_STR) - * @bit 1 Inclination (EKF2_MAG_CHK_INC) - * @bit 2 Wait for WMM - */ -PARAM_DEFINE_INT32(EKF2_MAG_CHECK, 1); - -/** - * Magnetic field strength check tolerance - * - * Maximum allowed deviation from the expected magnetic field strength to pass the check. - * - * @group EKF2 - * @min 0.0 - * @max 1.0 - * @unit gauss - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_STR, 0.2f); - -/** - * Magnetic field inclination check tolerance - * - * Maximum allowed deviation from the expected magnetic field inclination to pass the check. - * - * @group EKF2 - * @min 0.0 - * @max 90.0 - * @unit deg - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_MAG_CHK_INC, 20.f); - -/** - * Enable synthetic magnetometer Z component measurement. - * - * Use for vehicles where the measured body Z magnetic field is subject to strong magnetic interference. - * For magnetic heading fusion the magnetometer Z measurement will be replaced by a synthetic value calculated - * using the knowledge of the 3D magnetic field vector at the location of the drone. Therefore, this parameter - * will only have an effect if the global position of the drone is known. - * For 3D mag fusion the magnetometer Z measurement will simply be ignored instead of fusing the synthetic value. - * - * @group EKF2 - * @boolean -*/ -PARAM_DEFINE_INT32(EKF2_SYNT_MAG_Z, 0); - -/** - * Default value of true airspeed used in EKF-GSF AHRS calculation. - * - * If no airspeed measurements are available, the EKF-GSF AHRS calculation will assume this value of true airspeed when compensating for centripetal acceleration during turns. Set to zero to disable centripetal acceleration compensation during fixed wing flight modes. - * - * @group EKF2 - * @min 0.0 - * @unit m/s - * @max 100.0 - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GSF_TAS, 15.0f); - -/** - * Aux global position (AGP) sensor aiding - * - * Set bits in the following positions to enable: - * 0 : Horizontal position fusion - * 1 : Vertical position fusion - * - * @group EKF2 - * @min 0 - * @max 3 - * @bit 0 Horizontal position - * @bit 1 Vertical position - */ -PARAM_DEFINE_INT32(EKF2_AGP_CTRL, 1); - -/** - * Aux global position estimator delay relative to IMU measurements - * - * @group EKF2 - * @min 0 - * @max 300 - * @unit ms - * @reboot_required true - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_AGP_DELAY, 0); - -/** - * Measurement noise for aux global position observations used to lower bound or replace the uncertainty included in the message - * - * @group EKF2 - * @min 0.01 - * @unit m - * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_AGP_NOISE, 0.9f); - -/** - * Gate size for aux global position fusion - * - * Sets the number of standard deviations used by the innovation consistency test. - * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 -*/ -PARAM_DEFINE_FLOAT(EKF2_AGP_GATE, 3.0f); diff --git a/src/modules/ekf2/module.yaml b/src/modules/ekf2/module.yaml new file mode 100644 index 000000000000..8ed6e700dbad --- /dev/null +++ b/src/modules/ekf2/module.yaml @@ -0,0 +1,1343 @@ +module_name: ekf2 +parameters: +- group: EKF2 + definitions: + EKF2_EN: + description: + short: EKF2 enable + type: boolean + default: 1 + EKF2_PREDICT_US: + description: + short: EKF prediction period + long: EKF prediction period in microseconds. This should ideally be an integer + multiple of the IMU time delta. Actual filter update will be an integer + multiple of IMU update. + type: int32 + default: 10000 + min: 1000 + max: 20000 + unit: us + EKF2_IMU_CTRL: + description: + short: IMU control + type: bitmask + bit: + 0: Gyro Bias + 1: Accel Bias + 2: Gravity vector fusion + default: 7 + min: 0 + max: 7 + EKF2_DELAY_MAX: + description: + short: Maximum delay of all the aiding sensors + long: Defines the delay between the current time and the delayed-time horizon. + This value should be at least as large as the largest EKF2_XXX_DELAY parameter. + type: float + default: 200 + min: 0 + max: 1000 + unit: ms + reboot_required: true + decimal: 1 + EKF2_MAG_DELAY: + description: + short: Magnetometer measurement delay relative to IMU measurements + type: float + default: 0 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_BARO_DELAY: + description: + short: Barometer measurement delay relative to IMU measurements + type: float + default: 0 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_GPS_DELAY: + description: + short: GPS measurement delay relative to IMU measurements + type: float + default: 110 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_OF_DELAY: + description: + short: Optical flow measurement delay relative to IMU measurements + long: Assumes measurement is timestamped at trailing edge of integration period + type: float + default: 20 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_RNG_DELAY: + description: + short: Range finder measurement delay relative to IMU measurements + type: float + default: 5 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_ASP_DELAY: + description: + short: Airspeed measurement delay relative to IMU measurements + type: float + default: 100 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_EV_DELAY: + description: + short: Vision Position Estimator delay relative to IMU measurements + type: float + default: 0 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_AVEL_DELAY: + description: + short: Auxiliary Velocity Estimate delay relative to IMU measurements + type: float + default: 5 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_GPS_CHECK: + description: + short: Integer bitmask controlling GPS checks + long: 'Each threshold value is defined by the parameter indicated next to the check. + Drift and offset checks only run when the vehicle is on ground and stationary.' + type: bitmask + bit: + 0: Sat count (EKF2_REQ_NSATS) + 1: PDOP (EKF2_REQ_PDOP) + 2: EPH (EKF2_REQ_EPH) + 3: EPV (EKF2_REQ_EPV) + 4: Speed accuracy (EKF2_REQ_SACC) + 5: Horizontal position drift (EKF2_REQ_HDRIFT) + 6: Vertical position drift (EKF2_REQ_VDRIFT) + 7: Horizontal speed offset (EKF2_REQ_HDRIFT) + 8: Vertical speed offset (EKF2_REQ_VDRIFT) + 9: Spoofing + default: 1023 + min: 0 + max: 1023 + EKF2_REQ_EPH: + description: + short: Required EPH to use GPS + type: float + default: 3.0 + min: 2 + max: 100 + unit: m + decimal: 1 + EKF2_REQ_EPV: + description: + short: Required EPV to use GPS + type: float + default: 5.0 + min: 2 + max: 100 + unit: m + decimal: 1 + EKF2_REQ_SACC: + description: + short: Required speed accuracy to use GPS + type: float + default: 0.5 + min: 0.5 + max: 5.0 + unit: m/s + decimal: 2 + EKF2_REQ_NSATS: + description: + short: Required satellite count to use GPS + type: int32 + default: 6 + min: 4 + max: 12 + EKF2_REQ_PDOP: + description: + short: Maximum PDOP to use GPS + type: float + default: 2.5 + min: 1.5 + max: 5.0 + decimal: 1 + EKF2_REQ_HDRIFT: + description: + short: Maximum horizontal drift speed to use GPS + type: float + default: 0.1 + min: 0.1 + max: 1.0 + unit: m/s + decimal: 2 + EKF2_REQ_VDRIFT: + description: + short: Maximum vertical drift speed to use GPS + type: float + default: 0.2 + min: 0.1 + max: 1.5 + decimal: 2 + unit: m/s + EKF2_GYR_NOISE: + description: + short: Rate gyro noise for covariance prediction + type: float + default: 0.015 + min: 0.0001 + max: 0.1 + unit: rad/s + decimal: 4 + EKF2_ACC_NOISE: + description: + short: Accelerometer noise for covariance prediction + type: float + default: 0.35 + min: 0.01 + max: 1.0 + unit: m/s^2 + decimal: 2 + EKF2_GYR_B_NOISE: + description: + short: Process noise for IMU rate gyro bias prediction + type: float + default: 0.001 + min: 0.0 + max: 0.01 + unit: rad/s^2 + decimal: 6 + EKF2_ACC_B_NOISE: + description: + short: Process noise for IMU accelerometer bias prediction + type: float + default: 0.003 + min: 0.0 + max: 0.01 + unit: m/s^3 + decimal: 6 + EKF2_MAG_B_NOISE: + description: + short: Process noise for body magnetic field prediction + type: float + default: 0.0001 + min: 0.0 + max: 0.1 + unit: gauss/s + decimal: 6 + EKF2_MAG_E_NOISE: + description: + short: Process noise for earth magnetic field prediction + type: float + default: 0.001 + min: 0.0 + max: 0.1 + unit: gauss/s + decimal: 6 + EKF2_WIND_NSD: + description: + short: Process noise spectral density for wind velocity prediction + long: When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases + by this amount every second. + type: float + default: 0.05 + min: 0.0 + max: 1.0 + unit: m/s^2/sqrt(Hz) + decimal: 3 + EKF2_GPS_V_NOISE: + description: + short: Measurement noise for GNSS velocity + type: float + default: 0.3 + min: 0.01 + max: 5.0 + unit: m/s + decimal: 2 + EKF2_GPS_P_NOISE: + description: + short: Measurement noise for GNSS position + type: float + default: 0.5 + min: 0.01 + max: 10.0 + unit: m + decimal: 2 + EKF2_NOAID_NOISE: + description: + short: Measurement noise for non-aiding position hold + type: float + default: 10.0 + min: 0.5 + max: 50.0 + unit: m + decimal: 1 + EKF2_BARO_NOISE: + description: + short: Measurement noise for barometric altitude + type: float + default: 3.5 + min: 0.01 + max: 15.0 + unit: m + decimal: 2 + EKF2_HEAD_NOISE: + description: + short: Measurement noise for magnetic heading fusion + type: float + default: 0.3 + min: 0.01 + max: 1.0 + unit: rad + decimal: 2 + EKF2_MAG_NOISE: + description: + short: Measurement noise for magnetometer 3-axis fusion + type: float + default: 0.05 + min: 0.001 + max: 1.0 + unit: gauss + decimal: 3 + EKF2_EAS_NOISE: + description: + short: Measurement noise for airspeed fusion + type: float + default: 1.4 + min: 0.5 + max: 5.0 + unit: m/s + decimal: 1 + EKF2_BETA_GATE: + description: + short: Gate size for synthetic sideslip fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_BETA_NOISE: + description: + short: Noise for synthetic sideslip fusion + type: float + default: 0.3 + min: 0.1 + max: 1.0 + unit: m/s + decimal: 2 + EKF2_HDG_GATE: + description: + short: Gate size for heading fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 2.6 + min: 1.0 + unit: SD + decimal: 1 + EKF2_MAG_GATE: + description: + short: Gate size for magnetometer XYZ component fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 3.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_DECL_TYPE: + description: + short: Integer bitmask controlling handling of magnetic declination + long: 'Set bits in the following positions to enable functions. 0 : Set to + true to use the declination from the geo_lookup library when the GPS position + becomes available, set to false to always use the EKF2_MAG_DECL value. 1 + : Set to true to save the EKF2_MAG_DECL parameter to the value returned + by the EKF when the vehicle disarms.' + type: bitmask + bit: + 0: use geo_lookup declination + 1: save EKF2_MAG_DECL on disarm + default: 3 + min: 0 + max: 3 + reboot_required: true + EKF2_MAG_TYPE: + description: + short: Type of magnetometer fusion + long: Integer controlling the type of magnetometer fusion used - magnetic + heading or 3-component vector. The fusion of magnetometer data as a three + component vector enables vehicle body fixed hard iron errors to be learned, + but requires a stable earth field. If set to 'Automatic' magnetic heading + fusion is used when on-ground and 3-axis magnetic field fusion in-flight. + If set to 'Magnetic heading' magnetic heading fusion is used at all times. + If set to 'None' the magnetometer will not be used under any circumstance. + If no external source of yaw is available, it is possible to use post-takeoff + horizontal movement combined with GNSS velocity measurements to align the yaw angle. + If set to 'Init' the magnetometer is only used to initalize the heading. + type: enum + values: + 0: Automatic + 1: Magnetic heading + 5: None + 6: Init + default: 0 + reboot_required: true + EKF2_MAG_ACCLIM: + description: + short: Horizontal acceleration threshold used for heading observability check + long: The heading is assumed to be observable when the body acceleration is + greater than this parameter when a global position/velocity aiding source + is active. + type: float + default: 0.5 + min: 0.0 + max: 5.0 + unit: m/s^2 + decimal: 2 + EKF2_BARO_GATE: + description: + short: Gate size for barometric and GPS height fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_GND_EFF_DZ: + description: + short: Baro deadzone range for height fusion + long: Sets the value of deadzone applied to negative baro innovations. Deadzone + is enabled when EKF2_GND_EFF_DZ > 0. + type: float + default: 4.0 + min: 0.0 + max: 10.0 + unit: m + decimal: 1 + EKF2_GND_MAX_HGT: + description: + short: Height above ground level for ground effect zone + long: Sets the maximum distance to the ground level where negative baro innovations + are expected. + type: float + default: 0.5 + min: 0.0 + max: 5.0 + unit: m + decimal: 1 + EKF2_GPS_P_GATE: + description: + short: Gate size for GNSS position fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_GPS_V_GATE: + description: + short: Gate size for GNSS velocity fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_TAS_GATE: + description: + short: Gate size for TAS fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_HGT_REF: + description: + short: Determines the reference source of height data used by the EKF + long: When multiple height sources are enabled at the same time, the height + estimate will always converge towards the reference height source selected + by this parameter. The range sensor and vision options should only be used + when for operation over a flat surface as the local NED origin will move + up and down with ground level. + type: enum + values: + 0: Barometric pressure + 1: GPS + 2: Range sensor + 3: Vision + default: 1 + reboot_required: true + EKF2_BARO_CTRL: + description: + short: Barometric sensor height aiding + long: If this parameter is enabled then the estimator will make use of the + barometric height measurements to estimate its height in addition to other + height sources (if activated). + type: boolean + default: 1 + EKF2_EV_CTRL: + description: + short: External vision (EV) sensor aiding + long: 'Set bits in the following positions to enable: 0 : Horizontal position + fusion 1 : Vertical position fusion 2 : 3D velocity fusion 3 : Yaw' + type: bitmask + bit: + 0: Horizontal position + 1: Vertical position + 2: 3D velocity + 3: Yaw + default: 0 + min: 0 + max: 15 + EKF2_GPS_CTRL: + description: + short: GNSS sensor aiding + long: 'Set bits in the following positions to enable: 0 : Longitude and latitude + fusion 1 : Altitude fusion 2 : 3D velocity fusion 3 : Dual antenna heading + fusion' + type: bitmask + bit: + 0: Lon/lat + 1: Altitude + 2: 3D velocity + 3: Dual antenna heading + default: 7 + min: 0 + max: 15 + EKF2_RNG_CTRL: + description: + short: Range sensor height aiding + long: 'WARNING: Range finder measurements are less reliable and can experience + unexpected errors. For these reasons, if accurate control of height relative + to ground is required, it is recommended to use the MPC_ALT_MODE parameter + instead, unless baro errors are severe enough to cause problems with landing + and takeoff. If this parameter is enabled then the estimator + will make use of the range finder measurements to estimate its height in + addition to other height sources (if activated). Range sensor aiding can + be enabled (i.e.: always use) or set in "conditional" mode. Conditional + mode: This enables the range finder to be used during low speed (< EKF2_RNG_A_VMAX) + and low altitude (< EKF2_RNG_A_HMAX) operation, eg takeoff and landing, + where baro interference from rotor wash is excessive and can corrupt EKF + state estimates. It is intended to be used where a vertical takeoff and + landing is performed, and horizontal flight does not occur until above EKF2_RNG_A_HMAX.' + type: enum + values: + 0: Disable range fusion + 1: Enabled (conditional mode) + 2: Enabled + default: 1 + EKF2_NOAID_TOUT: + description: + short: Maximum inertial dead-reckoning time + long: Maximum lapsed time from last fusion of measurements that constrain + velocity drift before the EKF will report the horizontal nav solution as + invalid + type: int32 + default: 5000000 + min: 500000 + max: 10000000 + unit: us + EKF2_RNG_NOISE: + description: + short: Measurement noise for range finder fusion + type: float + default: 0.1 + min: 0.01 + unit: m + decimal: 2 + EKF2_RNG_SFE: + description: + short: Range finder range dependent noise scaler + long: Specifies the increase in range finder noise with range. + type: float + default: 0.05 + min: 0.0 + max: 0.2 + unit: m/m + EKF2_RNG_GATE: + description: + short: Gate size for range finder fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_MIN_RNG: + description: + short: Expected range finder reading when on ground + long: If the vehicle is on ground, is not moving as determined by the motion + test and the range finder is returning invalid or no data, then an assumed + range value of EKF2_MIN_RNG will be used by the terrain estimator so that + a terrain height estimate is available at the start of flight in situations + where the range finder may be inside its minimum measurements distance when + on ground. + type: float + default: 0.1 + min: 0.01 + unit: m + decimal: 2 + EKF2_EV_NOISE_MD: + description: + short: External vision (EV) noise mode + long: If set to 0 (default) the measurement noise is taken from the vision + message and the EV noise parameters are used as a lower bound. If set to + 1 the observation noise is set from the parameters directly, + type: enum + values: + 0: EV reported variance (parameter lower bound) + 1: EV noise parameters + default: 0 + EKF2_EV_QMIN: + description: + short: External vision (EV) minimum quality (optional) + long: External vision will only be started and fused if the quality metric + is above this threshold. The quality metric is a completely optional field + provided by some VIO systems. + type: int32 + default: 0 + min: 0 + max: 100 + decimal: 1 + EKF2_EVP_NOISE: + description: + short: Measurement noise for vision position measurements + long: Used to lower bound or replace the uncertainty included in the message + type: float + default: 0.1 + min: 0.01 + unit: m + decimal: 2 + EKF2_EVV_NOISE: + description: + short: Measurement noise for vision velocity measurements + long: Used to lower bound or replace the uncertainty included in the message + type: float + default: 0.1 + min: 0.01 + unit: m/s + decimal: 2 + EKF2_EVA_NOISE: + description: + short: Measurement noise for vision angle measurements + long: Used to lower bound or replace the uncertainty included in the message + type: float + default: 0.1 + min: 0.05 + unit: rad + decimal: 2 + EKF2_GRAV_NOISE: + description: + short: Accelerometer measurement noise for gravity based observations + type: float + default: 1.0 + min: 0.1 + max: 10.0 + unit: g0 + decimal: 2 + EKF2_OF_CTRL: + description: + short: Optical flow aiding + long: Enable optical flow fusion. + type: boolean + default: 1 + EKF2_OF_GYR_SRC: + description: + short: Optical flow angular rate compensation source + long: 'Auto: use gyro from optical flow message if available, internal gyro otherwise. + Internal: always use internal gyro' + type: enum + values: + 0: Auto + 1: Internal + default: 0 + EKF2_OF_N_MIN: + description: + short: Optical flow minimum noise + long: Measurement noise for the optical flow sensor when it's reported quality + metric is at the maximum + type: float + default: 0.15 + min: 0.05 + unit: rad/s + decimal: 2 + EKF2_OF_N_MAX: + description: + short: Optical flow maximum noise + long: Measurement noise for the optical flow sensor when it's reported quality + metric is at the minimum + type: float + default: 0.5 + min: 0.05 + unit: rad/s + decimal: 2 + EKF2_OF_QMIN: + description: + short: In air optical flow minimum quality + long: Optical Flow data will only be used in air if the sensor reports a + quality metric >= EKF2_OF_QMIN + type: int32 + default: 1 + min: 0 + max: 255 + EKF2_OF_QMIN_GND: + description: + short: On ground optical flow minimum quality + long: Optical Flow data will only be used on the ground if the sensor reports + a quality metric >= EKF2_OF_QMIN_GND + type: int32 + default: 0 + min: 0 + max: 255 + EKF2_OF_GATE: + description: + short: Gate size for optical flow fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 3.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_TERR_NOISE: + description: + short: Terrain altitude process noise + type: float + default: 5.0 + min: 0.5 + unit: m/s + decimal: 1 + EKF2_TERR_GRAD: + description: + short: Magnitude of terrain gradient + type: float + default: 0.5 + min: 0.0 + unit: m/m + decimal: 2 + EKF2_IMU_POS_X: + description: + short: X position of IMU in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_IMU_POS_Y: + description: + short: Y position of IMU in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_IMU_POS_Z: + description: + short: Z position of IMU in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_GPS_POS_X: + description: + short: X position of GPS antenna in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_GPS_POS_Y: + description: + short: Y position of GPS antenna in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_GPS_POS_Z: + description: + short: Z position of GPS antenna in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_RNG_POS_X: + description: + short: X position of range finder origin in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_RNG_POS_Y: + description: + short: Y position of range finder origin in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_RNG_POS_Z: + description: + short: Z position of range finder origin in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_OF_POS_X: + description: + short: X position of optical flow focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_OF_POS_Y: + description: + short: Y position of optical flow focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_OF_POS_Z: + description: + short: Z position of optical flow focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_EV_POS_X: + description: + short: X position of VI sensor focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_EV_POS_Y: + description: + short: Y position of VI sensor focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_EV_POS_Z: + description: + short: Z position of VI sensor focal point in body frame + long: Forward axis with origin relative to vehicle centre of gravity + type: float + default: 0.0 + unit: m + decimal: 3 + EKF2_ARSP_THR: + description: + short: Airspeed fusion threshold + long: Airspeed data is fused for wind estimation if above this threshold. + Set to 0 to disable airspeed fusion. For reliable wind estimation both sideslip + (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. Only applies + to fixed-wing vehicles (or VTOLs in fixed-wing mode). + type: float + default: 0.0 + min: 0.0 + unit: m/s + decimal: 1 + EKF2_FUSE_BETA: + description: + short: Enable synthetic sideslip fusion + long: 'For reliable wind estimation both sideslip and airspeed fusion (see + EKF2_ARSP_THR) should be enabled. Only applies to fixed-wing vehicles (or + VTOLs in fixed-wing mode). Note: side slip fusion is currently not supported + for tailsitters.' + type: boolean + default: 0 + EKF2_TAU_VEL: + description: + short: Time constant of the velocity output prediction and smoothing filter + type: float + default: 0.25 + max: 1.0 + unit: s + decimal: 2 + EKF2_TAU_POS: + description: + short: Output predictor position time constant + long: Controls how tightly the output track the EKF states + type: float + default: 0.25 + min: 0.1 + max: 1.0 + unit: s + decimal: 2 + EKF2_GBIAS_INIT: + description: + short: 1-sigma IMU gyro switch-on bias + type: float + default: 0.1 + min: 0.0 + max: 0.2 + unit: rad/s + reboot_required: true + decimal: 2 + EKF2_ABIAS_INIT: + description: + short: 1-sigma IMU accelerometer switch-on bias + type: float + default: 0.2 + min: 0.0 + max: 0.5 + unit: m/s^2 + reboot_required: true + decimal: 2 + EKF2_ANGERR_INIT: + description: + short: 1-sigma tilt angle uncertainty after gravity vector alignment + type: float + default: 0.1 + min: 0.0 + max: 0.5 + unit: rad + reboot_required: true + decimal: 3 + EKF2_RNG_PITCH: + description: + short: Range sensor pitch offset + type: float + default: 0.0 + min: -0.75 + max: 0.75 + unit: rad + decimal: 3 + EKF2_RNG_A_VMAX: + description: + short: Maximum horizontal velocity allowed for conditional range aid mode + long: If the vehicle horizontal speed exceeds this value then the estimator + will not fuse range measurements to estimate its height. This only applies + when conditional range aid mode is activated (EKF2_RNG_CTRL = 1). + type: float + default: 1.0 + min: 0.1 + max: 2 + unit: m/s + EKF2_RNG_A_HMAX: + description: + short: Maximum height above ground allowed for conditional range aid mode + long: If the vehicle absolute altitude exceeds this value then the estimator + will not fuse range measurements to estimate its height. This only applies + when conditional range aid mode is activated (EKF2_RNG_CTRL = 1). + type: float + default: 5.0 + min: 1.0 + max: 10.0 + unit: m + EKF2_RNG_A_IGATE: + description: + short: Gate size used for innovation consistency checks for range aid fusion + long: A lower value means HAGL needs to be more stable in order to use range + finder for height estimation in range aid mode + type: float + default: 1.0 + unit: SD + min: 0.1 + max: 5.0 + EKF2_RNG_QLTY_T: + description: + short: Minumum range validity period + long: Minimum duration during which the reported range finder signal quality + needs to be non-zero in order to be declared valid (s) + type: float + default: 1.0 + unit: s + min: 0.1 + max: 5 + EKF2_RNG_K_GATE: + description: + short: Gate size used for range finder kinematic consistency check + long: 'To be used, the time derivative of the distance sensor measurements + projected on the vertical axis needs to be statistically consistent with + the estimated vertical velocity of the drone. Decrease this value to make + the filter more robust against range finder faulty data (stuck, reflections, + ...). Note: tune the range finder noise parameters (EKF2_RNG_NOISE and EKF2_RNG_SFE) + before tuning this gate.' + type: float + default: 1.0 + unit: SD + min: 0.1 + max: 5.0 + EKF2_EVV_GATE: + description: + short: Gate size for vision velocity estimate fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 3.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_EVP_GATE: + description: + short: Gate size for vision position fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 5.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_DRAG_CTRL: + description: + short: Multirotor wind estimation selection + long: Activate wind speed estimation using specific-force measurements and + a drag model defined by EKF2_BCOEF_[XY] and EKF2_MCOEF. Only use on vehicles + that have their thrust aligned with the Z axis and no thrust in the XY plane. + type: boolean + default: 0 + EKF2_DRAG_NOISE: + description: + short: Specific drag force observation noise variance + long: Used by the multi-rotor specific drag force model. + Increasing this makes the multi-rotor wind estimates adjust more slowly. + type: float + default: 2.5 + min: 0.5 + max: 10.0 + unit: (m/s^2)^2 + decimal: 2 + EKF2_BCOEF_X: + description: + short: X-axis ballistic coefficient used for multi-rotor wind estimation + long: This parameter controls the prediction of drag produced by bluff body + drag along the forward/reverse axis when flying a multi-copter which enables + estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The + drag produced by this effect scales with speed squared. The predicted drag + from the rotors is specified separately by the EKF2_MCOEF parameter. Set + this parameter to zero to turn off the bluff body drag model for this axis. + type: float + default: 100.0 + min: 0.0 + max: 200.0 + unit: kg/m^2 + decimal: 1 + EKF2_BCOEF_Y: + description: + short: Y-axis ballistic coefficient used for multi-rotor wind estimation + long: This parameter controls the prediction of drag produced by bluff body + drag along the right/left axis when flying a multi-copter, which enables + estimation of wind drift when enabled by the EKF2_DRAG_CTRL parameter. The + drag produced by this effect scales with speed squared. The predicted drag + from the rotors is specified separately by the EKF2_MCOEF parameter. Set + this parameter to zero to turn off the bluff body drag model for this axis. + type: float + default: 100.0 + min: 0.0 + max: 200.0 + unit: kg/m^2 + decimal: 1 + EKF2_MCOEF: + description: + short: Propeller momentum drag coefficient for multi-rotor wind estimation + long: This parameter controls the prediction of drag produced by the propellers + when flying a multi-copter, which enables estimation of wind drift when + enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect + scales with speed not speed squared and is produced because some of the + air velocity normal to the propeller axis of rotation is lost when passing + through the rotor disc. This changes the momentum of the flow which creates + a drag reaction force. When comparing un-ducted propellers of the same diameter, + the effect is roughly proportional to the area of the propeller blades when + viewed side on and changes with propeller selection. Momentum drag is significantly + higher for ducted rotors. To account for the drag produced by the body which + scales with speed squared, see documentation for the EKF2_BCOEF_X and EKF2_BCOEF_Y + parameters. Set this parameter to zero to turn off the momentum drag model + for both axis. + type: float + default: 0.15 + min: 0 + max: 1.0 + unit: 1/s + decimal: 2 + EKF2_ASPD_MAX: + description: + short: Maximum airspeed used for baro static pressure compensation + type: float + default: 20.0 + min: 5.0 + max: 50.0 + unit: m/s + decimal: 1 + EKF2_PCOEF_XP: + description: + short: Static pressure position error coefficient for the positive X axis + long: This is the ratio of static pressure error to dynamic pressure generated + by a positive wind relative velocity along the X body axis. If the baro + height estimate rises during forward flight, then this will be a negative + number. + type: float + default: 0.0 + min: -0.5 + max: 0.5 + decimal: 2 + EKF2_PCOEF_XN: + description: + short: Static pressure position error coefficient for the negative X axis + long: This is the ratio of static pressure error to dynamic pressure generated + by a negative wind relative velocity along the X body axis. If the baro + height estimate rises during backwards flight, then this will be a negative + number. + type: float + default: 0.0 + min: -0.5 + max: 0.5 + decimal: 2 + EKF2_PCOEF_YP: + description: + short: Pressure position error coefficient for the positive Y axis + long: This is the ratio of static pressure error to dynamic pressure generated + by a wind relative velocity along the positive Y (RH) body axis. If the + baro height estimate rises during sideways flight to the right, then this + will be a negative number. + type: float + default: 0.0 + min: -0.5 + max: 0.5 + decimal: 2 + EKF2_PCOEF_YN: + description: + short: Pressure position error coefficient for the negative Y axis + long: This is the ratio of static pressure error to dynamic pressure generated + by a wind relative velocity along the negative Y (LH) body axis. If the + baro height estimate rises during sideways flight to the left, then this + will be a negative number. + type: float + default: 0.0 + min: -0.5 + max: 0.5 + decimal: 2 + EKF2_PCOEF_Z: + description: + short: Static pressure position error coefficient for the Z axis + long: This is the ratio of static pressure error to dynamic pressure generated + by a wind relative velocity along the Z body axis. + type: float + default: 0.0 + min: -0.5 + max: 0.5 + decimal: 2 + EKF2_ABL_LIM: + description: + short: Accelerometer bias learning limit + long: The ekf accel bias states will be limited to within a range equivalent + to +- of this value. + type: float + default: 0.4 + min: 0.0 + max: 0.8 + unit: m/s^2 + decimal: 2 + EKF2_ABL_ACCLIM: + description: + short: Maximum IMU accel magnitude that allows IMU bias learning + long: If the magnitude of the IMU accelerometer vector exceeds this value, + the EKF accel bias state estimation will be inhibited. This reduces the + adverse effect of high manoeuvre accelerations and IMU nonlinerity and scale + factor errors on the accel bias estimates. + type: float + default: 25.0 + min: 20.0 + max: 200.0 + unit: m/s^2 + decimal: 1 + EKF2_ABL_GYRLIM: + description: + short: Maximum IMU gyro angular rate magnitude that allows IMU bias learning + long: If the magnitude of the IMU angular rate vector exceeds this value, + the EKF accel bias state estimation will be inhibited. This reduces the + adverse effect of rapid rotation rates and associated errors on the accel + bias estimates. + type: float + default: 3.0 + min: 2.0 + max: 20.0 + unit: rad/s + decimal: 1 + EKF2_ABL_TAU: + description: + short: Accel bias learning inhibit time constant + long: The vector magnitude of angular rate and acceleration used to check + if learning should be inhibited has a peak hold filter applied to it with + an exponential decay. This parameter controls the time constant of the decay. + type: float + default: 0.5 + min: 0.1 + max: 1.0 + unit: s + decimal: 2 + EKF2_GYR_B_LIM: + description: + short: Gyro bias learning limit + long: The ekf gyro bias states will be limited to within a range equivalent + to +- of this value. + type: float + default: 0.15 + min: 0.0 + max: 0.4 + unit: rad/s + decimal: 3 + EKF2_REQ_GPS_H: + description: + short: Required GPS health time on startup + long: Minimum continuous period without GPS failure required to mark a healthy + GPS status. It can be reduced to speed up initialization, but it's recommended + to keep this unchanged for a vehicle. + type: float + default: 10.0 + min: 0.1 + decimal: 1 + unit: s + reboot_required: true + EKF2_MAG_CHECK: + description: + short: Magnetic field strength test selection + long: 'Bitmask to set which check is used to decide whether the magnetometer + data is valid. If GNSS data is received, the magnetic field is compared + to a World Magnetic Model (WMM), otherwise an average value is used. This + check is useful to reject occasional hard iron disturbance. Set bits to + 1 to enable checks. Checks enabled by the following bit positions 0 : Magnetic + field strength. Set tolerance using EKF2_MAG_CHK_STR 1 : Magnetic field + inclination. Set tolerance using EKF2_MAG_CHK_INC 2 : Wait for GNSS to find + the theoretical strength and inclination using the WMM' + type: bitmask + bit: + 0: Strength (EKF2_MAG_CHK_STR) + 1: Inclination (EKF2_MAG_CHK_INC) + 2: Wait for WMM + default: 1 + min: 0 + max: 7 + EKF2_MAG_CHK_STR: + description: + short: Magnetic field strength check tolerance + long: Maximum allowed deviation from the expected magnetic field strength + to pass the check. + type: float + default: 0.2 + min: 0.0 + max: 1.0 + unit: gauss + decimal: 2 + EKF2_MAG_CHK_INC: + description: + short: Magnetic field inclination check tolerance + long: Maximum allowed deviation from the expected magnetic field inclination + to pass the check. + type: float + default: 20.0 + min: 0.0 + max: 90.0 + unit: deg + decimal: 1 + EKF2_SYNT_MAG_Z: + description: + short: Enable synthetic magnetometer Z component measurement + long: Use for vehicles where the measured body Z magnetic field is subject + to strong magnetic interference. For magnetic heading fusion the magnetometer + Z measurement will be replaced by a synthetic value calculated using the + knowledge of the 3D magnetic field vector at the location of the drone. + Therefore, this parameter will only have an effect if the global position + of the drone is known. For 3D mag fusion the magnetometer Z measurement + will simply be ignored instead of fusing the synthetic value. + type: boolean + default: 0 + EKF2_GSF_TAS: + description: + short: Default value of true airspeed used in EKF-GSF AHRS calculation + long: If no airspeed measurements are available, the EKF-GSF AHRS calculation + will assume this value of true airspeed when compensating for centripetal + acceleration during turns. Set to zero to disable centripetal acceleration + compensation during fixed wing flight modes. + type: float + default: 15.0 + min: 0.0 + unit: m/s + max: 100.0 + decimal: 1 + EKF2_AGP_CTRL: + description: + short: Aux global position (AGP) sensor aiding + long: 'Set bits in the following positions to enable: 0 : Horizontal position + fusion 1 : Vertical position fusion' + type: bitmask + bit: + 0: Horizontal position + 1: Vertical position + default: 0 + min: 0 + max: 3 + EKF2_AGP_DELAY: + description: + short: Aux global position estimator delay relative to IMU measurements + type: float + default: 0 + min: 0 + max: 300 + unit: ms + reboot_required: true + decimal: 1 + EKF2_AGP_NOISE: + description: + short: Measurement noise for aux global position measurements + long: Used to lower bound or replace the uncertainty included in the message + type: float + default: 0.9 + min: 0.01 + unit: m + decimal: 2 + EKF2_AGP_GATE: + description: + short: Gate size for aux global position fusion + long: Sets the number of standard deviations used by the innovation consistency + test. + type: float + default: 3.0 + min: 1.0 + unit: SD + decimal: 1 + EKF2_LOG_VERBOSE: + description: + short: Verbose logging + type: boolean + default: 1 diff --git a/src/modules/ekf2/params_multi.yaml b/src/modules/ekf2/params_multi.yaml new file mode 100644 index 000000000000..04280ce1489c --- /dev/null +++ b/src/modules/ekf2/params_multi.yaml @@ -0,0 +1,24 @@ +module_name: ekf2 +parameters: +- group: EKF2 + definitions: + EKF2_MULTI_IMU: + description: + short: Multi-EKF IMUs + long: Maximum number of IMUs to use for Multi-EKF. Set 0 to disable. Requires + SENS_IMU_MODE 0. + type: int32 + default: 0 + reboot_required: true + min: 0 + max: 4 + EKF2_MULTI_MAG: + description: + short: Multi-EKF Magnetometers + long: Maximum number of magnetometers to use for Multi-EKF. Set 0 to disable. + Requires SENS_MAG_MODE 0. + type: int32 + default: 0 + reboot_required: true + min: 0 + max: 4 diff --git a/src/modules/ekf2/params_selector.yaml b/src/modules/ekf2/params_selector.yaml new file mode 100644 index 000000000000..d6b00eb3ff2c --- /dev/null +++ b/src/modules/ekf2/params_selector.yaml @@ -0,0 +1,47 @@ +module_name: ekf2 +parameters: +- group: EKF2 + definitions: + EKF2_SEL_ERR_RED: + description: + short: Selector error reduce threshold + long: EKF2 instances have to be better than the selected by at least this + amount before their relative score can be reduced. + type: float + default: 0.2 + EKF2_SEL_IMU_RAT: + description: + short: Selector angular rate threshold + long: EKF2 selector angular rate error threshold for comparing gyros. Angular + rate vector differences larger than this will result in accumulated angular + error. + type: float + default: 7.0 + unit: deg/s + EKF2_SEL_IMU_ANG: + description: + short: Selector angular threshold + long: EKF2 selector maximum accumulated angular error threshold for comparing + gyros. Accumulated angular error larger than this will result in the sensor + being declared faulty. + type: float + default: 15.0 + unit: deg + EKF2_SEL_IMU_ACC: + description: + short: Selector acceleration threshold + long: EKF2 selector acceleration error threshold for comparing accelerometers. + Acceleration vector differences larger than this will result in accumulated + velocity error. + type: float + default: 1.0 + unit: m/s^2 + EKF2_SEL_IMU_VEL: + description: + short: Selector angular threshold + long: EKF2 selector maximum accumulated velocity threshold for comparing accelerometers. + Accumulated velocity error larger than this will result in the sensor being + declared faulty. + type: float + default: 2.0 + unit: m/s diff --git a/src/modules/ekf2/params_volatile.yaml b/src/modules/ekf2/params_volatile.yaml new file mode 100644 index 000000000000..327ca770ae00 --- /dev/null +++ b/src/modules/ekf2/params_volatile.yaml @@ -0,0 +1,13 @@ +module_name: ekf2 +parameters: +- group: EKF2 + definitions: + EKF2_MAG_DECL: + description: + short: Magnetic declination + category: System + type: float + default: 0 + volatile: true + unit: deg + decimal: 1 diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 84a665ab0e01..800b568d96d7 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -41,10 +41,11 @@ px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_flow.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_flow_generated.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_fusionLogic.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_gps.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) -px4_add_unit_gtest(SRC test_EKF_gps_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_gnss_yaw.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_gnss_yaw_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_height_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_imuSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) @@ -53,7 +54,7 @@ px4_add_unit_gtest(SRC test_EKF_mag.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_measurementSampling.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_ringbuffer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) -px4_add_unit_gtest(SRC test_EKF_terrain_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) +px4_add_unit_gtest(SRC test_EKF_terrain.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_utils.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_withReplayData.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) diff --git a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv index 93742373c006..ba51b8a17aba 100644 --- a/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv +++ b/src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv @@ -1,391 +1,391 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,0.011,0.011,0.00093,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,0.016,0.016,0.00063,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.018,0.018,0.0008,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.019,0.019,0.00053,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.021,0.021,0.00065,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.023,0.023,0.00047,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00042,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.025,0.025,0.00055,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00042,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0.0027,0.00088,-0.064,8.4e-05,-0.00019,-5.6e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.026,0.026,0.00042,0.88,0.88,2,0.15,0.15,1.4,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0.005,0.0013,-0.078,8.4e-05,-0.00019,-5.6e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.028,0.028,0.00048,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1490000,1,-0.01,-0.014,0.00015,0.024,0.0029,-0.16,0.0037,0.00083,-0.093,0.00014,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,0.95,0.95,2,0.14,0.14,2.1,0.0089,0.0089,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1590000,1,-0.01,-0.014,0.00015,0.03,0.0035,-0.18,0.0064,0.0012,-0.11,0.00014,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.03,0.03,0.00043,1.3,1.3,2,0.21,0.21,2.6,0.0089,0.0089,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1690000,1,-0.011,-0.014,0.00012,0.028,-0.0001,-0.19,0.0045,0.00062,-0.13,0.0002,-0.00087,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.026,0.026,0.00034,1,1,2,0.14,0.14,3,0.0079,0.0079,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1790000,1,-0.011,-0.014,9.5e-05,0.035,-0.002,-0.2,0.0075,0.00054,-0.15,0.0002,-0.00087,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.028,0.028,0.00038,1.3,1.3,2,0.21,0.21,3.5,0.0079,0.0079,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1890000,1,-0.011,-0.015,7.5e-05,0.043,-0.0032,-0.22,0.011,0.00029,-0.17,0.0002,-0.00087,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.031,0.031,0.00043,1.7,1.7,2,0.31,0.31,4.1,0.0079,0.0079,0.00015,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1990000,1,-0.011,-0.014,8.5e-05,0.036,-0.0046,-0.23,0.0081,-0.00027,-0.19,0.00021,-0.0014,-3.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.025,0.025,0.00034,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,0.00011,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2090000,1,-0.011,-0.014,4.7e-05,0.041,-0.0071,-0.24,0.012,-0.00085,-0.22,0.00021,-0.0014,-3.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,0.00011,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2190000,1,-0.011,-0.014,5.8e-05,0.033,-0.0068,-0.26,0.0079,-0.00096,-0.24,0.00017,-0.002,-4.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.02,0.021,0.00031,1.2,1.2,2.1,0.2,0.2,6,0.0056,0.0056,8.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2290000,1,-0.011,-0.014,4.5e-05,0.039,-0.0093,-0.27,0.011,-0.0017,-0.27,0.00017,-0.002,-4.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.022,0.022,0.00034,1.5,1.5,2.1,0.3,0.3,6.7,0.0056,0.0056,8.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2390000,1,-0.011,-0.013,6.2e-05,0.03,-0.0086,-0.29,0.0074,-0.0015,-0.3,8.9e-05,-0.0025,-5.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.017,0.017,0.00028,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,7.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2490000,1,-0.011,-0.013,4.4e-05,0.035,-0.011,-0.3,0.011,-0.0024,-0.32,8.9e-05,-0.0025,-5.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.018,0.018,0.00031,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,7.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2590000,1,-0.011,-0.013,5.8e-05,0.027,-0.009,-0.31,0.0068,-0.0018,-0.36,-1.5e-05,-0.0029,-5.8e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.014,0.014,0.00026,0.88,0.88,2.1,0.18,0.18,9.1,0.0038,0.0038,5.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2690000,1,-0.011,-0.013,5.4e-05,0.031,-0.01,-0.33,0.0097,-0.0028,-0.39,-1.5e-05,-0.0029,-5.8e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.015,0.015,0.00028,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,5.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2790000,1,-0.011,-0.013,4.8e-05,0.024,-0.0093,-0.34,0.0062,-0.0019,-0.42,-0.00012,-0.0033,-6.3e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.012,0.012,0.00024,0.76,0.76,2.2,0.16,0.16,11,0.0032,0.0032,4.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2890000,1,-0.011,-0.013,-1.9e-06,0.027,-0.011,-0.35,0.0087,-0.0029,-0.46,-0.00012,-0.0033,-6.3e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.013,0.013,0.00026,0.94,0.94,2.2,0.23,0.23,12,0.0032,0.0032,4.8e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2990000,1,-0.011,-0.013,4.6e-05,0.022,-0.0095,-0.36,0.0057,-0.0021,-0.49,-0.00023,-0.0036,-6.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.01,0.01,0.00022,0.66,0.66,2.2,0.15,0.15,13,0.0027,0.0027,4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3090000,1,-0.011,-0.013,4.8e-05,0.025,-0.011,-0.38,0.0081,-0.0031,-0.53,-0.00023,-0.0036,-6.7e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.81,0.81,2.2,0.22,0.22,14,0.0027,0.0027,4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3190000,1,-0.011,-0.013,-8.9e-06,0.02,-0.0086,-0.39,0.0053,-0.0021,-0.57,-0.00034,-0.0039,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0089,0.0089,0.00021,0.58,0.58,2.3,0.14,0.14,15,0.0023,0.0023,3.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3290000,1,-0.011,-0.013,3.1e-05,0.023,-0.01,-0.4,0.0074,-0.0031,-0.61,-0.00034,-0.0039,-7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0097,0.0097,0.00022,0.72,0.72,2.3,0.2,0.2,16,0.0023,0.0023,3.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3390000,1,-0.011,-0.012,2.6e-06,0.018,-0.0091,-0.42,0.005,-0.0021,-0.65,-0.00045,-0.0041,-7.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0079,0.0079,0.00019,0.52,0.52,2.3,0.14,0.14,18,0.002,0.002,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3490000,1,-0.011,-0.013,-4.9e-06,0.022,-0.012,-0.43,0.007,-0.0031,-0.69,-0.00045,-0.0041,-7.3e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00021,0.64,0.64,2.3,0.19,0.19,19,0.002,0.002,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3590000,1,-0.011,-0.012,1.8e-05,0.017,-0.011,-0.44,0.0047,-0.0023,-0.73,-0.00055,-0.0044,-7.5e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.007,0.007,0.00018,0.48,0.48,2.4,0.13,0.13,20,0.0017,0.0017,2.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3690000,1,-0.011,-0.012,0.00014,0.02,-0.014,-0.46,0.0066,-0.0035,-0.78,-0.00055,-0.0044,-7.5e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.0077,0.0077,0.00019,0.58,0.58,2.4,0.17,0.17,22,0.0017,0.0017,2.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3790000,1,-0.011,-0.012,0.00019,0.016,-0.013,-0.47,0.0044,-0.0026,-0.82,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0063,0.0063,0.00017,0.44,0.44,2.4,0.12,0.12,23,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3890000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.48,0.0061,-0.004,-0.87,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0068,0.0068,0.00018,0.54,0.54,2.4,0.17,0.17,24,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3990000,1,-0.011,-0.012,0.00016,0.02,-0.016,-0.5,0.0079,-0.0055,-0.92,-0.00067,-0.0046,-7.6e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0074,0.0074,0.00019,0.65,0.65,2.5,0.22,0.22,26,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4090000,1,-0.011,-0.012,0.00015,0.017,-0.014,-0.51,0.0057,-0.0041,-0.97,-0.0008,-0.0048,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0061,0.0061,0.00017,0.5,0.5,2.5,0.16,0.16,28,0.0012,0.0012,1.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4190000,1,-0.011,-0.012,0.00013,0.02,-0.016,-0.53,0.0075,-0.0056,-1,-0.0008,-0.0048,-7.7e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0066,0.0066,0.00018,0.6,0.6,2.5,0.21,0.21,29,0.0012,0.0012,1.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4290000,1,-0.01,-0.012,8.2e-05,0.016,-0.012,-0.54,0.0054,-0.0041,-1.1,-0.00093,-0.0049,-7.8e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0053,0.0053,0.00016,0.46,0.46,2.6,0.15,0.15,31,0.00094,0.00095,1.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4390000,1,-0.01,-0.012,0.0001,0.018,-0.013,-0.55,0.0071,-0.0053,-1.1,-0.00093,-0.0049,-7.8e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0058,0.0058,0.00017,0.55,0.55,2.6,0.2,0.2,33,0.00094,0.00095,1.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4490000,1,-0.01,-0.012,0.00016,0.014,-0.0097,-0.57,0.0051,-0.0037,-1.2,-0.001,-0.0051,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,0.43,0.43,2.6,0.14,0.14,34,0.00077,0.00077,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4590000,1,-0.01,-0.012,0.00019,0.017,-0.011,-0.58,0.0066,-0.0047,-1.2,-0.001,-0.0051,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.005,0.005,0.00016,0.51,0.51,2.7,0.19,0.19,36,0.00077,0.00077,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4690000,1,-0.01,-0.012,0.0002,0.013,-0.0094,-0.6,0.0047,-0.0033,-1.3,-0.0011,-0.0052,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.004,0.004,0.00015,0.4,0.4,2.7,0.14,0.14,38,0.00062,0.00062,1.3e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4790000,1,-0.01,-0.012,0.00019,0.015,-0.011,-0.61,0.0061,-0.0043,-1.4,-0.0011,-0.0052,-7.8e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0043,0.0043,0.00016,0.47,0.47,2.7,0.18,0.18,40,0.00062,0.00062,1.3e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4890000,1,-0.01,-0.011,0.00017,0.012,-0.0095,-0.63,0.0044,-0.0031,-1.4,-0.0012,-0.0053,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0035,0.0035,0.00014,0.37,0.37,2.8,0.13,0.13,42,0.00049,0.00049,1.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4990000,1,-0.01,-0.011,0.00015,0.014,-0.01,-0.64,0.0057,-0.0041,-1.5,-0.0012,-0.0053,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0037,0.0037,0.00015,0.43,0.43,2.8,0.17,0.17,44,0.00049,0.00049,1.1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5090000,1,-0.01,-0.011,0.0002,0.011,-0.0078,-0.66,0.004,-0.0029,-1.6,-0.0013,-0.0054,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.003,0.003,0.00014,0.34,0.34,2.8,0.12,0.12,47,0.00039,0.00039,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5190000,1,-0.01,-0.011,0.00022,0.012,-0.0091,-0.67,0.0052,-0.0038,-1.6,-0.0013,-0.0054,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,0.4,0.4,2.9,0.16,0.16,49,0.00039,0.00039,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5290000,1,-0.0099,-0.011,0.00021,0.0079,-0.0066,-0.68,0.0036,-0.0027,-1.7,-0.0013,-0.0055,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0025,0.0025,0.00013,0.31,0.31,2.9,0.12,0.12,51,0.00031,0.00031,9.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5390000,1,-0.0099,-0.011,0.00027,0.0073,-0.0074,-0.7,0.0043,-0.0034,-1.8,-0.0013,-0.0055,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0027,0.0027,0.00014,0.36,0.36,3,0.15,0.15,54,0.00031,0.00031,9.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5490000,1,-0.0097,-0.011,0.00028,0.0047,-0.0055,-0.71,0.0029,-0.0024,-1.8,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00013,0.28,0.28,3,0.11,0.11,56,0.00024,0.00024,8.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5590000,1,-0.0097,-0.011,0.00026,0.0053,-0.0058,-0.73,0.0034,-0.0029,-1.9,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0023,0.0023,0.00013,0.33,0.33,3,0.15,0.15,59,0.00024,0.00024,8.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5690000,1,-0.0096,-0.011,0.00034,0.0034,-0.0032,-0.74,0.0023,-0.002,-2,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0019,0.0019,0.00012,0.26,0.26,3.1,0.11,0.11,61,0.00019,0.00019,7.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5790000,1,-0.0095,-0.011,0.00033,0.0036,-0.0021,-0.75,0.0026,-0.0023,-2,-0.0014,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.002,0.002,0.00013,0.3,0.3,3.1,0.14,0.14,64,0.00019,0.00019,7.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5890000,1,-0.0094,-0.011,0.00031,0.003,-0.0004,0.0028,0.0017,-0.0015,-3.7e+02,-0.0015,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00012,0.24,0.24,9.8,0.1,0.1,0.52,0.00015,0.00015,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5990000,1,-0.0094,-0.011,0.00033,0.0032,0.0011,0.015,0.002,-0.0014,-3.7e+02,-0.0015,-0.0056,-7.9e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00012,0.28,0.28,8.8,0.13,0.13,0.33,0.00015,0.00015,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6090000,1,-0.0094,-0.011,0.00032,0.0042,0.0023,-0.011,0.0024,-0.0012,-3.7e+02,-0.0015,-0.0056,-7.9e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00013,0.32,0.32,7,0.17,0.17,0.33,0.00015,0.00015,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6190000,1,-0.0094,-0.011,0.00024,0.003,0.0046,-0.005,0.0017,-0.00032,-3.7e+02,-0.0015,-0.0057,-7.9e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00012,0.25,0.25,4.9,0.13,0.13,0.32,0.00012,0.00012,6.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6290000,1,-0.0094,-0.011,0.00022,0.0042,0.0047,-0.012,0.0021,0.00013,-3.7e+02,-0.0015,-0.0057,-7.9e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00012,0.29,0.29,3.2,0.16,0.16,0.3,0.00012,0.00012,6.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6390000,0.77,-0.024,0.0046,-0.63,-0.012,-0.0013,-0.05,0.0011,-0.001,-3.7e+02,-0.0014,-0.0057,-8.1e-05,0,0,-0.0001,-0.097,-0.021,0.51,-0.00012,-0.08,-0.061,0,0,0.0012,0.0012,0.065,0.21,0.21,2.3,0.12,0.12,0.29,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.04,0.0014,0.00038,0.0014,0.001,0.0014,0.0014,1,1 -6490000,0.78,-0.024,0.005,-0.63,-0.038,-0.012,-0.052,-0.0019,-0.0044,-3.7e+02,-0.0013,-0.0057,-8.5e-05,0,0,-0.00015,-0.1,-0.022,0.51,-0.00039,-0.084,-0.064,0,0,0.0012,0.0012,0.056,0.21,0.21,1.5,0.15,0.15,0.26,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00022,0.0013,0.00092,0.0014,0.0013,1,1 -6590000,0.78,-0.024,0.0052,-0.63,-0.064,-0.021,-0.098,-0.0077,-0.0087,-3.7e+02,-0.0012,-0.0057,-8.8e-05,-3.6e-05,0.00025,2.5e-05,-0.1,-0.023,0.51,-0.00086,-0.085,-0.066,0,0,0.0012,0.0012,0.053,0.22,0.22,1.1,0.18,0.18,0.23,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00017,0.0013,0.00088,0.0013,0.0013,1,1 -6690000,0.78,-0.024,0.0053,-0.63,-0.091,-0.035,-0.075,-0.017,-0.017,-3.7e+02,-0.001,-0.0057,-9.4e-05,9.3e-05,0.00099,-0.0003,-0.1,-0.023,0.5,-0.001,-0.086,-0.067,0,0,0.0012,0.0012,0.051,0.23,0.23,0.78,0.22,0.22,0.21,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00014,0.0013,0.00087,0.0013,0.0013,1,1 -6790000,0.78,-0.024,0.0054,-0.63,-0.12,-0.045,-0.11,-0.026,-0.026,-3.7e+02,-0.00089,-0.0057,-9.7e-05,-1.3e-05,0.0015,-6.5e-05,-0.1,-0.023,0.5,-0.00099,-0.086,-0.068,0,0,0.0012,0.0012,0.051,0.25,0.25,0.6,0.26,0.26,0.2,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00013,0.0013,0.00086,0.0013,0.0013,1,1 -6890000,0.78,-0.024,0.0054,-0.63,-0.14,-0.053,-0.12,-0.039,-0.033,-3.7e+02,-0.00083,-0.0057,-9.8e-05,-7.5e-05,0.0017,-0.00011,-0.1,-0.023,0.5,-0.001,-0.086,-0.068,0,0,0.0012,0.0012,0.05,0.26,0.26,0.46,0.31,0.31,0.18,9.7e-05,9.7e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00012,0.0013,0.00086,0.0013,0.0013,1,1 -6990000,0.78,-0.024,0.0054,-0.63,-0.17,-0.065,-0.12,-0.059,-0.045,-3.7e+02,-0.0007,-0.0058,-0.00011,0.00036,0.0023,-0.00042,-0.1,-0.023,0.5,-0.0011,-0.086,-0.068,0,0,0.0012,0.0012,0.05,0.28,0.28,0.36,0.36,0.36,0.16,9.6e-05,9.6e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.00011,0.0013,0.00085,0.0013,0.0013,1,1 -7090000,0.78,-0.024,0.0056,-0.63,-0.2,-0.077,-0.12,-0.084,-0.062,-3.7e+02,-0.0005,-0.0059,-0.00011,0.00085,0.0031,-0.00078,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0012,0.0013,0.049,0.31,0.31,0.29,0.42,0.42,0.16,9.5e-05,9.6e-05,5.8e-06,0.04,0.04,0.04,0.0013,0.0001,0.0013,0.00085,0.0013,0.0013,1,1 -7190000,0.78,-0.024,0.0057,-0.63,-0.23,-0.085,-0.14,-0.11,-0.073,-3.7e+02,-0.00044,-0.006,-0.00012,0.001,0.0033,-0.00056,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0013,0.0013,0.049,0.34,0.33,0.24,0.49,0.48,0.15,9.4e-05,9.5e-05,5.8e-06,0.04,0.04,0.04,0.0013,9.9e-05,0.0013,0.00085,0.0013,0.0013,1,1 -7290000,0.78,-0.024,0.0056,-0.63,-0.25,-0.081,-0.14,-0.13,-0.068,-3.7e+02,-0.00066,-0.006,-0.00011,0.00095,0.0024,-0.0012,-0.1,-0.023,0.5,-0.0011,-0.086,-0.068,0,0,0.0013,0.0013,0.049,0.37,0.36,0.2,0.56,0.55,0.14,9.2e-05,9.3e-05,5.8e-06,0.04,0.04,0.04,0.0013,9.5e-05,0.0013,0.00085,0.0013,0.0013,1,1 -7390000,0.78,-0.024,0.0056,-0.63,-0.28,-0.086,-0.16,-0.16,-0.075,-3.7e+02,-0.0007,-0.0059,-0.00011,0.00082,0.0023,-0.0014,-0.1,-0.023,0.5,-0.0011,-0.086,-0.068,0,0,0.0013,0.0013,0.049,0.4,0.39,0.18,0.63,0.63,0.13,9.1e-05,9.2e-05,5.8e-06,0.04,0.04,0.039,0.0013,9.2e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7490000,0.78,-0.024,0.0057,-0.63,-0.3,-0.1,-0.16,-0.19,-0.096,-3.7e+02,-0.00051,-0.0059,-0.00011,0.00079,0.0031,-0.0021,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0013,0.0013,0.049,0.44,0.43,0.15,0.72,0.71,0.12,8.9e-05,9e-05,5.8e-06,0.04,0.04,0.039,0.0013,9e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7590000,0.78,-0.024,0.0056,-0.63,-0.32,-0.1,-0.16,-0.21,-0.1,-3.7e+02,-0.0006,-0.0058,-0.00011,0.00035,0.0028,-0.003,-0.1,-0.023,0.5,-0.0011,-0.086,-0.068,0,0,0.0013,0.0013,0.048,0.47,0.47,0.14,0.81,0.8,0.12,8.6e-05,8.7e-05,5.8e-06,0.04,0.04,0.039,0.0013,8.7e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7690000,0.78,-0.024,0.0056,-0.63,-0.35,-0.12,-0.16,-0.24,-0.12,-3.7e+02,-0.00048,-0.0058,-0.00011,0.00031,0.0033,-0.005,-0.1,-0.023,0.5,-0.0011,-0.086,-0.068,0,0,0.0013,0.0014,0.048,0.52,0.51,0.13,0.91,0.9,0.11,8.4e-05,8.5e-05,5.8e-06,0.04,0.04,0.039,0.0013,8.6e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7790000,0.78,-0.024,0.0058,-0.63,-0.39,-0.12,-0.16,-0.3,-0.14,-3.7e+02,-0.00043,-0.006,-0.00012,0.00098,0.0036,-0.007,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0014,0.0014,0.048,0.56,0.55,0.12,1,1,0.11,8.1e-05,8.2e-05,5.8e-06,0.04,0.04,0.038,0.0013,8.4e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7890000,0.78,-0.025,0.0058,-0.63,-0.41,-0.13,-0.15,-0.33,-0.15,-3.7e+02,-0.00039,-0.0059,-0.00012,0.00061,0.0039,-0.0095,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0014,0.0014,0.048,0.61,0.6,0.11,1.1,1.1,0.1,7.8e-05,8e-05,5.8e-06,0.04,0.04,0.038,0.0013,8.3e-05,0.0013,0.00084,0.0013,0.0013,1,1 -7990000,0.78,-0.025,0.0057,-0.63,-0.43,-0.14,-0.16,-0.36,-0.16,-3.7e+02,-0.00041,-0.0058,-0.00011,0.00024,0.0038,-0.011,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0014,0.0014,0.048,0.66,0.65,0.1,1.3,1.2,0.099,7.5e-05,7.6e-05,5.8e-06,0.04,0.04,0.038,0.0013,8.1e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8090000,0.78,-0.025,0.0057,-0.63,-0.45,-0.15,-0.17,-0.4,-0.18,-3.7e+02,-0.00037,-0.0057,-0.00011,-0.00031,0.004,-0.011,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0014,0.0014,0.048,0.72,0.71,0.1,1.4,1.4,0.097,7.2e-05,7.4e-05,5.8e-06,0.04,0.04,0.037,0.0013,8e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8190000,0.78,-0.025,0.0058,-0.63,-0.48,-0.15,-0.17,-0.46,-0.19,-3.7e+02,-0.0004,-0.0058,-0.00011,0.00015,0.004,-0.013,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.78,0.76,0.099,1.6,1.5,0.094,6.8e-05,7e-05,5.7e-06,0.04,0.04,0.037,0.0013,7.9e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8290000,0.78,-0.025,0.006,-0.63,-0.022,-0.005,-0.17,-0.47,-0.2,-3.7e+02,-0.00028,-0.0059,-0.00012,0.00023,0.0041,-0.017,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0014,0.0015,0.048,25,25,0.097,1e+02,1e+02,0.092,6.5e-05,6.7e-05,5.7e-06,0.04,0.04,0.036,0.0013,7.8e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8390000,0.78,-0.025,0.0059,-0.63,-0.048,-0.012,-0.17,-0.47,-0.2,-3.7e+02,-0.00026,-0.0058,-0.00012,0.00023,0.0041,-0.021,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0014,0.0015,0.048,25,25,0.097,1e+02,1e+02,0.091,6.2e-05,6.4e-05,5.7e-06,0.04,0.04,0.035,0.0013,7.8e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8490000,0.78,-0.025,0.006,-0.63,-0.075,-0.019,-0.17,-0.48,-0.2,-3.7e+02,-0.00026,-0.0058,-0.00012,0.00023,0.0041,-0.025,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.096,51,51,0.089,5.8e-05,6.1e-05,5.7e-06,0.04,0.04,0.034,0.0013,7.7e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8590000,0.78,-0.025,0.0059,-0.63,-0.099,-0.026,-0.16,-0.48,-0.2,-3.7e+02,-0.0005,-0.006,-0.00012,0.00023,0.0041,-0.029,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.095,52,52,0.088,5.5e-05,5.7e-05,5.7e-06,0.04,0.04,0.034,0.0013,7.6e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8690000,0.78,-0.025,0.0058,-0.63,-0.12,-0.034,-0.16,-0.49,-0.21,-3.7e+02,-0.00048,-0.0058,-0.00011,0.00023,0.0041,-0.034,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.096,35,35,0.088,5.2e-05,5.5e-05,5.7e-06,0.04,0.04,0.033,0.0013,7.6e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8790000,0.78,-0.025,0.006,-0.63,-0.15,-0.041,-0.15,-0.5,-0.21,-3.7e+02,-0.00044,-0.0058,-0.00011,0.00023,0.0041,-0.04,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0015,0.048,25,25,0.096,37,37,0.087,4.9e-05,5.1e-05,5.7e-06,0.04,0.04,0.032,0.0013,7.5e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8890000,0.78,-0.025,0.0061,-0.63,-0.17,-0.046,-0.15,-0.51,-0.21,-3.7e+02,-0.00044,-0.0059,-0.00012,0.00023,0.0041,-0.044,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0015,0.0015,0.048,24,24,0.095,28,28,0.086,4.6e-05,4.8e-05,5.7e-06,0.04,0.04,0.03,0.0013,7.4e-05,0.0013,0.00084,0.0013,0.0013,1,1 -8990000,0.78,-0.025,0.0062,-0.63,-0.2,-0.05,-0.14,-0.53,-0.21,-3.7e+02,-0.00036,-0.006,-0.00012,0.00023,0.0041,-0.05,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0015,0.0015,0.048,24,24,0.096,30,30,0.087,4.4e-05,4.6e-05,5.7e-06,0.04,0.04,0.029,0.0013,7.4e-05,0.0013,0.00084,0.0013,0.0013,1,1 -9090000,0.78,-0.025,0.0062,-0.63,-0.22,-0.053,-0.14,-0.53,-0.22,-3.7e+02,-0.00043,-0.0061,-0.00012,0.00023,0.0041,-0.052,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0015,0.0016,0.048,23,23,0.095,25,25,0.086,4.1e-05,4.3e-05,5.7e-06,0.04,0.04,0.028,0.0013,7.4e-05,0.0013,0.00084,0.0013,0.0013,1,1 -9190000,0.78,-0.025,0.006,-0.63,-0.25,-0.066,-0.14,-0.55,-0.22,-3.7e+02,-0.00041,-0.0058,-0.00011,0.00023,0.0041,-0.055,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,23,23,0.094,27,27,0.085,3.8e-05,4e-05,5.7e-06,0.04,0.04,0.027,0.0013,7.3e-05,0.0013,0.00084,0.0013,0.0013,1,1 -9290000,0.78,-0.025,0.0061,-0.63,-0.27,-0.071,-0.13,-0.56,-0.22,-3.7e+02,-0.00036,-0.0058,-0.00011,0.00023,0.0041,-0.06,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,21,21,0.093,23,23,0.085,3.6e-05,3.8e-05,5.7e-06,0.04,0.04,0.025,0.0013,7.3e-05,0.0013,0.00084,0.0013,0.0013,1,1 -9390000,0.78,-0.025,0.0062,-0.63,-0.3,-0.08,-0.13,-0.59,-0.23,-3.7e+02,-0.00032,-0.0058,-0.00012,0.00023,0.0041,-0.063,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0015,0.0016,0.048,21,21,0.093,26,26,0.086,3.4e-05,3.6e-05,5.7e-06,0.04,0.04,0.024,0.0013,7.2e-05,0.0013,0.00084,0.0013,0.0013,1,1 -9490000,0.78,-0.026,0.0061,-0.63,-0.31,-0.087,-0.13,-0.59,-0.23,-3.7e+02,-0.00027,-0.0056,-0.00011,0.00023,0.0041,-0.067,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,19,19,0.091,22,22,0.085,3.2e-05,3.4e-05,5.7e-06,0.04,0.04,0.023,0.0013,7.2e-05,0.0013,0.00083,0.0013,0.0013,1,1 -9590000,0.78,-0.025,0.0059,-0.63,-0.33,-0.095,-0.12,-0.62,-0.24,-3.7e+02,-0.0004,-0.0056,-0.00011,0.00023,0.0041,-0.07,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,19,19,0.09,25,25,0.085,3e-05,3.2e-05,5.7e-06,0.04,0.04,0.022,0.0013,7.2e-05,0.0013,0.00083,0.0013,0.0013,1,1 -9690000,0.78,-0.025,0.006,-0.63,-0.33,-0.092,-0.12,-0.61,-0.24,-3.7e+02,-0.00046,-0.0058,-0.00011,0.00023,0.0041,-0.075,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,17,17,0.089,22,22,0.086,2.8e-05,3e-05,5.7e-06,0.04,0.04,0.02,0.0013,7.1e-05,0.0013,0.00083,0.0013,0.0013,1,1 -9790000,0.78,-0.025,0.006,-0.63,-0.36,-0.1,-0.11,-0.65,-0.25,-3.7e+02,-0.00044,-0.0057,-0.00011,0.00023,0.0041,-0.08,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,17,17,0.087,25,25,0.085,2.6e-05,2.8e-05,5.7e-06,0.04,0.04,0.019,0.0013,7.1e-05,0.0013,0.00083,0.0013,0.0013,1,1 -9890000,0.78,-0.025,0.0059,-0.63,-0.36,-0.1,-0.1,-0.64,-0.25,-3.7e+02,-0.0005,-0.0057,-0.00011,0.00023,0.0041,-0.083,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,15,15,0.084,22,22,0.085,2.5e-05,2.6e-05,5.7e-06,0.04,0.04,0.018,0.0013,7.1e-05,0.0013,0.00083,0.0013,0.0013,1,1 -9990000,0.78,-0.025,0.006,-0.63,-0.39,-0.11,-0.097,-0.67,-0.26,-3.7e+02,-0.00052,-0.0057,-0.00011,0.00023,0.0041,-0.086,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,15,15,0.083,25,25,0.086,2.3e-05,2.5e-05,5.7e-06,0.04,0.04,0.017,0.0013,7.1e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10090000,0.78,-0.025,0.0058,-0.63,-0.41,-0.11,-0.093,-0.71,-0.27,-3.7e+02,-0.0006,-0.0057,-0.00011,0.00023,0.0041,-0.089,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0016,0.048,15,15,0.081,28,28,0.085,2.2e-05,2.3e-05,5.7e-06,0.04,0.04,0.016,0.0013,7e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10190000,0.78,-0.025,0.0061,-0.63,-0.41,-0.11,-0.093,-0.7,-0.26,-3.7e+02,-0.00061,-0.0059,-0.00011,0.00023,0.0041,-0.09,-0.1,-0.023,0.5,-0.0012,-0.086,-0.069,0,0,0.0015,0.0015,0.048,14,14,0.078,24,24,0.084,2.1e-05,2.2e-05,5.7e-06,0.04,0.04,0.015,0.0013,7e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10290000,0.78,-0.025,0.0063,-0.63,-0.44,-0.11,-0.08,-0.74,-0.27,-3.7e+02,-0.00062,-0.006,-0.00011,0.00023,0.0041,-0.096,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0015,0.0015,0.048,14,14,0.076,27,27,0.085,1.9e-05,2.1e-05,5.7e-06,0.04,0.04,0.014,0.0013,7e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10390000,0.78,-0.025,0.0064,-0.63,-0.016,-0.026,0.0097,-0.00026,-0.0021,-3.7e+02,-0.00059,-0.006,-0.00011,0.00018,0.0044,-0.099,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0015,0.0015,0.048,0.25,0.25,0.56,0.25,0.25,0.078,1.8e-05,2e-05,5.7e-06,0.04,0.039,0.013,0.0013,7e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10490000,0.78,-0.025,0.0065,-0.63,-0.044,-0.032,0.023,-0.0033,-0.005,-3.7e+02,-0.0006,-0.006,-0.00012,0.00039,0.0046,-0.1,-0.1,-0.023,0.5,-0.0012,-0.086,-0.068,0,0,0.0015,0.0015,0.048,0.26,0.26,0.55,0.26,0.26,0.08,1.7e-05,1.9e-05,5.7e-06,0.04,0.039,0.012,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10590000,0.78,-0.025,0.0062,-0.63,-0.042,-0.022,0.026,0.0012,-0.001,-3.7e+02,-0.00075,-0.006,-0.00011,0.00062,0.0031,-0.1,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.13,0.13,0.27,0.13,0.13,0.073,1.6e-05,1.7e-05,5.7e-06,0.039,0.039,0.012,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10690000,0.78,-0.025,0.0062,-0.63,-0.069,-0.026,0.03,-0.0044,-0.0034,-3.7e+02,-0.00074,-0.006,-0.00011,0.00064,0.0031,-0.1,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.14,0.14,0.26,0.14,0.14,0.078,1.5e-05,1.7e-05,5.7e-06,0.039,0.039,0.011,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10790000,0.78,-0.024,0.006,-0.63,-0.065,-0.021,0.024,5e-05,-0.0014,-3.7e+02,-0.00079,-0.006,-0.00011,0.00084,0.00042,-0.1,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.096,0.096,0.17,0.09,0.09,0.072,1.4e-05,1.6e-05,5.7e-06,0.039,0.039,0.011,0.0013,6.9e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10890000,0.78,-0.024,0.006,-0.63,-0.092,-0.027,0.02,-0.0078,-0.0038,-3.7e+02,-0.00079,-0.006,-0.00011,0.0008,0.00041,-0.1,-0.1,-0.023,0.5,-0.0011,-0.086,-0.069,0,0,0.0014,0.0015,0.048,0.11,0.11,0.16,0.096,0.096,0.075,1.4e-05,1.5e-05,5.7e-06,0.039,0.039,0.011,0.0013,6.8e-05,0.0013,0.00083,0.0013,0.0013,1,1 -10990000,0.78,-0.024,0.0055,-0.63,-0.08,-0.021,0.014,-0.0033,-0.0021,-3.7e+02,-0.00082,-0.0059,-0.0001,0.0011,-0.0047,-0.11,-0.1,-0.023,0.5,-0.001,-0.087,-0.069,0,0,0.0013,0.0014,0.048,0.086,0.086,0.12,0.099,0.099,0.071,1.3e-05,1.4e-05,5.7e-06,0.038,0.038,0.011,0.0013,6.8e-05,0.0013,0.00082,0.0013,0.0013,1,1 -11090000,0.78,-0.024,0.0053,-0.63,-0.1,-0.029,0.019,-0.012,-0.0048,-3.7e+02,-0.00085,-0.0058,-9.9e-05,0.00083,-0.0047,-0.11,-0.1,-0.023,0.5,-0.00098,-0.087,-0.069,0,0,0.0013,0.0014,0.048,0.099,0.1,0.11,0.11,0.11,0.074,1.2e-05,1.3e-05,5.7e-06,0.038,0.038,0.011,0.0013,6.8e-05,0.0013,0.00082,0.0013,0.0013,1,1 -11190000,0.78,-0.023,0.0047,-0.63,-0.09,-0.022,0.0078,-0.0044,-0.0017,-3.7e+02,-0.00094,-0.0059,-9.5e-05,0.0015,-0.012,-0.11,-0.11,-0.023,0.5,-0.00093,-0.087,-0.069,0,0,0.0012,0.0013,0.048,0.083,0.083,0.084,0.11,0.11,0.069,1.1e-05,1.2e-05,5.7e-06,0.037,0.037,0.011,0.0013,6.8e-05,0.0013,0.00081,0.0013,0.0013,1,1 -11290000,0.78,-0.023,0.0048,-0.63,-0.11,-0.025,0.0076,-0.015,-0.0038,-3.7e+02,-0.00089,-0.0059,-9.9e-05,0.0016,-0.011,-0.11,-0.11,-0.023,0.5,-0.00097,-0.087,-0.069,0,0,0.0012,0.0012,0.047,0.098,0.098,0.078,0.12,0.12,0.072,1.1e-05,1.2e-05,5.7e-06,0.037,0.037,0.01,0.0013,6.7e-05,0.0013,0.00081,0.0013,0.0013,1,1 -11390000,0.78,-0.022,0.0042,-0.63,-0.098,-0.02,0.002,-0.0023,-0.00077,-3.7e+02,-0.00099,-0.006,-9.6e-05,0.0016,-0.018,-0.11,-0.11,-0.023,0.5,-0.001,-0.088,-0.069,0,0,0.0011,0.0011,0.047,0.079,0.079,0.063,0.082,0.082,0.068,1e-05,1.1e-05,5.7e-06,0.036,0.036,0.01,0.0012,6.7e-05,0.0013,0.0008,0.0013,0.0013,1,1 -11490000,0.78,-0.022,0.0044,-0.63,-0.12,-0.022,0.0029,-0.013,-0.0025,-3.7e+02,-0.00095,-0.0061,-0.0001,0.0016,-0.018,-0.11,-0.11,-0.023,0.5,-0.0011,-0.088,-0.069,0,0,0.0011,0.0011,0.047,0.094,0.094,0.058,0.089,0.089,0.069,9.6e-06,1e-05,5.7e-06,0.036,0.036,0.01,0.0012,6.7e-05,0.0013,0.00079,0.0013,0.0013,1,1 -11590000,0.78,-0.021,0.0038,-0.63,-0.097,-0.019,-0.003,-0.0042,-0.00076,-3.7e+02,-0.001,-0.0061,-9.9e-05,0.0014,-0.025,-0.11,-0.11,-0.023,0.5,-0.0011,-0.088,-0.069,0,0,0.00095,0.00099,0.047,0.078,0.078,0.049,0.068,0.068,0.066,9.1e-06,9.8e-06,5.7e-06,0.035,0.035,0.01,0.0012,6.6e-05,0.0013,0.00078,0.0013,0.0013,1,1 -11690000,0.78,-0.021,0.0039,-0.63,-0.11,-0.022,-0.0074,-0.015,-0.0027,-3.7e+02,-0.00094,-0.0061,-0.0001,0.0014,-0.025,-0.11,-0.11,-0.023,0.5,-0.0012,-0.088,-0.069,0,0,0.00095,0.00099,0.047,0.092,0.092,0.046,0.075,0.075,0.066,8.6e-06,9.4e-06,5.7e-06,0.035,0.035,0.01,0.0012,6.6e-05,0.0013,0.00078,0.0013,0.0013,1,1 -11790000,0.78,-0.02,0.0034,-0.63,-0.096,-0.014,-0.0092,-0.008,0.0004,-3.7e+02,-0.00097,-0.0061,-9.8e-05,0.0019,-0.031,-0.11,-0.11,-0.023,0.5,-0.0011,-0.089,-0.069,0,0,0.00084,0.00087,0.047,0.076,0.076,0.039,0.06,0.06,0.063,8.1e-06,8.8e-06,5.7e-06,0.034,0.034,0.01,0.0012,6.6e-05,0.0013,0.00077,0.0013,0.0013,1,1 -11890000,0.78,-0.02,0.0035,-0.63,-0.11,-0.015,-0.01,-0.019,-0.00091,-3.7e+02,-0.00095,-0.0062,-0.0001,0.0018,-0.031,-0.11,-0.11,-0.023,0.5,-0.0012,-0.089,-0.069,0,0,0.00084,0.00087,0.047,0.089,0.089,0.037,0.067,0.067,0.063,7.8e-06,8.4e-06,5.7e-06,0.034,0.034,0.01,0.0012,6.6e-05,0.0013,0.00077,0.0013,0.0013,1,1 -11990000,0.78,-0.019,0.0028,-0.63,-0.092,-0.0097,-0.015,-0.011,0.0014,-3.7e+02,-0.0011,-0.0061,-9.4e-05,0.002,-0.036,-0.11,-0.11,-0.023,0.5,-0.0011,-0.089,-0.069,0,0,0.00075,0.00077,0.047,0.074,0.073,0.033,0.055,0.055,0.061,7.4e-06,8e-06,5.7e-06,0.033,0.033,0.01,0.0012,6.5e-05,0.0013,0.00075,0.0013,0.0013,1,1 -12090000,0.78,-0.019,0.0027,-0.63,-0.1,-0.013,-0.021,-0.02,4.4e-05,-3.7e+02,-0.0011,-0.0061,-9.1e-05,0.0022,-0.036,-0.11,-0.11,-0.023,0.5,-0.001,-0.089,-0.069,0,0,0.00075,0.00077,0.047,0.086,0.086,0.031,0.063,0.063,0.061,7e-06,7.6e-06,5.7e-06,0.033,0.033,0.01,0.0012,6.5e-05,0.0013,0.00075,0.0013,0.0013,1,1 -12190000,0.78,-0.019,0.0021,-0.63,-0.081,-0.013,-0.016,-0.01,0.00049,-3.7e+02,-0.0011,-0.0061,-9e-05,0.0018,-0.041,-0.11,-0.11,-0.024,0.5,-0.001,-0.09,-0.069,0,0,0.00067,0.00069,0.046,0.07,0.07,0.028,0.052,0.052,0.059,6.7e-06,7.2e-06,5.7e-06,0.032,0.033,0.01,0.0012,6.5e-05,0.0012,0.00074,0.0013,0.0012,1,1 -12290000,0.78,-0.019,0.0022,-0.63,-0.089,-0.015,-0.015,-0.019,-0.001,-3.7e+02,-0.0011,-0.006,-9e-05,0.0018,-0.042,-0.11,-0.11,-0.024,0.5,-0.001,-0.09,-0.069,0,0,0.00067,0.00069,0.046,0.081,0.081,0.028,0.06,0.06,0.059,6.4e-06,7e-06,5.7e-06,0.032,0.033,0.01,0.0012,6.5e-05,0.0012,0.00074,0.0013,0.0012,1,1 -12390000,0.78,-0.018,0.0017,-0.63,-0.07,-0.012,-0.013,-0.0093,8.2e-05,-3.7e+02,-0.0011,-0.0061,-8.9e-05,0.0013,-0.046,-0.11,-0.11,-0.024,0.5,-0.0011,-0.09,-0.069,0,0,0.0006,0.00062,0.046,0.066,0.066,0.026,0.05,0.05,0.057,6.1e-06,6.6e-06,5.7e-06,0.032,0.032,0.01,0.0012,6.5e-05,0.0012,0.00073,0.0013,0.0012,1,1 -12490000,0.78,-0.018,0.0019,-0.63,-0.078,-0.013,-0.016,-0.017,-0.00097,-3.7e+02,-0.0011,-0.0061,-9.2e-05,0.001,-0.046,-0.11,-0.11,-0.024,0.5,-0.0012,-0.09,-0.069,0,0,0.0006,0.00062,0.046,0.076,0.076,0.026,0.058,0.058,0.057,5.9e-06,6.4e-06,5.7e-06,0.032,0.032,0.01,0.0012,6.4e-05,0.0012,0.00073,0.0013,0.0012,1,1 -12590000,0.78,-0.018,0.0016,-0.63,-0.071,-0.012,-0.022,-0.014,-0.00011,-3.7e+02,-0.0012,-0.0061,-9e-05,0.0011,-0.048,-0.11,-0.11,-0.024,0.5,-0.0012,-0.09,-0.069,0,0,0.00055,0.00057,0.046,0.062,0.062,0.025,0.049,0.049,0.055,5.6e-06,6.1e-06,5.7e-06,0.031,0.032,0.0099,0.0012,6.4e-05,0.0012,0.00072,0.0013,0.0012,1,1 -12690000,0.78,-0.018,0.0016,-0.63,-0.076,-0.013,-0.025,-0.021,-0.0011,-3.7e+02,-0.0012,-0.0061,-8.9e-05,0.0009,-0.047,-0.11,-0.11,-0.024,0.5,-0.0013,-0.09,-0.069,0,0,0.00056,0.00057,0.046,0.071,0.071,0.025,0.057,0.057,0.055,5.4e-06,5.9e-06,5.7e-06,0.031,0.032,0.0099,0.0012,6.4e-05,0.0012,0.00072,0.0013,0.0012,1,1 -12790000,0.78,-0.018,0.0014,-0.63,-0.07,-0.011,-0.028,-0.018,-0.00038,-3.7e+02,-0.0012,-0.0061,-8.9e-05,0.001,-0.049,-0.11,-0.11,-0.024,0.5,-0.0012,-0.09,-0.069,0,0,0.00052,0.00053,0.046,0.058,0.058,0.024,0.048,0.048,0.053,5.1e-06,5.6e-06,5.7e-06,0.031,0.031,0.0097,0.0012,6.4e-05,0.0012,0.00071,0.0013,0.0012,1,1 -12890000,0.78,-0.018,0.0015,-0.63,-0.077,-0.011,-0.027,-0.026,-0.0015,-3.7e+02,-0.0011,-0.0061,-9e-05,0.0011,-0.05,-0.11,-0.11,-0.024,0.5,-0.0012,-0.09,-0.069,0,0,0.00052,0.00053,0.046,0.066,0.066,0.025,0.056,0.056,0.054,5e-06,5.5e-06,5.7e-06,0.031,0.031,0.0097,0.0012,6.4e-05,0.0012,0.00071,0.0013,0.0012,1,1 -12990000,0.78,-0.018,0.0011,-0.63,-0.062,-0.0099,-0.028,-0.019,-0.0012,-3.7e+02,-0.0012,-0.0061,-8.7e-05,0.0011,-0.052,-0.11,-0.11,-0.024,0.5,-0.0012,-0.09,-0.069,0,0,0.00049,0.0005,0.046,0.058,0.058,0.025,0.058,0.058,0.052,4.8e-06,5.3e-06,5.7e-06,0.031,0.031,0.0094,0.0012,6.4e-05,0.0012,0.0007,0.0013,0.0012,1,1 -13090000,0.78,-0.018,0.0012,-0.63,-0.068,-0.0096,-0.028,-0.026,-0.0019,-3.7e+02,-0.0011,-0.0061,-9e-05,0.0007,-0.053,-0.11,-0.11,-0.024,0.5,-0.0013,-0.09,-0.069,0,0,0.00049,0.0005,0.046,0.065,0.065,0.025,0.066,0.066,0.052,4.6e-06,5.1e-06,5.7e-06,0.031,0.031,0.0094,0.0012,6.3e-05,0.0012,0.0007,0.0013,0.0012,1,1 -13190000,0.78,-0.017,0.00095,-0.63,-0.054,-0.0091,-0.025,-0.017,-0.0013,-3.7e+02,-0.0012,-0.0061,-9e-05,0.0004,-0.055,-0.11,-0.11,-0.024,0.5,-0.0013,-0.091,-0.069,0,0,0.00047,0.00048,0.046,0.057,0.057,0.025,0.067,0.067,0.051,4.4e-06,4.9e-06,5.7e-06,0.031,0.031,0.0091,0.0012,6.3e-05,0.0012,0.0007,0.0013,0.0012,1,1 -13290000,0.78,-0.017,0.00097,-0.63,-0.059,-0.011,-0.021,-0.023,-0.0026,-3.7e+02,-0.0011,-0.0061,-9e-05,0.00068,-0.055,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00047,0.00048,0.046,0.064,0.064,0.027,0.077,0.077,0.051,4.3e-06,4.7e-06,5.7e-06,0.03,0.031,0.0091,0.0012,6.3e-05,0.0012,0.0007,0.0013,0.0012,1,1 -13390000,0.78,-0.017,0.00082,-0.63,-0.048,-0.01,-0.017,-0.016,-0.0018,-3.7e+02,-0.0011,-0.0061,-8.7e-05,0.00081,-0.057,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00045,0.00046,0.046,0.056,0.056,0.026,0.077,0.077,0.05,4.1e-06,4.6e-06,5.7e-06,0.03,0.031,0.0088,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 -13490000,0.78,-0.017,0.00079,-0.63,-0.051,-0.011,-0.016,-0.021,-0.0031,-3.7e+02,-0.0011,-0.0061,-8.7e-05,0.00098,-0.057,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00045,0.00046,0.046,0.062,0.062,0.028,0.088,0.088,0.05,3.9e-06,4.4e-06,5.7e-06,0.03,0.03,0.0087,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 -13590000,0.78,-0.017,0.00066,-0.63,-0.041,-0.01,-0.018,-0.014,-0.0018,-3.7e+02,-0.0011,-0.0061,-8.7e-05,0.00069,-0.058,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00043,0.00044,0.046,0.054,0.054,0.028,0.087,0.087,0.05,3.8e-06,4.3e-06,5.7e-06,0.03,0.03,0.0084,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 -13690000,0.78,-0.017,0.00066,-0.63,-0.044,-0.013,-0.022,-0.019,-0.0032,-3.7e+02,-0.0011,-0.006,-8.6e-05,0.001,-0.058,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00043,0.00044,0.046,0.059,0.059,0.029,0.098,0.098,0.05,3.7e-06,4.1e-06,5.7e-06,0.03,0.03,0.0083,0.0012,6.3e-05,0.0012,0.00069,0.0013,0.0012,1,1 -13790000,0.78,-0.017,0.00048,-0.63,-0.032,-0.012,-0.024,-0.0065,-0.0029,-3.7e+02,-0.0012,-0.0061,-8.6e-05,0.00076,-0.059,-0.12,-0.11,-0.024,0.5,-0.0012,-0.091,-0.069,0,0,0.00042,0.00043,0.046,0.045,0.045,0.029,0.072,0.072,0.049,3.5e-06,4e-06,5.7e-06,0.03,0.03,0.0079,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 -13890000,0.78,-0.017,0.00054,-0.63,-0.036,-0.013,-0.028,-0.01,-0.0043,-3.7e+02,-0.0011,-0.006,-8.6e-05,0.00094,-0.059,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00042,0.00043,0.046,0.049,0.049,0.03,0.081,0.081,0.05,3.4e-06,3.9e-06,5.7e-06,0.03,0.03,0.0078,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 -13990000,0.78,-0.017,0.00039,-0.63,-0.027,-0.013,-0.027,-0.0033,-0.004,-3.7e+02,-0.0011,-0.006,-8.6e-05,0.00081,-0.06,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.04,0.04,0.03,0.063,0.063,0.05,3.3e-06,3.8e-06,5.7e-06,0.03,0.03,0.0074,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 -14090000,0.78,-0.017,0.00034,-0.63,-0.029,-0.014,-0.028,-0.006,-0.0055,-3.7e+02,-0.0012,-0.006,-8.4e-05,0.0011,-0.06,-0.12,-0.11,-0.024,0.5,-0.0011,-0.091,-0.069,0,0,0.00041,0.00042,0.046,0.043,0.043,0.031,0.07,0.07,0.05,3.2e-06,3.7e-06,5.7e-06,0.03,0.03,0.0073,0.0012,6.2e-05,0.0012,0.00068,0.0013,0.0012,1,1 -14190000,0.78,-0.017,0.00027,-0.63,-0.023,-0.012,-0.03,-0.00016,-0.0036,-3.7e+02,-0.0012,-0.006,-8.2e-05,0.0014,-0.061,-0.12,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.036,0.036,0.03,0.057,0.057,0.05,3.1e-06,3.5e-06,5.6e-06,0.03,0.03,0.0069,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 -14290000,0.78,-0.017,0.00024,-0.63,-0.024,-0.014,-0.029,-0.0024,-0.0049,-3.7e+02,-0.0012,-0.006,-8.2e-05,0.0014,-0.06,-0.12,-0.11,-0.024,0.5,-0.001,-0.091,-0.069,0,0,0.0004,0.00041,0.046,0.04,0.04,0.032,0.064,0.064,0.051,3e-06,3.4e-06,5.7e-06,0.03,0.03,0.0067,0.0012,6.2e-05,0.0012,0.00068,0.0012,0.0012,1,1 -14390000,0.78,-0.017,0.0002,-0.63,-0.019,-0.014,-0.031,0.0017,-0.0036,-3.7e+02,-0.0012,-0.006,-8e-05,0.0019,-0.061,-0.12,-0.11,-0.024,0.5,-0.00095,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.034,0.034,0.031,0.053,0.053,0.05,2.9e-06,3.3e-06,5.6e-06,0.03,0.03,0.0063,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14490000,0.78,-0.017,0.00026,-0.63,-0.021,-0.016,-0.034,-0.00062,-0.0052,-3.7e+02,-0.0011,-0.006,-8.1e-05,0.0019,-0.062,-0.12,-0.11,-0.024,0.5,-0.00093,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.037,0.037,0.032,0.06,0.06,0.051,2.8e-06,3.2e-06,5.6e-06,0.03,0.03,0.0062,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14590000,0.78,-0.016,0.00034,-0.63,-0.021,-0.017,-0.034,-0.0013,-0.0051,-3.7e+02,-0.0011,-0.006,-8.1e-05,0.0019,-0.062,-0.12,-0.11,-0.024,0.5,-0.00092,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.032,0.032,0.031,0.051,0.051,0.051,2.7e-06,3.2e-06,5.6e-06,0.03,0.03,0.0058,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14690000,0.78,-0.017,0.00036,-0.63,-0.025,-0.015,-0.031,-0.0037,-0.0068,-3.7e+02,-0.0011,-0.0059,-8e-05,0.0021,-0.062,-0.12,-0.11,-0.024,0.5,-0.0009,-0.091,-0.069,0,0,0.00039,0.0004,0.046,0.035,0.035,0.032,0.056,0.056,0.051,2.6e-06,3.1e-06,5.6e-06,0.03,0.03,0.0056,0.0012,6.2e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14790000,0.78,-0.016,0.00037,-0.63,-0.024,-0.015,-0.027,-0.0037,-0.0065,-3.7e+02,-0.0011,-0.006,-8e-05,0.0021,-0.063,-0.12,-0.11,-0.024,0.5,-0.00089,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.03,0.03,0.031,0.049,0.049,0.051,2.6e-06,3e-06,5.6e-06,0.03,0.03,0.0053,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14890000,0.78,-0.016,0.00038,-0.63,-0.027,-0.018,-0.03,-0.0063,-0.0082,-3.7e+02,-0.0011,-0.0059,-8e-05,0.0022,-0.063,-0.12,-0.11,-0.024,0.5,-0.00088,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.033,0.033,0.031,0.054,0.054,0.052,2.5e-06,2.9e-06,5.6e-06,0.03,0.03,0.0051,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -14990000,0.78,-0.016,0.00042,-0.63,-0.025,-0.015,-0.026,-0.0047,-0.0063,-3.7e+02,-0.0011,-0.006,-8e-05,0.0022,-0.063,-0.12,-0.11,-0.024,0.5,-0.00086,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.029,0.029,0.03,0.047,0.047,0.051,2.4e-06,2.8e-06,5.6e-06,0.029,0.03,0.0048,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15090000,0.78,-0.016,0.00051,-0.63,-0.026,-0.016,-0.029,-0.0074,-0.0078,-3.7e+02,-0.001,-0.006,-8e-05,0.0021,-0.063,-0.12,-0.11,-0.024,0.5,-0.00087,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.031,0.031,0.031,0.052,0.052,0.052,2.3e-06,2.8e-06,5.6e-06,0.029,0.03,0.0046,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15190000,0.78,-0.016,0.00054,-0.63,-0.025,-0.015,-0.026,-0.0058,-0.0062,-3.7e+02,-0.001,-0.006,-8e-05,0.0021,-0.064,-0.12,-0.11,-0.024,0.5,-0.00086,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.027,0.028,0.03,0.046,0.046,0.052,2.3e-06,2.7e-06,5.6e-06,0.029,0.03,0.0043,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15290000,0.78,-0.016,0.00054,-0.63,-0.026,-0.016,-0.024,-0.0083,-0.0078,-3.7e+02,-0.001,-0.006,-7.9e-05,0.0022,-0.063,-0.12,-0.11,-0.024,0.5,-0.00086,-0.091,-0.069,0,0,0.00038,0.00039,0.046,0.03,0.03,0.03,0.051,0.051,0.052,2.2e-06,2.6e-06,5.6e-06,0.029,0.03,0.0042,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15390000,0.78,-0.016,0.0005,-0.63,-0.025,-0.017,-0.022,-0.0079,-0.008,-3.7e+02,-0.0011,-0.0059,-7.5e-05,0.0026,-0.063,-0.12,-0.11,-0.024,0.5,-0.00082,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.028,0.029,0.029,0.054,0.054,0.051,2.1e-06,2.5e-06,5.6e-06,0.029,0.03,0.0039,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15490000,0.78,-0.016,0.00052,-0.63,-0.028,-0.017,-0.022,-0.011,-0.0093,-3.7e+02,-0.0011,-0.006,-7.7e-05,0.0023,-0.063,-0.12,-0.11,-0.024,0.5,-0.00083,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.031,0.031,0.029,0.06,0.06,0.053,2.1e-06,2.5e-06,5.6e-06,0.029,0.03,0.0037,0.0012,6.1e-05,0.0012,0.00067,0.0012,0.0012,1,1 -15590000,0.78,-0.016,0.00055,-0.63,-0.026,-0.015,-0.021,-0.0099,-0.0087,-3.7e+02,-0.0011,-0.006,-7.7e-05,0.0021,-0.064,-0.12,-0.11,-0.024,0.5,-0.00082,-0.091,-0.069,0,0,0.00037,0.00038,0.046,0.029,0.03,0.028,0.062,0.062,0.052,2e-06,2.4e-06,5.6e-06,0.029,0.03,0.0035,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -15690000,0.78,-0.016,0.00053,-0.63,-0.027,-0.016,-0.021,-0.012,-0.01,-3.7e+02,-0.0011,-0.006,-7.7e-05,0.0021,-0.063,-0.12,-0.11,-0.024,0.5,-0.00082,-0.091,-0.069,0,0,0.00037,0.00039,0.046,0.031,0.032,0.028,0.069,0.069,0.052,2e-06,2.4e-06,5.6e-06,0.029,0.03,0.0033,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -15790000,0.78,-0.016,0.00052,-0.63,-0.025,-0.015,-0.024,-0.0087,-0.0087,-3.7e+02,-0.0011,-0.006,-7.7e-05,0.0021,-0.064,-0.12,-0.11,-0.024,0.5,-0.00081,-0.091,-0.069,0,0,0.00037,0.00038,0.046,0.026,0.027,0.027,0.056,0.057,0.051,1.9e-06,2.3e-06,5.6e-06,0.029,0.03,0.0031,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -15890000,0.78,-0.016,0.00054,-0.63,-0.026,-0.015,-0.022,-0.011,-0.01,-3.7e+02,-0.0011,-0.006,-7.7e-05,0.0021,-0.064,-0.12,-0.11,-0.024,0.5,-0.00082,-0.091,-0.069,0,0,0.00037,0.00038,0.046,0.028,0.029,0.027,0.063,0.063,0.052,1.9e-06,2.3e-06,5.6e-06,0.029,0.03,0.003,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -15990000,0.78,-0.016,0.00052,-0.63,-0.024,-0.015,-0.017,-0.0081,-0.0092,-3.7e+02,-0.0011,-0.006,-7.5e-05,0.0023,-0.064,-0.13,-0.11,-0.024,0.5,-0.0008,-0.091,-0.069,0,0,0.00037,0.00038,0.046,0.024,0.025,0.026,0.052,0.053,0.051,1.8e-06,2.2e-06,5.6e-06,0.029,0.03,0.0028,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16090000,0.78,-0.016,0.00045,-0.63,-0.026,-0.017,-0.014,-0.01,-0.011,-3.7e+02,-0.0011,-0.006,-7.2e-05,0.0026,-0.064,-0.13,-0.11,-0.024,0.5,-0.00077,-0.091,-0.069,0,0,0.00037,0.00038,0.046,0.026,0.026,0.025,0.058,0.058,0.052,1.8e-06,2.1e-06,5.6e-06,0.029,0.03,0.0027,0.0012,6.1e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16190000,0.78,-0.016,0.00044,-0.63,-0.024,-0.016,-0.013,-0.0077,-0.0086,-3.7e+02,-0.0011,-0.006,-7e-05,0.0027,-0.064,-0.13,-0.11,-0.024,0.5,-0.00075,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.023,0.023,0.025,0.05,0.05,0.051,1.7e-06,2.1e-06,5.6e-06,0.029,0.03,0.0025,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16290000,0.78,-0.016,0.00037,-0.63,-0.026,-0.017,-0.014,-0.01,-0.011,-3.7e+02,-0.0011,-0.0059,-6.7e-05,0.003,-0.064,-0.13,-0.11,-0.024,0.5,-0.00073,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.024,0.025,0.024,0.055,0.055,0.052,1.7e-06,2e-06,5.6e-06,0.029,0.03,0.0024,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16390000,0.78,-0.016,0.0004,-0.63,-0.023,-0.014,-0.013,-0.0078,-0.0083,-3.7e+02,-0.0012,-0.006,-6.6e-05,0.0029,-0.063,-0.13,-0.11,-0.024,0.5,-0.00072,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.021,0.022,0.023,0.047,0.047,0.051,1.6e-06,2e-06,5.6e-06,0.029,0.03,0.0022,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16490000,0.78,-0.016,0.00035,-0.63,-0.022,-0.015,-0.016,-0.0098,-0.0097,-3.7e+02,-0.0012,-0.006,-6.5e-05,0.0029,-0.063,-0.13,-0.11,-0.024,0.5,-0.00072,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.023,0.023,0.023,0.052,0.052,0.052,1.6e-06,2e-06,5.6e-06,0.029,0.03,0.0022,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16590000,0.78,-0.016,0.00032,-0.63,-0.022,-0.011,-0.017,-0.01,-0.0058,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.003,-0.063,-0.13,-0.11,-0.024,0.5,-0.00067,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.02,0.021,0.022,0.046,0.046,0.051,1.5e-06,1.9e-06,5.6e-06,0.029,0.03,0.002,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16690000,0.78,-0.016,0.00036,-0.63,-0.024,-0.012,-0.013,-0.013,-0.0068,-3.7e+02,-0.0012,-0.006,-6.1e-05,0.0028,-0.063,-0.13,-0.11,-0.024,0.5,-0.00067,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.022,0.022,0.022,0.05,0.051,0.051,1.5e-06,1.9e-06,5.6e-06,0.029,0.03,0.0019,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16790000,0.78,-0.016,0.0004,-0.63,-0.023,-0.0088,-0.012,-0.012,-0.0036,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0029,-0.063,-0.13,-0.11,-0.024,0.5,-0.00062,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.02,0.02,0.021,0.044,0.044,0.05,1.5e-06,1.8e-06,5.6e-06,0.029,0.03,0.0018,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16890000,0.78,-0.016,0.00039,-0.63,-0.024,-0.0098,-0.0097,-0.015,-0.0044,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0028,-0.063,-0.13,-0.11,-0.024,0.5,-0.00063,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.021,0.022,0.021,0.049,0.049,0.051,1.4e-06,1.8e-06,5.6e-06,0.029,0.03,0.0017,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -16990000,0.78,-0.016,0.00043,-0.63,-0.023,-0.0096,-0.0092,-0.013,-0.0044,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.0026,-0.063,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.019,0.019,0.02,0.043,0.043,0.05,1.4e-06,1.7e-06,5.6e-06,0.029,0.029,0.0016,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17090000,0.78,-0.016,0.00043,-0.63,-0.024,-0.011,-0.0091,-0.015,-0.0054,-3.7e+02,-0.0012,-0.006,-5.8e-05,0.0026,-0.063,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.02,0.021,0.02,0.048,0.048,0.05,1.4e-06,1.7e-06,5.6e-06,0.029,0.029,0.0016,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17190000,0.78,-0.016,0.00037,-0.63,-0.023,-0.014,-0.01,-0.014,-0.0057,-3.7e+02,-0.0012,-0.006,-5.7e-05,0.0027,-0.063,-0.13,-0.11,-0.024,0.5,-0.00064,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.018,0.019,0.019,0.042,0.043,0.049,1.3e-06,1.7e-06,5.6e-06,0.029,0.029,0.0015,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17290000,0.78,-0.016,0.0004,-0.63,-0.025,-0.015,-0.0055,-0.016,-0.0068,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.0025,-0.063,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.019,0.02,0.019,0.047,0.047,0.049,1.3e-06,1.6e-06,5.6e-06,0.029,0.029,0.0014,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17390000,0.78,-0.016,0.00033,-0.63,-0.023,-0.017,-0.0036,-0.014,-0.0071,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0027,-0.063,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.018,0.018,0.018,0.042,0.042,0.048,1.3e-06,1.6e-06,5.6e-06,0.029,0.029,0.0013,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17490000,0.78,-0.016,0.00035,-0.63,-0.025,-0.017,-0.0019,-0.016,-0.0089,-3.7e+02,-0.0012,-0.006,-5.6e-05,0.0027,-0.063,-0.13,-0.11,-0.024,0.5,-0.00065,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.019,0.02,0.018,0.046,0.046,0.049,1.2e-06,1.6e-06,5.6e-06,0.029,0.029,0.0013,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17590000,0.78,-0.016,0.00036,-0.63,-0.023,-0.018,0.0035,-0.014,-0.0086,-3.7e+02,-0.0012,-0.006,-5.5e-05,0.0027,-0.063,-0.13,-0.11,-0.024,0.5,-0.00063,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.017,0.018,0.017,0.041,0.041,0.048,1.2e-06,1.5e-06,5.6e-06,0.029,0.029,0.0012,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17690000,0.78,-0.016,0.00039,-0.63,-0.025,-0.02,0.0029,-0.016,-0.011,-3.7e+02,-0.0012,-0.006,-5.4e-05,0.0028,-0.063,-0.13,-0.11,-0.024,0.5,-0.00063,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.018,0.019,0.017,0.045,0.045,0.048,1.2e-06,1.5e-06,5.6e-06,0.029,0.029,0.0012,0.0012,6e-05,0.0012,0.00066,0.0012,0.0012,1,1 -17790000,0.78,-0.016,0.00034,-0.63,-0.023,-0.02,0.0016,-0.014,-0.011,-3.7e+02,-0.0013,-0.006,-4.7e-05,0.0031,-0.063,-0.13,-0.11,-0.024,0.5,-0.00059,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.018,0.019,0.016,0.048,0.048,0.048,1.2e-06,1.5e-06,5.5e-06,0.029,0.029,0.0011,0.0012,6e-05,0.0012,0.00065,0.0012,0.0012,1,1 -17890000,0.78,-0.016,0.00032,-0.63,-0.026,-0.022,0.0017,-0.017,-0.014,-3.7e+02,-0.0012,-0.006,-4.5e-05,0.0033,-0.063,-0.13,-0.11,-0.024,0.5,-0.00059,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.019,0.02,0.016,0.052,0.053,0.048,1.1e-06,1.5e-06,5.5e-06,0.029,0.029,0.0011,0.0012,6e-05,0.0012,0.00065,0.0012,0.0012,1,1 -17990000,0.78,-0.016,0.00035,-0.63,-0.025,-0.019,0.0029,-0.015,-0.014,-3.7e+02,-0.0013,-0.006,-4.4e-05,0.0032,-0.063,-0.13,-0.11,-0.024,0.5,-0.00057,-0.091,-0.069,0,0,0.00036,0.00037,0.046,0.019,0.02,0.016,0.055,0.055,0.047,1.1e-06,1.4e-06,5.5e-06,0.029,0.029,0.001,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18090000,0.78,-0.016,0.00039,-0.63,-0.026,-0.019,0.0052,-0.018,-0.015,-3.7e+02,-0.0013,-0.006,-4.8e-05,0.003,-0.063,-0.13,-0.11,-0.024,0.5,-0.00059,-0.091,-0.069,0,0,0.00036,0.00038,0.046,0.02,0.021,0.016,0.06,0.061,0.047,1.1e-06,1.4e-06,5.5e-06,0.029,0.029,0.00098,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18190000,0.78,-0.016,0.0004,-0.63,-0.023,-0.019,0.0065,-0.013,-0.012,-3.7e+02,-0.0013,-0.006,-4.1e-05,0.0031,-0.063,-0.13,-0.11,-0.024,0.5,-0.00055,-0.091,-0.069,0,0,0.00035,0.00037,0.046,0.018,0.018,0.015,0.051,0.051,0.047,1.1e-06,1.4e-06,5.5e-06,0.029,0.029,0.00092,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18290000,0.78,-0.016,0.00049,-0.63,-0.024,-0.019,0.0077,-0.016,-0.014,-3.7e+02,-0.0013,-0.006,-4.3e-05,0.003,-0.063,-0.13,-0.11,-0.024,0.5,-0.00056,-0.091,-0.069,0,0,0.00035,0.00037,0.046,0.019,0.02,0.015,0.056,0.056,0.046,1e-06,1.3e-06,5.5e-06,0.029,0.029,0.00089,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18390000,0.78,-0.016,0.00045,-0.63,-0.023,-0.02,0.0089,-0.012,-0.012,-3.7e+02,-0.0013,-0.006,-3.6e-05,0.0032,-0.064,-0.13,-0.11,-0.024,0.5,-0.00052,-0.091,-0.069,0,0,0.00035,0.00037,0.046,0.017,0.017,0.014,0.048,0.048,0.046,1e-06,1.3e-06,5.5e-06,0.029,0.029,0.00085,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18490000,0.78,-0.016,0.00041,-0.63,-0.023,-0.021,0.0085,-0.014,-0.014,-3.7e+02,-0.0013,-0.006,-3.5e-05,0.0032,-0.063,-0.13,-0.11,-0.024,0.5,-0.00052,-0.091,-0.069,0,0,0.00035,0.00037,0.046,0.018,0.019,0.014,0.052,0.053,0.046,9.9e-07,1.3e-06,5.5e-06,0.029,0.029,0.00082,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18590000,0.78,-0.016,0.0004,-0.63,-0.022,-0.021,0.0066,-0.011,-0.012,-3.7e+02,-0.0013,-0.006,-2.5e-05,0.0034,-0.063,-0.13,-0.11,-0.024,0.5,-0.00047,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.016,0.017,0.014,0.046,0.046,0.045,9.6e-07,1.3e-06,5.5e-06,0.029,0.029,0.00078,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18690000,0.78,-0.016,0.00046,-0.63,-0.023,-0.021,0.0047,-0.014,-0.014,-3.7e+02,-0.0013,-0.006,-2.7e-05,0.0034,-0.064,-0.13,-0.11,-0.024,0.5,-0.00047,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.017,0.018,0.013,0.05,0.05,0.045,9.5e-07,1.2e-06,5.5e-06,0.029,0.029,0.00076,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18790000,0.78,-0.016,0.00048,-0.63,-0.022,-0.02,0.0044,-0.012,-0.012,-3.7e+02,-0.0013,-0.006,-2.3e-05,0.0033,-0.064,-0.13,-0.11,-0.024,0.5,-0.00046,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.013,0.044,0.044,0.045,9.2e-07,1.2e-06,5.4e-06,0.029,0.029,0.00072,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18890000,0.78,-0.016,0.00039,-0.63,-0.022,-0.022,0.005,-0.014,-0.014,-3.7e+02,-0.0013,-0.006,-1.9e-05,0.0035,-0.063,-0.13,-0.11,-0.024,0.5,-0.00045,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.016,0.017,0.013,0.048,0.048,0.045,9.1e-07,1.2e-06,5.4e-06,0.029,0.029,0.0007,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -18990000,0.78,-0.016,0.00033,-0.63,-0.018,-0.022,0.0036,-0.0095,-0.012,-3.7e+02,-0.0013,-0.006,-1.1e-05,0.0036,-0.063,-0.13,-0.11,-0.024,0.5,-0.00041,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.012,0.042,0.043,0.044,8.8e-07,1.2e-06,5.4e-06,0.029,0.029,0.00067,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19090000,0.78,-0.016,0.00033,-0.63,-0.018,-0.024,0.0066,-0.011,-0.015,-3.7e+02,-0.0013,-0.006,-1.1e-05,0.0036,-0.063,-0.13,-0.11,-0.024,0.5,-0.00042,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.016,0.017,0.012,0.046,0.047,0.044,8.7e-07,1.1e-06,5.4e-06,0.029,0.029,0.00065,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19190000,0.78,-0.015,0.00028,-0.63,-0.015,-0.023,0.0066,-0.0074,-0.013,-3.7e+02,-0.0013,-0.006,-3.5e-06,0.0036,-0.063,-0.13,-0.11,-0.024,0.5,-0.00038,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.012,0.041,0.042,0.044,8.5e-07,1.1e-06,5.4e-06,0.029,0.029,0.00062,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19290000,0.78,-0.015,0.00028,-0.63,-0.016,-0.023,0.0094,-0.0091,-0.015,-3.7e+02,-0.0013,-0.006,-6.1e-06,0.0035,-0.063,-0.13,-0.11,-0.024,0.5,-0.0004,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.012,0.045,0.046,0.044,8.4e-07,1.1e-06,5.4e-06,0.029,0.029,0.00061,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19390000,0.78,-0.015,0.00032,-0.63,-0.015,-0.021,0.013,-0.0082,-0.013,-3.7e+02,-0.0013,-0.006,3e-06,0.0036,-0.063,-0.13,-0.11,-0.024,0.5,-0.00036,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.012,0.04,0.041,0.043,8.2e-07,1.1e-06,5.4e-06,0.029,0.029,0.00058,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19490000,0.78,-0.015,0.00027,-0.63,-0.014,-0.022,0.0095,-0.0097,-0.016,-3.7e+02,-0.0013,-0.006,7.4e-06,0.0038,-0.063,-0.13,-0.11,-0.024,0.5,-0.00034,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.011,0.044,0.045,0.043,8.1e-07,1.1e-06,5.4e-06,0.029,0.029,0.00057,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19590000,0.78,-0.015,0.00022,-0.63,-0.013,-0.021,0.0088,-0.0082,-0.014,-3.7e+02,-0.0013,-0.006,2.1e-05,0.0039,-0.063,-0.13,-0.11,-0.024,0.5,-0.0003,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.011,0.04,0.04,0.042,7.8e-07,1e-06,5.3e-06,0.029,0.029,0.00054,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19690000,0.78,-0.015,0.00021,-0.63,-0.013,-0.019,0.01,-0.009,-0.016,-3.7e+02,-0.0013,-0.006,1.8e-05,0.0038,-0.063,-0.13,-0.11,-0.024,0.5,-0.00031,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.011,0.043,0.044,0.042,7.7e-07,1e-06,5.3e-06,0.029,0.029,0.00053,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19790000,0.78,-0.015,0.00017,-0.63,-0.011,-0.017,0.011,-0.0077,-0.014,-3.7e+02,-0.0014,-0.006,2.5e-05,0.0038,-0.063,-0.13,-0.11,-0.024,0.5,-0.00028,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.011,0.039,0.039,0.042,7.6e-07,1e-06,5.3e-06,0.029,0.029,0.00051,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19890000,0.78,-0.015,0.0002,-0.63,-0.011,-0.019,0.012,-0.0093,-0.017,-3.7e+02,-0.0013,-0.006,3.2e-05,0.004,-0.063,-0.13,-0.11,-0.024,0.5,-0.00026,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.011,0.043,0.043,0.042,7.5e-07,9.9e-07,5.3e-06,0.029,0.029,0.0005,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -19990000,0.78,-0.015,0.00021,-0.63,-0.0091,-0.018,0.015,-0.0081,-0.015,-3.7e+02,-0.0013,-0.0059,4.9e-05,0.0043,-0.063,-0.13,-0.11,-0.024,0.5,-0.00021,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.015,0.01,0.038,0.039,0.041,7.3e-07,9.7e-07,5.3e-06,0.029,0.029,0.00048,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20090000,0.78,-0.015,0.00016,-0.63,-0.0091,-0.019,0.015,-0.0088,-0.018,-3.7e+02,-0.0013,-0.0059,5.8e-05,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,-0.0002,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.016,0.01,0.042,0.043,0.042,7.2e-07,9.6e-07,5.3e-06,0.029,0.029,0.00047,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20190000,0.78,-0.015,0.00012,-0.63,-0.0096,-0.017,0.017,-0.009,-0.016,-3.7e+02,-0.0013,-0.0059,7e-05,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-0.00016,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.015,0.01,0.038,0.039,0.041,7e-07,9.3e-07,5.2e-06,0.029,0.029,0.00045,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20290000,0.78,-0.015,0.00013,-0.63,-0.0084,-0.017,0.015,-0.0094,-0.018,-3.7e+02,-0.0013,-0.0059,7.3e-05,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-0.00016,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.016,0.0099,0.042,0.042,0.041,6.9e-07,9.3e-07,5.2e-06,0.029,0.029,0.00044,0.0012,5.9e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20390000,0.78,-0.015,0.00017,-0.63,-0.008,-0.015,0.017,-0.0092,-0.016,-3.7e+02,-0.0013,-0.0059,8.1e-05,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-0.00013,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.014,0.0097,0.038,0.038,0.041,6.8e-07,9e-07,5.2e-06,0.029,0.029,0.00043,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20490000,0.78,-0.015,0.00013,-0.63,-0.0082,-0.015,0.017,-0.01,-0.017,-3.7e+02,-0.0013,-0.0059,7.8e-05,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,-0.00013,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.0096,0.041,0.042,0.041,6.7e-07,9e-07,5.2e-06,0.029,0.029,0.00042,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20590000,0.78,-0.015,0.00013,-0.63,-0.0077,-0.012,0.014,-0.0087,-0.015,-3.7e+02,-0.0013,-0.0059,8.3e-05,0.0045,-0.063,-0.13,-0.11,-0.024,0.5,-0.00011,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.014,0.0093,0.037,0.038,0.04,6.5e-07,8.7e-07,5.2e-06,0.029,0.029,0.0004,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20690000,0.78,-0.016,8.6e-05,-0.63,-0.0084,-0.013,0.015,-0.0095,-0.016,-3.7e+02,-0.0013,-0.0059,8.5e-05,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,-0.00011,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.0093,0.041,0.042,0.04,6.4e-07,8.6e-07,5.2e-06,0.029,0.029,0.0004,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20790000,0.78,-0.015,6.4e-05,-0.63,-0.0061,-0.012,0.016,-0.008,-0.014,-3.7e+02,-0.0014,-0.0059,9.4e-05,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,-9e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.014,0.0091,0.037,0.038,0.04,6.3e-07,8.4e-07,5.1e-06,0.029,0.029,0.00038,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20890000,0.78,-0.016,4.6e-05,-0.63,-0.0064,-0.012,0.015,-0.0085,-0.016,-3.7e+02,-0.0013,-0.0059,0.0001,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-8.4e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.009,0.041,0.041,0.04,6.2e-07,8.4e-07,5.1e-06,0.029,0.029,0.00037,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -20990000,0.78,-0.016,3.1e-05,-0.63,-0.0048,-0.0096,0.015,-0.0082,-0.016,-3.7e+02,-0.0014,-0.0059,0.0001,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-7.6e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.0088,0.043,0.044,0.039,6.1e-07,8.2e-07,5.1e-06,0.029,0.029,0.00036,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21090000,0.78,-0.016,2.5e-05,-0.63,-0.0059,-0.0093,0.016,-0.0091,-0.018,-3.7e+02,-0.0014,-0.0059,0.00011,0.0048,-0.063,-0.13,-0.11,-0.024,0.5,-6.4e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.0088,0.047,0.048,0.039,6e-07,8.1e-07,5.1e-06,0.029,0.029,0.00036,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21190000,0.78,-0.016,1.9e-05,-0.63,-0.006,-0.009,0.015,-0.0096,-0.018,-3.7e+02,-0.0013,-0.0059,0.00011,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,-5.6e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.016,0.0086,0.049,0.05,0.039,5.9e-07,7.9e-07,5e-06,0.029,0.029,0.00035,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21290000,0.78,-0.016,-6.8e-05,-0.63,-0.0055,-0.0093,0.017,-0.0096,-0.02,-3.7e+02,-0.0014,-0.0059,0.00012,0.0049,-0.063,-0.13,-0.11,-0.024,0.5,-5.1e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.016,0.017,0.0085,0.053,0.055,0.039,5.9e-07,7.9e-07,5e-06,0.029,0.029,0.00034,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21390000,0.78,-0.015,-2.1e-05,-0.63,-0.0049,-0.0049,0.016,-0.0084,-0.015,-3.7e+02,-0.0014,-0.0059,0.00013,0.0048,-0.063,-0.13,-0.11,-0.024,0.5,-2.2e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.016,0.0084,0.046,0.047,0.039,5.7e-07,7.6e-07,5e-06,0.029,0.029,0.00033,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21490000,0.78,-0.015,-3e-05,-0.63,-0.0055,-0.0057,0.016,-0.0095,-0.016,-3.7e+02,-0.0013,-0.0059,0.00013,0.0049,-0.063,-0.13,-0.11,-0.024,0.5,-1.9e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.015,0.017,0.0083,0.05,0.052,0.038,5.7e-07,7.6e-07,5e-06,0.029,0.029,0.00032,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21590000,0.78,-0.015,7.3e-06,-0.63,-0.0042,-0.0041,0.016,-0.0079,-0.012,-3.7e+02,-0.0013,-0.0059,0.00014,0.0048,-0.063,-0.13,-0.11,-0.024,0.5,4.4e-06,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.015,0.0081,0.044,0.045,0.038,5.5e-07,7.4e-07,4.9e-06,0.029,0.029,0.00032,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21690000,0.78,-0.015,4.5e-07,-0.63,-0.0058,-0.005,0.017,-0.0092,-0.013,-3.7e+02,-0.0013,-0.0059,0.00014,0.0049,-0.064,-0.13,-0.11,-0.024,0.5,1.1e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.016,0.0081,0.048,0.049,0.038,5.5e-07,7.3e-07,4.9e-06,0.029,0.029,0.00031,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21790000,0.78,-0.015,9.5e-05,-0.63,-0.0049,-0.0028,0.016,-0.0081,-0.0081,-3.7e+02,-0.0013,-0.0059,0.00015,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,4.7e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.014,0.008,0.042,0.043,0.038,5.3e-07,7.1e-07,4.9e-06,0.029,0.029,0.0003,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21890000,0.78,-0.015,9.7e-05,-0.63,-0.0055,-0.0036,0.016,-0.0088,-0.0085,-3.7e+02,-0.0013,-0.0059,0.00015,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,5e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.014,0.015,0.0079,0.046,0.047,0.038,5.3e-07,7.1e-07,4.9e-06,0.029,0.029,0.0003,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -21990000,0.78,-0.015,0.00013,-0.63,-0.0054,-0.00082,0.017,-0.0082,-0.0047,-3.7e+02,-0.0013,-0.0059,0.00016,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,8.3e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.014,0.0078,0.041,0.042,0.038,5.2e-07,6.9e-07,4.8e-06,0.029,0.029,0.00029,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -22090000,0.78,-0.015,0.00011,-0.63,-0.0051,-0.0023,0.015,-0.0086,-0.0048,-3.7e+02,-0.0013,-0.0059,0.00016,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,8.3e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.015,0.0078,0.044,0.046,0.037,5.1e-07,6.8e-07,4.8e-06,0.029,0.029,0.00029,0.0012,5.8e-05,0.0012,0.00065,0.0012,0.0012,1,1 -22190000,0.78,-0.015,0.00013,-0.63,-0.0038,-0.003,0.016,-0.0072,-0.0044,-3.7e+02,-0.0014,-0.0059,0.00017,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9.3e-05,-0.091,-0.068,0,0,0.00034,0.00037,0.046,0.012,0.014,0.0076,0.04,0.041,0.037,5e-07,6.6e-07,4.8e-06,0.029,0.029,0.00028,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22290000,0.78,-0.015,9.5e-05,-0.63,-0.0034,-0.0025,0.016,-0.0078,-0.0045,-3.7e+02,-0.0014,-0.0059,0.00017,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9.2e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.046,0.013,0.015,0.0076,0.043,0.045,0.037,5e-07,6.6e-07,4.8e-06,0.029,0.029,0.00028,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22390000,0.78,-0.015,8.7e-05,-0.63,-0.00089,-0.0026,0.017,-0.006,-0.004,-3.7e+02,-0.0014,-0.0059,0.00017,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9.3e-05,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.012,0.014,0.0075,0.039,0.04,0.037,4.9e-07,6.4e-07,4.7e-06,0.029,0.029,0.00027,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22490000,0.78,-0.015,6.5e-05,-0.63,0.00026,-0.0031,0.018,-0.0055,-0.0042,-3.7e+02,-0.0014,-0.0059,0.00017,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9.2e-05,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.013,0.015,0.0074,0.042,0.044,0.037,4.8e-07,6.4e-07,4.7e-06,0.029,0.029,0.00027,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22590000,0.78,-0.015,6.3e-05,-0.63,0.0021,-0.002,0.018,-0.0037,-0.0035,-3.7e+02,-0.0014,-0.0059,0.00018,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9.8e-05,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.013,0.015,0.0073,0.045,0.046,0.036,4.8e-07,6.3e-07,4.7e-06,0.029,0.029,0.00026,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22690000,0.78,-0.015,1.9e-07,-0.63,0.0036,-0.0032,0.019,-0.0031,-0.0042,-3.7e+02,-0.0014,-0.0059,0.00019,0.0048,-0.063,-0.13,-0.11,-0.024,0.5,9.8e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.047,0.014,0.016,0.0073,0.048,0.05,0.036,4.7e-07,6.3e-07,4.7e-06,0.029,0.029,0.00026,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22790000,0.78,-0.015,4.2e-05,-0.63,0.0047,-0.0026,0.02,-0.0025,-0.0029,-3.7e+02,-0.0014,-0.006,0.00017,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,9e-05,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.014,0.016,0.0072,0.051,0.053,0.036,4.6e-07,6.2e-07,4.6e-06,0.029,0.029,0.00025,0.0012,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22890000,0.78,-0.015,4.8e-05,-0.63,0.0053,-0.0034,0.021,-0.0027,-0.0033,-3.7e+02,-0.0014,-0.006,0.00017,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,9.6e-05,-0.091,-0.068,0,0,0.00035,0.00037,0.047,0.015,0.017,0.0072,0.055,0.057,0.036,4.6e-07,6.2e-07,4.6e-06,0.029,0.029,0.00025,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -22990000,0.78,-0.015,5.5e-05,-0.63,0.005,-0.0035,0.022,-0.0028,-0.004,-3.7e+02,-0.0014,-0.0059,0.00018,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,0.0001,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.015,0.017,0.0071,0.057,0.06,0.036,4.5e-07,6e-07,4.6e-06,0.029,0.029,0.00024,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23090000,0.78,-0.015,0.00012,-0.63,0.0053,-0.0031,0.023,-0.0025,-0.0035,-3.7e+02,-0.0014,-0.006,0.00017,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,0.00011,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.016,0.018,0.007,0.062,0.065,0.036,4.5e-07,6e-07,4.6e-06,0.029,0.029,0.00024,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23190000,0.78,-0.015,0.0001,-0.63,0.0029,-0.002,0.024,-0.0053,-0.0034,-3.7e+02,-0.0014,-0.006,0.00018,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,0.00013,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.015,0.018,0.0069,0.064,0.067,0.035,4.4e-07,5.9e-07,4.6e-06,0.029,0.029,0.00024,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23290000,0.78,-0.015,0.00017,-0.63,0.0025,-0.0016,0.025,-0.0056,-0.0039,-3.7e+02,-0.0014,-0.006,0.00018,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,0.00013,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.016,0.019,0.0069,0.07,0.073,0.036,4.4e-07,5.9e-07,4.5e-06,0.029,0.029,0.00023,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23390000,0.78,-0.015,0.00014,-0.63,-0.00084,-0.0013,0.022,-0.0097,-0.0041,-3.7e+02,-0.0014,-0.006,0.00018,0.0046,-0.063,-0.13,-0.11,-0.024,0.5,0.00015,-0.091,-0.068,0,0,0.00034,0.00037,0.047,0.016,0.018,0.0068,0.072,0.075,0.035,4.3e-07,5.8e-07,4.5e-06,0.029,0.029,0.00023,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23490000,0.78,-0.013,-0.002,-0.63,0.0046,-0.00053,-0.011,-0.01,-0.0051,-3.7e+02,-0.0014,-0.006,0.00019,0.0047,-0.063,-0.13,-0.11,-0.024,0.5,0.00014,-0.091,-0.068,0,0,0.00034,0.00035,0.047,0.017,0.02,0.0068,0.078,0.082,0.035,4.3e-07,5.7e-07,4.5e-06,0.029,0.029,0.00023,0.0011,5.8e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23590000,0.78,-0.0041,-0.0063,-0.63,0.015,0.0031,-0.043,-0.0096,-0.0024,-3.7e+02,-0.0013,-0.006,0.00019,0.0046,-0.064,-0.13,-0.11,-0.024,0.5,0.00015,-0.091,-0.068,0,0,0.00034,0.00033,0.047,0.014,0.016,0.0067,0.062,0.064,0.035,4.2e-07,5.5e-07,4.4e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23690000,0.78,0.0015,-0.0053,-0.63,0.042,0.017,-0.093,-0.0073,-0.0018,-3.7e+02,-0.0013,-0.006,0.0002,0.0048,-0.064,-0.13,-0.11,-0.024,0.5,8.3e-05,-0.091,-0.068,0,0,0.00033,0.00033,0.047,0.015,0.017,0.0067,0.066,0.069,0.035,4.2e-07,5.5e-07,4.4e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23790000,0.78,-0.0021,-0.0027,-0.63,0.063,0.034,-0.15,-0.0072,-0.00047,-3.7e+02,-0.0013,-0.006,0.00021,0.005,-0.064,-0.13,-0.11,-0.024,0.5,-1.3e-05,-0.09,-0.067,0,0,0.00033,0.00033,0.047,0.013,0.015,0.0066,0.055,0.057,0.035,4.1e-07,5.3e-07,4.3e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23890000,0.78,-0.0084,-0.00076,-0.63,0.077,0.046,-0.2,0.0003,0.0036,-3.7e+02,-0.0013,-0.006,0.00021,0.0051,-0.065,-0.13,-0.11,-0.024,0.5,-6.5e-05,-0.09,-0.067,0,0,0.00033,0.00034,0.047,0.014,0.016,0.0066,0.059,0.061,0.035,4.1e-07,5.3e-07,4.3e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -23990000,0.78,-0.013,0.00014,-0.63,0.072,0.046,-0.25,-0.0051,0.0022,-3.7e+02,-0.0013,-0.0059,0.0002,0.0053,-0.065,-0.13,-0.11,-0.024,0.5,-3.2e-05,-0.09,-0.067,0,0,0.00034,0.00036,0.047,0.014,0.016,0.0066,0.061,0.064,0.035,4e-07,5.2e-07,4.3e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -24090000,0.78,-0.012,-0.00099,-0.63,0.072,0.046,-0.3,0.0012,0.0061,-3.7e+02,-0.0013,-0.0059,0.00021,0.0054,-0.065,-0.13,-0.11,-0.024,0.5,-8.2e-05,-0.09,-0.067,0,0,0.00034,0.00035,0.047,0.015,0.017,0.0065,0.066,0.069,0.035,4e-07,5.2e-07,4.3e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -24190000,0.78,-0.0096,-0.0018,-0.63,0.07,0.045,-0.35,-0.0059,0.0038,-3.7e+02,-0.0013,-0.0059,0.0002,0.0057,-0.066,-0.13,-0.11,-0.024,0.5,-6.4e-05,-0.09,-0.067,0,0,0.00034,0.00034,0.046,0.015,0.017,0.0065,0.068,0.071,0.034,3.9e-07,5.1e-07,4.3e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -24290000,0.78,-0.0087,-0.0022,-0.63,0.077,0.05,-0.4,0.00046,0.0086,-3.7e+02,-0.0013,-0.0059,0.0002,0.0057,-0.066,-0.13,-0.11,-0.024,0.5,-7.7e-05,-0.09,-0.067,0,0,0.00034,0.00034,0.046,0.016,0.019,0.0065,0.074,0.077,0.034,3.9e-07,5.1e-07,4.3e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.00064,0.0012,0.0012,1,1 -24390000,0.78,-0.0092,-0.0023,-0.63,0.075,0.048,-0.46,-0.012,0.0019,-3.7e+02,-0.0013,-0.0059,0.00016,0.0063,-0.067,-0.13,-0.11,-0.024,0.5,-6.1e-05,-0.089,-0.068,0,0,0.00034,0.00034,0.046,0.016,0.018,0.0064,0.076,0.079,0.034,3.9e-07,5e-07,4.2e-06,0.029,0.029,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 -24490000,0.78,-0.0049,-0.0027,-0.63,0.086,0.055,-0.51,-0.0038,0.0071,-3.7e+02,-0.0013,-0.0059,0.00016,0.0063,-0.067,-0.13,-0.11,-0.024,0.5,-8.7e-05,-0.089,-0.068,0,0,0.00033,0.00033,0.046,0.017,0.02,0.0064,0.082,0.085,0.034,3.8e-07,5e-07,4.2e-06,0.029,0.029,0.0002,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 -24590000,0.78,-0.0015,-0.0028,-0.63,0.09,0.059,-0.56,-0.017,-0.0021,-3.7e+02,-0.0012,-0.0059,0.00015,0.0069,-0.069,-0.13,-0.11,-0.024,0.5,-0.00014,-0.089,-0.068,0,0,0.00033,0.00033,0.046,0.017,0.019,0.0063,0.084,0.088,0.034,3.8e-07,4.9e-07,4.2e-06,0.029,0.029,0.00019,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 -24690000,0.78,-0.0006,-0.0027,-0.63,0.11,0.075,-0.64,-0.0078,0.0034,-3.7e+02,-0.0012,-0.0059,0.00016,0.0071,-0.069,-0.13,-0.11,-0.024,0.5,-0.00031,-0.089,-0.068,0,0,0.00033,0.00033,0.046,0.018,0.021,0.0063,0.09,0.094,0.034,3.8e-07,4.9e-07,4.2e-06,0.029,0.029,0.00019,0.0011,5.6e-05,0.0012,0.00063,0.0012,0.0012,1,1 -24790000,0.78,-0.0022,-0.0025,-0.63,0.11,0.084,-0.73,-0.027,-0.0015,-3.7e+02,-0.0012,-0.0059,0.00014,0.0079,-0.071,-0.13,-0.11,-0.025,0.5,-0.00014,-0.089,-0.068,0,0,0.00033,0.00033,0.046,0.018,0.021,0.0062,0.092,0.097,0.034,3.7e-07,4.8e-07,4.1e-06,0.029,0.029,0.00019,0.0011,5.6e-05,0.0012,0.00062,0.0012,0.0012,1,1 -24890000,0.78,-0.00032,-0.004,-0.63,0.13,0.098,-0.75,-0.016,0.0076,-3.7e+02,-0.0012,-0.0059,0.00013,0.0081,-0.071,-0.13,-0.11,-0.025,0.5,-0.00022,-0.088,-0.068,0,0,0.00033,0.00033,0.046,0.019,0.022,0.0062,0.099,0.1,0.034,3.7e-07,4.8e-07,4.1e-06,0.029,0.029,0.00019,0.0011,5.6e-05,0.0012,0.00062,0.0012,0.0012,1,1 -24990000,0.78,0.0014,-0.0056,-0.62,0.13,0.11,-0.81,-0.037,0.00092,-3.7e+02,-0.0012,-0.0059,9.7e-05,0.0091,-0.074,-0.13,-0.11,-0.025,0.5,-2.7e-05,-0.088,-0.068,0,0,0.00033,0.00033,0.045,0.019,0.022,0.0062,0.1,0.11,0.034,3.7e-07,4.7e-07,4.1e-06,0.029,0.029,0.00019,0.0011,5.5e-05,0.0012,0.00061,0.0012,0.0012,1,1 -25090000,0.78,0.00078,-0.006,-0.62,0.16,0.12,-0.86,-0.023,0.013,-3.7e+02,-0.0012,-0.0059,9e-05,0.0093,-0.074,-0.13,-0.11,-0.025,0.5,-3.2e-05,-0.087,-0.068,0,0,0.00033,0.00033,0.045,0.02,0.023,0.0062,0.11,0.11,0.034,3.7e-07,4.7e-07,4.1e-06,0.029,0.029,0.00018,0.0011,5.5e-05,0.0012,0.00061,0.0012,0.0012,1,1 -25190000,0.78,-0.0013,-0.0055,-0.62,0.15,0.11,-0.91,-0.067,-0.01,-3.7e+02,-0.0011,-0.0059,4.4e-05,0.011,-0.078,-0.13,-0.11,-0.025,0.5,-8.3e-06,-0.086,-0.069,0,0,0.00033,0.00032,0.044,0.019,0.023,0.0061,0.11,0.11,0.033,3.6e-07,4.6e-07,4e-06,0.029,0.029,0.00018,0.0011,5.5e-05,0.0012,0.0006,0.0011,0.0012,1,1 -25290000,0.78,0.0056,-0.0067,-0.62,0.18,0.13,-0.96,-0.05,0.0014,-3.7e+02,-0.0011,-0.0059,4.9e-05,0.011,-0.078,-0.13,-0.11,-0.025,0.5,-0.00012,-0.086,-0.069,0,0,0.00032,0.00033,0.044,0.021,0.024,0.0061,0.12,0.12,0.033,3.6e-07,4.6e-07,4e-06,0.029,0.029,0.00018,0.0011,5.5e-05,0.0012,0.0006,0.0011,0.0012,1,1 -25390000,0.78,0.012,-0.0071,-0.62,0.18,0.13,-1,-0.098,-0.023,-3.7e+02,-0.001,-0.0058,3.9e-06,0.013,-0.083,-0.13,-0.11,-0.025,0.5,-0.00017,-0.085,-0.069,0,0,0.00032,0.00035,0.043,0.02,0.024,0.0061,0.12,0.12,0.033,3.6e-07,4.5e-07,4e-06,0.029,0.029,0.00018,0.0011,5.4e-05,0.0012,0.00059,0.0011,0.0012,1,1 -25490000,0.78,0.013,-0.0073,-0.63,0.22,0.16,-1.1,-0.079,-0.01,-3.7e+02,-0.001,-0.0058,2.1e-05,0.014,-0.083,-0.13,-0.11,-0.025,0.5,-0.00051,-0.084,-0.069,0,0,0.00032,0.00035,0.043,0.022,0.026,0.0061,0.13,0.13,0.033,3.5e-07,4.5e-07,4e-06,0.029,0.029,0.00018,0.0011,5.3e-05,0.0012,0.00058,0.0011,0.0012,1,1 -25590000,0.78,0.011,-0.0071,-0.63,0.25,0.19,-1.1,-0.056,0.0066,-3.7e+02,-0.001,-0.0058,3.1e-05,0.014,-0.084,-0.13,-0.12,-0.025,0.5,-0.00078,-0.084,-0.068,0,0,0.00032,0.00034,0.043,0.023,0.028,0.0061,0.14,0.14,0.033,3.5e-07,4.5e-07,3.9e-06,0.029,0.029,0.00018,0.0011,5.3e-05,0.0011,0.00058,0.0011,0.0011,1,1 -25690000,0.78,0.018,-0.01,-0.63,0.29,0.21,-1.2,-0.029,0.025,-3.7e+02,-0.001,-0.0058,4.4e-05,0.014,-0.084,-0.13,-0.12,-0.026,0.5,-0.0011,-0.083,-0.067,0,0,0.00032,0.00038,0.042,0.025,0.03,0.0061,0.14,0.15,0.033,3.5e-07,4.5e-07,3.9e-06,0.029,0.029,0.00017,0.001,5.2e-05,0.0011,0.00058,0.0011,0.0011,1,1 -25790000,0.78,0.024,-0.012,-0.63,0.35,0.25,-1.2,0.0032,0.046,-3.7e+02,-0.001,-0.0058,6.7e-05,0.015,-0.085,-0.13,-0.12,-0.026,0.5,-0.0016,-0.081,-0.067,0,0,0.00033,0.00042,0.041,0.027,0.033,0.0061,0.15,0.16,0.033,3.5e-07,4.5e-07,3.9e-06,0.029,0.029,0.00017,0.001,5.1e-05,0.0011,0.00057,0.0011,0.0011,1,1 -25890000,0.77,0.025,-0.012,-0.63,0.41,0.28,-1.3,0.042,0.069,-3.7e+02,-0.001,-0.0058,9.3e-05,0.015,-0.085,-0.13,-0.12,-0.026,0.5,-0.0023,-0.08,-0.066,0,0,0.00033,0.00043,0.041,0.029,0.037,0.0061,0.16,0.18,0.033,3.5e-07,4.5e-07,3.9e-06,0.029,0.029,0.00017,0.001,5.1e-05,0.0011,0.00056,0.0011,0.0011,1,1 -25990000,0.77,0.021,-0.012,-0.63,0.47,0.32,-1.3,0.086,0.097,-3.7e+02,-0.001,-0.0058,0.00011,0.015,-0.086,-0.13,-0.12,-0.027,0.5,-0.0027,-0.079,-0.065,0,0,0.00033,0.0004,0.04,0.031,0.04,0.0061,0.18,0.19,0.033,3.5e-07,4.5e-07,3.9e-06,0.029,0.029,0.00017,0.00099,5e-05,0.0011,0.00055,0.001,0.0011,1,1 -26090000,0.78,0.032,-0.016,-0.63,0.53,0.35,-1.3,0.13,0.13,-3.7e+02,-0.001,-0.0058,9.6e-05,0.016,-0.086,-0.13,-0.12,-0.027,0.5,-0.0026,-0.077,-0.064,0,0,0.00033,0.00049,0.039,0.033,0.043,0.0061,0.19,0.2,0.033,3.5e-07,4.6e-07,3.8e-06,0.029,0.029,0.00017,0.00097,4.9e-05,0.0011,0.00054,0.001,0.0011,1,1 -26190000,0.78,0.042,-0.017,-0.63,0.6,0.4,-1.3,0.19,0.17,-3.7e+02,-0.00099,-0.0058,0.00011,0.017,-0.087,-0.13,-0.13,-0.028,0.5,-0.0032,-0.074,-0.062,0,0,0.00035,0.00057,0.038,0.036,0.047,0.0061,0.2,0.22,0.033,3.5e-07,4.6e-07,3.8e-06,0.029,0.029,0.00017,0.00094,4.7e-05,0.001,0.00053,0.00098,0.001,1,1 -26290000,0.78,0.044,-0.018,-0.63,0.68,0.45,-1.3,0.25,0.21,-3.7e+02,-0.00099,-0.0058,0.0001,0.018,-0.087,-0.13,-0.13,-0.028,0.49,-0.0035,-0.071,-0.061,0,0,0.00035,0.00059,0.036,0.039,0.052,0.0061,0.21,0.23,0.033,3.5e-07,4.6e-07,3.8e-06,0.029,0.029,0.00016,0.00091,4.6e-05,0.001,0.00052,0.00095,0.001,1,1 -26390000,0.77,0.04,-0.018,-0.63,0.76,0.5,-1.3,0.32,0.25,-3.7e+02,-0.00098,-0.0058,0.00011,0.019,-0.088,-0.13,-0.13,-0.028,0.49,-0.0039,-0.07,-0.06,0,0,0.00035,0.00054,0.035,0.041,0.056,0.0061,0.23,0.25,0.033,3.5e-07,4.6e-07,3.8e-06,0.029,0.029,0.00016,0.00088,4.4e-05,0.00098,0.0005,0.00091,0.00098,1,1 -26490000,0.77,0.057,-0.024,-0.63,0.84,0.56,-1.3,0.4,0.3,-3.7e+02,-0.00098,-0.0058,0.00011,0.02,-0.088,-0.13,-0.13,-0.029,0.49,-0.0041,-0.068,-0.058,0,0,0.00037,0.00074,0.033,0.044,0.061,0.0061,0.24,0.27,0.033,3.6e-07,4.6e-07,3.8e-06,0.029,0.029,0.00016,0.00084,4.2e-05,0.00094,0.00048,0.00088,0.00094,1,1 -26590000,0.77,0.074,-0.029,-0.63,0.96,0.64,-1.3,0.49,0.37,-3.7e+02,-0.00097,-0.0058,8.4e-05,0.022,-0.089,-0.13,-0.13,-0.03,0.49,-0.0036,-0.064,-0.056,0,0,0.0004,0.00097,0.031,0.047,0.067,0.0061,0.26,0.29,0.033,3.6e-07,4.6e-07,3.7e-06,0.029,0.029,0.00016,0.0008,4e-05,0.00089,0.00046,0.00083,0.00089,1,1 -26690000,0.77,0.076,-0.03,-0.64,1.1,0.71,-1.3,0.59,0.43,-3.7e+02,-0.00097,-0.0058,9.8e-05,0.023,-0.09,-0.13,-0.14,-0.031,0.49,-0.0046,-0.059,-0.052,0,0,0.0004,0.00097,0.029,0.051,0.073,0.0061,0.28,0.31,0.033,3.6e-07,4.6e-07,3.7e-06,0.029,0.029,0.00016,0.00074,3.8e-05,0.00083,0.00044,0.00077,0.00083,1,1 -26790000,0.77,0.07,-0.029,-0.64,1.2,0.79,-1.3,0.7,0.51,-3.7e+02,-0.00097,-0.0058,8.1e-05,0.025,-0.089,-0.13,-0.14,-0.032,0.48,-0.0044,-0.056,-0.049,0,0,0.00038,0.00083,0.026,0.054,0.079,0.0061,0.3,0.33,0.033,3.6e-07,4.6e-07,3.7e-06,0.029,0.029,0.00016,0.0007,3.6e-05,0.00079,0.00041,0.00073,0.00078,1,1 -26890000,0.76,0.093,-0.036,-0.64,1.4,0.87,-1.3,0.84,0.59,-3.7e+02,-0.00096,-0.0058,8.6e-05,0.026,-0.089,-0.13,-0.15,-0.032,0.48,-0.0049,-0.052,-0.047,0,0,0.00043,0.0011,0.024,0.057,0.085,0.0061,0.31,0.35,0.033,3.6e-07,4.6e-07,3.7e-06,0.029,0.029,0.00016,0.00066,3.4e-05,0.00074,0.00039,0.00068,0.00074,1,1 -26990000,0.76,0.12,-0.041,-0.64,1.5,0.98,-1.3,0.98,0.68,-3.7e+02,-0.00096,-0.0058,7.5e-05,0.028,-0.09,-0.13,-0.15,-0.034,0.48,-0.0052,-0.046,-0.043,0,0,0.00048,0.0014,0.021,0.061,0.091,0.0061,0.33,0.38,0.033,3.6e-07,4.6e-07,3.6e-06,0.029,0.029,0.00016,0.0006,3.1e-05,0.00067,0.00036,0.00062,0.00067,1,1 -27090000,0.76,0.12,-0.041,-0.64,1.7,1.1,-1.2,1.1,0.78,-3.7e+02,-0.00097,-0.0058,5.8e-05,0.03,-0.09,-0.13,-0.16,-0.035,0.47,-0.0052,-0.041,-0.038,0,0,0.00048,0.0013,0.019,0.064,0.098,0.0061,0.36,0.4,0.033,3.6e-07,4.6e-07,3.6e-06,0.029,0.029,0.00015,0.00055,2.8e-05,0.0006,0.00033,0.00056,0.0006,1,1 -27190000,0.76,0.11,-0.039,-0.64,1.9,1.2,-1.2,1.3,0.91,-3.7e+02,-0.00097,-0.0058,4e-05,0.031,-0.089,-0.13,-0.16,-0.035,0.47,-0.0049,-0.038,-0.035,0,0,0.00044,0.0011,0.017,0.067,0.1,0.0062,0.38,0.43,0.034,3.6e-07,4.6e-07,3.6e-06,0.029,0.029,0.00015,0.00051,2.6e-05,0.00056,0.00031,0.00052,0.00056,1,1 -27290000,0.76,0.093,-0.035,-0.64,2,1.3,-1.2,1.5,1,-3.7e+02,-0.00097,-0.0058,3.4e-05,0.032,-0.087,-0.13,-0.16,-0.036,0.47,-0.005,-0.035,-0.033,0,0,0.0004,0.00082,0.015,0.069,0.11,0.0062,0.4,0.46,0.033,3.6e-07,4.7e-07,3.6e-06,0.029,0.029,0.00015,0.00048,2.5e-05,0.00053,0.00029,0.00049,0.00052,1,1 -27390000,0.76,0.077,-0.03,-0.64,2.1,1.4,-1.2,1.7,1.2,-3.7e+02,-0.00097,-0.0058,2.9e-05,0.032,-0.085,-0.13,-0.17,-0.036,0.47,-0.005,-0.033,-0.032,0,0,0.00037,0.00064,0.014,0.071,0.11,0.0062,0.43,0.49,0.033,3.6e-07,4.7e-07,3.6e-06,0.029,0.029,0.00015,0.00046,2.4e-05,0.00051,0.00027,0.00047,0.00051,1,1 -27490000,0.76,0.062,-0.026,-0.64,2.2,1.4,-1.2,1.9,1.3,-3.7e+02,-0.00097,-0.0058,2e-05,0.033,-0.083,-0.13,-0.17,-0.037,0.47,-0.0048,-0.032,-0.031,0,0,0.00035,0.00051,0.013,0.071,0.11,0.0062,0.45,0.52,0.033,3.6e-07,4.7e-07,3.6e-06,0.029,0.029,0.00015,0.00044,2.3e-05,0.0005,0.00025,0.00046,0.0005,1,1 -27590000,0.77,0.049,-0.022,-0.64,2.3,1.5,-1.2,2.2,1.5,-3.7e+02,-0.00097,-0.0058,9.3e-06,0.033,-0.081,-0.13,-0.17,-0.037,0.47,-0.0044,-0.031,-0.031,0,0,0.00034,0.00044,0.012,0.072,0.11,0.0062,0.48,0.56,0.033,3.6e-07,4.7e-07,3.6e-06,0.029,0.029,0.00015,0.00044,2.3e-05,0.00049,0.00024,0.00045,0.00049,1,1 -27690000,0.77,0.047,-0.021,-0.64,2.3,1.5,-1.2,2.4,1.6,-3.7e+02,-0.00097,-0.0058,-8.3e-07,0.033,-0.079,-0.13,-0.17,-0.037,0.47,-0.004,-0.031,-0.031,0,0,0.00034,0.00043,0.011,0.072,0.11,0.0063,0.51,0.59,0.033,3.6e-07,4.7e-07,3.6e-06,0.028,0.029,0.00015,0.00043,2.2e-05,0.00049,0.00023,0.00045,0.00049,1,1 -27790000,0.77,0.049,-0.021,-0.64,2.3,1.6,-1.2,2.6,1.8,-3.7e+02,-0.00097,-0.0058,-1.6e-05,0.034,-0.078,-0.13,-0.17,-0.037,0.47,-0.0035,-0.03,-0.03,0,0,0.00034,0.00043,0.0098,0.073,0.11,0.0063,0.54,0.63,0.033,3.6e-07,4.7e-07,3.6e-06,0.028,0.029,0.00015,0.00042,2.2e-05,0.00048,0.00022,0.00044,0.00048,1,1 -27890000,0.77,0.047,-0.021,-0.64,2.4,1.6,-1.2,2.8,2,-3.7e+02,-0.00097,-0.0058,-1.6e-05,0.034,-0.076,-0.13,-0.17,-0.037,0.47,-0.0035,-0.03,-0.03,0,0,0.00034,0.00042,0.0093,0.074,0.11,0.0063,0.57,0.67,0.034,3.6e-07,4.7e-07,3.6e-06,0.028,0.029,0.00014,0.00041,2.2e-05,0.00048,0.00022,0.00043,0.00048,1,1 -27990000,0.77,0.043,-0.02,-0.64,2.4,1.6,-1.2,3.1,2.1,-3.7e+02,-0.00097,-0.0058,-1.9e-05,0.034,-0.075,-0.13,-0.17,-0.037,0.47,-0.0035,-0.03,-0.03,0,0,0.00034,0.0004,0.0087,0.075,0.11,0.0064,0.61,0.71,0.033,3.6e-07,4.7e-07,3.6e-06,0.028,0.029,0.00014,0.00041,2.1e-05,0.00048,0.00021,0.00043,0.00048,1,1 -28090000,0.77,0.057,-0.025,-0.63,2.4,1.6,-1.2,3.3,2.3,-3.7e+02,-0.00097,-0.0058,-3.2e-05,0.034,-0.073,-0.13,-0.17,-0.038,0.47,-0.0031,-0.029,-0.03,0,0,0.00034,0.00044,0.0081,0.076,0.11,0.0064,0.64,0.75,0.033,3.6e-07,4.7e-07,3.6e-06,0.028,0.029,0.00014,0.0004,2.1e-05,0.00048,0.0002,0.00042,0.00047,1,1 -28190000,0.77,0.071,-0.028,-0.63,2.5,1.7,-0.93,3.6,2.4,-3.7e+02,-0.00096,-0.0058,-3.4e-05,0.034,-0.071,-0.13,-0.17,-0.038,0.46,-0.0031,-0.029,-0.03,0,0,0.00035,0.00048,0.0077,0.077,0.11,0.0065,0.68,0.8,0.034,3.7e-07,4.7e-07,3.6e-06,0.028,0.029,0.00014,0.00039,2.1e-05,0.00047,0.0002,0.00042,0.00047,1,1 -28290000,0.77,0.053,-0.022,-0.64,2.5,1.7,-0.065,3.8,2.6,-3.7e+02,-0.00096,-0.0058,-4.4e-05,0.034,-0.068,-0.13,-0.17,-0.038,0.46,-0.0028,-0.028,-0.029,0,0,0.00034,0.00042,0.0074,0.076,0.1,0.0065,0.72,0.84,0.034,3.7e-07,4.7e-07,3.6e-06,0.028,0.029,0.00014,0.00038,2e-05,0.00046,0.0002,0.00041,0.00046,1,1 -28390000,0.77,0.02,-0.0095,-0.64,2.5,1.7,0.79,4,2.8,-3.7e+02,-0.00097,-0.0058,-5.2e-05,0.034,-0.065,-0.13,-0.17,-0.038,0.46,-0.0027,-0.027,-0.029,0,0,0.00033,0.00036,0.0071,0.076,0.1,0.0066,0.75,0.89,0.033,3.6e-07,4.8e-07,3.6e-06,0.028,0.029,0.00014,0.00038,2e-05,0.00046,0.00019,0.00041,0.00046,1,1 -28490000,0.77,0.0016,-0.0026,-0.64,2.4,1.7,1.1,4.3,3,-3.7e+02,-0.00098,-0.0058,-5.9e-05,0.034,-0.063,-0.13,-0.17,-0.038,0.46,-0.0027,-0.027,-0.029,0,0,0.00033,0.00035,0.0068,0.076,0.1,0.0066,0.79,0.94,0.034,3.6e-07,4.8e-07,3.5e-06,0.028,0.029,0.00014,0.00038,2e-05,0.00046,0.00019,0.0004,0.00046,1,1 -28590000,0.77,-0.0021,-0.0012,-0.64,2.4,1.6,0.99,4.5,3.1,-3.7e+02,-0.00097,-0.0058,-6e-05,0.034,-0.061,-0.12,-0.17,-0.038,0.46,-0.0027,-0.027,-0.029,0,0,0.00033,0.00035,0.0065,0.077,0.1,0.0067,0.83,0.99,0.034,3.6e-07,4.8e-07,3.5e-06,0.028,0.028,0.00014,0.00038,2e-05,0.00046,0.00018,0.0004,0.00046,1,1 -28690000,0.77,-0.003,-0.00065,-0.63,2.3,1.6,0.99,4.8,3.3,-3.7e+02,-0.00098,-0.0058,-6.8e-05,0.034,-0.061,-0.12,-0.17,-0.038,0.46,-0.0026,-0.027,-0.029,0,0,0.00033,0.00035,0.0063,0.077,0.1,0.0067,0.87,1,0.034,3.6e-07,4.8e-07,3.5e-06,0.028,0.028,0.00014,0.00038,2e-05,0.00046,0.00018,0.0004,0.00046,1,1 -28790000,0.77,-0.0033,-0.00044,-0.63,2.2,1.6,1,5,3.5,-3.7e+02,-0.00099,-0.0058,-7.7e-05,0.033,-0.059,-0.12,-0.17,-0.038,0.46,-0.0024,-0.027,-0.029,0,0,0.00033,0.00036,0.006,0.078,0.1,0.0067,0.91,1.1,0.034,3.5e-07,4.8e-07,3.5e-06,0.028,0.028,0.00013,0.00038,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 -28890000,0.77,-0.003,-0.00045,-0.63,2.2,1.5,0.99,5.2,3.6,-3.7e+02,-0.001,-0.0058,-8.4e-05,0.033,-0.058,-0.12,-0.17,-0.038,0.46,-0.0024,-0.027,-0.028,0,0,0.00033,0.00036,0.0059,0.079,0.1,0.0068,0.96,1.2,0.034,3.5e-07,4.8e-07,3.5e-06,0.028,0.028,0.00013,0.00038,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 -28990000,0.78,-0.0025,-0.00063,-0.63,2.1,1.5,0.98,5.5,3.8,-3.7e+02,-0.001,-0.0058,-9.7e-05,0.032,-0.055,-0.12,-0.17,-0.038,0.46,-0.0022,-0.027,-0.028,0,0,0.00033,0.00036,0.0057,0.081,0.1,0.0068,1,1.2,0.034,3.5e-07,4.8e-07,3.5e-06,0.028,0.028,0.00013,0.00037,2e-05,0.00045,0.00018,0.0004,0.00045,1,1 -29090000,0.78,-0.002,-0.00079,-0.63,2.1,1.5,0.97,5.7,3.9,-3.7e+02,-0.001,-0.0058,-0.0001,0.032,-0.054,-0.12,-0.17,-0.038,0.46,-0.0021,-0.027,-0.028,0,0,0.00033,0.00036,0.0055,0.082,0.11,0.0069,1,1.3,0.034,3.5e-07,4.8e-07,3.5e-06,0.028,0.028,0.00013,0.00037,2e-05,0.00045,0.00017,0.0004,0.00045,1,1 -29190000,0.78,-0.0017,-0.00087,-0.63,2,1.5,0.97,5.9,4.1,-3.7e+02,-0.001,-0.0058,-0.0001,0.031,-0.053,-0.12,-0.17,-0.038,0.46,-0.0021,-0.027,-0.028,0,0,0.00033,0.00036,0.0054,0.083,0.11,0.0069,1.1,1.3,0.034,3.5e-07,4.8e-07,3.5e-06,0.028,0.028,0.00013,0.00037,2e-05,0.00045,0.00017,0.0004,0.00045,1,1 -29290000,0.78,-0.00077,-0.0012,-0.63,1.9,1.4,1,6.1,4.2,-3.7e+02,-0.001,-0.0058,-0.00011,0.03,-0.051,-0.12,-0.17,-0.038,0.46,-0.002,-0.027,-0.028,0,0,0.00032,0.00036,0.0053,0.085,0.11,0.0069,1.1,1.4,0.034,3.4e-07,4.8e-07,3.5e-06,0.028,0.027,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29390000,0.78,0.00066,-0.0015,-0.63,1.9,1.4,1,6.3,4.4,-3.7e+02,-0.001,-0.0058,-0.00013,0.03,-0.049,-0.12,-0.17,-0.038,0.46,-0.0017,-0.027,-0.028,0,0,0.00032,0.00036,0.0051,0.086,0.11,0.007,1.2,1.5,0.034,3.4e-07,4.9e-07,3.5e-06,0.028,0.027,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29490000,0.78,0.0019,-0.0019,-0.63,1.8,1.4,1,6.5,4.5,-3.7e+02,-0.001,-0.0058,-0.00013,0.029,-0.048,-0.12,-0.17,-0.038,0.46,-0.0017,-0.027,-0.028,0,0,0.00032,0.00036,0.005,0.088,0.12,0.007,1.3,1.5,0.034,3.4e-07,4.9e-07,3.5e-06,0.028,0.027,0.00013,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29590000,0.78,0.003,-0.0021,-0.63,1.8,1.4,1,6.6,4.7,-3.7e+02,-0.001,-0.0058,-0.00013,0.028,-0.046,-0.12,-0.17,-0.038,0.46,-0.0016,-0.028,-0.028,0,0,0.00032,0.00036,0.0049,0.09,0.12,0.007,1.3,1.6,0.034,3.4e-07,4.9e-07,3.5e-06,0.028,0.027,0.00012,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29690000,0.78,0.0037,-0.0024,-0.63,1.8,1.4,0.99,6.8,4.8,-3.7e+02,-0.001,-0.0058,-0.00014,0.027,-0.043,-0.12,-0.17,-0.038,0.46,-0.0015,-0.028,-0.028,0,0,0.00032,0.00036,0.0048,0.092,0.12,0.0071,1.4,1.7,0.034,3.4e-07,4.9e-07,3.5e-06,0.028,0.027,0.00012,0.00037,2e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29790000,0.78,0.0043,-0.0026,-0.63,1.7,1.3,0.98,7,4.9,-3.7e+02,-0.0011,-0.0058,-0.00014,0.027,-0.04,-0.12,-0.17,-0.038,0.46,-0.0015,-0.028,-0.027,0,0,0.00032,0.00036,0.0048,0.094,0.13,0.0071,1.4,1.8,0.034,3.3e-07,4.9e-07,3.5e-06,0.028,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00017,0.0004,0.00044,1,1 -29890000,0.78,0.0046,-0.0027,-0.63,1.7,1.3,0.97,7.2,5.1,-3.7e+02,-0.0011,-0.0058,-0.00015,0.026,-0.036,-0.12,-0.17,-0.038,0.46,-0.0013,-0.028,-0.027,0,0,0.00032,0.00037,0.0047,0.096,0.13,0.0071,1.5,1.9,0.034,3.3e-07,4.9e-07,3.5e-06,0.028,0.027,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00044,1,1 -29990000,0.78,0.0048,-0.0028,-0.63,1.7,1.3,0.95,7.3,5.2,-3.7e+02,-0.0011,-0.0058,-0.00016,0.025,-0.033,-0.12,-0.17,-0.038,0.46,-0.0012,-0.028,-0.027,0,0,0.00032,0.00037,0.0046,0.098,0.14,0.0071,1.5,2,0.034,3.3e-07,4.9e-07,3.5e-06,0.028,0.026,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00044,1,1 -30090000,0.78,0.0047,-0.0028,-0.63,1.6,1.3,0.94,7.5,5.4,-3.7e+02,-0.0011,-0.0058,-0.00016,0.024,-0.031,-0.12,-0.17,-0.038,0.46,-0.0011,-0.028,-0.027,0,0,0.00031,0.00037,0.0045,0.1,0.14,0.0071,1.6,2,0.034,3.3e-07,4.9e-07,3.5e-06,0.028,0.026,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 -30190000,0.78,0.0045,-0.0027,-0.63,1.6,1.3,0.93,7.7,5.5,-3.7e+02,-0.0011,-0.0058,-0.00016,0.023,-0.031,-0.12,-0.17,-0.038,0.46,-0.0011,-0.028,-0.028,0,0,0.00031,0.00037,0.0045,0.1,0.15,0.0072,1.7,2.1,0.035,3.3e-07,4.9e-07,3.5e-06,0.028,0.026,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 -30290000,0.78,0.0043,-0.0026,-0.63,1.5,1.3,0.92,7.8,5.6,-3.7e+02,-0.0011,-0.0058,-0.00017,0.022,-0.029,-0.12,-0.17,-0.038,0.46,-0.001,-0.028,-0.028,0,0,0.00031,0.00037,0.0044,0.1,0.15,0.0072,1.8,2.2,0.035,3.3e-07,4.9e-07,3.5e-06,0.028,0.026,0.00012,0.00037,1.9e-05,0.00044,0.00016,0.0004,0.00043,1,1 -30390000,0.78,0.0042,-0.0027,-0.63,1.5,1.3,0.9,8,5.7,-3.7e+02,-0.0011,-0.0058,-0.00017,0.021,-0.027,-0.12,-0.17,-0.038,0.46,-0.00098,-0.028,-0.027,0,0,0.00031,0.00037,0.0044,0.11,0.16,0.0072,1.8,2.4,0.034,3.2e-07,4.9e-07,3.5e-06,0.028,0.026,0.00012,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30490000,0.78,0.004,-0.0026,-0.63,1.5,1.2,0.89,8.2,5.9,-3.7e+02,-0.0011,-0.0058,-0.00017,0.02,-0.025,-0.12,-0.17,-0.038,0.46,-0.00097,-0.028,-0.027,0,0,0.00031,0.00037,0.0043,0.11,0.16,0.0072,1.9,2.5,0.035,3.2e-07,5e-07,3.5e-06,0.028,0.026,0.00011,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30590000,0.78,0.0037,-0.0026,-0.63,1.5,1.2,0.85,8.3,6,-3.7e+02,-0.0011,-0.0058,-0.00017,0.019,-0.021,-0.12,-0.17,-0.038,0.46,-0.00096,-0.028,-0.027,0,0,0.00031,0.00037,0.0043,0.11,0.17,0.0072,2,2.6,0.035,3.2e-07,5e-07,3.5e-06,0.028,0.026,0.00011,0.00037,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30690000,0.78,0.0035,-0.0025,-0.63,1.4,1.2,0.84,8.5,6.1,-3.7e+02,-0.0011,-0.0058,-0.00017,0.018,-0.018,-0.12,-0.17,-0.038,0.46,-0.0009,-0.028,-0.027,0,0,0.00031,0.00038,0.0042,0.11,0.18,0.0072,2.1,2.7,0.035,3.2e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30790000,0.78,0.0031,-0.0023,-0.63,1.4,1.2,0.83,8.6,6.2,-3.7e+02,-0.0011,-0.0058,-0.00018,0.017,-0.017,-0.12,-0.17,-0.038,0.46,-0.00081,-0.028,-0.027,0,0,0.00031,0.00038,0.0042,0.12,0.18,0.0072,2.1,2.8,0.035,3.2e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30890000,0.78,0.0028,-0.0023,-0.63,1.4,1.2,0.82,8.8,6.3,-3.7e+02,-0.0011,-0.0058,-0.00018,0.016,-0.015,-0.12,-0.17,-0.038,0.46,-0.00076,-0.028,-0.027,0,0,0.0003,0.00038,0.0042,0.12,0.19,0.0072,2.2,3,0.035,3.2e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -30990000,0.78,0.0024,-0.0022,-0.63,1.3,1.2,0.81,8.9,6.5,-3.7e+02,-0.0011,-0.0058,-0.00019,0.015,-0.011,-0.12,-0.17,-0.038,0.46,-0.0007,-0.028,-0.027,0,0,0.0003,0.00038,0.0041,0.12,0.2,0.0071,2.3,3.1,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -31090000,0.78,0.0019,-0.0021,-0.63,1.3,1.2,0.8,9,6.6,-3.7e+02,-0.0011,-0.0058,-0.00019,0.014,-0.0084,-0.12,-0.17,-0.038,0.46,-0.0006,-0.028,-0.027,0,0,0.0003,0.00038,0.0041,0.12,0.2,0.0072,2.4,3.2,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.0004,0.00043,1,1 -31190000,0.78,0.0017,-0.002,-0.63,1.3,1.1,0.79,9.2,6.7,-3.7e+02,-0.0011,-0.0058,-0.00019,0.013,-0.0048,-0.12,-0.17,-0.038,0.46,-0.00052,-0.029,-0.027,0,0,0.0003,0.00038,0.0041,0.13,0.21,0.0071,2.5,3.4,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.025,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.00039,0.00043,1,1 -31290000,0.78,0.0012,-0.0019,-0.63,1.2,1.1,0.79,9.3,6.8,-3.7e+02,-0.0011,-0.0058,-0.00019,0.011,-0.002,-0.12,-0.17,-0.038,0.46,-0.00044,-0.029,-0.027,0,0,0.0003,0.00038,0.0041,0.13,0.22,0.0071,2.6,3.5,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.024,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.00039,0.00043,1,1 -31390000,0.78,0.00064,-0.0017,-0.63,1.2,1.1,0.79,9.4,6.9,-3.7e+02,-0.0011,-0.0058,-0.00019,0.01,0.00086,-0.12,-0.17,-0.038,0.46,-0.00037,-0.029,-0.027,0,0,0.0003,0.00039,0.004,0.13,0.23,0.0071,2.7,3.7,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.024,0.00011,0.00036,1.9e-05,0.00043,0.00016,0.00039,0.00042,1,1 -31490000,0.78,0.00013,-0.0016,-0.63,1.2,1.1,0.79,9.5,7,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0088,0.0035,-0.12,-0.17,-0.038,0.46,-0.00025,-0.029,-0.027,0,0,0.00029,0.00039,0.004,0.13,0.23,0.0071,2.8,3.9,0.035,3.1e-07,5e-07,3.5e-06,0.028,0.024,0.0001,0.00036,1.9e-05,0.00043,0.00016,0.00039,0.00042,1,1 -31590000,0.78,-0.00017,-0.0015,-0.63,1.1,1.1,0.78,9.7,7.1,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0075,0.0055,-0.11,-0.17,-0.038,0.46,-0.0002,-0.029,-0.027,0,0,0.00029,0.00039,0.004,0.14,0.24,0.007,2.9,4.1,0.035,3.1e-07,5.1e-07,3.5e-06,0.028,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -31690000,0.78,-0.00078,-0.0013,-0.63,1.1,1.1,0.79,9.8,7.2,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0062,0.0082,-0.11,-0.17,-0.038,0.46,-0.00013,-0.029,-0.027,0,0,0.00029,0.00039,0.004,0.14,0.25,0.007,3,4.2,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.024,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -31790000,0.78,-0.0014,-0.0012,-0.63,1.1,1,0.79,9.9,7.4,-3.7e+02,-0.0011,-0.0058,-0.0002,0.0049,0.011,-0.11,-0.17,-0.038,0.46,-6.7e-05,-0.029,-0.027,0,0,0.00029,0.00039,0.004,0.14,0.26,0.007,3.1,4.4,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -31890000,0.78,-0.002,-0.0011,-0.63,1.1,1,0.78,10,7.5,-3.7e+02,-0.0011,-0.0058,-0.00021,0.0036,0.014,-0.11,-0.17,-0.038,0.46,2.1e-05,-0.029,-0.027,0,0,0.00029,0.00039,0.0039,0.15,0.27,0.007,3.2,4.6,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -31990000,0.78,-0.0024,-0.00095,-0.63,1,1,0.78,10,7.6,-3.7e+02,-0.0012,-0.0058,-0.00021,0.0021,0.018,-0.11,-0.18,-0.038,0.46,0.00013,-0.029,-0.027,0,0,0.00029,0.0004,0.0039,0.15,0.28,0.0069,3.3,4.8,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,0.0001,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -32090000,0.78,-0.003,-0.00075,-0.63,1,0.99,0.79,10,7.7,-3.7e+02,-0.0012,-0.0058,-0.00022,0.0006,0.02,-0.11,-0.18,-0.038,0.46,0.00024,-0.029,-0.026,0,0,0.00028,0.0004,0.0039,0.15,0.29,0.0069,3.4,5.1,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,9.9e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -32190000,0.78,-0.0038,-0.00056,-0.63,0.97,0.98,0.78,10,7.8,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.001,0.024,-0.11,-0.18,-0.038,0.46,0.00038,-0.029,-0.026,0,0,0.00028,0.0004,0.0039,0.15,0.3,0.0069,3.6,5.3,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,9.8e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -32290000,0.78,-0.0044,-0.00047,-0.63,0.94,0.96,0.78,11,7.9,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0027,0.028,-0.11,-0.18,-0.038,0.46,0.00048,-0.03,-0.026,0,0,0.00028,0.0004,0.0039,0.16,0.31,0.0068,3.7,5.5,0.035,3e-07,5.1e-07,3.5e-06,0.028,0.023,9.7e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -32390000,0.78,-0.0049,-0.00037,-0.63,0.91,0.94,0.78,11,8,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0036,0.029,-0.11,-0.18,-0.038,0.46,0.00053,-0.03,-0.026,0,0,0.00028,0.0004,0.0039,0.16,0.32,0.0068,3.8,5.8,0.035,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.7e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00042,1,1 -32490000,0.78,-0.0051,-0.00031,-0.63,0.88,0.93,0.78,11,8.1,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.005,0.032,-0.11,-0.18,-0.039,0.46,0.00062,-0.03,-0.026,0,0,0.00028,0.00041,0.0039,0.16,0.33,0.0068,3.9,6,0.035,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.6e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00041,1,1 -32590000,0.78,-0.0053,-0.00026,-0.63,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0058,0.033,-0.11,-0.18,-0.039,0.46,0.00068,-0.03,-0.026,0,0,0.00028,0.00041,0.0039,0.25,0.25,0.56,0.25,0.25,0.037,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.5e-05,0.00036,1.9e-05,0.00042,0.00016,0.00039,0.00041,1,1 -32690000,0.78,-0.0053,-0.00029,-0.63,-1.6,-0.85,0.61,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0067,0.035,-0.11,-0.18,-0.039,0.46,0.00075,-0.03,-0.026,0,0,0.00027,0.00041,0.0039,0.25,0.25,0.55,0.26,0.26,0.048,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.5e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -32790000,0.78,-0.0052,-0.00031,-0.63,-1.5,-0.83,0.62,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0075,0.036,-0.11,-0.18,-0.039,0.46,0.0008,-0.03,-0.026,0,0,0.00027,0.00041,0.0038,0.13,0.13,0.27,0.26,0.26,0.048,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -32890000,0.78,-0.005,-0.00043,-0.63,-1.6,-0.85,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00025,-0.0083,0.038,-0.11,-0.18,-0.039,0.46,0.0009,-0.03,-0.025,0,0,0.00027,0.00041,0.0038,0.13,0.13,0.26,0.27,0.27,0.059,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.4e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -32990000,0.78,-0.005,-0.00055,-0.63,-1.5,-0.84,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0091,0.039,-0.11,-0.18,-0.039,0.46,0.00092,-0.03,-0.025,0,0,0.00027,0.00041,0.0038,0.084,0.085,0.17,0.27,0.27,0.057,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.3e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -33090000,0.78,-0.005,-0.00052,-0.63,-1.6,-0.86,0.58,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0095,0.039,-0.11,-0.18,-0.038,0.46,0.00094,-0.03,-0.025,0,0,0.00027,0.00041,0.0038,0.084,0.086,0.16,0.28,0.28,0.065,2.9e-07,5.1e-07,3.5e-06,0.028,0.022,9.3e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -33190000,0.78,-0.0036,-0.0038,-0.62,-1.5,-0.84,0.53,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.0097,0.04,-0.11,-0.18,-0.038,0.46,0.00094,-0.03,-0.025,0,0,0.00027,0.00041,0.0038,0.063,0.065,0.11,0.28,0.28,0.062,2.8e-07,5.1e-07,3.5e-06,0.028,0.022,9.3e-05,0.00036,1.9e-05,0.00041,0.00016,0.00039,0.00041,1,1 -33290000,0.82,-0.0015,-0.016,-0.57,-1.5,-0.86,0.5,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.0094,0.04,-0.11,-0.18,-0.039,0.46,0.00091,-0.03,-0.025,0,0,0.00027,0.00041,0.0038,0.064,0.066,0.11,0.29,0.29,0.067,2.8e-07,5.1e-07,3.5e-06,0.028,0.021,9.3e-05,0.00035,1.9e-05,0.00041,0.00015,0.00039,0.00041,1,1 -33390000,0.89,-0.0018,-0.013,-0.45,-1.5,-0.85,0.7,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.014,0.037,-0.11,-0.18,-0.039,0.46,0.0015,-0.028,-0.025,0,0,0.00028,0.00039,0.0037,0.051,0.053,0.083,0.29,0.29,0.065,2.8e-07,5.1e-07,3.4e-06,0.027,0.021,9.3e-05,0.00033,1.7e-05,0.00041,0.00015,0.00035,0.00041,1,1 -33490000,0.95,-0.00034,-0.0053,-0.31,-1.5,-0.86,0.72,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00024,-0.016,0.037,-0.11,-0.18,-0.04,0.46,0.002,-0.02,-0.025,0,0,0.00031,0.00035,0.0034,0.052,0.055,0.075,0.3,0.3,0.068,2.8e-07,5.1e-07,3.4e-06,0.027,0.021,9.3e-05,0.00025,1.4e-05,0.00041,0.00013,0.00026,0.00041,1,1 -33590000,0.99,-0.0031,0.0014,-0.14,-1.5,-0.84,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00021,-0.016,0.037,-0.11,-0.19,-0.042,0.46,0.0028,-0.013,-0.026,0,0,0.00034,0.00031,0.0029,0.044,0.047,0.061,0.3,0.3,0.065,2.8e-07,5.1e-07,3.4e-06,0.027,0.021,9.3e-05,0.00017,9.9e-06,0.00041,9.4e-05,0.00016,0.0004,1,1 -33690000,1,-0.0066,0.0049,0.025,-1.6,-0.86,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00023,-0.016,0.037,-0.11,-0.19,-0.043,0.46,0.002,-0.0093,-0.026,0,0,0.00037,0.00027,0.0026,0.045,0.05,0.056,0.31,0.31,0.068,2.8e-07,5.1e-07,3.3e-06,0.027,0.021,9.3e-05,0.00013,7.8e-06,0.0004,6.9e-05,0.0001,0.0004,1,1 -33790000,0.98,-0.0075,0.0068,0.19,-1.6,-0.86,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.016,0.037,-0.11,-0.2,-0.043,0.46,0.0021,-0.0067,-0.027,0,0,0.00037,0.00025,0.0023,0.04,0.045,0.047,0.31,0.31,0.064,2.8e-07,5e-07,3.3e-06,0.027,0.021,9.3e-05,9.6e-05,6.3e-06,0.0004,4.8e-05,6.3e-05,0.0004,1,1 -33890000,0.93,-0.0078,0.0082,0.35,-1.7,-0.9,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.016,0.037,-0.11,-0.2,-0.043,0.46,0.002,-0.0054,-0.027,0,0,0.00036,0.00026,0.0022,0.044,0.051,0.043,0.32,0.32,0.065,2.8e-07,5e-07,3.3e-06,0.027,0.021,9.3e-05,8e-05,5.6e-06,0.0004,3.4e-05,4.2e-05,0.0004,1,1 -33990000,0.87,-0.0098,0.0057,0.49,-1.7,-0.91,0.64,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.014,0.036,-0.11,-0.2,-0.044,0.46,0.0017,-0.004,-0.027,0,0,0.00032,0.00026,0.002,0.041,0.049,0.036,0.32,0.32,0.062,2.8e-07,4.9e-07,3.3e-06,0.027,0.021,9.3e-05,7e-05,5.1e-06,0.0004,2.6e-05,3e-05,0.0004,1,1 -34090000,0.81,-0.011,0.0045,0.59,-1.7,-0.97,0.65,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.0087,0.035,-0.11,-0.2,-0.044,0.46,0.0012,-0.0034,-0.027,0,0,0.00029,0.00028,0.002,0.047,0.056,0.033,0.33,0.33,0.063,2.8e-07,4.9e-07,3.2e-06,0.026,0.021,9.3e-05,6.5e-05,4.8e-06,0.0004,2.1e-05,2.4e-05,0.0004,1,1 -34190000,0.76,-0.0084,0.003,0.65,-1.7,-0.97,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.038,0.052,-0.11,-0.2,-0.044,0.46,0.0013,-0.0028,-0.027,0,0,0.00026,0.00027,0.0018,0.045,0.054,0.029,0.33,0.33,0.06,2.8e-07,4.8e-07,3.2e-06,0.025,0.02,9.2e-05,5.9e-05,4.5e-06,0.0004,1.7e-05,1.9e-05,0.0004,1,1 -34290000,0.72,-0.0055,0.0042,0.69,-1.7,-1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.036,0.05,-0.11,-0.2,-0.044,0.46,0.0011,-0.0024,-0.027,0,0,0.00025,0.00028,0.0018,0.053,0.063,0.027,0.34,0.34,0.06,2.8e-07,4.8e-07,3.2e-06,0.025,0.02,9.2e-05,5.6e-05,4.4e-06,0.0004,1.4e-05,1.6e-05,0.0004,1,1 -34390000,0.7,-0.0027,0.0056,0.71,-1.8,-1.1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.035,0.049,-0.11,-0.2,-0.044,0.46,0.00096,-0.0021,-0.027,0,0,0.00024,0.00029,0.0017,0.062,0.075,0.025,0.35,0.35,0.06,2.8e-07,4.8e-07,3.2e-06,0.024,0.02,9.2e-05,5.4e-05,4.3e-06,0.0004,1.3e-05,1.4e-05,0.0004,1,1 -34490000,0.68,-0.0006,0.0067,0.73,-1.8,-1.1,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.034,0.049,-0.11,-0.2,-0.044,0.46,0.00084,-0.0021,-0.027,0,0,0.00024,0.00029,0.0017,0.072,0.088,0.023,0.36,0.36,0.06,2.8e-07,4.8e-07,3.2e-06,0.024,0.02,9.3e-05,5.3e-05,4.2e-06,0.0004,1.1e-05,1.2e-05,0.0004,1,1 -34590000,0.68,0.00068,0.0076,0.74,-1.9,-1.2,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.032,0.049,-0.11,-0.2,-0.044,0.46,0.00068,-0.002,-0.027,0,0,0.00024,0.0003,0.0017,0.084,0.1,0.021,0.38,0.38,0.059,2.8e-07,4.8e-07,3.2e-06,0.024,0.02,9.3e-05,5.1e-05,4.2e-06,0.0004,1e-05,1.1e-05,0.0004,1,1 -34690000,0.67,0.0015,0.008,0.74,-1.9,-1.2,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.033,0.048,-0.11,-0.2,-0.044,0.46,0.00071,-0.0018,-0.027,0,0,0.00023,0.0003,0.0017,0.097,0.12,0.019,0.39,0.4,0.059,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,5.1e-05,4.1e-06,0.0004,9.7e-06,1e-05,0.0004,1,1 -34790000,0.67,0.0021,0.0083,0.75,-2,-1.3,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00081,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.11,0.14,0.018,0.41,0.42,0.058,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,5e-05,4.1e-06,0.0004,9e-06,9.2e-06,0.0004,1,1 -34890000,0.66,0.0022,0.0083,0.75,-2,-1.3,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00019,-0.033,0.046,-0.11,-0.2,-0.044,0.46,0.00072,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.13,0.16,0.017,0.43,0.44,0.056,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.9e-05,4.1e-06,0.0004,8.4e-06,8.5e-06,0.0004,1,1 -34990000,0.66,-0.0011,0.016,0.75,-3,-2.2,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00082,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.16,0.22,0.016,0.46,0.47,0.056,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.8e-05,4e-06,0.0004,8e-06,8e-06,0.0004,1,1 -35090000,0.66,-0.0012,0.016,0.75,-3.1,-2.3,-0.2,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00081,-0.0017,-0.027,0,0,0.00023,0.00031,0.0017,0.18,0.25,0.015,0.49,0.51,0.055,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.8e-05,4e-06,0.0004,7.6e-06,7.5e-06,0.0004,1,1 -35190000,0.66,-0.0013,0.016,0.75,-3.1,-2.3,-0.19,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00084,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.2,0.27,0.014,0.52,0.55,0.054,2.8e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.7e-05,4e-06,0.0004,7.2e-06,7.1e-06,0.0004,1,1 -35290000,0.66,-0.0014,0.016,0.75,-3.2,-2.4,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00087,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.22,0.3,0.013,0.56,0.6,0.052,2.9e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.7e-05,4e-06,0.0004,6.9e-06,6.7e-06,0.0004,1,1 -35390000,0.66,-0.0014,0.016,0.75,-3.2,-2.4,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0002,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00094,-0.0017,-0.027,0,0,0.00023,0.0003,0.0017,0.25,0.33,0.013,0.61,0.66,0.052,2.9e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.6e-05,3.9e-06,0.0004,6.7e-06,6.4e-06,0.0004,1,1 -35490000,0.66,-0.0014,0.016,0.75,-3.2,-2.5,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.27,0.36,0.012,0.66,0.73,0.051,2.9e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.6e-05,3.9e-06,0.0004,6.4e-06,6.1e-06,0.0004,1,1 -35590000,0.66,-0.0014,0.016,0.75,-3.2,-2.5,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.034,0.046,-0.11,-0.2,-0.044,0.46,0.00095,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.3,0.4,0.011,0.72,0.8,0.05,2.9e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.5e-05,3.9e-06,0.0004,6.2e-06,5.9e-06,0.0004,1,1 -35690000,0.66,-0.0014,0.016,0.75,-3.3,-2.5,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.034,0.045,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.33,0.43,0.011,0.78,0.89,0.049,2.9e-07,4.9e-07,3.2e-06,0.024,0.02,9.3e-05,4.5e-05,3.9e-06,0.0004,6.1e-06,5.7e-06,0.0004,1,1 -35790000,0.66,-0.0014,0.016,0.75,-3.3,-2.6,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.034,0.045,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.36,0.47,0.01,0.86,0.99,0.048,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.5e-05,3.9e-06,0.0004,5.9e-06,5.5e-06,0.0004,1,1 -35890000,0.66,-0.0015,0.016,0.75,-3.3,-2.6,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.033,0.044,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00022,0.0003,0.0017,0.39,0.51,0.0099,0.94,1.1,0.047,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.4e-05,3.8e-06,0.0004,5.8e-06,5.3e-06,0.0004,1,1 -35990000,0.66,-0.0015,0.016,0.75,-3.4,-2.7,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.033,0.044,-0.11,-0.2,-0.044,0.46,0.00097,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.42,0.55,0.0096,1,1.2,0.047,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.4e-05,3.8e-06,0.0004,5.7e-06,5.1e-06,0.0004,1,1 -36090000,0.66,-0.0015,0.016,0.75,-3.4,-2.7,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00021,-0.032,0.044,-0.11,-0.2,-0.044,0.46,0.00099,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.46,0.59,0.0093,1.1,1.4,0.046,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.4e-05,3.8e-06,0.0004,5.5e-06,5e-06,0.0004,1,1 -36190000,0.66,-0.0016,0.016,0.75,-3.4,-2.8,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00023,-0.033,0.043,-0.11,-0.2,-0.044,0.46,0.0011,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.49,0.63,0.009,1.3,1.5,0.045,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.4e-05,3.8e-06,0.0004,5.4e-06,4.8e-06,0.0004,1,1 -36290000,0.66,-0.0015,0.016,0.75,-3.5,-2.8,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00023,-0.032,0.041,-0.11,-0.2,-0.044,0.46,0.0011,-0.0017,-0.027,0,0,0.00022,0.00029,0.0017,0.53,0.68,0.0088,1.4,1.7,0.045,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.3e-05,4.3e-05,3.8e-06,0.00039,5.4e-06,4.7e-06,0.0004,1,1 -36390000,0.66,-0.0016,0.016,0.75,-3.5,-2.9,-0.096,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00023,-0.032,0.041,-0.11,-0.2,-0.044,0.46,0.0011,-0.0017,-0.027,0,0,0.00021,0.00029,0.0017,0.56,0.73,0.0085,1.5,1.9,0.044,2.9e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.3e-05,3.8e-06,0.00039,5.3e-06,4.6e-06,0.0004,1,1 -36490000,0.66,-0.0017,0.016,0.75,-3.5,-2.9,-0.089,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00022,-0.031,0.041,-0.11,-0.2,-0.044,0.46,0.001,-0.0017,-0.027,0,0,0.00021,0.00029,0.0017,0.6,0.77,0.0083,1.7,2.1,0.043,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.3e-05,3.7e-06,0.00039,5.2e-06,4.5e-06,0.0004,1,1 -36590000,0.66,-0.0016,0.016,0.75,-3.6,-3,-0.079,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00023,-0.031,0.04,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.64,0.82,0.0082,1.9,2.4,0.042,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,5.1e-06,4.3e-06,0.0004,1,1 -36690000,0.66,-0.0017,0.016,0.75,-3.6,-3,-0.072,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00024,-0.03,0.04,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.68,0.87,0.008,2.1,2.6,0.042,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,5.1e-06,4.3e-06,0.0004,1,1 -36790000,0.66,-0.0017,0.016,0.75,-3.7,-3.1,-0.062,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00025,-0.03,0.038,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00029,0.0017,0.72,0.93,0.0079,2.3,2.9,0.041,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,5e-06,4.2e-06,0.0004,1,1 -36890000,0.66,-0.0017,0.016,0.75,-3.7,-3.1,-0.055,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00026,-0.03,0.038,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.77,0.98,0.0078,2.5,3.2,0.041,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.2e-05,3.7e-06,0.00039,5e-06,4.1e-06,0.0004,1,1 -36990000,0.66,-0.0017,0.017,0.75,-3.7,-3.2,-0.048,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00026,-0.029,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.81,1,0.0077,2.8,3.5,0.04,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.1e-05,3.7e-06,0.00039,4.9e-06,4e-06,0.0004,1,1 -37090000,0.66,-0.0017,0.017,0.75,-3.8,-3.2,-0.04,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00027,-0.029,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.86,1.1,0.0076,3.1,3.9,0.04,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.1e-05,3.7e-06,0.00039,4.9e-06,3.9e-06,0.0004,1,1 -37190000,0.66,-0.0017,0.017,0.75,-3.8,-3.3,-0.032,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00026,-0.028,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.9,1.1,0.0075,3.4,4.3,0.039,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.1e-05,3.7e-06,0.00039,4.8e-06,3.8e-06,0.0004,1,1 -37290000,0.66,-0.0018,0.017,0.75,-3.8,-3.4,-0.025,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00027,-0.028,0.037,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.00021,0.00028,0.0017,0.95,1.2,0.0075,3.7,4.7,0.039,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.1e-05,3.6e-06,0.00039,4.8e-06,3.8e-06,0.0004,1,1 -37390000,0.66,-0.0017,0.017,0.75,-3.9,-3.4,-0.018,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00028,-0.028,0.036,-0.11,-0.2,-0.044,0.46,0.0011,-0.0016,-0.027,0,0,0.0002,0.00028,0.0017,1,1.3,0.0075,4,5.2,0.039,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4.1e-05,3.6e-06,0.00039,4.8e-06,3.7e-06,0.0004,1,1 -37490000,0.66,-0.0017,0.017,0.75,-3.9,-3.5,-0.011,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.028,0.035,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00028,0.0017,1,1.3,0.0074,4.4,5.7,0.038,3e-07,4.9e-07,3.2e-06,0.024,0.019,9.4e-05,4e-05,3.6e-06,0.00039,4.7e-06,3.7e-06,0.0004,1,1 -37590000,0.66,-0.0017,0.017,0.75,-3.9,-3.5,-0.0024,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00029,-0.027,0.034,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00028,0.0017,1.1,1.4,0.0074,4.8,6.2,0.038,3e-07,4.9e-07,3.1e-06,0.024,0.018,9.4e-05,4e-05,3.6e-06,0.00039,4.7e-06,3.6e-06,0.0004,1,1 -37690000,0.66,-0.0018,0.017,0.75,-4,-3.6,0.0069,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.0003,-0.026,0.033,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.1,1.5,0.0074,5.2,6.8,0.038,3.1e-07,4.9e-07,3.1e-06,0.024,0.018,9.4e-05,4e-05,3.6e-06,0.00039,4.7e-06,3.5e-06,0.0004,1,1 -37790000,0.66,-0.0018,0.017,0.75,-4,-3.6,0.016,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00031,-0.026,0.032,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.2,1.5,0.0074,5.7,7.4,0.037,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,4e-05,3.6e-06,0.00039,4.7e-06,3.5e-06,0.0004,1,1 -37890000,0.66,-0.0019,0.017,0.75,-4,-3.7,0.023,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00031,-0.025,0.032,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.3,1.6,0.0074,6.2,8.1,0.037,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.9e-05,3.6e-06,0.00039,4.6e-06,3.4e-06,0.0004,1,1 -37990000,0.66,-0.0019,0.017,0.75,-4.1,-3.7,0.032,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0059,-0.00031,-0.025,0.031,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.3,1.6,0.0074,6.7,8.8,0.037,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.9e-05,3.6e-06,0.00039,4.6e-06,3.4e-06,0.0004,1,1 -38090000,0.66,-0.0019,0.017,0.75,-4.1,-3.8,0.042,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.024,0.03,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.4,1.7,0.0074,7.3,9.5,0.037,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.9e-05,3.5e-06,0.00039,4.6e-06,3.4e-06,0.0004,1,1 -38190000,0.66,-0.0019,0.017,0.75,-4.1,-3.8,0.049,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.024,0.03,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.4,1.8,0.0074,7.9,10,0.036,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.9e-05,3.5e-06,0.00039,4.6e-06,3.3e-06,0.0004,1,1 -38290000,0.66,-0.0019,0.017,0.75,-4.2,-3.9,0.057,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.024,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.5,1.9,0.0074,8.5,11,0.037,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.9e-05,3.5e-06,0.00039,4.6e-06,3.3e-06,0.0004,1,1 -38390000,0.66,-0.0019,0.017,0.75,-4.2,-3.9,0.064,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.023,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.0002,0.00027,0.0016,1.5,1.9,0.0074,9.2,12,0.036,3.1e-07,4.9e-07,3.1e-06,0.023,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.6e-06,3.2e-06,0.0004,1,1 -38490000,0.66,-0.0019,0.017,0.75,-4.3,-4,0.07,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.023,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00019,0.00026,0.0016,1.6,2,0.0074,9.9,13,0.036,3.1e-07,4.8e-07,3.1e-06,0.023,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.5e-06,3.2e-06,0.0004,1,1 -38590000,0.66,-0.0018,0.017,0.75,-4.3,-4,0.077,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00032,-0.023,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00019,0.00026,0.0016,1.7,2.1,0.0075,11,14,0.036,3.1e-07,4.8e-07,3e-06,0.023,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.5e-06,3.2e-06,0.0004,1,1 -38690000,0.66,-0.0019,0.017,0.75,-4.3,-4.1,0.083,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00033,-0.024,0.029,-0.11,-0.2,-0.044,0.46,0.0012,-0.0014,-0.027,0,0,0.00019,0.00026,0.0016,1.7,2.1,0.0075,11,15,0.036,3.1e-07,4.8e-07,3e-06,0.023,0.018,9.4e-05,3.8e-05,3.5e-06,0.00039,4.5e-06,3.1e-06,0.0004,1,1 -38790000,0.66,-0.0018,0.017,0.75,-4.4,-4.1,0.09,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00033,-0.024,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0014,-0.027,0,0,0.00019,0.00026,0.0016,1.8,2.2,0.0075,12,16,0.036,3.1e-07,4.8e-07,3e-06,0.023,0.018,9.4e-05,3.7e-05,3.5e-06,0.00039,4.5e-06,3.1e-06,0.0004,1,1 -38890000,0.66,-0.0019,0.018,0.75,-4.4,-4.2,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0058,-0.00033,-0.024,0.028,-0.11,-0.2,-0.044,0.46,0.0012,-0.0015,-0.027,0,0,0.00019,0.00026,0.0016,1.8,2.3,0.0075,13,17,0.036,3.1e-07,4.8e-07,3e-06,0.023,0.018,9.4e-05,3.7e-05,3.4e-06,0.00039,4.5e-06,3.1e-06,0.0004,1,1 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.6e-13,0,0,-5e-08,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-1.7e-11,0,0,-2.5e-06,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.0094,-0.011,7e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,2.9e-10,0,0,-1.5e-05,0,0,0,0,0,0,0,0,0.1,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.0095,-0.012,2e-05,0.0039,0.0048,-0.06,0.00024,0.00049,-0.011,1.6e-06,-3.7e-07,-4.2e-08,0,0,-4.1e-05,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.0095,-0.012,2.6e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,-4e-08,0,0,-7.3e-05,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.0096,-0.012,8.6e-05,0.0063,0.0052,-0.059,0.0005,0.00055,-0.013,5.5e-06,-3.2e-06,-1.8e-07,0,0,-0.00012,0,0,0,0,0,0,0,0,0.1,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.0097,-0.013,9.9e-05,0.0086,0.0073,-0.063,0.0012,0.0012,-0.014,5.4e-06,-3.1e-06,-1.8e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00077,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.0098,-0.013,0.00012,0.01,0.006,-0.077,0.00096,0.00072,-0.021,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.019,0.019,0.00051,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0064,-0.092,0.0022,0.0014,-0.029,1.6e-05,-1.5e-05,-6.5e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.021,0.021,0.00062,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0051,-0.11,0.0018,0.00086,-0.039,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.023,0.023,0.00043,0.93,0.93,2,0.17,0.17,0.84,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.01,-0.014,0.0001,0.02,0.0053,-0.12,0.0035,0.0014,-0.051,4.1e-05,-6.2e-05,-2.1e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00051,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0044,-0.14,0.0027,0.00089,-0.064,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.00038,0.89,0.89,2,0.15,0.15,1.4,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0042,-0.15,0.0051,0.0013,-0.078,8.5e-05,-0.00019,-5.5e-06,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00043,1.2,1.2,2,0.21,0.21,1.7,0.0095,0.0095,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0029,-0.16,0.0038,0.00083,-0.093,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00033,0.96,0.96,2,0.14,0.14,2.1,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.01,-0.014,0.00014,0.03,0.0035,-0.18,0.0065,0.0012,-0.11,0.00015,-0.00045,-1.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.03,0.03,0.00037,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.011,-0.014,0.00012,0.028,-0.00014,-0.19,0.0045,0.00062,-0.13,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.026,0.026,0.0003,1,1,2,0.14,0.14,3,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.011,-0.014,9.3e-05,0.035,-0.002,-0.2,0.0076,0.00053,-0.15,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.028,0.028,0.00033,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.011,-0.015,7.3e-05,0.043,-0.0033,-0.22,0.011,0.00027,-0.17,0.0002,-0.00088,-2.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.031,0.031,0.00037,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.0001,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,8.4e-05,0.036,-0.0048,-0.23,0.0082,-0.00029,-0.19,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.025,0.025,0.00029,1.3,1.3,2.1,0.2,0.2,4.7,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,4.5e-05,0.041,-0.0073,-0.24,0.012,-0.00089,-0.22,0.00021,-0.0014,-3.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.027,0.027,0.00032,1.7,1.7,2.1,0.31,0.31,5.3,0.0067,0.0067,7.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,5.6e-05,0.033,-0.007,-0.26,0.0079,-0.00099,-0.24,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.02,0.02,0.00027,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,4.2e-05,0.039,-0.0095,-0.27,0.011,-0.0018,-0.27,0.00017,-0.002,-4.2e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.022,0.022,0.00029,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,5.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,5.9e-05,0.03,-0.0089,-0.29,0.0074,-0.0015,-0.3,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.017,0.017,0.00024,1,1,2.1,0.19,0.19,7.4,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.013,4.1e-05,0.035,-0.011,-0.3,0.011,-0.0025,-0.32,8.5e-05,-0.0025,-5e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.018,0.018,0.00026,1.3,1.3,2.1,0.28,0.28,8.2,0.0046,0.0046,4.5e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.011,-0.013,5.5e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.014,0.014,0.00022,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.011,-0.013,5.1e-05,0.03,-0.011,-0.33,0.0097,-0.0028,-0.39,-2.2e-05,-0.0029,-5.6e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.015,0.015,0.00024,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,3.6e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.011,-0.013,4.5e-05,0.023,-0.0096,-0.34,0.0062,-0.002,-0.42,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.011,-0.013,-5.4e-06,0.027,-0.012,-0.35,0.0087,-0.003,-0.46,-0.00013,-0.0033,-6.1e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.013,0.013,0.00022,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,2.9e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.011,-0.013,4.2e-05,0.022,-0.0098,-0.36,0.0057,-0.0021,-0.49,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.0099,0.0099,0.00019,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.011,-0.013,4.4e-05,0.025,-0.011,-0.38,0.008,-0.0032,-0.53,-0.00025,-0.0036,-6.4e-05,0,0,-0.00018,0,0,0,0,0,0,0,0,0.1,0.011,0.011,0.00021,0.83,0.83,2.2,0.22,0.22,14,0.0027,0.0027,2.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.011,-0.013,-1.3e-05,0.02,-0.009,-0.39,0.0053,-0.0022,-0.57,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0087,0.0087,0.00018,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.011,-0.013,2.6e-05,0.023,-0.011,-0.4,0.0074,-0.0032,-0.61,-0.00036,-0.0039,-6.7e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0096,0.0096,0.00019,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.011,-0.012,-2.6e-06,0.018,-0.0095,-0.42,0.0049,-0.0022,-0.65,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0078,0.0078,0.00017,0.53,0.53,2.3,0.14,0.14,18,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.011,-0.013,-1.1e-05,0.022,-0.012,-0.43,0.0069,-0.0033,-0.69,-0.00047,-0.0041,-6.9e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0086,0.0086,0.00018,0.66,0.66,2.3,0.19,0.19,19,0.002,0.002,1.7e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.011,-0.012,1.2e-05,0.017,-0.011,-0.44,0.0047,-0.0023,-0.73,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.007,0.007,0.00016,0.49,0.49,2.4,0.13,0.13,20,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.011,-0.012,0.00013,0.019,-0.014,-0.46,0.0065,-0.0036,-0.78,-0.00058,-0.0044,-7.1e-05,0,0,-0.00017,0,0,0,0,0,0,0,0,0.1,0.0077,0.0077,0.00017,0.6,0.6,2.4,0.18,0.18,22,0.0017,0.0017,1.4e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.011,-0.012,0.00018,0.016,-0.014,-0.47,0.0044,-0.0027,-0.82,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0064,0.0064,0.00015,0.45,0.45,2.4,0.12,0.12,23,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.011,-0.012,0.00015,0.017,-0.015,-0.48,0.006,-0.0041,-0.87,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0069,0.0069,0.00016,0.55,0.55,2.4,0.17,0.17,24,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.011,-0.012,0.00015,0.02,-0.017,-0.5,0.0079,-0.0057,-0.92,-0.0007,-0.0046,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0075,0.0075,0.00017,0.67,0.67,2.5,0.23,0.23,26,0.0014,0.0014,1.2e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.011,-0.012,0.00014,0.017,-0.015,-0.51,0.0056,-0.0042,-0.97,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0062,0.0062,0.00015,0.51,0.51,2.5,0.16,0.16,28,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.011,-0.012,0.00012,0.02,-0.017,-0.53,0.0075,-0.0058,-1,-0.00083,-0.0047,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0067,0.0067,0.00016,0.61,0.61,2.5,0.21,0.21,29,0.0012,0.0012,1e-05,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.01,-0.012,7.2e-05,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.0056,0.0056,0.00014,0.47,0.47,2.6,0.15,0.15,31,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.01,-0.012,9.4e-05,0.018,-0.013,-0.55,0.0071,-0.0055,-1.1,-0.00095,-0.0049,-7.2e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,0.1,0.006,0.006,0.00015,0.56,0.56,2.6,0.2,0.2,33,0.00097,0.00097,9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.01,-0.012,0.00015,0.014,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0049,0.0049,0.00014,0.43,0.43,2.6,0.14,0.14,34,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.01,-0.012,0.00018,0.017,-0.012,-0.58,0.0067,-0.0049,-1.2,-0.0011,-0.005,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0053,0.0053,0.00014,0.52,0.52,2.7,0.19,0.19,36,0.0008,0.0008,7.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.01,-0.012,0.00018,0.014,-0.01,-0.6,0.0048,-0.0035,-1.3,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0044,0.0044,0.00013,0.4,0.4,2.7,0.14,0.14,38,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.01,-0.012,0.00017,0.015,-0.012,-0.61,0.0062,-0.0045,-1.4,-0.0012,-0.0052,-7.2e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,0.1,0.0047,0.0047,0.00014,0.48,0.48,2.7,0.18,0.18,40,0.00065,0.00065,6.9e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.01,-0.011,0.00016,0.012,-0.01,-0.63,0.0044,-0.0033,-1.4,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0039,0.0039,0.00012,0.37,0.37,2.8,0.13,0.13,42,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.01,-0.012,0.00014,0.015,-0.011,-0.64,0.0058,-0.0043,-1.5,-0.0012,-0.0053,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0041,0.0041,0.00013,0.44,0.44,2.8,0.17,0.17,44,0.00053,0.00053,6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.01,-0.011,0.00018,0.011,-0.0085,-0.66,0.0041,-0.0031,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0034,0.0034,0.00012,0.34,0.34,2.8,0.12,0.12,47,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.01,-0.011,0.00021,0.013,-0.0098,-0.67,0.0053,-0.004,-1.6,-0.0013,-0.0054,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,0.1,0.0036,0.0036,0.00012,0.4,0.4,2.9,0.16,0.16,49,0.00043,0.00043,5.3e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.01,-0.011,0.0002,0.0086,-0.0073,-0.68,0.0037,-0.0029,-1.7,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.003,0.003,0.00011,0.31,0.31,2.9,0.12,0.12,51,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0099,-0.011,0.00026,0.0081,-0.0081,-0.7,0.0045,-0.0036,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0032,0.0032,0.00012,0.36,0.36,3,0.16,0.16,54,0.00034,0.00034,4.7e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0098,-0.011,0.00027,0.0055,-0.0062,-0.71,0.0031,-0.0026,-1.8,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0026,0.0026,0.00011,0.28,0.28,3,0.11,0.11,56,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0097,-0.011,0.00025,0.0061,-0.0066,-0.73,0.0036,-0.0032,-1.9,-0.0014,-0.0055,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0028,0.0028,0.00011,0.33,0.33,3,0.15,0.15,59,0.00028,0.00028,4.2e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0096,-0.011,0.00032,0.0041,-0.0038,-0.74,0.0025,-0.0022,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0023,0.0023,0.00011,0.26,0.26,3.1,0.11,0.11,61,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0095,-0.011,0.00032,0.0044,-0.0028,-0.75,0.0029,-0.0025,-2,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,0.1,0.0025,0.0025,0.00011,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,3.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0095,-0.011,0.00029,0.0038,-0.00092,0.0028,0.002,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0021,0.0021,0.0001,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0094,-0.011,0.00032,0.0041,0.00056,0.015,0.0023,-0.0016,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00014,0,0,0,0,0,0,0,0,-3.6e+02,0.0022,0.0022,0.00011,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0094,-0.011,0.0003,0.0051,0.0017,-0.011,0.0028,-0.0015,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00013,0,0,0,0,0,0,0,0,-3.6e+02,0.0023,0.0023,0.00011,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,3.4e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0094,-0.011,0.00022,0.0038,0.0042,-0.005,0.002,-0.00053,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0019,0.0019,0.0001,0.25,0.25,4.9,0.13,0.13,0.32,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0094,-0.011,0.0002,0.005,0.0043,-0.012,0.0025,-0.00012,-3.7e+02,-0.0015,-0.0056,-7.3e-05,0,0,-0.00016,0,0,0,0,0,0,0,0,-3.6e+02,0.002,0.002,0.00011,0.28,0.28,3.2,0.16,0.16,0.3,0.00015,0.00015,3.1e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0093,-0.011,0.00022,0.0043,0.0053,-0.05,0.0019,0.00038,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,-3.6e+02,0.0017,0.0017,9.9e-05,0.22,0.22,2.3,0.12,0.12,0.29,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0093,-0.011,0.00022,0.0049,0.0055,-0.052,0.0023,0.00093,-3.7e+02,-0.0015,-0.0057,-7.3e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-3.6e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0093,-0.011,0.00015,0.0037,0.0055,-0.099,0.0018,0.001,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,3e-05,0,0,0,0,0,0,0,0,-3.6e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0093,-0.011,7.8e-05,0.0046,0.0051,-0.076,0.0022,0.0015,-3.7e+02,-0.0015,-0.0057,-7.4e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-3.6e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,0.78,-0.024,0.0048,-0.62,-0.22,-0.05,-0.11,-0.13,-0.029,-3.7e+02,-0.00035,-0.01,-0.0002,0,0,0.0012,-0.092,-0.021,0.51,-0.00079,-0.075,-0.061,0,0,-3.6e+02,0.0013,0.0013,0.075,0.18,0.18,0.6,0.11,0.11,0.2,7.5e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.0005,0.0014,0.0011,0.0015,0.0014,1,1,1.7 +6890000,0.78,-0.025,0.0057,-0.62,-0.27,-0.065,-0.12,-0.16,-0.04,-3.7e+02,-0.00017,-0.011,-0.00022,0,0,0.0012,-0.1,-0.023,0.51,-0.0012,-0.083,-0.067,0,0,-3.6e+02,0.0012,0.0013,0.062,0.2,0.2,0.46,0.13,0.13,0.18,7.4e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00025,0.0013,0.00089,0.0014,0.0013,1,1,1.8 +6990000,0.78,-0.025,0.0059,-0.62,-0.3,-0.076,-0.12,-0.2,-0.05,-3.7e+02,-7e-05,-0.011,-0.00022,-0.00057,-7.3e-05,0.00095,-0.1,-0.024,0.5,-0.0016,-0.084,-0.067,0,0,-3.6e+02,0.0013,0.0013,0.059,0.23,0.23,0.36,0.16,0.16,0.16,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00018,0.0013,0.00085,0.0014,0.0013,1,1,1.8 +7090000,0.78,-0.025,0.0062,-0.62,-0.33,-0.088,-0.12,-0.23,-0.063,-3.7e+02,4.4e-05,-0.011,-0.00023,-0.0013,-0.0003,0.00061,-0.1,-0.024,0.5,-0.002,-0.085,-0.068,0,0,-3.6e+02,0.0013,0.0013,0.058,0.26,0.26,0.29,0.2,0.2,0.16,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00016,0.0013,0.00083,0.0014,0.0013,1,1,1.8 +7190000,0.78,-0.025,0.0064,-0.62,-0.36,-0.097,-0.15,-0.26,-0.075,-3.7e+02,0.00011,-0.011,-0.00023,-0.0017,-0.00046,0.00083,-0.1,-0.024,0.5,-0.002,-0.085,-0.068,0,0,-3.6e+02,0.0013,0.0013,0.057,0.3,0.29,0.24,0.25,0.24,0.15,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00014,0.0013,0.00082,0.0013,0.0013,1,1,1.8 +7290000,0.78,-0.025,0.0067,-0.62,-0.38,-0.098,-0.14,-0.3,-0.082,-3.7e+02,5.3e-05,-0.011,-0.00022,-0.0016,-0.00042,0.00015,-0.1,-0.024,0.5,-0.0019,-0.085,-0.068,0,0,-3.6e+02,0.0013,0.0013,0.056,0.33,0.33,0.2,0.3,0.3,0.14,7.3e-05,7.5e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00013,0.0013,0.00081,0.0013,0.0013,1,1,1.9 +7390000,0.78,-0.024,0.0068,-0.62,-0.41,-0.11,-0.16,-0.34,-0.093,-3.7e+02,7.4e-05,-0.011,-0.00022,-0.0017,-0.00046,8.6e-07,-0.1,-0.024,0.5,-0.0018,-0.085,-0.068,0,0,-3.6e+02,0.0013,0.0014,0.056,0.37,0.37,0.18,0.36,0.36,0.13,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00012,0.0013,0.00081,0.0013,0.0013,1,1,1.9 +7490000,0.78,-0.024,0.0069,-0.62,-0.43,-0.12,-0.16,-0.38,-0.11,-3.7e+02,0.00021,-0.011,-0.00023,-0.0019,-0.00054,-0.00076,-0.1,-0.024,0.5,-0.002,-0.085,-0.069,0,0,-3.6e+02,0.0013,0.0014,0.055,0.42,0.42,0.15,0.43,0.43,0.12,7.2e-05,7.4e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00011,0.0013,0.0008,0.0013,0.0013,1,1,1.9 +7590000,0.78,-0.024,0.007,-0.62,-0.45,-0.12,-0.16,-0.41,-0.12,-3.7e+02,0.0002,-0.011,-0.00022,-0.0018,-0.00052,-0.0016,-0.1,-0.024,0.5,-0.0019,-0.085,-0.068,0,0,-3.6e+02,0.0013,0.0014,0.055,0.46,0.46,0.14,0.52,0.51,0.12,7.2e-05,7.3e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00011,0.0013,0.0008,0.0013,0.0013,1,1,1.9 +7690000,0.78,-0.024,0.007,-0.62,-0.011,-0.0016,-0.16,-0.44,-0.14,-3.7e+02,0.00029,-0.011,-0.00022,-0.0019,-0.00051,-0.0036,-0.1,-0.024,0.5,-0.002,-0.085,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,25,25,0.13,1e+02,1e+02,0.11,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0001,0.0013,0.0008,0.0013,0.0013,1,1,2 +7790000,0.78,-0.024,0.0073,-0.62,-0.038,-0.0061,-0.16,-0.44,-0.14,-3.7e+02,0.00035,-0.011,-0.00023,-0.0019,-0.00051,-0.0056,-0.1,-0.024,0.5,-0.002,-0.085,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,25,25,0.12,1e+02,1e+02,0.11,7e-05,7.2e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.0001,0.0013,0.00079,0.0013,0.0013,1,1,2 +7890000,0.78,-0.024,0.0072,-0.62,-0.063,-0.011,-0.15,-0.44,-0.14,-3.7e+02,0.00039,-0.011,-0.00022,-0.0019,-0.00051,-0.0081,-0.1,-0.024,0.5,-0.0021,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,25,25,0.11,50,50,0.1,6.9e-05,7e-05,2.4e-06,0.04,0.04,0.038,0.0013,9.8e-05,0.0013,0.00079,0.0013,0.0013,1,1,2 +7990000,0.78,-0.024,0.0072,-0.62,-0.089,-0.016,-0.16,-0.45,-0.14,-3.7e+02,0.0004,-0.011,-0.00022,-0.0019,-0.00051,-0.0094,-0.1,-0.024,0.5,-0.0021,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,25,25,0.1,51,51,0.099,6.7e-05,6.9e-05,2.4e-06,0.04,0.04,0.038,0.0013,9.5e-05,0.0013,0.00079,0.0013,0.0013,1,1,2 +8090000,0.78,-0.024,0.0071,-0.62,-0.11,-0.021,-0.17,-0.45,-0.14,-3.7e+02,0.00044,-0.01,-0.00021,-0.0019,-0.00051,-0.0096,-0.1,-0.024,0.5,-0.0022,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,24,24,0.1,35,35,0.097,6.6e-05,6.8e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.4e-05,0.0013,0.00079,0.0013,0.0013,1,1,2.1 +8190000,0.78,-0.024,0.0073,-0.62,-0.14,-0.025,-0.18,-0.47,-0.14,-3.7e+02,0.00043,-0.01,-0.00021,-0.0019,-0.00051,-0.012,-0.1,-0.024,0.5,-0.0022,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.055,25,25,0.099,36,36,0.094,6.4e-05,6.6e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.2e-05,0.0013,0.00078,0.0013,0.0013,1,1,2.1 +8290000,0.78,-0.024,0.0073,-0.62,-0.16,-0.03,-0.17,-0.47,-0.14,-3.7e+02,0.00053,-0.01,-0.00021,-0.0019,-0.00051,-0.016,-0.1,-0.024,0.5,-0.0023,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,24,24,0.097,28,28,0.091,6.2e-05,6.4e-05,2.4e-06,0.04,0.04,0.036,0.0013,9e-05,0.0013,0.00078,0.0013,0.0013,1,1,2.1 +8390000,0.78,-0.024,0.007,-0.63,-0.18,-0.036,-0.17,-0.48,-0.15,-3.7e+02,0.00055,-0.01,-0.00021,-0.0019,-0.00051,-0.019,-0.1,-0.024,0.5,-0.0024,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,24,24,0.097,29,29,0.091,6.1e-05,6.3e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.9e-05,0.0013,0.00078,0.0013,0.0013,1,1,2.1 +8490000,0.78,-0.024,0.007,-0.63,-0.2,-0.04,-0.17,-0.49,-0.15,-3.7e+02,0.00056,-0.0099,-0.0002,-0.0019,-0.00051,-0.024,-0.1,-0.024,0.5,-0.0024,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,23,23,0.096,24,24,0.089,5.9e-05,6.1e-05,2.4e-06,0.04,0.04,0.034,0.0013,8.8e-05,0.0013,0.00077,0.0013,0.0013,1,1,2.2 +8590000,0.78,-0.023,0.0072,-0.63,-0.22,-0.038,-0.17,-0.51,-0.15,-3.7e+02,0.00036,-0.0099,-0.0002,-0.0019,-0.00051,-0.028,-0.1,-0.024,0.5,-0.0022,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,23,23,0.095,26,26,0.088,5.6e-05,5.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,8.7e-05,0.0013,0.00077,0.0013,0.0013,1,1,2.2 +8690000,0.78,-0.023,0.0068,-0.63,-0.23,-0.044,-0.16,-0.53,-0.15,-3.7e+02,0.00037,-0.0096,-0.00019,-0.0019,-0.00051,-0.033,-0.1,-0.024,0.5,-0.0024,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,23,23,0.096,29,29,0.088,5.5e-05,5.7e-05,2.4e-06,0.04,0.04,0.033,0.0013,8.6e-05,0.0013,0.00077,0.0013,0.0013,1,1,2.2 +8790000,0.78,-0.023,0.0069,-0.63,-0.25,-0.047,-0.15,-0.53,-0.15,-3.7e+02,0.0004,-0.0095,-0.00019,-0.0019,-0.00051,-0.039,-0.1,-0.024,0.5,-0.0024,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,21,21,0.096,25,25,0.087,5.2e-05,5.4e-05,2.4e-06,0.04,0.04,0.032,0.0013,8.5e-05,0.0013,0.00076,0.0013,0.0013,1,1,2.2 +8890000,0.78,-0.023,0.0068,-0.63,-0.27,-0.051,-0.15,-0.55,-0.16,-3.7e+02,0.0004,-0.0094,-0.00018,-0.0019,-0.00051,-0.043,-0.1,-0.024,0.5,-0.0025,-0.086,-0.069,0,0,-3.6e+02,0.0014,0.0014,0.054,21,21,0.095,27,27,0.086,5e-05,5.2e-05,2.4e-06,0.04,0.04,0.03,0.0012,8.4e-05,0.0013,0.00076,0.0013,0.0013,1,1,2.3 +8990000,0.78,-0.023,0.0068,-0.63,-0.27,-0.053,-0.14,-0.55,-0.16,-3.7e+02,0.00045,-0.0093,-0.00018,-0.0019,-0.00051,-0.049,-0.1,-0.024,0.5,-0.0026,-0.086,-0.069,0,0,-3.6e+02,0.0013,0.0014,0.054,19,19,0.096,24,24,0.087,4.8e-05,5e-05,2.4e-06,0.04,0.04,0.029,0.0012,8.3e-05,0.0013,0.00075,0.0013,0.0013,1,1,2.3 +9090000,0.78,-0.022,0.0068,-0.63,-0.29,-0.054,-0.14,-0.58,-0.17,-3.7e+02,0.00037,-0.0092,-0.00018,-0.0019,-0.00051,-0.052,-0.1,-0.024,0.5,-0.0025,-0.086,-0.069,0,0,-3.6e+02,0.0013,0.0014,0.054,19,19,0.095,27,27,0.086,4.6e-05,4.8e-05,2.4e-06,0.04,0.04,0.028,0.0012,8.2e-05,0.0013,0.00075,0.0013,0.0013,1,1,2.3 +9190000,0.78,-0.022,0.0061,-0.63,-0.3,-0.064,-0.14,-0.6,-0.17,-3.7e+02,0.00038,-0.0088,-0.00017,-0.0018,-0.0003,-0.055,-0.11,-0.025,0.5,-0.0027,-0.087,-0.069,0,0,-3.6e+02,0.0013,0.0013,0.054,19,19,0.094,30,30,0.085,4.3e-05,4.5e-05,2.4e-06,0.04,0.04,0.027,0.0012,8.2e-05,0.0013,0.00074,0.0013,0.0013,1,1,2.3 +9290000,0.78,-0.022,0.0058,-0.63,-0.31,-0.073,-0.14,-0.62,-0.18,-3.7e+02,0.0004,-0.0086,-0.00016,-0.0018,-6.6e-05,-0.059,-0.11,-0.025,0.5,-0.0029,-0.087,-0.069,0,0,-3.6e+02,0.0013,0.0013,0.054,19,19,0.093,34,34,0.085,4.1e-05,4.3e-05,2.4e-06,0.04,0.04,0.025,0.0012,8.1e-05,0.0013,0.00074,0.0013,0.0013,1,1,2.4 +9390000,0.78,-0.022,0.0057,-0.63,-0.33,-0.082,-0.13,-0.65,-0.19,-3.7e+02,0.00043,-0.0085,-0.00016,-0.0019,0.00012,-0.063,-0.11,-0.025,0.5,-0.003,-0.087,-0.069,0,0,-3.6e+02,0.0013,0.0013,0.054,19,19,0.093,38,38,0.086,3.9e-05,4.1e-05,2.4e-06,0.04,0.04,0.024,0.0012,8e-05,0.0013,0.00074,0.0013,0.0013,1,1,2.4 +9490000,0.78,-0.022,0.0052,-0.63,-0.33,-0.095,-0.13,-0.67,-0.21,-3.7e+02,0.00046,-0.0082,-0.00015,-0.0018,0.00038,-0.067,-0.11,-0.025,0.5,-0.0032,-0.087,-0.069,0,0,-3.6e+02,0.0013,0.0013,0.054,19,19,0.091,42,42,0.085,3.7e-05,3.9e-05,2.4e-06,0.04,0.04,0.023,0.0012,8e-05,0.0013,0.00073,0.0013,0.0013,1,1,2.4 +9590000,0.78,-0.022,0.0049,-0.63,-0.34,-0.094,-0.13,-0.7,-0.22,-3.7e+02,0.00031,-0.008,-0.00014,-0.0018,0.00066,-0.07,-0.11,-0.025,0.5,-0.0031,-0.087,-0.069,0,0,-3.6e+02,0.0012,0.0013,0.054,20,20,0.09,47,47,0.085,3.5e-05,3.7e-05,2.4e-06,0.04,0.04,0.022,0.0012,7.9e-05,0.0013,0.00073,0.0013,0.0013,1,1,2.4 +9690000,0.78,-0.022,0.0051,-0.63,-0.36,-0.092,-0.12,-0.73,-0.22,-3.7e+02,0.00022,-0.008,-0.00014,-0.0021,0.0009,-0.075,-0.11,-0.025,0.5,-0.003,-0.087,-0.069,0,0,-3.6e+02,0.0012,0.0012,0.054,20,20,0.089,53,53,0.086,3.3e-05,3.5e-05,2.4e-06,0.04,0.04,0.02,0.0012,7.9e-05,0.0013,0.00072,0.0013,0.0013,1,1,2.5 +9790000,0.78,-0.022,0.0047,-0.63,-0.37,-0.1,-0.11,-0.75,-0.24,-3.7e+02,0.00023,-0.0078,-0.00014,-0.0021,0.0012,-0.081,-0.11,-0.025,0.5,-0.0031,-0.087,-0.069,0,0,-3.6e+02,0.0012,0.0012,0.054,20,20,0.087,58,58,0.085,3.1e-05,3.3e-05,2.4e-06,0.04,0.04,0.019,0.0012,7.8e-05,0.0013,0.00072,0.0013,0.0013,1,1,2.5 +9890000,0.78,-0.021,0.0045,-0.63,-0.37,-0.11,-0.11,-0.78,-0.24,-3.7e+02,0.00014,-0.0077,-0.00013,-0.0021,0.0014,-0.083,-0.11,-0.025,0.5,-0.0031,-0.087,-0.069,0,0,-3.6e+02,0.0012,0.0012,0.054,20,20,0.084,64,64,0.085,2.9e-05,3.2e-05,2.4e-06,0.04,0.04,0.018,0.0012,7.8e-05,0.0013,0.00072,0.0013,0.0013,1,1,2.5 +9990000,0.78,-0.021,0.0045,-0.63,-0.39,-0.11,-0.1,-0.81,-0.25,-3.7e+02,9.8e-05,-0.0077,-0.00013,-0.0023,0.0016,-0.087,-0.11,-0.025,0.5,-0.0031,-0.088,-0.069,0,0,-3.6e+02,0.0012,0.0012,0.054,20,20,0.083,71,71,0.086,2.8e-05,3e-05,2.4e-06,0.04,0.04,0.017,0.0012,7.8e-05,0.0013,0.00071,0.0013,0.0013,1,1,2.5 +10090000,0.78,-0.021,0.0043,-0.63,-0.39,-0.11,-0.096,-0.84,-0.26,-3.7e+02,-9.2e-06,-0.0075,-0.00012,-0.0023,0.0018,-0.09,-0.11,-0.025,0.5,-0.003,-0.088,-0.069,0,0,-3.6e+02,0.0012,0.0012,0.054,20,20,0.08,78,78,0.085,2.6e-05,2.9e-05,2.4e-06,0.04,0.04,0.016,0.0012,7.7e-05,0.0013,0.00071,0.0013,0.0013,1,1,2.6 +10190000,0.78,-0.021,0.0045,-0.63,-0.42,-0.1,-0.096,-0.88,-0.26,-3.7e+02,-4.1e-05,-0.0076,-0.00012,-0.0024,0.0019,-0.091,-0.11,-0.025,0.5,-0.0029,-0.088,-0.069,0,0,-3.6e+02,0.0011,0.0011,0.054,20,20,0.078,85,85,0.084,2.5e-05,2.7e-05,2.4e-06,0.04,0.04,0.015,0.0012,7.7e-05,0.0013,0.00071,0.0013,0.0013,1,1,2.6 +10290000,0.78,-0.021,0.0047,-0.63,-0.44,-0.1,-0.083,-0.92,-0.27,-3.7e+02,-6.8e-05,-0.0076,-0.00012,-0.0026,0.0022,-0.097,-0.11,-0.025,0.5,-0.0029,-0.088,-0.069,0,0,-3.6e+02,0.0011,0.0011,0.054,20,20,0.076,92,92,0.085,2.4e-05,2.6e-05,2.4e-06,0.04,0.04,0.014,0.0012,7.6e-05,0.0013,0.0007,0.0013,0.0013,1,1,2.6 +10390000,0.78,-0.021,0.0045,-0.63,-0.0085,-0.022,0.0097,8e-05,-0.0019,-3.7e+02,-5.8e-05,-0.0075,-0.00012,-0.0026,0.0023,-0.1,-0.11,-0.025,0.5,-0.003,-0.088,-0.069,0,0,-3.6e+02,0.0011,0.0011,0.054,0.25,0.25,0.56,0.25,0.25,0.078,2.2e-05,2.4e-05,2.3e-06,0.04,0.04,0.013,0.0012,7.6e-05,0.0013,0.0007,0.0013,0.0013,1,1,2.6 +10490000,0.78,-0.021,0.0046,-0.63,-0.028,-0.025,0.023,-0.0017,-0.0043,-3.7e+02,-8e-05,-0.0075,-0.00012,-0.0028,0.0025,-0.1,-0.11,-0.025,0.5,-0.0029,-0.088,-0.069,0,0,-3.6e+02,0.0011,0.0011,0.054,0.25,0.25,0.55,0.26,0.26,0.08,2.1e-05,2.3e-05,2.3e-06,0.04,0.04,0.012,0.0012,7.6e-05,0.0013,0.0007,0.0013,0.0013,1,1,2.7 +10590000,0.78,-0.02,0.0043,-0.63,-0.026,-0.014,0.026,0.0015,-0.00091,-3.7e+02,-0.00026,-0.0074,-0.00011,-0.0019,0.0024,-0.1,-0.11,-0.025,0.5,-0.0028,-0.088,-0.069,0,0,-3.6e+02,0.0011,0.0011,0.054,0.13,0.13,0.27,0.13,0.13,0.073,2e-05,2.2e-05,2.3e-06,0.04,0.04,0.012,0.0012,7.6e-05,0.0013,0.0007,0.0013,0.0013,1,1,2.7 +10690000,0.78,-0.02,0.0042,-0.63,-0.043,-0.015,0.03,-0.002,-0.0024,-3.7e+02,-0.00028,-0.0073,-0.00011,-0.0019,0.0025,-0.11,-0.11,-0.025,0.5,-0.0028,-0.088,-0.069,0,0,-3.6e+02,0.001,0.001,0.054,0.13,0.13,0.26,0.14,0.14,0.078,1.9e-05,2.1e-05,2.3e-06,0.04,0.04,0.011,0.0012,7.6e-05,0.0013,0.00069,0.0013,0.0013,1,1,2.7 +10790000,0.78,-0.02,0.0038,-0.63,-0.04,-0.011,0.024,0.001,-0.0011,-3.7e+02,-0.00033,-0.0072,-0.0001,-0.00015,0.0013,-0.11,-0.11,-0.025,0.5,-0.0029,-0.088,-0.069,0,0,-3.6e+02,0.001,0.001,0.054,0.091,0.091,0.17,0.09,0.09,0.072,1.7e-05,1.9e-05,2.3e-06,0.039,0.039,0.011,0.0012,7.6e-05,0.0013,0.00069,0.0013,0.0013,1,1,2.7 +10890000,0.78,-0.02,0.0037,-0.63,-0.057,-0.013,0.02,-0.0036,-0.0024,-3.7e+02,-0.00035,-0.0071,-0.0001,-9.7e-05,0.0014,-0.11,-0.11,-0.025,0.5,-0.0029,-0.088,-0.069,0,0,-3.6e+02,0.00099,0.00099,0.054,0.099,0.099,0.16,0.096,0.096,0.075,1.7e-05,1.8e-05,2.3e-06,0.039,0.039,0.011,0.0012,7.6e-05,0.0013,0.00069,0.0013,0.0013,1,1,2.8 +10990000,0.78,-0.02,0.003,-0.63,-0.048,-0.011,0.015,0.00017,-0.0012,-3.7e+02,-0.00038,-0.0069,-9.7e-05,0.0036,-0.00085,-0.11,-0.11,-0.025,0.5,-0.0031,-0.089,-0.069,0,0,-3.6e+02,0.00093,0.00094,0.053,0.078,0.078,0.12,0.098,0.098,0.071,1.5e-05,1.7e-05,2.3e-06,0.038,0.038,0.011,0.0012,7.6e-05,0.0013,0.00069,0.0013,0.0013,1,1,2.8 +11090000,0.78,-0.02,0.0026,-0.63,-0.06,-0.015,0.02,-0.0047,-0.0027,-3.7e+02,-0.00042,-0.0068,-9.2e-05,0.0037,-0.00042,-0.11,-0.11,-0.025,0.5,-0.0031,-0.089,-0.07,0,0,-3.6e+02,0.00092,0.00092,0.053,0.087,0.088,0.11,0.11,0.11,0.074,1.5e-05,1.6e-05,2.3e-06,0.038,0.038,0.011,0.0012,7.6e-05,0.0013,0.00068,0.0013,0.0013,1,1,2.8 +11190000,0.78,-0.019,0.0021,-0.63,-0.053,-0.011,0.0082,0.00075,-0.00057,-3.7e+02,-0.00052,-0.0067,-8.9e-05,0.0082,-0.0036,-0.11,-0.11,-0.025,0.5,-0.0031,-0.089,-0.07,0,0,-3.6e+02,0.00085,0.00086,0.053,0.073,0.073,0.084,0.11,0.11,0.069,1.3e-05,1.5e-05,2.3e-06,0.037,0.037,0.011,0.0012,7.6e-05,0.0013,0.00068,0.0013,0.0013,1,1,2.8 +11290000,0.78,-0.019,0.0022,-0.63,-0.069,-0.013,0.008,-0.0056,-0.0018,-3.7e+02,-0.00049,-0.0067,-9.1e-05,0.0083,-0.0038,-0.11,-0.11,-0.025,0.5,-0.0032,-0.089,-0.07,0,0,-3.6e+02,0.00084,0.00085,0.053,0.083,0.084,0.078,0.12,0.12,0.072,1.3e-05,1.4e-05,2.3e-06,0.037,0.037,0.01,0.0012,7.6e-05,0.0013,0.00068,0.0013,0.0012,1,1,2.9 +11390000,0.78,-0.019,0.0018,-0.63,-0.064,-0.011,0.0024,0.00037,-0.00044,-3.7e+02,-0.00058,-0.0067,-8.9e-05,0.012,-0.0074,-0.11,-0.11,-0.025,0.5,-0.0032,-0.089,-0.07,0,0,-3.6e+02,0.00076,0.00077,0.052,0.067,0.068,0.063,0.081,0.081,0.068,1.2e-05,1.3e-05,2.3e-06,0.035,0.035,0.01,0.0012,7.5e-05,0.0013,0.00068,0.0013,0.0012,1,1,2.9 +11490000,0.78,-0.019,0.002,-0.63,-0.078,-0.012,0.0032,-0.0071,-0.0014,-3.7e+02,-0.00056,-0.0068,-9.2e-05,0.012,-0.0079,-0.11,-0.11,-0.025,0.5,-0.0032,-0.089,-0.07,0,0,-3.6e+02,0.00076,0.00076,0.052,0.078,0.079,0.058,0.088,0.088,0.069,1.1e-05,1.2e-05,2.3e-06,0.035,0.035,0.01,0.0012,7.5e-05,0.0013,0.00067,0.0013,0.0012,1,1,2.9 +11590000,0.78,-0.019,0.0015,-0.63,-0.069,-0.012,-0.0027,-0.0021,-0.00084,-3.7e+02,-0.00061,-0.0067,-9.1e-05,0.017,-0.012,-0.11,-0.11,-0.025,0.5,-0.0033,-0.089,-0.07,0,0,-3.6e+02,0.00068,0.00069,0.052,0.066,0.066,0.049,0.068,0.068,0.066,1e-05,1.1e-05,2.3e-06,0.033,0.033,0.01,0.0012,7.5e-05,0.0013,0.00067,0.0013,0.0012,1,1,2.9 +11690000,0.78,-0.019,0.0016,-0.63,-0.08,-0.016,-0.0071,-0.0097,-0.0025,-3.7e+02,-0.00057,-0.0067,-9.2e-05,0.017,-0.012,-0.11,-0.11,-0.025,0.5,-0.0034,-0.089,-0.07,0,0,-3.6e+02,0.00067,0.00068,0.052,0.076,0.077,0.046,0.074,0.074,0.066,9.9e-06,1.1e-05,2.3e-06,0.033,0.033,0.01,0.0012,7.5e-05,0.0013,0.00067,0.0013,0.0012,1,1,3 +11790000,0.78,-0.019,0.001,-0.63,-0.072,-0.011,-0.0089,-0.0061,-0.00025,-3.7e+02,-0.00061,-0.0066,-9.1e-05,0.023,-0.015,-0.11,-0.11,-0.025,0.5,-0.0036,-0.09,-0.07,0,0,-3.6e+02,0.0006,0.00061,0.051,0.064,0.065,0.039,0.06,0.06,0.063,9.1e-06,9.9e-06,2.3e-06,0.03,0.03,0.01,0.0012,7.5e-05,0.0013,0.00067,0.0013,0.0012,1,1,3 +11890000,0.78,-0.019,0.0011,-0.63,-0.083,-0.012,-0.0098,-0.014,-0.0014,-3.7e+02,-0.00061,-0.0066,-9.1e-05,0.023,-0.016,-0.11,-0.11,-0.025,0.5,-0.0036,-0.09,-0.07,0,0,-3.6e+02,0.00059,0.00061,0.051,0.075,0.075,0.037,0.066,0.066,0.063,8.7e-06,9.5e-06,2.3e-06,0.03,0.03,0.01,0.0012,7.5e-05,0.0013,0.00067,0.0013,0.0012,1,1,3 +11990000,0.78,-0.019,0.00053,-0.63,-0.071,-0.0068,-0.015,-0.009,0.00086,-3.7e+02,-0.00076,-0.0065,-8.7e-05,0.027,-0.018,-0.11,-0.11,-0.025,0.5,-0.0035,-0.09,-0.07,0,0,-3.6e+02,0.00052,0.00054,0.051,0.063,0.064,0.033,0.055,0.055,0.061,8e-06,8.8e-06,2.3e-06,0.028,0.028,0.01,0.0012,7.5e-05,0.0013,0.00066,0.0013,0.0012,1,1,3 +12090000,0.78,-0.018,0.00037,-0.63,-0.078,-0.0091,-0.021,-0.016,-3.2e-05,-3.7e+02,-0.00081,-0.0065,-8.3e-05,0.026,-0.017,-0.11,-0.11,-0.025,0.5,-0.0034,-0.09,-0.07,0,0,-3.6e+02,0.00052,0.00054,0.051,0.073,0.074,0.031,0.061,0.061,0.061,7.7e-06,8.4e-06,2.3e-06,0.028,0.028,0.01,0.0012,7.5e-05,0.0013,0.00066,0.0013,0.0012,1,1,3.1 +12190000,0.78,-0.018,-0.00031,-0.62,-0.064,-0.011,-0.016,-0.0085,-0.0004,-3.7e+02,-0.00082,-0.0064,-8.3e-05,0.032,-0.022,-0.11,-0.11,-0.025,0.5,-0.0037,-0.091,-0.07,0,0,-3.6e+02,0.00046,0.00048,0.051,0.062,0.062,0.028,0.052,0.052,0.059,7.2e-06,7.8e-06,2.3e-06,0.026,0.026,0.01,0.0012,7.5e-05,0.0012,0.00066,0.0013,0.0012,1,1,3.1 +12290000,0.78,-0.019,-0.00032,-0.62,-0.07,-0.014,-0.015,-0.015,-0.0021,-3.7e+02,-0.00079,-0.0064,-8.3e-05,0.033,-0.021,-0.11,-0.11,-0.025,0.5,-0.0037,-0.091,-0.07,0,0,-3.6e+02,0.00046,0.00047,0.051,0.07,0.071,0.028,0.058,0.058,0.059,6.9e-06,7.5e-06,2.3e-06,0.026,0.026,0.01,0.0012,7.5e-05,0.0012,0.00066,0.0013,0.0012,1,1,3.1 +12390000,0.78,-0.018,-0.00073,-0.62,-0.057,-0.012,-0.013,-0.0083,-0.001,-3.7e+02,-0.00086,-0.0063,-8.3e-05,0.037,-0.026,-0.11,-0.11,-0.025,0.5,-0.0038,-0.091,-0.07,0,0,-3.6e+02,0.0004,0.00042,0.05,0.059,0.06,0.026,0.05,0.05,0.057,6.4e-06,7e-06,2.3e-06,0.024,0.024,0.01,0.0012,7.5e-05,0.0012,0.00066,0.0013,0.0012,1,1,3.1 +12490000,0.78,-0.018,-0.00059,-0.62,-0.064,-0.013,-0.016,-0.015,-0.0022,-3.7e+02,-0.00083,-0.0064,-8.5e-05,0.038,-0.026,-0.11,-0.11,-0.025,0.5,-0.0038,-0.091,-0.07,0,0,-3.6e+02,0.0004,0.00042,0.05,0.067,0.068,0.026,0.056,0.057,0.057,6.2e-06,6.8e-06,2.3e-06,0.024,0.024,0.01,0.0012,7.5e-05,0.0012,0.00066,0.0013,0.0012,1,1,3.2 +12590000,0.78,-0.018,-0.00079,-0.62,-0.06,-0.011,-0.022,-0.013,-0.001,-3.7e+02,-0.00092,-0.0063,-8.3e-05,0.039,-0.027,-0.11,-0.11,-0.025,0.5,-0.0037,-0.091,-0.07,0,0,-3.6e+02,0.00036,0.00038,0.05,0.057,0.057,0.025,0.048,0.048,0.055,5.8e-06,6.4e-06,2.3e-06,0.022,0.023,0.0099,0.0012,7.4e-05,0.0012,0.00065,0.0013,0.0012,1,1,3.2 +12690000,0.78,-0.018,-0.00072,-0.62,-0.065,-0.011,-0.025,-0.019,-0.0013,-3.7e+02,-0.00099,-0.0064,-8.2e-05,0.036,-0.027,-0.11,-0.11,-0.025,0.5,-0.0035,-0.091,-0.07,0,0,-3.6e+02,0.00036,0.00037,0.05,0.064,0.064,0.025,0.055,0.055,0.055,5.6e-06,6.1e-06,2.3e-06,0.022,0.023,0.0099,0.0012,7.4e-05,0.0012,0.00065,0.0013,0.0012,1,1,3.2 +12790000,0.78,-0.018,-0.001,-0.62,-0.061,-0.0098,-0.029,-0.017,-0.0012,-3.7e+02,-0.00097,-0.0063,-8.2e-05,0.04,-0.029,-0.11,-0.11,-0.025,0.5,-0.0036,-0.091,-0.07,0,0,-3.6e+02,0.00032,0.00034,0.05,0.054,0.054,0.024,0.048,0.048,0.053,5.3e-06,5.8e-06,2.3e-06,0.021,0.021,0.0097,0.0012,7.4e-05,0.0012,0.00065,0.0013,0.0012,1,1,3.2 +12890000,0.78,-0.018,-0.00099,-0.62,-0.068,-0.011,-0.028,-0.024,-0.0027,-3.7e+02,-0.00093,-0.0063,-8.2e-05,0.041,-0.029,-0.11,-0.11,-0.025,0.5,-0.0038,-0.091,-0.07,0,0,-3.6e+02,0.00032,0.00034,0.05,0.06,0.061,0.025,0.055,0.055,0.054,5.1e-06,5.6e-06,2.3e-06,0.02,0.021,0.0097,0.0012,7.4e-05,0.0012,0.00065,0.0013,0.0012,1,1,3.3 +12990000,0.78,-0.018,-0.0015,-0.62,-0.055,-0.01,-0.028,-0.018,-0.0026,-3.7e+02,-0.00097,-0.0062,-8e-05,0.045,-0.031,-0.11,-0.11,-0.025,0.5,-0.0038,-0.091,-0.07,0,0,-3.6e+02,0.00029,0.00031,0.05,0.054,0.054,0.025,0.057,0.057,0.052,4.9e-06,5.3e-06,2.3e-06,0.019,0.02,0.0094,0.0012,7.4e-05,0.0012,0.00065,0.0013,0.0012,1,1,3.3 +13090000,0.78,-0.018,-0.0014,-0.62,-0.061,-0.01,-0.028,-0.024,-0.0034,-3.7e+02,-0.00095,-0.0063,-8.3e-05,0.045,-0.032,-0.11,-0.11,-0.025,0.5,-0.0039,-0.091,-0.07,0,0,-3.6e+02,0.00029,0.00031,0.05,0.061,0.061,0.025,0.065,0.065,0.052,4.7e-06,5.2e-06,2.3e-06,0.019,0.02,0.0094,0.0012,7.4e-05,0.0012,0.00065,0.0012,0.0012,1,1,3.3 +13190000,0.78,-0.018,-0.0017,-0.62,-0.049,-0.0096,-0.025,-0.017,-0.0028,-3.7e+02,-0.001,-0.0063,-8.3e-05,0.047,-0.035,-0.11,-0.11,-0.025,0.5,-0.0039,-0.091,-0.07,0,0,-3.6e+02,0.00027,0.00029,0.05,0.054,0.054,0.025,0.066,0.066,0.051,4.5e-06,4.9e-06,2.3e-06,0.018,0.019,0.0091,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.3 +13290000,0.78,-0.018,-0.0018,-0.62,-0.053,-0.013,-0.022,-0.022,-0.0049,-3.7e+02,-0.00095,-0.0062,-8.2e-05,0.05,-0.034,-0.12,-0.11,-0.025,0.5,-0.004,-0.091,-0.07,0,0,-3.6e+02,0.00027,0.00028,0.05,0.06,0.06,0.027,0.075,0.075,0.051,4.4e-06,4.8e-06,2.3e-06,0.018,0.019,0.0091,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.4 +13390000,0.78,-0.018,-0.002,-0.62,-0.044,-0.012,-0.018,-0.016,-0.004,-3.7e+02,-0.00099,-0.0062,-8e-05,0.051,-0.035,-0.12,-0.11,-0.025,0.5,-0.004,-0.091,-0.07,0,0,-3.6e+02,0.00025,0.00027,0.05,0.053,0.053,0.026,0.076,0.076,0.05,4.2e-06,4.6e-06,2.3e-06,0.017,0.018,0.0088,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.4 +13490000,0.78,-0.018,-0.0021,-0.62,-0.046,-0.013,-0.016,-0.021,-0.0057,-3.7e+02,-0.00097,-0.0062,-7.9e-05,0.052,-0.034,-0.12,-0.11,-0.025,0.5,-0.0041,-0.091,-0.07,0,0,-3.6e+02,0.00025,0.00026,0.05,0.059,0.059,0.028,0.086,0.086,0.05,4e-06,4.4e-06,2.3e-06,0.017,0.018,0.0087,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.4 +13590000,0.78,-0.018,-0.0022,-0.62,-0.038,-0.012,-0.019,-0.014,-0.0042,-3.7e+02,-0.001,-0.0062,-8e-05,0.053,-0.036,-0.12,-0.11,-0.025,0.5,-0.0041,-0.091,-0.07,0,0,-3.6e+02,0.00023,0.00025,0.049,0.052,0.052,0.028,0.086,0.086,0.05,3.9e-06,4.3e-06,2.3e-06,0.016,0.017,0.0084,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.4 +13690000,0.78,-0.018,-0.0023,-0.62,-0.041,-0.015,-0.023,-0.018,-0.006,-3.7e+02,-0.001,-0.0061,-7.9e-05,0.054,-0.035,-0.12,-0.11,-0.025,0.5,-0.0041,-0.091,-0.07,0,0,-3.6e+02,0.00023,0.00025,0.049,0.057,0.057,0.029,0.096,0.096,0.05,3.7e-06,4.1e-06,2.3e-06,0.016,0.017,0.0083,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.5 +13790000,0.78,-0.018,-0.0024,-0.62,-0.029,-0.013,-0.024,-0.0067,-0.0046,-3.7e+02,-0.001,-0.0061,-7.9e-05,0.054,-0.037,-0.12,-0.11,-0.025,0.5,-0.004,-0.092,-0.07,0,0,-3.6e+02,0.00021,0.00023,0.049,0.044,0.044,0.029,0.071,0.071,0.049,3.6e-06,4e-06,2.3e-06,0.015,0.016,0.0079,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.5 +13890000,0.78,-0.018,-0.0025,-0.62,-0.033,-0.015,-0.029,-0.01,-0.0066,-3.7e+02,-0.001,-0.0061,-7.9e-05,0.056,-0.037,-0.12,-0.11,-0.025,0.5,-0.0041,-0.092,-0.07,0,0,-3.6e+02,0.00021,0.00023,0.049,0.048,0.048,0.03,0.079,0.079,0.05,3.5e-06,3.9e-06,2.3e-06,0.015,0.016,0.0078,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.5 +13990000,0.78,-0.018,-0.0026,-0.62,-0.025,-0.014,-0.028,-0.0036,-0.0056,-3.7e+02,-0.001,-0.0061,-7.9e-05,0.057,-0.038,-0.12,-0.11,-0.025,0.5,-0.0041,-0.092,-0.07,0,0,-3.6e+02,0.0002,0.00021,0.049,0.039,0.039,0.03,0.062,0.062,0.05,3.3e-06,3.7e-06,2.3e-06,0.014,0.015,0.0074,0.0012,7.4e-05,0.0012,0.00064,0.0012,0.0012,1,1,3.5 +14090000,0.78,-0.018,-0.0027,-0.62,-0.026,-0.015,-0.029,-0.0058,-0.0071,-3.7e+02,-0.0011,-0.0061,-7.6e-05,0.056,-0.036,-0.12,-0.11,-0.025,0.5,-0.004,-0.092,-0.07,0,0,-3.6e+02,0.0002,0.00021,0.049,0.042,0.042,0.031,0.07,0.07,0.05,3.2e-06,3.6e-06,2.4e-06,0.014,0.015,0.0073,0.0012,7.4e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.6 +14190000,0.78,-0.017,-0.0028,-0.62,-0.021,-0.013,-0.031,-0.00023,-0.005,-3.7e+02,-0.0011,-0.0061,-7.5e-05,0.058,-0.036,-0.12,-0.11,-0.026,0.5,-0.004,-0.092,-0.069,0,0,-3.6e+02,0.00019,0.0002,0.049,0.036,0.036,0.03,0.057,0.057,0.05,3.1e-06,3.5e-06,2.4e-06,0.014,0.015,0.0069,0.0012,7.4e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.6 +14290000,0.78,-0.017,-0.0029,-0.62,-0.022,-0.015,-0.03,-0.0022,-0.0063,-3.7e+02,-0.0011,-0.006,-7.4e-05,0.057,-0.036,-0.12,-0.11,-0.026,0.5,-0.004,-0.092,-0.069,0,0,-3.6e+02,0.00019,0.0002,0.049,0.039,0.039,0.032,0.063,0.063,0.051,3e-06,3.4e-06,2.4e-06,0.013,0.014,0.0067,0.0012,7.4e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.6 +14390000,0.78,-0.017,-0.003,-0.62,-0.017,-0.015,-0.032,0.0018,-0.0049,-3.7e+02,-0.0011,-0.006,-7.3e-05,0.06,-0.035,-0.12,-0.11,-0.026,0.5,-0.004,-0.092,-0.069,0,0,-3.6e+02,0.00018,0.00019,0.049,0.033,0.033,0.031,0.053,0.053,0.05,2.9e-06,3.3e-06,2.4e-06,0.013,0.014,0.0063,0.0012,7.4e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.6 +14490000,0.78,-0.017,-0.0031,-0.62,-0.019,-0.018,-0.035,-0.00044,-0.0069,-3.7e+02,-0.001,-0.006,-7.4e-05,0.062,-0.036,-0.12,-0.11,-0.026,0.5,-0.0041,-0.092,-0.069,0,0,-3.6e+02,0.00018,0.00019,0.049,0.036,0.036,0.032,0.059,0.059,0.051,2.8e-06,3.2e-06,2.4e-06,0.013,0.014,0.0062,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.7 +14590000,0.78,-0.017,-0.003,-0.62,-0.02,-0.018,-0.035,-0.0013,-0.0065,-3.7e+02,-0.001,-0.006,-7.4e-05,0.063,-0.036,-0.12,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00017,0.00019,0.049,0.031,0.031,0.031,0.05,0.05,0.051,2.8e-06,3.1e-06,2.4e-06,0.012,0.013,0.0058,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.7 +14690000,0.78,-0.017,-0.0031,-0.62,-0.023,-0.017,-0.032,-0.0034,-0.0085,-3.7e+02,-0.00099,-0.006,-7.3e-05,0.064,-0.035,-0.12,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00017,0.00019,0.049,0.034,0.034,0.032,0.056,0.056,0.051,2.7e-06,3e-06,2.4e-06,0.012,0.013,0.0056,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.7 +14790000,0.78,-0.017,-0.0031,-0.62,-0.023,-0.016,-0.028,-0.0036,-0.0079,-3.7e+02,-0.00099,-0.006,-7.3e-05,0.065,-0.035,-0.12,-0.11,-0.026,0.5,-0.0042,-0.091,-0.069,0,0,-3.6e+02,0.00016,0.00018,0.049,0.03,0.03,0.031,0.048,0.048,0.051,2.6e-06,2.9e-06,2.4e-06,0.012,0.013,0.0053,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.7 +14890000,0.78,-0.017,-0.0031,-0.62,-0.026,-0.019,-0.031,-0.006,-0.0098,-3.7e+02,-0.00098,-0.006,-7.3e-05,0.065,-0.035,-0.12,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00016,0.00018,0.049,0.032,0.033,0.031,0.054,0.054,0.052,2.5e-06,2.9e-06,2.4e-06,0.012,0.013,0.0051,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.8 +14990000,0.78,-0.017,-0.0031,-0.62,-0.024,-0.016,-0.027,-0.0046,-0.0077,-3.7e+02,-0.00098,-0.006,-7.3e-05,0.066,-0.036,-0.12,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00016,0.00017,0.049,0.029,0.029,0.03,0.047,0.047,0.051,2.4e-06,2.8e-06,2.4e-06,0.012,0.012,0.0048,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.8 +15090000,0.78,-0.017,-0.003,-0.62,-0.026,-0.017,-0.03,-0.0072,-0.0093,-3.7e+02,-0.00098,-0.006,-7.4e-05,0.066,-0.036,-0.12,-0.11,-0.026,0.5,-0.0042,-0.091,-0.069,0,0,-3.6e+02,0.00016,0.00017,0.049,0.031,0.031,0.031,0.052,0.052,0.052,2.4e-06,2.7e-06,2.4e-06,0.011,0.012,0.0046,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.8 +15190000,0.78,-0.017,-0.003,-0.62,-0.024,-0.016,-0.027,-0.0058,-0.0075,-3.7e+02,-0.00097,-0.006,-7.4e-05,0.067,-0.037,-0.12,-0.11,-0.026,0.5,-0.0043,-0.091,-0.069,0,0,-3.6e+02,0.00015,0.00017,0.049,0.027,0.027,0.03,0.046,0.046,0.052,2.3e-06,2.6e-06,2.4e-06,0.011,0.012,0.0043,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.8 +15290000,0.78,-0.017,-0.003,-0.62,-0.025,-0.018,-0.025,-0.0081,-0.0092,-3.7e+02,-0.00098,-0.006,-7.3e-05,0.067,-0.036,-0.12,-0.11,-0.026,0.5,-0.0042,-0.091,-0.069,0,0,-3.6e+02,0.00015,0.00017,0.049,0.03,0.03,0.03,0.051,0.051,0.052,2.2e-06,2.6e-06,2.4e-06,0.011,0.012,0.0041,0.0012,7.3e-05,0.0012,0.00063,0.0012,0.0012,1,1,3.9 +15390000,0.78,-0.017,-0.0031,-0.62,-0.025,-0.018,-0.023,-0.0075,-0.0094,-3.7e+02,-0.001,-0.006,-7e-05,0.067,-0.035,-0.13,-0.11,-0.026,0.5,-0.0041,-0.091,-0.069,0,0,-3.6e+02,0.00015,0.00016,0.049,0.028,0.029,0.029,0.053,0.053,0.051,2.2e-06,2.5e-06,2.4e-06,0.011,0.012,0.0038,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,3.9 +15490000,0.78,-0.017,-0.003,-0.62,-0.027,-0.018,-0.023,-0.01,-0.011,-3.7e+02,-0.001,-0.006,-7.2e-05,0.067,-0.036,-0.13,-0.11,-0.026,0.5,-0.0042,-0.091,-0.069,0,0,-3.6e+02,0.00015,0.00016,0.049,0.031,0.031,0.029,0.06,0.06,0.053,2.1e-06,2.4e-06,2.4e-06,0.011,0.012,0.0037,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,3.9 +15590000,0.78,-0.017,-0.0029,-0.62,-0.026,-0.016,-0.022,-0.0097,-0.01,-3.7e+02,-0.001,-0.006,-7.2e-05,0.067,-0.037,-0.13,-0.11,-0.026,0.5,-0.0042,-0.091,-0.069,0,0,-3.6e+02,0.00015,0.00016,0.049,0.029,0.029,0.028,0.062,0.062,0.052,2.1e-06,2.4e-06,2.4e-06,0.011,0.011,0.0035,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,3.9 +15690000,0.78,-0.017,-0.0029,-0.62,-0.027,-0.017,-0.022,-0.012,-0.012,-3.7e+02,-0.001,-0.006,-7.2e-05,0.066,-0.037,-0.13,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00014,0.00016,0.049,0.031,0.032,0.028,0.069,0.069,0.052,2e-06,2.3e-06,2.4e-06,0.01,0.011,0.0033,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4 +15790000,0.78,-0.017,-0.003,-0.62,-0.025,-0.016,-0.025,-0.0085,-0.0098,-3.7e+02,-0.001,-0.006,-7.2e-05,0.067,-0.038,-0.12,-0.11,-0.026,0.5,-0.0042,-0.092,-0.069,0,0,-3.6e+02,0.00014,0.00016,0.049,0.026,0.027,0.027,0.056,0.056,0.051,1.9e-06,2.2e-06,2.4e-06,0.01,0.011,0.0031,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4 +15890000,0.78,-0.017,-0.0029,-0.62,-0.026,-0.016,-0.023,-0.011,-0.011,-3.7e+02,-0.001,-0.006,-7.2e-05,0.066,-0.037,-0.13,-0.11,-0.026,0.5,-0.0041,-0.092,-0.069,0,0,-3.6e+02,0.00014,0.00015,0.049,0.028,0.028,0.027,0.062,0.062,0.052,1.9e-06,2.2e-06,2.4e-06,0.01,0.011,0.003,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4 +15990000,0.78,-0.017,-0.003,-0.62,-0.024,-0.016,-0.018,-0.0078,-0.01,-3.7e+02,-0.0011,-0.006,-7e-05,0.066,-0.037,-0.13,-0.11,-0.026,0.5,-0.0041,-0.092,-0.069,0,0,-3.6e+02,0.00014,0.00015,0.049,0.024,0.024,0.026,0.052,0.052,0.051,1.8e-06,2.1e-06,2.4e-06,0.0099,0.011,0.0028,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4 +16090000,0.78,-0.017,-0.0031,-0.62,-0.026,-0.018,-0.015,-0.0099,-0.012,-3.7e+02,-0.0011,-0.006,-6.7e-05,0.066,-0.035,-0.13,-0.11,-0.026,0.5,-0.004,-0.092,-0.069,0,0,-3.6e+02,0.00014,0.00015,0.049,0.026,0.026,0.025,0.058,0.058,0.052,1.8e-06,2.1e-06,2.4e-06,0.0097,0.011,0.0027,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.1 +16190000,0.78,-0.017,-0.0031,-0.62,-0.024,-0.016,-0.014,-0.0073,-0.0094,-3.7e+02,-0.0011,-0.006,-6.6e-05,0.066,-0.035,-0.13,-0.11,-0.026,0.5,-0.004,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00015,0.049,0.023,0.023,0.025,0.049,0.05,0.051,1.7e-06,2e-06,2.4e-06,0.0096,0.011,0.0025,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.1 +16290000,0.78,-0.017,-0.0032,-0.62,-0.026,-0.018,-0.015,-0.0098,-0.012,-3.7e+02,-0.0011,-0.0059,-6.4e-05,0.066,-0.034,-0.13,-0.11,-0.026,0.5,-0.0039,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00015,0.049,0.024,0.025,0.024,0.055,0.055,0.052,1.7e-06,2e-06,2.4e-06,0.0095,0.01,0.0024,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.1 +16390000,0.78,-0.017,-0.0031,-0.62,-0.023,-0.015,-0.014,-0.0073,-0.009,-3.7e+02,-0.0011,-0.006,-6.3e-05,0.065,-0.034,-0.13,-0.11,-0.026,0.5,-0.0039,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.022,0.022,0.023,0.047,0.047,0.051,1.6e-06,1.9e-06,2.4e-06,0.0093,0.01,0.0022,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.1 +16490000,0.78,-0.017,-0.0031,-0.62,-0.022,-0.016,-0.017,-0.0093,-0.01,-3.7e+02,-0.0011,-0.006,-6.3e-05,0.064,-0.034,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.023,0.023,0.023,0.052,0.052,0.052,1.6e-06,1.9e-06,2.4e-06,0.0092,0.01,0.0021,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.2 +16590000,0.78,-0.017,-0.0031,-0.62,-0.022,-0.012,-0.018,-0.0097,-0.0065,-3.7e+02,-0.0011,-0.006,-6e-05,0.064,-0.034,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.021,0.021,0.022,0.046,0.046,0.051,1.6e-06,1.8e-06,2.4e-06,0.0091,0.01,0.002,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.2 +16690000,0.78,-0.017,-0.0031,-0.62,-0.024,-0.013,-0.014,-0.012,-0.0075,-3.7e+02,-0.0011,-0.006,-6.1e-05,0.064,-0.035,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.022,0.022,0.022,0.05,0.05,0.051,1.5e-06,1.8e-06,2.4e-06,0.009,0.0099,0.0019,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.2 +16790000,0.78,-0.017,-0.003,-0.62,-0.023,-0.0094,-0.013,-0.012,-0.0042,-3.7e+02,-0.0011,-0.006,-5.9e-05,0.065,-0.035,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.02,0.02,0.021,0.044,0.044,0.05,1.5e-06,1.8e-06,2.4e-06,0.0089,0.0098,0.0018,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.2 +16890000,0.78,-0.017,-0.003,-0.62,-0.024,-0.01,-0.01,-0.014,-0.005,-3.7e+02,-0.0011,-0.006,-6e-05,0.064,-0.035,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00013,0.00014,0.049,0.021,0.021,0.021,0.049,0.049,0.051,1.5e-06,1.7e-06,2.4e-06,0.0088,0.0097,0.0017,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.3 +16990000,0.78,-0.017,-0.0029,-0.62,-0.023,-0.01,-0.0099,-0.013,-0.0049,-3.7e+02,-0.0011,-0.006,-6.1e-05,0.064,-0.036,-0.13,-0.11,-0.026,0.5,-0.0039,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00014,0.049,0.019,0.019,0.02,0.043,0.043,0.05,1.4e-06,1.7e-06,2.4e-06,0.0087,0.0095,0.0016,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.3 +17090000,0.78,-0.017,-0.0029,-0.62,-0.024,-0.012,-0.0098,-0.015,-0.0059,-3.7e+02,-0.0012,-0.006,-6e-05,0.063,-0.035,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00014,0.049,0.02,0.021,0.02,0.048,0.048,0.05,1.4e-06,1.7e-06,2.4e-06,0.0086,0.0094,0.0015,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.3 +17190000,0.78,-0.017,-0.003,-0.62,-0.023,-0.014,-0.011,-0.013,-0.0062,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0037,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.018,0.019,0.019,0.042,0.042,0.049,1.3e-06,1.6e-06,2.4e-06,0.0085,0.0093,0.0015,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.3 +17290000,0.78,-0.017,-0.0029,-0.62,-0.026,-0.016,-0.0061,-0.016,-0.0073,-3.7e+02,-0.0012,-0.006,-6e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0038,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.02,0.02,0.019,0.047,0.047,0.049,1.3e-06,1.6e-06,2.4e-06,0.0084,0.0093,0.0014,0.0012,7.3e-05,0.0012,0.00062,0.0012,0.0012,1,1,4.4 +17390000,0.78,-0.017,-0.003,-0.62,-0.023,-0.017,-0.0042,-0.013,-0.0075,-3.7e+02,-0.0012,-0.006,-5.8e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0037,-0.091,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.018,0.018,0.018,0.042,0.042,0.048,1.3e-06,1.5e-06,2.4e-06,0.0083,0.0091,0.0013,0.0012,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.4 +17490000,0.78,-0.017,-0.003,-0.62,-0.025,-0.018,-0.0025,-0.016,-0.0094,-3.7e+02,-0.0012,-0.006,-5.9e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0037,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.019,0.019,0.018,0.046,0.046,0.049,1.3e-06,1.5e-06,2.4e-06,0.0082,0.0091,0.0013,0.0012,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.4 +17590000,0.78,-0.017,-0.003,-0.62,-0.024,-0.018,0.003,-0.013,-0.009,-3.7e+02,-0.0012,-0.006,-5.8e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0037,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.017,0.018,0.017,0.041,0.041,0.048,1.2e-06,1.5e-06,2.4e-06,0.0081,0.009,0.0012,0.0012,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.4 +17690000,0.78,-0.017,-0.0029,-0.62,-0.025,-0.02,0.0024,-0.016,-0.011,-3.7e+02,-0.0012,-0.006,-5.7e-05,0.062,-0.035,-0.13,-0.11,-0.026,0.5,-0.0037,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.019,0.019,0.017,0.045,0.045,0.048,1.2e-06,1.4e-06,2.4e-06,0.008,0.0089,0.0011,0.0011,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.5 +17790000,0.78,-0.017,-0.003,-0.62,-0.023,-0.021,0.001,-0.014,-0.012,-3.7e+02,-0.0012,-0.006,-5.3e-05,0.062,-0.033,-0.13,-0.11,-0.026,0.5,-0.0035,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.018,0.019,0.016,0.048,0.048,0.048,1.2e-06,1.4e-06,2.4e-06,0.008,0.0088,0.0011,0.0011,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.5 +17890000,0.78,-0.017,-0.0031,-0.62,-0.026,-0.022,0.0011,-0.017,-0.015,-3.7e+02,-0.0012,-0.006,-5.2e-05,0.062,-0.033,-0.13,-0.11,-0.026,0.5,-0.0035,-0.092,-0.069,0,0,-3.6e+02,0.00012,0.00013,0.049,0.02,0.02,0.016,0.052,0.053,0.048,1.2e-06,1.4e-06,2.4e-06,0.0079,0.0087,0.001,0.0011,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.5 +17990000,0.78,-0.017,-0.003,-0.62,-0.025,-0.02,0.0023,-0.015,-0.014,-3.7e+02,-0.0012,-0.006,-5.1e-05,0.062,-0.033,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.019,0.02,0.016,0.055,0.055,0.047,1.1e-06,1.4e-06,2.4e-06,0.0078,0.0086,0.00099,0.0011,7.3e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.5 +18090000,0.78,-0.017,-0.003,-0.62,-0.027,-0.02,0.0047,-0.018,-0.016,-3.7e+02,-0.0012,-0.006,-5.4e-05,0.062,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.021,0.021,0.016,0.06,0.061,0.047,1.1e-06,1.3e-06,2.4e-06,0.0078,0.0086,0.00096,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.6 +18190000,0.78,-0.017,-0.003,-0.62,-0.023,-0.019,0.0061,-0.013,-0.013,-3.7e+02,-0.0012,-0.006,-5e-05,0.062,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.018,0.018,0.015,0.051,0.051,0.047,1.1e-06,1.3e-06,2.3e-06,0.0077,0.0085,0.0009,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.6 +18290000,0.78,-0.017,-0.0029,-0.62,-0.025,-0.02,0.0072,-0.016,-0.015,-3.7e+02,-0.0012,-0.006,-5.2e-05,0.062,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.019,0.019,0.015,0.056,0.056,0.046,1.1e-06,1.3e-06,2.4e-06,0.0076,0.0084,0.00087,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.6 +18390000,0.78,-0.017,-0.003,-0.62,-0.023,-0.021,0.0084,-0.012,-0.012,-3.7e+02,-0.0012,-0.006,-4.8e-05,0.063,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.017,0.017,0.014,0.048,0.048,0.046,1e-06,1.2e-06,2.3e-06,0.0075,0.0083,0.00083,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.6 +18490000,0.78,-0.017,-0.003,-0.62,-0.024,-0.022,0.0081,-0.014,-0.015,-3.7e+02,-0.0012,-0.006,-4.7e-05,0.062,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.018,0.018,0.014,0.053,0.053,0.046,1e-06,1.2e-06,2.3e-06,0.0075,0.0083,0.0008,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.7 +18590000,0.78,-0.016,-0.003,-0.62,-0.022,-0.022,0.0062,-0.011,-0.013,-3.7e+02,-0.0013,-0.006,-4.2e-05,0.062,-0.033,-0.13,-0.11,-0.026,0.5,-0.0035,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.016,0.016,0.014,0.046,0.046,0.045,9.8e-07,1.2e-06,2.3e-06,0.0074,0.0082,0.00076,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.7 +18690000,0.78,-0.017,-0.003,-0.62,-0.024,-0.022,0.0043,-0.014,-0.015,-3.7e+02,-0.0012,-0.006,-4.3e-05,0.063,-0.033,-0.13,-0.11,-0.026,0.5,-0.0035,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.017,0.018,0.013,0.05,0.05,0.045,9.6e-07,1.2e-06,2.3e-06,0.0074,0.0081,0.00074,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.7 +18790000,0.78,-0.016,-0.0029,-0.62,-0.022,-0.021,0.004,-0.012,-0.012,-3.7e+02,-0.0012,-0.006,-4.2e-05,0.062,-0.034,-0.13,-0.11,-0.026,0.5,-0.0036,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.015,0.016,0.013,0.044,0.044,0.045,9.4e-07,1.1e-06,2.3e-06,0.0073,0.008,0.0007,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.7 +18890000,0.78,-0.016,-0.003,-0.62,-0.022,-0.023,0.0046,-0.013,-0.015,-3.7e+02,-0.0013,-0.006,-3.9e-05,0.062,-0.033,-0.13,-0.11,-0.026,0.5,-0.0035,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00012,0.049,0.016,0.017,0.013,0.048,0.048,0.045,9.2e-07,1.1e-06,2.3e-06,0.0072,0.008,0.00068,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.8 +18990000,0.78,-0.016,-0.003,-0.62,-0.019,-0.023,0.0033,-0.0092,-0.013,-3.7e+02,-0.0013,-0.006,-3.5e-05,0.061,-0.032,-0.13,-0.11,-0.026,0.5,-0.0034,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00011,0.049,0.015,0.015,0.012,0.043,0.043,0.044,9e-07,1.1e-06,2.3e-06,0.0072,0.0079,0.00065,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.8 +19090000,0.78,-0.016,-0.003,-0.62,-0.018,-0.025,0.0063,-0.011,-0.015,-3.7e+02,-0.0013,-0.006,-3.5e-05,0.061,-0.032,-0.13,-0.11,-0.026,0.5,-0.0034,-0.092,-0.069,0,0,-3.6e+02,0.00011,0.00011,0.049,0.016,0.016,0.012,0.046,0.047,0.044,8.9e-07,1.1e-06,2.3e-06,0.0071,0.0079,0.00063,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.8 +19190000,0.78,-0.016,-0.0031,-0.62,-0.015,-0.024,0.0063,-0.0072,-0.014,-3.7e+02,-0.0013,-0.006,-3.1e-05,0.06,-0.032,-0.13,-0.11,-0.026,0.5,-0.0033,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.015,0.015,0.012,0.041,0.042,0.044,8.6e-07,1.1e-06,2.3e-06,0.0071,0.0078,0.0006,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.8 +19290000,0.78,-0.016,-0.003,-0.62,-0.016,-0.024,0.0091,-0.0089,-0.016,-3.7e+02,-0.0013,-0.006,-3.2e-05,0.06,-0.032,-0.13,-0.11,-0.026,0.5,-0.0034,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.015,0.016,0.012,0.045,0.045,0.044,8.5e-07,1e-06,2.3e-06,0.007,0.0077,0.00058,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.9 +19390000,0.78,-0.016,-0.003,-0.62,-0.015,-0.022,0.013,-0.0079,-0.014,-3.7e+02,-0.0013,-0.006,-2.8e-05,0.06,-0.032,-0.13,-0.11,-0.026,0.5,-0.0034,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.014,0.015,0.012,0.04,0.041,0.043,8.3e-07,1e-06,2.3e-06,0.007,0.0077,0.00056,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.9 +19490000,0.78,-0.016,-0.0031,-0.62,-0.015,-0.024,0.0093,-0.0095,-0.017,-3.7e+02,-0.0013,-0.006,-2.5e-05,0.061,-0.032,-0.13,-0.11,-0.026,0.5,-0.0033,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.015,0.016,0.011,0.044,0.044,0.043,8.2e-07,1e-06,2.3e-06,0.0069,0.0076,0.00055,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.9 +19590000,0.78,-0.016,-0.0032,-0.62,-0.013,-0.022,0.0085,-0.0079,-0.015,-3.7e+02,-0.0013,-0.0059,-1.8e-05,0.061,-0.031,-0.13,-0.11,-0.026,0.5,-0.0032,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.014,0.015,0.011,0.04,0.04,0.042,7.9e-07,9.7e-07,2.3e-06,0.0069,0.0075,0.00052,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,4.9 +19690000,0.78,-0.016,-0.0031,-0.62,-0.013,-0.021,0.01,-0.0088,-0.017,-3.7e+02,-0.0013,-0.006,-2e-05,0.06,-0.031,-0.13,-0.11,-0.026,0.5,-0.0033,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.015,0.016,0.011,0.043,0.044,0.042,7.8e-07,9.6e-07,2.3e-06,0.0068,0.0075,0.00051,0.0011,7.2e-05,0.0012,0.00061,0.0012,0.0012,1,1,5 +19790000,0.78,-0.016,-0.0032,-0.62,-0.012,-0.018,0.01,-0.0075,-0.015,-3.7e+02,-0.0013,-0.006,-1.6e-05,0.06,-0.031,-0.13,-0.11,-0.026,0.5,-0.0033,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.014,0.014,0.011,0.039,0.039,0.042,7.7e-07,9.4e-07,2.3e-06,0.0068,0.0074,0.00049,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5 +19890000,0.78,-0.016,-0.0032,-0.62,-0.011,-0.02,0.012,-0.0091,-0.018,-3.7e+02,-0.0013,-0.0059,-1.3e-05,0.061,-0.031,-0.13,-0.11,-0.026,0.5,-0.0032,-0.092,-0.068,0,0,-3.6e+02,0.0001,0.00011,0.049,0.015,0.015,0.011,0.043,0.043,0.042,7.6e-07,9.3e-07,2.3e-06,0.0067,0.0074,0.00048,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5 +19990000,0.78,-0.016,-0.0032,-0.62,-0.0095,-0.02,0.014,-0.0079,-0.017,-3.7e+02,-0.0013,-0.0059,-3.8e-06,0.061,-0.03,-0.13,-0.11,-0.026,0.5,-0.0031,-0.092,-0.068,0,0,-3.6e+02,9.9e-05,0.00011,0.049,0.014,0.014,0.01,0.039,0.039,0.041,7.4e-07,9e-07,2.3e-06,0.0067,0.0073,0.00046,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5 +20090000,0.78,-0.016,-0.0033,-0.62,-0.0095,-0.021,0.015,-0.0085,-0.02,-3.7e+02,-0.0013,-0.0059,9.8e-07,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.9e-05,0.00011,0.049,0.014,0.015,0.01,0.042,0.042,0.042,7.3e-07,8.9e-07,2.3e-06,0.0066,0.0073,0.00045,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.1 +20190000,0.78,-0.016,-0.0034,-0.62,-0.01,-0.019,0.017,-0.0087,-0.018,-3.7e+02,-0.0013,-0.0059,6.9e-06,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.8e-05,0.00011,0.048,0.013,0.014,0.01,0.038,0.038,0.041,7.1e-07,8.7e-07,2.3e-06,0.0066,0.0072,0.00043,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.1 +20290000,0.78,-0.016,-0.0034,-0.62,-0.0088,-0.019,0.015,-0.0091,-0.02,-3.7e+02,-0.0013,-0.0059,8.7e-06,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.8e-05,0.00011,0.048,0.014,0.015,0.0099,0.042,0.042,0.041,7e-07,8.6e-07,2.3e-06,0.0065,0.0072,0.00042,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.1 +20390000,0.78,-0.016,-0.0033,-0.62,-0.0084,-0.016,0.017,-0.009,-0.017,-3.7e+02,-0.0013,-0.0059,1.3e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.7e-05,0.0001,0.048,0.013,0.014,0.0097,0.038,0.038,0.041,6.8e-07,8.4e-07,2.3e-06,0.0065,0.0071,0.00041,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.1 +20490000,0.78,-0.016,-0.0034,-0.62,-0.0087,-0.017,0.017,-0.0099,-0.019,-3.7e+02,-0.0013,-0.0059,1.1e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.7e-05,0.0001,0.048,0.014,0.015,0.0096,0.041,0.042,0.041,6.8e-07,8.3e-07,2.3e-06,0.0065,0.0071,0.0004,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.2 +20590000,0.78,-0.016,-0.0033,-0.62,-0.0081,-0.014,0.014,-0.0085,-0.016,-3.7e+02,-0.0013,-0.0059,1.3e-05,0.061,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.5e-05,0.0001,0.048,0.013,0.014,0.0093,0.037,0.038,0.04,6.6e-07,8.1e-07,2.3e-06,0.0064,0.007,0.00038,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.2 +20690000,0.78,-0.016,-0.0034,-0.62,-0.0089,-0.015,0.015,-0.0094,-0.018,-3.7e+02,-0.0013,-0.0059,1.4e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.6e-05,0.0001,0.048,0.014,0.015,0.0093,0.041,0.041,0.04,6.5e-07,8e-07,2.3e-06,0.0064,0.007,0.00037,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.2 +20790000,0.78,-0.016,-0.0034,-0.62,-0.0065,-0.014,0.015,-0.0078,-0.016,-3.7e+02,-0.0013,-0.0059,1.9e-05,0.061,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.6e+02,9.4e-05,0.0001,0.048,0.013,0.014,0.0091,0.037,0.038,0.04,6.4e-07,7.8e-07,2.3e-06,0.0063,0.0069,0.00036,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.2 +20890000,0.78,-0.016,-0.0035,-0.62,-0.0068,-0.014,0.014,-0.0083,-0.018,-3.7e+02,-0.0013,-0.0059,2.2e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.5e-05,0.0001,0.048,0.014,0.015,0.009,0.041,0.041,0.04,6.3e-07,7.7e-07,2.3e-06,0.0063,0.0069,0.00035,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.3 +20990000,0.78,-0.016,-0.0035,-0.62,-0.0052,-0.012,0.015,-0.008,-0.018,-3.7e+02,-0.0013,-0.0059,2.5e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.4e-05,0.0001,0.048,0.014,0.015,0.0088,0.043,0.043,0.039,6.2e-07,7.6e-07,2.3e-06,0.0063,0.0069,0.00034,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.3 +21090000,0.78,-0.016,-0.0035,-0.62,-0.0063,-0.012,0.015,-0.0089,-0.02,-3.7e+02,-0.0013,-0.0059,2.7e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.4e-05,0.0001,0.048,0.015,0.016,0.0088,0.047,0.047,0.039,6.1e-07,7.5e-07,2.3e-06,0.0062,0.0068,0.00034,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.3 +21190000,0.78,-0.016,-0.0035,-0.62,-0.0064,-0.011,0.014,-0.0094,-0.02,-3.7e+02,-0.0013,-0.0059,2.8e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.3e-05,0.0001,0.048,0.015,0.016,0.0086,0.049,0.05,0.039,6e-07,7.3e-07,2.3e-06,0.0062,0.0068,0.00033,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.3 +21290000,0.78,-0.016,-0.0036,-0.62,-0.0059,-0.012,0.016,-0.0094,-0.023,-3.7e+02,-0.0013,-0.0059,3.3e-05,0.062,-0.027,-0.13,-0.11,-0.026,0.5,-0.0028,-0.092,-0.068,0,0,-3.6e+02,9.3e-05,0.0001,0.048,0.016,0.017,0.0085,0.053,0.054,0.039,5.9e-07,7.3e-07,2.3e-06,0.0062,0.0068,0.00032,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.4 +21390000,0.78,-0.016,-0.0036,-0.62,-0.0053,-0.0073,0.016,-0.0082,-0.017,-3.7e+02,-0.0013,-0.0059,3.6e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.2e-05,9.8e-05,0.048,0.014,0.015,0.0084,0.046,0.047,0.039,5.8e-07,7.1e-07,2.3e-06,0.0061,0.0067,0.00031,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.4 +21490000,0.78,-0.016,-0.0036,-0.62,-0.006,-0.0083,0.016,-0.0093,-0.019,-3.7e+02,-0.0013,-0.0059,3.8e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.2e-05,9.8e-05,0.048,0.015,0.016,0.0083,0.05,0.051,0.038,5.7e-07,7e-07,2.3e-06,0.0061,0.0067,0.0003,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.4 +21590000,0.78,-0.016,-0.0035,-0.62,-0.0046,-0.0066,0.016,-0.0077,-0.014,-3.7e+02,-0.0013,-0.0059,4.2e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9e-05,9.7e-05,0.048,0.013,0.014,0.0081,0.044,0.045,0.038,5.6e-07,6.8e-07,2.3e-06,0.0061,0.0066,0.0003,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.4 +21690000,0.78,-0.016,-0.0036,-0.62,-0.0061,-0.0077,0.017,-0.009,-0.016,-3.7e+02,-0.0013,-0.0059,4.4e-05,0.063,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9.1e-05,9.7e-05,0.048,0.014,0.015,0.0081,0.048,0.049,0.038,5.5e-07,6.8e-07,2.3e-06,0.006,0.0066,0.00029,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.5 +21790000,0.78,-0.016,-0.0034,-0.62,-0.0052,-0.0054,0.016,-0.0079,-0.01,-3.7e+02,-0.0013,-0.0059,4.8e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,8.9e-05,9.5e-05,0.048,0.013,0.014,0.008,0.042,0.043,0.038,5.4e-07,6.6e-07,2.3e-06,0.006,0.0065,0.00028,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.5 +21890000,0.78,-0.016,-0.0034,-0.62,-0.0058,-0.0063,0.016,-0.0086,-0.011,-3.7e+02,-0.0013,-0.0059,4.9e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.6e+02,9e-05,9.6e-05,0.048,0.014,0.015,0.0079,0.046,0.047,0.038,5.3e-07,6.5e-07,2.3e-06,0.006,0.0065,0.00028,0.0011,7.2e-05,0.0012,0.0006,0.0012,0.0012,1,1,5.5 +21990000,0.78,-0.016,-0.0034,-0.62,-0.0057,-0.0035,0.017,-0.008,-0.0069,-3.7e+02,-0.0013,-0.0059,5.6e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.7e+02,8.8e-05,9.4e-05,0.048,0.013,0.013,0.0078,0.041,0.042,0.038,5.2e-07,6.4e-07,2.2e-06,0.0059,0.0065,0.00027,0.0011,7.1e-05,0.0012,0.0006,0.0012,0.0012,1,1,0.01 +22090000,0.78,-0.016,-0.0034,-0.62,-0.0054,-0.0051,0.015,-0.0084,-0.0074,-3.7e+02,-0.0013,-0.0059,5.6e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.9e-05,9.5e-05,0.048,0.013,0.014,0.0078,0.045,0.045,0.037,5.2e-07,6.3e-07,2.2e-06,0.0059,0.0065,0.00027,0.0011,7.1e-05,0.0012,0.0006,0.0012,0.0012,1,1,0.01 +22190000,0.78,-0.016,-0.0034,-0.62,-0.0041,-0.0056,0.015,-0.007,-0.0066,-3.7e+02,-0.0013,-0.0059,5.9e-05,0.062,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.7e-05,9.3e-05,0.048,0.012,0.013,0.0076,0.04,0.04,0.037,5.1e-07,6.2e-07,2.2e-06,0.0059,0.0064,0.00026,0.0011,7.1e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22290000,0.78,-0.016,-0.0034,-0.62,-0.0036,-0.0053,0.016,-0.0077,-0.007,-3.7e+02,-0.0013,-0.0059,5.8e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.8e-05,9.3e-05,0.048,0.013,0.014,0.0076,0.043,0.044,0.037,5e-07,6.1e-07,2.2e-06,0.0059,0.0064,0.00025,0.0011,7.1e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22390000,0.78,-0.016,-0.0034,-0.62,-0.0011,-0.0052,0.017,-0.0059,-0.0063,-3.7e+02,-0.0013,-0.0059,6.2e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.6e-05,9.2e-05,0.048,0.012,0.013,0.0075,0.039,0.04,0.037,4.9e-07,6e-07,2.2e-06,0.0058,0.0064,0.00025,0.0011,7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22490000,0.78,-0.016,-0.0034,-0.62,1.4e-05,-0.0059,0.018,-0.0053,-0.0067,-3.7e+02,-0.0014,-0.0059,6.2e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.7e-05,9.2e-05,0.048,0.013,0.014,0.0074,0.042,0.043,0.037,4.9e-07,5.9e-07,2.2e-06,0.0058,0.0063,0.00024,0.0011,7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22590000,0.78,-0.016,-0.0034,-0.62,0.0019,-0.0048,0.017,-0.0036,-0.0061,-3.7e+02,-0.0014,-0.0059,6.6e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.6e-05,9.2e-05,0.048,0.013,0.014,0.0073,0.045,0.045,0.036,4.8e-07,5.8e-07,2.2e-06,0.0058,0.0063,0.00024,0.0011,7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22690000,0.78,-0.016,-0.0035,-0.62,0.0034,-0.0061,0.019,-0.0029,-0.0072,-3.7e+02,-0.0014,-0.0059,7e-05,0.06,-0.027,-0.13,-0.11,-0.026,0.5,-0.0028,-0.092,-0.068,0,0,-3.7e+02,8.7e-05,9.2e-05,0.048,0.014,0.015,0.0073,0.048,0.049,0.036,4.8e-07,5.8e-07,2.2e-06,0.0058,0.0063,0.00024,0.0011,7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22790000,0.78,-0.016,-0.0034,-0.62,0.0045,-0.0054,0.02,-0.0024,-0.0058,-3.7e+02,-0.0014,-0.0059,6.3e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.6e-05,9.1e-05,0.048,0.014,0.015,0.0072,0.051,0.052,0.036,4.7e-07,5.7e-07,2.2e-06,0.0057,0.0062,0.00023,0.0011,7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22890000,0.78,-0.016,-0.0034,-0.62,0.0051,-0.0063,0.021,-0.0025,-0.0066,-3.7e+02,-0.0014,-0.0059,6.3e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.6e-05,9.2e-05,0.048,0.015,0.016,0.0072,0.055,0.056,0.036,4.7e-07,5.7e-07,2.2e-06,0.0057,0.0062,0.00023,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +22990000,0.78,-0.016,-0.0034,-0.62,0.0048,-0.0065,0.022,-0.0026,-0.0074,-3.7e+02,-0.0014,-0.0059,6.8e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.5e-05,9.1e-05,0.048,0.015,0.016,0.0071,0.057,0.059,0.036,4.6e-07,5.6e-07,2.2e-06,0.0057,0.0062,0.00022,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23090000,0.78,-0.016,-0.0033,-0.62,0.0051,-0.0062,0.023,-0.0023,-0.0071,-3.7e+02,-0.0014,-0.0059,6.4e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.6e-05,9.1e-05,0.048,0.016,0.017,0.007,0.062,0.064,0.036,4.6e-07,5.6e-07,2.2e-06,0.0057,0.0062,0.00022,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23190000,0.78,-0.016,-0.0034,-0.62,0.0027,-0.0051,0.024,-0.0051,-0.0071,-3.7e+02,-0.0014,-0.0059,6.6e-05,0.06,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.5e-05,9e-05,0.048,0.016,0.017,0.0069,0.065,0.066,0.035,4.5e-07,5.4e-07,2.2e-06,0.0057,0.0061,0.00021,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23290000,0.78,-0.016,-0.0033,-0.62,0.0023,-0.0048,0.025,-0.0054,-0.008,-3.7e+02,-0.0014,-0.0059,6.8e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.5e-05,9.1e-05,0.048,0.016,0.018,0.0069,0.07,0.071,0.036,4.5e-07,5.4e-07,2.2e-06,0.0056,0.0061,0.00021,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23390000,0.78,-0.016,-0.0034,-0.62,-0.001,-0.0046,0.022,-0.0095,-0.0082,-3.7e+02,-0.0014,-0.0059,7e-05,0.061,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.093,-0.068,0,0,-3.7e+02,8.5e-05,9e-05,0.048,0.016,0.017,0.0068,0.072,0.074,0.035,4.4e-07,5.3e-07,2.2e-06,0.0056,0.0061,0.00021,0.0011,6.9e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23490000,0.78,-0.013,-0.0055,-0.62,0.0046,-0.0042,-0.011,-0.01,-0.0098,-3.7e+02,-0.0013,-0.0059,7.4e-05,0.061,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.5e-05,9e-05,0.048,0.017,0.018,0.0068,0.078,0.08,0.035,4.3e-07,5.3e-07,2.2e-06,0.0056,0.0061,0.0002,0.0011,6.8e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23590000,0.78,-0.0046,-0.0097,-0.62,0.015,-0.00011,-0.043,-0.0094,-0.0059,-3.7e+02,-0.0013,-0.0059,7.7e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.092,-0.068,0,0,-3.7e+02,8.3e-05,8.8e-05,0.047,0.014,0.015,0.0067,0.062,0.063,0.035,4.2e-07,5.1e-07,2.2e-06,0.0056,0.006,0.0002,0.0011,6.8e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23690000,0.78,0.0011,-0.0087,-0.62,0.043,0.014,-0.093,-0.007,-0.0057,-3.7e+02,-0.0013,-0.0059,7.9e-05,0.062,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.068,0,0,-3.7e+02,8.3e-05,8.8e-05,0.047,0.015,0.016,0.0067,0.066,0.068,0.035,4.2e-07,5.1e-07,2.2e-06,0.0056,0.006,0.0002,0.0011,6.8e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23790000,0.78,-0.0026,-0.0062,-0.62,0.064,0.031,-0.15,-0.007,-0.0037,-3.7e+02,-0.0013,-0.0059,8.5e-05,0.063,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.092,-0.067,0,0,-3.7e+02,8.2e-05,8.7e-05,0.047,0.013,0.014,0.0066,0.055,0.056,0.035,4.1e-07,4.9e-07,2.1e-06,0.0055,0.006,0.00019,0.0011,6.7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23890000,0.78,-0.0089,-0.0043,-0.62,0.077,0.043,-0.2,0.0005,6.3e-05,-3.7e+02,-0.0013,-0.0059,8.6e-05,0.063,-0.029,-0.13,-0.11,-0.026,0.5,-0.0031,-0.091,-0.067,0,0,-3.7e+02,8.2e-05,8.7e-05,0.047,0.014,0.015,0.0066,0.059,0.06,0.035,4.1e-07,4.9e-07,2.1e-06,0.0055,0.006,0.00019,0.0011,6.7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +23990000,0.78,-0.014,-0.0035,-0.62,0.072,0.043,-0.25,-0.005,-0.0014,-3.7e+02,-0.0013,-0.0059,8.4e-05,0.064,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.091,-0.068,0,0,-3.7e+02,8.2e-05,8.7e-05,0.047,0.014,0.015,0.0066,0.061,0.063,0.035,4e-07,4.9e-07,2.1e-06,0.0055,0.006,0.00019,0.0011,6.7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +24090000,0.78,-0.012,-0.0046,-0.62,0.073,0.042,-0.3,0.0014,0.002,-3.7e+02,-0.0013,-0.0059,8.8e-05,0.064,-0.029,-0.13,-0.11,-0.026,0.5,-0.003,-0.091,-0.067,0,0,-3.7e+02,8.2e-05,8.7e-05,0.047,0.015,0.016,0.0065,0.066,0.067,0.035,4e-07,4.9e-07,2.1e-06,0.0055,0.006,0.00019,0.0011,6.7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +24190000,0.78,-0.01,-0.0055,-0.62,0.07,0.041,-0.35,-0.0058,-0.00021,-3.7e+02,-0.0013,-0.0059,8.6e-05,0.065,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.091,-0.068,0,0,-3.7e+02,8.2e-05,8.6e-05,0.047,0.015,0.016,0.0065,0.069,0.07,0.034,4e-07,4.8e-07,2.1e-06,0.0055,0.0059,0.00018,0.0011,6.7e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +24290000,0.78,-0.0092,-0.0058,-0.62,0.078,0.046,-0.4,0.00065,0.0042,-3.7e+02,-0.0013,-0.0059,8.4e-05,0.065,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.091,-0.068,0,0,-3.7e+02,8.2e-05,8.7e-05,0.047,0.016,0.017,0.0065,0.074,0.075,0.034,3.9e-07,4.8e-07,2.1e-06,0.0055,0.0059,0.00018,0.0011,6.6e-05,0.0012,0.00059,0.0012,0.0012,1,1,0.01 +24390000,0.79,-0.0096,-0.006,-0.62,0.075,0.044,-0.46,-0.012,-0.0022,-3.7e+02,-0.0012,-0.0059,7e-05,0.067,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.091,-0.068,0,0,-3.7e+02,8.1e-05,8.6e-05,0.047,0.016,0.017,0.0064,0.076,0.078,0.034,3.9e-07,4.7e-07,2.1e-06,0.0055,0.0059,0.00018,0.0011,6.6e-05,0.0012,0.00058,0.0012,0.0012,1,1,0.01 +24490000,0.79,-0.0054,-0.0064,-0.62,0.086,0.051,-0.51,-0.0037,0.0025,-3.7e+02,-0.0012,-0.0059,7e-05,0.067,-0.029,-0.13,-0.11,-0.026,0.5,-0.0029,-0.091,-0.068,0,0,-3.7e+02,8.2e-05,8.6e-05,0.047,0.017,0.018,0.0064,0.082,0.083,0.034,3.9e-07,4.7e-07,2.1e-06,0.0054,0.0059,0.00018,0.0011,6.6e-05,0.0012,0.00058,0.0012,0.0012,1,1,0.01 +24590000,0.79,-0.0019,-0.0065,-0.62,0.09,0.055,-0.56,-0.017,-0.0063,-3.7e+02,-0.0012,-0.0059,6.5e-05,0.068,-0.028,-0.13,-0.11,-0.026,0.5,-0.0028,-0.091,-0.068,0,0,-3.7e+02,8.1e-05,8.6e-05,0.047,0.017,0.018,0.0063,0.084,0.086,0.034,3.8e-07,4.6e-07,2.1e-06,0.0054,0.0059,0.00017,0.0011,6.6e-05,0.0012,0.00058,0.0012,0.0012,1,1,0.01 +24690000,0.79,-0.00097,-0.0065,-0.62,0.11,0.071,-0.64,-0.0079,-0.0013,-3.7e+02,-0.0012,-0.0059,7.2e-05,0.068,-0.028,-0.13,-0.11,-0.026,0.5,-0.0029,-0.09,-0.068,0,0,-3.7e+02,8.2e-05,8.6e-05,0.047,0.018,0.019,0.0063,0.09,0.092,0.034,3.8e-07,4.6e-07,2.1e-06,0.0054,0.0059,0.00017,0.0011,6.5e-05,0.0012,0.00058,0.0011,0.0012,1,1,0.01 +24790000,0.79,-0.0025,-0.0064,-0.62,0.11,0.08,-0.73,-0.027,-0.006,-3.7e+02,-0.0012,-0.0059,6.1e-05,0.07,-0.029,-0.13,-0.11,-0.026,0.5,-0.0026,-0.09,-0.068,0,0,-3.7e+02,8.1e-05,8.5e-05,0.047,0.018,0.018,0.0062,0.092,0.094,0.034,3.7e-07,4.6e-07,2.1e-06,0.0054,0.0059,0.00017,0.0011,6.5e-05,0.0012,0.00058,0.0011,0.0012,1,1,0.01 +24890000,0.79,-0.00061,-0.0079,-0.62,0.13,0.095,-0.75,-0.016,0.0028,-3.7e+02,-0.0012,-0.0059,6.1e-05,0.07,-0.029,-0.13,-0.11,-0.026,0.5,-0.0028,-0.089,-0.068,0,0,-3.7e+02,8.2e-05,8.6e-05,0.047,0.019,0.02,0.0062,0.099,0.1,0.034,3.7e-07,4.6e-07,2.1e-06,0.0054,0.0058,0.00017,0.0011,6.5e-05,0.0012,0.00058,0.0011,0.0012,1,1,0.01 +24990000,0.79,0.0012,-0.0095,-0.62,0.13,0.1,-0.81,-0.038,-0.0035,-3.7e+02,-0.0011,-0.0059,4.6e-05,0.072,-0.029,-0.13,-0.11,-0.026,0.5,-0.0025,-0.089,-0.069,0,0,-3.7e+02,8.1e-05,8.5e-05,0.047,0.019,0.019,0.0062,0.1,0.1,0.034,3.7e-07,4.5e-07,2.1e-06,0.0054,0.0058,0.00016,0.0011,6.4e-05,0.0012,0.00058,0.0011,0.0012,1,1,0.01 +25090000,0.79,0.00058,-0.0099,-0.62,0.16,0.12,-0.86,-0.024,0.0079,-3.7e+02,-0.0011,-0.0059,4.4e-05,0.072,-0.029,-0.13,-0.11,-0.027,0.5,-0.0026,-0.088,-0.069,0,0,-3.7e+02,8.1e-05,8.5e-05,0.047,0.02,0.021,0.0062,0.11,0.11,0.034,3.7e-07,4.5e-07,2.1e-06,0.0054,0.0058,0.00016,0.0011,6.4e-05,0.0012,0.00058,0.0011,0.0012,1,1,0.01 +25190000,0.79,-0.0014,-0.0096,-0.61,0.15,0.11,-0.91,-0.068,-0.014,-3.7e+02,-0.0011,-0.0058,2.5e-05,0.076,-0.029,-0.13,-0.11,-0.027,0.5,-0.0023,-0.088,-0.069,0,0,-3.7e+02,8.1e-05,8.5e-05,0.047,0.019,0.02,0.0061,0.11,0.11,0.033,3.6e-07,4.4e-07,2.1e-06,0.0054,0.0058,0.00016,0.0011,6.4e-05,0.0012,0.00057,0.0011,0.0012,1,1,0.01 +25290000,0.79,0.0056,-0.011,-0.62,0.18,0.13,-0.96,-0.051,-0.0029,-3.7e+02,-0.0011,-0.0058,2.7e-05,0.076,-0.029,-0.13,-0.11,-0.027,0.5,-0.0024,-0.087,-0.069,0,0,-3.7e+02,8.1e-05,8.5e-05,0.047,0.02,0.021,0.0061,0.12,0.12,0.033,3.6e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00016,0.0011,6.3e-05,0.0012,0.00057,0.0011,0.0012,1,1,0.01 +25390000,0.79,0.012,-0.011,-0.61,0.18,0.13,-1,-0.099,-0.027,-3.7e+02,-0.001,-0.0058,8.4e-06,0.079,-0.029,-0.13,-0.12,-0.027,0.5,-0.0022,-0.087,-0.07,0,0,-3.7e+02,8.1e-05,8.5e-05,0.046,0.02,0.021,0.0061,0.12,0.12,0.033,3.6e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00016,0.001,6.3e-05,0.0012,0.00057,0.0011,0.0012,1,1,0.01 +25490000,0.79,0.013,-0.011,-0.61,0.22,0.16,-1.1,-0.081,-0.014,-3.7e+02,-0.001,-0.0058,1.7e-05,0.079,-0.029,-0.13,-0.12,-0.027,0.5,-0.0025,-0.086,-0.069,0,0,-3.7e+02,8.2e-05,8.5e-05,0.046,0.022,0.022,0.0061,0.13,0.13,0.033,3.6e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00015,0.001,6.2e-05,0.0012,0.00057,0.0011,0.0011,1,1,0.01 +25590000,0.79,0.011,-0.011,-0.62,0.25,0.19,-1.1,-0.058,0.0026,-3.7e+02,-0.001,-0.0058,2.3e-05,0.08,-0.029,-0.13,-0.12,-0.027,0.5,-0.0028,-0.085,-0.068,0,0,-3.7e+02,8.2e-05,8.6e-05,0.046,0.023,0.024,0.0061,0.14,0.14,0.033,3.5e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00015,0.001,6.1e-05,0.0011,0.00057,0.0011,0.0011,1,1,0.01 +25690000,0.79,0.018,-0.014,-0.62,0.29,0.22,-1.2,-0.031,0.021,-3.7e+02,-0.00099,-0.0058,3e-05,0.08,-0.029,-0.13,-0.12,-0.027,0.5,-0.0031,-0.084,-0.068,0,0,-3.7e+02,8.3e-05,8.6e-05,0.046,0.025,0.026,0.0061,0.14,0.15,0.033,3.5e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00015,0.001,6e-05,0.0011,0.00056,0.0011,0.0011,1,1,0.01 +25790000,0.78,0.025,-0.016,-0.62,0.35,0.25,-1.2,0.0015,0.042,-3.7e+02,-0.00099,-0.0058,4.2e-05,0.08,-0.029,-0.13,-0.12,-0.028,0.5,-0.0036,-0.083,-0.067,0,0,-3.7e+02,8.4e-05,8.6e-05,0.045,0.027,0.028,0.0061,0.15,0.16,0.033,3.5e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00015,0.00099,5.9e-05,0.0011,0.00056,0.001,0.0011,1,1,0.01 +25890000,0.78,0.025,-0.016,-0.62,0.41,0.29,-1.3,0.041,0.065,-3.7e+02,-0.00099,-0.0058,5.7e-05,0.08,-0.029,-0.13,-0.12,-0.028,0.5,-0.0042,-0.081,-0.066,0,0,-3.7e+02,8.4e-05,8.7e-05,0.045,0.029,0.03,0.0061,0.16,0.17,0.033,3.5e-07,4.4e-07,2.1e-06,0.0053,0.0058,0.00015,0.00097,5.8e-05,0.0011,0.00056,0.001,0.0011,1,1,0.01 +25990000,0.78,0.022,-0.016,-0.62,0.46,0.32,-1.3,0.084,0.093,-3.7e+02,-0.00099,-0.0058,6.6e-05,0.08,-0.03,-0.13,-0.12,-0.028,0.5,-0.0046,-0.08,-0.065,0,0,-3.7e+02,8.5e-05,8.8e-05,0.045,0.031,0.033,0.0061,0.18,0.18,0.033,3.5e-07,4.4e-07,2e-06,0.0053,0.0058,0.00015,0.00095,5.7e-05,0.0011,0.00055,0.001,0.0011,1,1,0.01 +26090000,0.78,0.032,-0.019,-0.62,0.52,0.36,-1.3,0.13,0.13,-3.7e+02,-0.00099,-0.0058,6.4e-05,0.08,-0.03,-0.13,-0.12,-0.029,0.5,-0.0047,-0.079,-0.065,0,0,-3.7e+02,8.6e-05,8.8e-05,0.044,0.033,0.036,0.0061,0.19,0.19,0.033,3.5e-07,4.4e-07,2e-06,0.0053,0.0058,0.00015,0.00093,5.6e-05,0.0011,0.00055,0.00098,0.0011,1,1,0.01 +26190000,0.78,0.042,-0.021,-0.62,0.59,0.41,-1.3,0.19,0.16,-3.7e+02,-0.00098,-0.0058,7.4e-05,0.08,-0.03,-0.13,-0.13,-0.03,0.5,-0.0055,-0.075,-0.063,0,0,-3.7e+02,8.7e-05,8.9e-05,0.043,0.036,0.039,0.0061,0.2,0.21,0.033,3.5e-07,4.4e-07,2e-06,0.0053,0.0058,0.00014,0.0009,5.4e-05,0.001,0.00054,0.00094,0.001,1,1,0.01 +26290000,0.78,0.044,-0.022,-0.62,0.67,0.46,-1.3,0.25,0.2,-3.7e+02,-0.00097,-0.0058,7.8e-05,0.081,-0.03,-0.13,-0.13,-0.03,0.49,-0.0058,-0.073,-0.061,0,0,-3.7e+02,8.8e-05,8.9e-05,0.042,0.039,0.043,0.0061,0.21,0.22,0.033,3.5e-07,4.4e-07,2e-06,0.0053,0.0058,0.00014,0.00087,5.2e-05,0.001,0.00053,0.00091,0.00099,1,1,0.01 +26390000,0.78,0.041,-0.021,-0.62,0.75,0.51,-1.3,0.32,0.25,-3.7e+02,-0.00097,-0.0058,8.5e-05,0.081,-0.031,-0.13,-0.13,-0.03,0.49,-0.0064,-0.071,-0.06,0,0,-3.7e+02,8.8e-05,9e-05,0.041,0.042,0.046,0.0061,0.23,0.23,0.033,3.5e-07,4.5e-07,2e-06,0.0053,0.0058,0.00014,0.00084,5.1e-05,0.00097,0.00051,0.00088,0.00096,1,1,0.01 +26490000,0.78,0.057,-0.028,-0.63,0.84,0.57,-1.3,0.4,0.3,-3.7e+02,-0.00097,-0.0058,9e-05,0.081,-0.031,-0.13,-0.13,-0.031,0.49,-0.0066,-0.069,-0.058,0,0,-3.7e+02,8.9e-05,9.1e-05,0.039,0.045,0.05,0.0061,0.24,0.25,0.033,3.6e-07,4.5e-07,2e-06,0.0053,0.0058,0.00014,0.00081,4.9e-05,0.00093,0.00049,0.00084,0.00093,1,1,0.01 +26590000,0.78,0.074,-0.032,-0.62,0.95,0.65,-1.3,0.49,0.36,-3.7e+02,-0.00096,-0.0058,8.5e-05,0.081,-0.031,-0.13,-0.14,-0.032,0.49,-0.0063,-0.066,-0.057,0,0,-3.7e+02,9e-05,9.1e-05,0.038,0.049,0.056,0.0061,0.26,0.27,0.033,3.6e-07,4.5e-07,2e-06,0.0053,0.0058,0.00014,0.00076,4.6e-05,0.00088,0.00047,0.0008,0.00088,1,1,0.01 +26690000,0.77,0.077,-0.033,-0.63,1.1,0.72,-1.3,0.59,0.42,-3.7e+02,-0.00096,-0.0058,0.0001,0.081,-0.031,-0.13,-0.14,-0.033,0.49,-0.0074,-0.061,-0.052,0,0,-3.7e+02,9.1e-05,9.2e-05,0.035,0.053,0.062,0.0061,0.28,0.29,0.033,3.6e-07,4.5e-07,2e-06,0.0053,0.0058,0.00014,0.00071,4.3e-05,0.00082,0.00045,0.00074,0.00082,1,1,0.01 +26790000,0.77,0.071,-0.032,-0.63,1.2,0.8,-1.3,0.71,0.5,-3.7e+02,-0.00095,-0.0058,0.0001,0.081,-0.031,-0.13,-0.14,-0.034,0.48,-0.0072,-0.057,-0.049,0,0,-3.7e+02,9.2e-05,9.3e-05,0.032,0.056,0.067,0.0061,0.3,0.3,0.033,3.6e-07,4.5e-07,2e-06,0.0053,0.0057,0.00014,0.00067,4.1e-05,0.00078,0.00042,0.00069,0.00077,1,1,0.01 +26890000,0.77,0.094,-0.039,-0.63,1.3,0.88,-1.3,0.84,0.58,-3.7e+02,-0.00095,-0.0058,0.00011,0.081,-0.031,-0.13,-0.15,-0.035,0.48,-0.0078,-0.053,-0.047,0,0,-3.7e+02,9.2e-05,9.3e-05,0.03,0.059,0.072,0.0061,0.32,0.32,0.033,3.6e-07,4.5e-07,2e-06,0.0052,0.0057,0.00014,0.00063,3.9e-05,0.00073,0.00039,0.00065,0.00073,1,1,0.01 +26990000,0.76,0.12,-0.044,-0.63,1.5,0.99,-1.3,0.98,0.67,-3.7e+02,-0.00095,-0.0058,0.00011,0.081,-0.031,-0.13,-0.15,-0.036,0.48,-0.008,-0.047,-0.043,0,0,-3.7e+02,9.3e-05,9.4e-05,0.027,0.063,0.079,0.0061,0.34,0.35,0.033,3.6e-07,4.5e-07,2e-06,0.0052,0.0057,0.00013,0.00057,3.5e-05,0.00066,0.00035,0.00059,0.00066,1,1,0.01 +27090000,0.76,0.12,-0.045,-0.63,1.7,1.1,-1.3,1.1,0.77,-3.7e+02,-0.00096,-0.0058,0.00011,0.081,-0.03,-0.13,-0.16,-0.037,0.47,-0.0079,-0.043,-0.039,0,0,-3.7e+02,9.3e-05,9.4e-05,0.024,0.067,0.085,0.0061,0.36,0.37,0.033,3.6e-07,4.4e-07,2e-06,0.0052,0.0057,0.00013,0.00052,3.2e-05,0.00059,0.00032,0.00053,0.00059,1,1,0.01 +27190000,0.76,0.11,-0.042,-0.63,1.9,1.2,-1.2,1.3,0.89,-3.7e+02,-0.00096,-0.0058,0.00011,0.081,-0.03,-0.13,-0.16,-0.038,0.47,-0.0075,-0.04,-0.035,0,0,-3.7e+02,9.4e-05,9.5e-05,0.021,0.071,0.091,0.0061,0.38,0.39,0.034,3.6e-07,4.4e-07,2e-06,0.0052,0.0057,0.00013,0.00048,3e-05,0.00055,0.00029,0.00049,0.00054,1,1,0.01 +27290000,0.76,0.094,-0.038,-0.64,2,1.3,-1.2,1.5,1,-3.7e+02,-0.00096,-0.0058,0.00011,0.081,-0.029,-0.13,-0.17,-0.039,0.47,-0.0076,-0.036,-0.033,0,0,-3.7e+02,9.4e-05,9.5e-05,0.019,0.072,0.094,0.0061,0.4,0.42,0.033,3.6e-07,4.4e-07,2e-06,0.0052,0.0056,0.00013,0.00045,2.9e-05,0.00052,0.00026,0.00046,0.00051,1,1,0.01 +27390000,0.77,0.078,-0.033,-0.64,2.1,1.4,-1.2,1.7,1.2,-3.7e+02,-0.00096,-0.0058,0.00011,0.081,-0.029,-0.13,-0.17,-0.039,0.47,-0.0075,-0.035,-0.032,0,0,-3.7e+02,9.5e-05,9.6e-05,0.017,0.073,0.094,0.0061,0.43,0.44,0.033,3.6e-07,4.4e-07,2e-06,0.0052,0.0056,0.00013,0.00043,2.8e-05,0.0005,0.00023,0.00044,0.00049,1,1,0.01 +27490000,0.77,0.062,-0.029,-0.64,2.2,1.5,-1.2,1.9,1.3,-3.7e+02,-0.00096,-0.0058,0.00011,0.082,-0.028,-0.13,-0.17,-0.039,0.47,-0.0072,-0.034,-0.032,0,0,-3.7e+02,9.6e-05,9.6e-05,0.015,0.074,0.093,0.0061,0.45,0.47,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00013,0.00042,2.7e-05,0.00049,0.00021,0.00043,0.00048,1,1,0.01 +27590000,0.77,0.05,-0.025,-0.63,2.3,1.5,-1.2,2.2,1.5,-3.7e+02,-0.00095,-0.0058,0.0001,0.082,-0.027,-0.13,-0.17,-0.039,0.47,-0.0067,-0.033,-0.031,0,0,-3.7e+02,9.6e-05,9.7e-05,0.014,0.074,0.092,0.0061,0.48,0.5,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00013,0.00041,2.6e-05,0.00048,0.0002,0.00042,0.00048,1,1,0.01 +27690000,0.77,0.048,-0.025,-0.63,2.3,1.5,-1.2,2.4,1.6,-3.7e+02,-0.00095,-0.0058,0.0001,0.082,-0.026,-0.13,-0.17,-0.04,0.47,-0.0063,-0.032,-0.031,0,0,-3.7e+02,9.7e-05,9.7e-05,0.013,0.074,0.09,0.0061,0.51,0.53,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00013,0.0004,2.6e-05,0.00048,0.00019,0.00042,0.00047,1,1,0.01 +27790000,0.77,0.05,-0.025,-0.63,2.3,1.6,-1.2,2.6,1.8,-3.7e+02,-0.00095,-0.0058,9.6e-05,0.083,-0.026,-0.13,-0.17,-0.04,0.47,-0.0057,-0.032,-0.031,0,0,-3.7e+02,9.8e-05,9.8e-05,0.012,0.074,0.089,0.0061,0.54,0.56,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00013,0.00039,2.6e-05,0.00047,0.00017,0.00041,0.00047,1,1,0.01 +27890000,0.77,0.048,-0.024,-0.63,2.3,1.6,-1.2,2.8,1.9,-3.7e+02,-0.00095,-0.0058,9.5e-05,0.083,-0.025,-0.13,-0.17,-0.04,0.46,-0.0057,-0.031,-0.031,0,0,-3.7e+02,9.9e-05,9.8e-05,0.011,0.075,0.089,0.0061,0.57,0.6,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00012,0.00039,2.5e-05,0.00047,0.00016,0.0004,0.00047,1,1,0.01 +27990000,0.77,0.044,-0.023,-0.63,2.4,1.6,-1.2,3.1,2.1,-3.7e+02,-0.00095,-0.0058,9.3e-05,0.083,-0.025,-0.13,-0.17,-0.04,0.47,-0.0056,-0.031,-0.031,0,0,-3.7e+02,0.0001,9.9e-05,0.01,0.075,0.088,0.0061,0.61,0.63,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00012,0.00038,2.5e-05,0.00047,0.00016,0.0004,0.00046,1,1,0.01 +28090000,0.78,0.058,-0.028,-0.63,2.4,1.6,-1.2,3.3,2.3,-3.7e+02,-0.00095,-0.0058,8.8e-05,0.083,-0.024,-0.13,-0.17,-0.04,0.46,-0.0051,-0.03,-0.03,0,0,-3.7e+02,0.0001,9.9e-05,0.0096,0.076,0.088,0.0061,0.64,0.67,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0056,0.00012,0.00038,2.4e-05,0.00046,0.00015,0.00039,0.00046,1,1,0.01 +28190000,0.77,0.071,-0.031,-0.63,2.4,1.7,-0.93,3.5,2.4,-3.7e+02,-0.00095,-0.0058,8.7e-05,0.084,-0.024,-0.13,-0.17,-0.04,0.46,-0.0051,-0.03,-0.03,0,0,-3.7e+02,0.0001,0.0001,0.0091,0.077,0.088,0.0061,0.68,0.71,0.033,3.7e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00037,2.4e-05,0.00046,0.00014,0.00038,0.00045,1,1,0.01 +28290000,0.78,0.054,-0.025,-0.63,2.4,1.7,-0.069,3.8,2.6,-3.7e+02,-0.00094,-0.0058,8.3e-05,0.084,-0.023,-0.13,-0.17,-0.04,0.46,-0.0049,-0.029,-0.029,0,0,-3.7e+02,0.0001,0.0001,0.0086,0.077,0.086,0.0061,0.71,0.75,0.033,3.7e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00036,2.4e-05,0.00045,0.00014,0.00038,0.00045,1,1,0.01 +28390000,0.78,0.021,-0.013,-0.63,2.4,1.7,0.79,4,2.8,-3.7e+02,-0.00095,-0.0058,7.9e-05,0.084,-0.022,-0.13,-0.17,-0.041,0.46,-0.0046,-0.028,-0.029,0,0,-3.7e+02,0.0001,0.0001,0.0082,0.076,0.084,0.0061,0.75,0.79,0.033,3.7e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00036,2.3e-05,0.00045,0.00013,0.00037,0.00045,1,1,0.01 +28490000,0.78,0.0015,-0.006,-0.63,2.4,1.7,1.1,4.3,3,-3.7e+02,-0.00096,-0.0058,7.4e-05,0.084,-0.021,-0.13,-0.17,-0.041,0.46,-0.0046,-0.028,-0.029,0,0,-3.7e+02,0.0001,0.0001,0.0079,0.077,0.083,0.0061,0.79,0.83,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00036,2.3e-05,0.00045,0.00013,0.00037,0.00045,1,1,0.01 +28590000,0.78,-0.0022,-0.0046,-0.63,2.3,1.7,0.98,4.5,3.1,-3.7e+02,-0.00096,-0.0058,7.2e-05,0.084,-0.021,-0.13,-0.17,-0.041,0.46,-0.0046,-0.028,-0.029,0,0,-3.7e+02,0.0001,0.0001,0.0075,0.077,0.082,0.0061,0.83,0.87,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00035,2.3e-05,0.00045,0.00012,0.00037,0.00044,1,1,0.01 +28690000,0.78,-0.0031,-0.004,-0.63,2.3,1.6,0.99,4.7,3.3,-3.7e+02,-0.00097,-0.0058,6.8e-05,0.083,-0.021,-0.12,-0.17,-0.041,0.46,-0.0045,-0.028,-0.029,0,0,-3.7e+02,0.00011,0.0001,0.0072,0.077,0.082,0.0061,0.87,0.91,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00035,2.3e-05,0.00045,0.00012,0.00037,0.00044,1,1,0.01 +28790000,0.78,-0.0035,-0.0038,-0.63,2.2,1.6,0.99,5,3.5,-3.7e+02,-0.00097,-0.0058,6.4e-05,0.083,-0.02,-0.12,-0.17,-0.041,0.46,-0.0043,-0.028,-0.029,0,0,-3.7e+02,0.00011,0.0001,0.0069,0.078,0.082,0.006,0.91,0.96,0.033,3.6e-07,4.4e-07,2e-06,0.0051,0.0055,0.00012,0.00035,2.3e-05,0.00044,0.00012,0.00037,0.00044,1,1,0.01 +28890000,0.78,-0.0032,-0.0038,-0.63,2.1,1.6,0.98,5.2,3.6,-3.7e+02,-0.00098,-0.0058,6e-05,0.082,-0.02,-0.12,-0.17,-0.041,0.46,-0.0042,-0.028,-0.029,0,0,-3.7e+02,0.00011,0.0001,0.0067,0.079,0.083,0.0061,0.96,1,0.033,3.5e-07,4.5e-07,2e-06,0.0051,0.0055,0.00011,0.00035,2.3e-05,0.00044,0.00011,0.00037,0.00044,1,1,0.01 +28990000,0.78,-0.0028,-0.0039,-0.62,2.1,1.5,0.98,5.4,3.8,-3.7e+02,-0.001,-0.0058,5.3e-05,0.082,-0.019,-0.12,-0.17,-0.041,0.46,-0.0039,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0065,0.08,0.084,0.006,1,1.1,0.033,3.5e-07,4.5e-07,2e-06,0.0051,0.0055,0.00011,0.00035,2.3e-05,0.00044,0.00011,0.00037,0.00043,1,1,0.01 +29090000,0.78,-0.0024,-0.004,-0.62,2,1.5,0.97,5.6,4,-3.7e+02,-0.001,-0.0058,4.9e-05,0.081,-0.018,-0.12,-0.17,-0.041,0.46,-0.0038,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0063,0.081,0.085,0.006,1,1.1,0.033,3.5e-07,4.5e-07,2e-06,0.005,0.0055,0.00011,0.00035,2.3e-05,0.00044,0.00011,0.00037,0.00043,1,1,0.01 +29190000,0.78,-0.0021,-0.0041,-0.62,2,1.5,0.97,5.8,4.1,-3.7e+02,-0.001,-0.0058,4.9e-05,0.081,-0.018,-0.12,-0.17,-0.041,0.46,-0.0039,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0061,0.082,0.087,0.006,1.1,1.2,0.033,3.5e-07,4.5e-07,2e-06,0.005,0.0055,0.00011,0.00035,2.3e-05,0.00044,0.00011,0.00037,0.00043,1,1,0.01 +29290000,0.78,-0.0012,-0.0044,-0.62,1.9,1.5,0.99,6,4.2,-3.7e+02,-0.001,-0.0058,4.4e-05,0.081,-0.018,-0.12,-0.17,-0.041,0.46,-0.0037,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.006,0.084,0.09,0.006,1.1,1.2,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0055,0.00011,0.00035,2.3e-05,0.00043,0.00011,0.00037,0.00043,1,1,0.01 +29390000,0.78,0.00011,-0.0047,-0.62,1.9,1.4,1,6.2,4.4,-3.7e+02,-0.001,-0.0058,3.8e-05,0.081,-0.017,-0.12,-0.17,-0.041,0.46,-0.0034,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0058,0.085,0.092,0.006,1.2,1.3,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0055,0.00011,0.00035,2.3e-05,0.00043,0.0001,0.00037,0.00043,1,1,0.01 +29490000,0.78,0.0013,-0.0051,-0.62,1.8,1.4,1,6.4,4.5,-3.7e+02,-0.001,-0.0058,3.5e-05,0.08,-0.016,-0.12,-0.17,-0.041,0.46,-0.0034,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0057,0.087,0.095,0.006,1.2,1.3,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0055,0.00011,0.00035,2.3e-05,0.00043,0.0001,0.00037,0.00043,1,1,0.01 +29590000,0.78,0.0023,-0.0053,-0.62,1.8,1.4,1,6.6,4.7,-3.7e+02,-0.001,-0.0058,3.2e-05,0.08,-0.016,-0.12,-0.17,-0.041,0.46,-0.0033,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0056,0.089,0.098,0.006,1.3,1.4,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0054,0.00011,0.00035,2.3e-05,0.00043,0.0001,0.00037,0.00043,1,1,0.01 +29690000,0.78,0.003,-0.0055,-0.62,1.7,1.4,0.99,6.7,4.8,-3.7e+02,-0.001,-0.0057,2.8e-05,0.08,-0.015,-0.12,-0.17,-0.041,0.46,-0.0032,-0.028,-0.028,0,0,-3.7e+02,0.00011,0.0001,0.0054,0.09,0.1,0.006,1.4,1.5,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0054,0.00011,0.00035,2.3e-05,0.00043,9.9e-05,0.00037,0.00043,1,1,0.01 +29790000,0.78,0.0034,-0.0057,-0.62,1.7,1.4,0.98,6.9,5,-3.7e+02,-0.001,-0.0057,2.4e-05,0.079,-0.014,-0.12,-0.17,-0.041,0.46,-0.0032,-0.028,-0.027,0,0,-3.7e+02,0.00011,0.0001,0.0053,0.092,0.11,0.006,1.4,1.5,0.033,3.4e-07,4.5e-07,2e-06,0.005,0.0054,0.00011,0.00035,2.3e-05,0.00043,9.8e-05,0.00037,0.00042,1,1,0.01 +29890000,0.78,0.0036,-0.0057,-0.62,1.7,1.3,0.97,7.1,5.1,-3.7e+02,-0.001,-0.0057,1.8e-05,0.079,-0.013,-0.12,-0.17,-0.041,0.46,-0.003,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0053,0.094,0.11,0.006,1.5,1.6,0.033,3.3e-07,4.5e-07,2e-06,0.005,0.0054,0.00011,0.00035,2.3e-05,0.00043,9.7e-05,0.00037,0.00042,1,1,0.01 +29990000,0.78,0.0036,-0.0058,-0.62,1.6,1.3,0.95,7.2,5.2,-3.7e+02,-0.001,-0.0057,1.4e-05,0.079,-0.013,-0.12,-0.17,-0.041,0.46,-0.003,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0052,0.096,0.11,0.006,1.5,1.7,0.033,3.3e-07,4.6e-07,2e-06,0.005,0.0054,0.00011,0.00035,2.3e-05,0.00043,9.6e-05,0.00036,0.00042,1,1,0.01 +30090000,0.78,0.0035,-0.0058,-0.62,1.6,1.3,0.94,7.4,5.4,-3.7e+02,-0.001,-0.0057,1e-05,0.079,-0.012,-0.12,-0.17,-0.041,0.46,-0.0028,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0051,0.098,0.12,0.006,1.6,1.7,0.033,3.3e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00035,2.3e-05,0.00043,9.5e-05,0.00036,0.00042,1,1,0.01 +30190000,0.78,0.0032,-0.0057,-0.62,1.6,1.3,0.93,7.6,5.5,-3.7e+02,-0.001,-0.0057,1.1e-05,0.078,-0.012,-0.12,-0.17,-0.041,0.46,-0.0029,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.005,0.1,0.12,0.006,1.7,1.8,0.033,3.3e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00035,2.2e-05,0.00043,9.4e-05,0.00036,0.00042,1,1,0.01 +30290000,0.78,0.003,-0.0056,-0.62,1.5,1.3,0.92,7.7,5.6,-3.7e+02,-0.001,-0.0057,8.3e-06,0.078,-0.012,-0.12,-0.17,-0.041,0.46,-0.0028,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0049,0.1,0.13,0.006,1.7,1.9,0.033,3.3e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00035,2.2e-05,0.00043,9.3e-05,0.00036,0.00042,1,1,0.01 +30390000,0.78,0.0028,-0.0056,-0.62,1.5,1.3,0.9,7.9,5.8,-3.7e+02,-0.0011,-0.0057,4.2e-06,0.077,-0.011,-0.12,-0.17,-0.041,0.46,-0.0028,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0049,0.1,0.13,0.006,1.8,2,0.033,3.3e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00035,2.2e-05,0.00042,9.2e-05,0.00036,0.00042,1,1,0.01 +30490000,0.78,0.0026,-0.0055,-0.62,1.5,1.2,0.89,8,5.9,-3.7e+02,-0.0011,-0.0057,3e-06,0.077,-0.01,-0.12,-0.17,-0.041,0.46,-0.0028,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0048,0.11,0.14,0.006,1.9,2.1,0.033,3.2e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00034,2.2e-05,0.00042,9.2e-05,0.00036,0.00042,1,1,0.01 +30590000,0.78,0.0021,-0.0054,-0.62,1.4,1.2,0.85,8.2,6,-3.7e+02,-0.0011,-0.0057,9.7e-07,0.077,-0.0095,-0.12,-0.17,-0.041,0.46,-0.0028,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.0001,0.0048,0.11,0.14,0.006,2,2.2,0.033,3.2e-07,4.6e-07,2e-06,0.005,0.0054,0.0001,0.00034,2.2e-05,0.00042,9.1e-05,0.00036,0.00042,1,1,0.01 +30690000,0.78,0.0018,-0.0053,-0.62,1.4,1.2,0.84,8.3,6.1,-3.7e+02,-0.0011,-0.0057,-2.3e-06,0.077,-0.0087,-0.12,-0.17,-0.041,0.46,-0.0027,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.00011,0.0047,0.11,0.15,0.006,2,2.3,0.033,3.2e-07,4.6e-07,2e-06,0.0049,0.0054,0.0001,0.00034,2.2e-05,0.00042,9e-05,0.00036,0.00042,1,1,0.01 +30790000,0.78,0.0014,-0.0052,-0.62,1.4,1.2,0.84,8.5,6.2,-3.7e+02,-0.0011,-0.0057,-5.4e-06,0.076,-0.0085,-0.12,-0.17,-0.041,0.46,-0.0026,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.00011,0.0047,0.11,0.15,0.006,2.1,2.4,0.033,3.2e-07,4.6e-07,2e-06,0.0049,0.0054,0.0001,0.00034,2.2e-05,0.00042,9e-05,0.00036,0.00042,1,1,0.01 +30890000,0.78,0.00095,-0.0051,-0.62,1.3,1.2,0.82,8.6,6.4,-3.7e+02,-0.0011,-0.0057,-8.4e-06,0.076,-0.0077,-0.12,-0.17,-0.041,0.46,-0.0026,-0.028,-0.027,0,0,-3.7e+02,0.00012,0.00011,0.0046,0.12,0.16,0.0059,2.2,2.5,0.033,3.2e-07,4.6e-07,2e-06,0.0049,0.0054,9.9e-05,0.00034,2.2e-05,0.00042,8.9e-05,0.00036,0.00042,1,1,0.01 +30990000,0.78,0.00035,-0.0051,-0.62,1.3,1.2,0.82,8.8,6.5,-3.7e+02,-0.0011,-0.0057,-1.2e-05,0.075,-0.0069,-0.12,-0.17,-0.041,0.46,-0.0025,-0.028,-0.027,0,0,-3.7e+02,0.00013,0.00011,0.0046,0.12,0.16,0.0059,2.3,2.6,0.033,3.2e-07,4.6e-07,2e-06,0.0049,0.0053,9.9e-05,0.00034,2.2e-05,0.00042,8.9e-05,0.00036,0.00041,1,1,0.01 +31090000,0.78,-0.00019,-0.0049,-0.62,1.3,1.1,0.81,8.9,6.6,-3.7e+02,-0.0011,-0.0057,-1.6e-05,0.075,-0.0062,-0.12,-0.17,-0.041,0.46,-0.0025,-0.028,-0.027,0,0,-3.7e+02,0.00013,0.00011,0.0045,0.12,0.17,0.0059,2.4,2.7,0.033,3.1e-07,4.6e-07,2e-06,0.0049,0.0053,9.8e-05,0.00034,2.2e-05,0.00042,8.8e-05,0.00036,0.00041,1,1,0.01 +31190000,0.78,-0.00061,-0.0048,-0.62,1.2,1.1,0.8,9,6.7,-3.7e+02,-0.0011,-0.0057,-1.9e-05,0.075,-0.0054,-0.12,-0.17,-0.041,0.46,-0.0024,-0.028,-0.027,0,0,-3.7e+02,0.00013,0.00011,0.0045,0.12,0.18,0.0059,2.5,2.9,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.7e-05,0.00034,2.2e-05,0.00042,8.8e-05,0.00036,0.00041,1,1,0.01 +31290000,0.78,-0.0012,-0.0046,-0.62,1.2,1.1,0.8,9.2,6.8,-3.7e+02,-0.0011,-0.0057,-2.1e-05,0.075,-0.0047,-0.12,-0.17,-0.041,0.46,-0.0024,-0.028,-0.027,0,0,-3.7e+02,0.00013,0.00011,0.0045,0.13,0.18,0.0059,2.6,3,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.7e-05,0.00034,2.2e-05,0.00042,8.8e-05,0.00036,0.00041,1,1,0.01 +31390000,0.78,-0.0019,-0.0044,-0.62,1.2,1.1,0.8,9.3,6.9,-3.7e+02,-0.0011,-0.0057,-2.4e-05,0.075,-0.0041,-0.12,-0.17,-0.041,0.46,-0.0024,-0.028,-0.027,0,0,-3.7e+02,0.00013,0.00011,0.0044,0.13,0.19,0.0059,2.6,3.1,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.6e-05,0.00034,2.2e-05,0.00042,8.7e-05,0.00036,0.00041,1,1,0.01 +31490000,0.78,-0.0025,-0.0043,-0.62,1.2,1.1,0.79,9.4,7,-3.7e+02,-0.0011,-0.0057,-3e-05,0.074,-0.0033,-0.12,-0.17,-0.041,0.46,-0.0023,-0.028,-0.026,0,0,-3.7e+02,0.00013,0.00011,0.0044,0.13,0.2,0.0059,2.7,3.3,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.6e-05,0.00034,2.2e-05,0.00042,8.7e-05,0.00036,0.00041,1,1,0.01 +31590000,0.78,-0.0029,-0.0043,-0.62,1.1,1,0.79,9.5,7.1,-3.7e+02,-0.0011,-0.0057,-3.1e-05,0.073,-0.0026,-0.12,-0.17,-0.041,0.46,-0.0022,-0.028,-0.026,0,0,-3.7e+02,0.00013,0.00011,0.0044,0.13,0.21,0.0059,2.8,3.4,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.5e-05,0.00034,2.2e-05,0.00041,8.7e-05,0.00036,0.00041,1,1,0.01 +31690000,0.78,-0.0037,-0.0041,-0.62,1.1,1,0.8,9.7,7.2,-3.7e+02,-0.0011,-0.0057,-3.3e-05,0.073,-0.0019,-0.12,-0.17,-0.041,0.46,-0.0022,-0.028,-0.026,0,0,-3.7e+02,0.00013,0.00011,0.0044,0.14,0.21,0.0059,2.9,3.5,0.033,3.1e-07,4.7e-07,2e-06,0.0049,0.0053,9.4e-05,0.00034,2.2e-05,0.00041,8.6e-05,0.00036,0.00041,1,1,0.01 +31790000,0.78,-0.0044,-0.0039,-0.62,1.1,1,0.8,9.8,7.3,-3.7e+02,-0.0011,-0.0057,-3.6e-05,0.072,-0.001,-0.12,-0.17,-0.041,0.46,-0.0021,-0.029,-0.026,0,0,-3.7e+02,0.00013,0.00011,0.0043,0.14,0.22,0.0059,3.1,3.7,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0053,9.4e-05,0.00034,2.2e-05,0.00041,8.6e-05,0.00036,0.00041,1,1,0.01 +31890000,0.78,-0.0051,-0.0038,-0.62,1,0.99,0.79,9.9,7.4,-3.7e+02,-0.0011,-0.0057,-4e-05,0.072,-0.00017,-0.12,-0.17,-0.041,0.46,-0.0021,-0.029,-0.026,0,0,-3.7e+02,0.00013,0.00011,0.0043,0.14,0.23,0.0059,3.2,3.9,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0053,9.3e-05,0.00034,2.2e-05,0.00041,8.6e-05,0.00036,0.00041,1,1,0.01 +31990000,0.78,-0.0056,-0.0036,-0.62,1,0.97,0.79,10,7.5,-3.7e+02,-0.0012,-0.0057,-4.5e-05,0.071,0.00076,-0.12,-0.17,-0.041,0.46,-0.002,-0.029,-0.026,0,0,-3.7e+02,0.00014,0.00011,0.0043,0.15,0.24,0.0058,3.3,4,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0053,9.3e-05,0.00034,2.2e-05,0.00041,8.6e-05,0.00036,0.00041,1,1,0.01 +32090000,0.78,-0.0064,-0.0034,-0.62,0.99,0.96,0.8,10,7.7,-3.7e+02,-0.0012,-0.0057,-4.9e-05,0.071,0.0015,-0.11,-0.17,-0.041,0.46,-0.0019,-0.029,-0.026,0,0,-3.7e+02,0.00014,0.00011,0.0043,0.15,0.25,0.0059,3.4,4.2,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0052,9.2e-05,0.00034,2.2e-05,0.00041,8.5e-05,0.00036,0.0004,1,1,0.01 +32190000,0.78,-0.0073,-0.0032,-0.62,0.96,0.94,0.8,10,7.8,-3.7e+02,-0.0012,-0.0057,-5.8e-05,0.07,0.0024,-0.11,-0.17,-0.041,0.46,-0.0018,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0043,0.15,0.25,0.0058,3.5,4.4,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0052,9.1e-05,0.00034,2.2e-05,0.00041,8.5e-05,0.00036,0.0004,1,1,0.01 +32290000,0.78,-0.0081,-0.0031,-0.62,0.93,0.92,0.79,10,7.8,-3.7e+02,-0.0012,-0.0057,-6.2e-05,0.07,0.0034,-0.11,-0.17,-0.041,0.46,-0.0017,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.15,0.26,0.0058,3.6,4.6,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0052,9.1e-05,0.00034,2.2e-05,0.00041,8.5e-05,0.00036,0.0004,1,1,0.01 +32390000,0.78,-0.0087,-0.003,-0.62,0.9,0.9,0.79,10,7.9,-3.7e+02,-0.0012,-0.0057,-6.3e-05,0.069,0.0041,-0.11,-0.17,-0.041,0.46,-0.0017,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.16,0.27,0.0058,3.7,4.8,0.033,3e-07,4.7e-07,2e-06,0.0049,0.0052,9e-05,0.00034,2.2e-05,0.00041,8.5e-05,0.00036,0.0004,1,1,0.01 +32490000,0.78,-0.0089,-0.003,-0.62,0.87,0.88,0.8,11,8,-3.7e+02,-0.0012,-0.0057,-6.5e-05,0.069,0.0048,-0.11,-0.17,-0.041,0.46,-0.0016,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.16,0.28,0.0058,3.9,5,0.033,2.9e-07,4.7e-07,2e-06,0.0049,0.0052,9e-05,0.00034,2.2e-05,0.0004,8.5e-05,0.00036,0.0004,1,1,0.01 +32590000,0.78,-0.0092,-0.0029,-0.62,-1.6,-0.84,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-6.9e-05,0.069,0.0052,-0.11,-0.17,-0.041,0.46,-0.0016,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.25,0.25,0.56,0.25,0.25,0.035,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.9e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.0004,1,1,0.01 +32690000,0.79,-0.0093,-0.0029,-0.62,-1.6,-0.86,0.61,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.2e-05,0.068,0.0058,-0.11,-0.17,-0.041,0.46,-0.0015,-0.029,-0.025,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.25,0.25,0.55,0.26,0.26,0.047,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.9e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.0004,1,1,0.01 +32790000,0.79,-0.0092,-0.0029,-0.62,-1.5,-0.84,0.61,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.4e-05,0.067,0.0062,-0.11,-0.17,-0.041,0.46,-0.0015,-0.029,-0.024,0,0,-3.7e+02,0.00014,0.00011,0.0042,0.13,0.13,0.27,0.26,0.26,0.047,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.9e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.0004,1,1,0.01 +32890000,0.79,-0.0091,-0.003,-0.62,-1.6,-0.86,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-8.3e-05,0.067,0.0067,-0.11,-0.17,-0.041,0.46,-0.0013,-0.029,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0042,0.13,0.13,0.26,0.27,0.27,0.058,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.8e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.00039,1,1,0.01 +32990000,0.79,-0.0091,-0.0031,-0.62,-1.5,-0.85,0.59,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.8e-05,0.066,0.0072,-0.11,-0.17,-0.041,0.46,-0.0013,-0.029,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0041,0.084,0.085,0.17,0.27,0.27,0.056,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.8e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.00039,1,1,0.01 +33090000,0.79,-0.0091,-0.0031,-0.62,-1.6,-0.87,0.57,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.5e-05,0.066,0.0074,-0.11,-0.17,-0.041,0.46,-0.0013,-0.029,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0041,0.084,0.085,0.16,0.28,0.28,0.065,2.9e-07,4.8e-07,2e-06,0.0048,0.0052,8.8e-05,0.00034,2.2e-05,0.0004,8.4e-05,0.00036,0.00039,1,1,0.01 +33190000,0.79,-0.0078,-0.0063,-0.61,-1.5,-0.85,0.52,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.2e-05,0.066,0.0075,-0.11,-0.17,-0.041,0.46,-0.0014,-0.029,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0041,0.063,0.065,0.11,0.28,0.28,0.062,2.8e-07,4.8e-07,2e-06,0.0048,0.0052,8.8e-05,0.00034,2.2e-05,0.0004,8.3e-05,0.00036,0.00039,1,1,0.01 +33290000,0.83,-0.0057,-0.018,-0.56,-1.5,-0.87,0.5,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-7.6e-05,0.066,0.0074,-0.11,-0.17,-0.041,0.46,-0.0014,-0.029,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0041,0.064,0.066,0.1,0.29,0.29,0.067,2.8e-07,4.8e-07,2e-06,0.0048,0.0052,8.8e-05,0.00034,2.2e-05,0.0004,8.3e-05,0.00035,0.00039,1,1,0.01 +33390000,0.9,-0.0064,-0.015,-0.44,-1.5,-0.86,0.7,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-6.3e-05,0.065,0.0071,-0.11,-0.18,-0.041,0.46,-0.00098,-0.027,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.004,0.051,0.053,0.082,0.29,0.29,0.065,2.8e-07,4.7e-07,2e-06,0.0048,0.0051,8.8e-05,0.00031,2e-05,0.0004,8.1e-05,0.00032,0.00039,1,1,0.01 +33490000,0.96,-0.0051,-0.0069,-0.3,-1.5,-0.88,0.71,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,-4.4e-05,0.065,0.0071,-0.11,-0.18,-0.043,0.46,-0.00048,-0.02,-0.024,0,0,-3.7e+02,0.00015,0.00011,0.0037,0.052,0.054,0.075,0.3,0.3,0.068,2.8e-07,4.7e-07,2e-06,0.0048,0.0051,8.8e-05,0.00023,1.6e-05,0.00039,7.3e-05,0.00024,0.00039,1,1,0.01 +33590000,0.99,-0.0083,-0.00017,-0.13,-1.5,-0.86,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,1.7e-05,0.065,0.0071,-0.11,-0.19,-0.044,0.46,0.00029,-0.013,-0.024,0,0,-3.7e+02,0.00015,0.0001,0.0032,0.044,0.046,0.061,0.3,0.3,0.065,2.8e-07,4.6e-07,2e-06,0.0048,0.0051,8.8e-05,0.00015,1.2e-05,0.00039,6e-05,0.00015,0.00039,1,1,0.01 +33690000,1,-0.012,0.004,0.038,-1.6,-0.88,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,2.3e-05,0.065,0.0071,-0.11,-0.19,-0.045,0.46,0.00013,-0.0091,-0.025,0,0,-3.7e+02,0.00015,0.0001,0.0028,0.044,0.048,0.056,0.31,0.31,0.067,2.8e-07,4.6e-07,2e-06,0.0048,0.0051,8.8e-05,0.0001,9e-06,0.00039,4.7e-05,9.7e-05,0.00038,1,1,0.01 +33790000,0.98,-0.013,0.0065,0.21,-1.6,-0.9,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,6.8e-05,0.065,0.0071,-0.11,-0.2,-0.046,0.46,0.00049,-0.0066,-0.025,0,0,-3.7e+02,0.00015,0.0001,0.0023,0.039,0.043,0.047,0.31,0.31,0.064,2.7e-07,4.5e-07,1.9e-06,0.0048,0.0051,8.8e-05,6.9e-05,7.2e-06,0.00038,3.5e-05,6.2e-05,0.00038,1,1,0.01 +33890000,0.93,-0.013,0.0085,0.37,-1.7,-0.95,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.2e-05,0.065,0.0071,-0.11,-0.2,-0.046,0.46,0.00071,-0.0053,-0.025,0,0,-3.7e+02,0.00015,0.0001,0.002,0.04,0.046,0.042,0.32,0.32,0.065,2.7e-07,4.5e-07,1.9e-06,0.0048,0.0051,8.8e-05,5e-05,6.2e-06,0.00038,2.7e-05,4.2e-05,0.00038,1,1,0.01 +33990000,0.86,-0.015,0.0071,0.51,-1.7,-0.98,0.64,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.065,0.0069,-0.11,-0.2,-0.046,0.46,0.0005,-0.0039,-0.025,0,0,-3.7e+02,0.00014,9.9e-05,0.0017,0.036,0.042,0.036,0.32,0.32,0.062,2.7e-07,4.4e-07,1.9e-06,0.0048,0.0051,8.8e-05,4e-05,5.6e-06,0.00038,2.1e-05,3e-05,0.00038,1,1,0.01 +34090000,0.8,-0.016,0.0063,0.6,-1.7,-1.1,0.64,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.066,0.0063,-0.11,-0.2,-0.046,0.46,0.00017,-0.0033,-0.025,0,0,-3.7e+02,0.00014,9.9e-05,0.0016,0.038,0.046,0.033,0.33,0.33,0.063,2.7e-07,4.3e-07,1.9e-06,0.0048,0.0051,8.9e-05,3.4e-05,5.3e-06,0.00038,1.7e-05,2.4e-05,0.00038,1,1,0.01 +34190000,0.75,-0.014,0.007,0.67,-1.8,-1.1,0.65,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.065,0.0061,-0.11,-0.2,-0.047,0.46,0.00018,-0.0027,-0.025,0,0,-3.7e+02,0.00014,9.9e-05,0.0015,0.041,0.051,0.031,0.34,0.34,0.063,2.8e-07,4.3e-07,1.9e-06,0.0047,0.0051,8.9e-05,3e-05,5e-06,0.00038,1.4e-05,1.9e-05,0.00038,1,1,0.01 +34290000,0.71,-0.011,0.0085,0.71,-1.8,-1.2,0.65,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.065,0.0058,-0.11,-0.2,-0.047,0.46,0.00014,-0.0023,-0.025,0,0,-3.7e+02,0.00014,9.8e-05,0.0014,0.044,0.056,0.028,0.35,0.35,0.062,2.8e-07,4.3e-07,1.9e-06,0.0047,0.0051,8.9e-05,2.7e-05,4.9e-06,0.00038,1.2e-05,1.6e-05,0.00038,1,1,0.01 +34390000,0.69,-0.0078,0.0099,0.73,-1.9,-1.3,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.0054,-0.11,-0.2,-0.047,0.46,7.3e-05,-0.002,-0.025,0,0,-3.7e+02,0.00014,9.8e-05,0.0013,0.048,0.061,0.026,0.36,0.37,0.063,2.8e-07,4.3e-07,1.9e-06,0.0047,0.0051,8.9e-05,2.5e-05,4.8e-06,0.00038,1.1e-05,1.4e-05,0.00038,1,1,0.01 +34490000,0.67,-0.0056,0.011,0.74,-1.9,-1.4,0.66,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0054,-0.11,-0.2,-0.047,0.46,3.2e-05,-0.002,-0.025,0,0,-3.7e+02,0.00014,9.7e-05,0.0013,0.052,0.068,0.024,0.38,0.38,0.062,2.8e-07,4.3e-07,1.8e-06,0.0047,0.0051,8.9e-05,2.3e-05,4.7e-06,0.00038,9.9e-06,1.2e-05,0.00038,1,1,0.01 +34590000,0.66,-0.0042,0.012,0.75,-2,-1.4,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0053,-0.11,-0.2,-0.047,0.46,-6.4e-05,-0.0019,-0.025,0,0,-3.7e+02,0.00014,9.7e-05,0.0012,0.057,0.074,0.022,0.39,0.4,0.06,2.8e-07,4.3e-07,1.8e-06,0.0047,0.0051,8.9e-05,2.2e-05,4.6e-06,0.00038,9e-06,1.1e-05,0.00038,1,1,0.01 +34690000,0.65,-0.0034,0.013,0.76,-2,-1.5,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0052,-0.11,-0.2,-0.047,0.46,-5.8e-07,-0.0016,-0.025,0,0,-3.7e+02,0.00014,9.6e-05,0.0012,0.062,0.082,0.02,0.41,0.41,0.06,2.8e-07,4.3e-07,1.8e-06,0.0047,0.0051,8.9e-05,2.1e-05,4.6e-06,0.00038,8.3e-06,9.9e-06,0.00038,1,1,0.01 +34790000,0.65,-0.0028,0.013,0.76,-2.1,-1.6,0.67,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0051,-0.11,-0.2,-0.047,0.46,0.00012,-0.0016,-0.025,0,0,-3.7e+02,0.00014,9.6e-05,0.0012,0.068,0.09,0.018,0.42,0.43,0.059,2.8e-07,4.3e-07,1.8e-06,0.0047,0.0051,8.9e-05,2.1e-05,4.5e-06,0.00038,7.8e-06,9.1e-06,0.00038,1,1,0.01 +34890000,0.65,-0.0027,0.013,0.76,-2.1,-1.7,0.68,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0049,-0.11,-0.2,-0.047,0.46,7.1e-05,-0.0016,-0.025,0,0,-3.7e+02,0.00014,9.6e-05,0.0012,0.075,0.099,0.017,0.44,0.46,0.058,2.8e-07,4.3e-07,1.8e-06,0.0047,0.0051,8.9e-05,2e-05,4.5e-06,0.00038,7.3e-06,8.4e-06,0.00038,1,1,0.01 +34990000,0.64,-0.0062,0.02,0.76,-3,-2.6,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00018,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.5e-05,0.0012,0.092,0.13,0.016,0.46,0.48,0.057,2.8e-07,4.4e-07,1.8e-06,0.0047,0.0051,8.9e-05,1.9e-05,4.5e-06,0.00038,6.9e-06,7.9e-06,0.00038,1,1,0.01 +35090000,0.64,-0.0063,0.02,0.76,-3.2,-2.7,-0.18,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.0002,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.5e-05,0.0012,0.1,0.14,0.015,0.49,0.51,0.056,2.8e-07,4.4e-07,1.8e-06,0.0047,0.0051,8.9e-05,1.9e-05,4.4e-06,0.00038,6.6e-06,7.4e-06,0.00038,1,1,0.01 +35190000,0.64,-0.0064,0.02,0.76,-3.2,-2.8,-0.17,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00024,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.4e-05,0.0011,0.11,0.15,0.014,0.51,0.54,0.055,2.8e-07,4.4e-07,1.8e-06,0.0047,0.0051,9e-05,1.8e-05,4.4e-06,0.00038,6.3e-06,7e-06,0.00038,1,1,0.01 +35290000,0.64,-0.0065,0.02,0.76,-3.2,-2.8,-0.16,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00029,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.4e-05,0.0011,0.12,0.17,0.013,0.54,0.58,0.053,2.8e-07,4.4e-07,1.8e-06,0.0047,0.0051,9e-05,1.8e-05,4.4e-06,0.00038,6e-06,6.6e-06,0.00038,1,1,0.01 +35390000,0.64,-0.0065,0.02,0.76,-3.2,-2.9,-0.15,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00036,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.4e-05,0.0011,0.13,0.18,0.012,0.58,0.62,0.053,2.8e-07,4.4e-07,1.8e-06,0.0047,0.0051,9e-05,1.8e-05,4.3e-06,0.00038,5.8e-06,6.3e-06,0.00037,1,1,0.01 +35490000,0.64,-0.0066,0.02,0.77,-3.3,-3,-0.14,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00043,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.3e-05,0.0011,0.14,0.19,0.012,0.61,0.67,0.052,2.9e-07,4.4e-07,1.8e-06,0.0047,0.0051,9e-05,1.8e-05,4.3e-06,0.00038,5.6e-06,6e-06,0.00037,1,1,0.01 +35590000,0.64,-0.0066,0.02,0.77,-3.3,-3.1,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.0004,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.3e-05,0.0011,0.15,0.2,0.011,0.65,0.72,0.05,2.9e-07,4.4e-07,1.8e-06,0.0047,0.0051,9e-05,1.7e-05,4.3e-06,0.00038,5.4e-06,5.8e-06,0.00037,1,1,0.01 +35690000,0.64,-0.0065,0.02,0.77,-3.3,-3.1,-0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0048,-0.11,-0.2,-0.047,0.46,0.00046,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.16,0.22,0.011,0.69,0.77,0.05,2.9e-07,4.4e-07,1.9e-06,0.0047,0.0051,9e-05,1.7e-05,4.3e-06,0.00038,5.3e-06,5.6e-06,0.00037,1,1,0.01 +35790000,0.64,-0.0066,0.02,0.77,-3.4,-3.2,-0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.065,0.0049,-0.11,-0.2,-0.047,0.46,0.00047,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.17,0.23,0.01,0.74,0.84,0.049,2.9e-07,4.4e-07,1.9e-06,0.0047,0.0051,9e-05,1.7e-05,4.3e-06,0.00038,5.1e-06,5.3e-06,0.00037,1,1,0.025 +35890000,0.64,-0.0067,0.02,0.77,-3.4,-3.3,-0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.065,0.005,-0.11,-0.2,-0.047,0.46,0.00048,-0.0015,-0.025,0,0,-3.7e+02,0.00013,9.2e-05,0.0011,0.18,0.25,0.0096,0.79,0.9,0.048,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.7e-05,4.3e-06,0.00038,5e-06,5.2e-06,0.00037,1,1,0.056 +35990000,0.64,-0.0067,0.02,0.77,-3.4,-3.3,-0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00014,0.066,0.0048,-0.11,-0.2,-0.047,0.46,0.00045,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.2,0.26,0.0093,0.84,0.98,0.047,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.3e-06,0.00038,4.9e-06,5e-06,0.00037,1,1,0.086 +36090000,0.64,-0.0067,0.02,0.77,-3.4,-3.4,-0.093,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0049,-0.11,-0.2,-0.047,0.46,0.00048,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.21,0.28,0.0089,0.91,1.1,0.046,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9e-05,1.6e-05,4.2e-06,0.00038,4.8e-06,4.8e-06,0.00037,1,1,0.12 +36190000,0.64,-0.0068,0.02,0.77,-3.5,-3.5,-0.083,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0051,-0.11,-0.2,-0.047,0.46,0.00055,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9.1e-05,0.0011,0.22,0.3,0.0086,0.97,1.1,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.6e-05,4.2e-06,0.00038,4.7e-06,4.7e-06,0.00037,1,1,0.15 +36290000,0.64,-0.0067,0.02,0.77,-3.5,-3.6,-0.073,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0048,-0.11,-0.2,-0.047,0.46,0.00057,-0.0016,-0.025,0,0,-3.7e+02,0.00013,9e-05,0.0011,0.24,0.31,0.0083,1,1.2,0.045,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.6e-05,4.2e-06,0.00038,4.6e-06,4.6e-06,0.00037,1,1,0.18 +36390000,0.64,-0.0068,0.02,0.77,-3.5,-3.6,-0.066,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.066,0.0049,-0.11,-0.2,-0.047,0.46,0.00056,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.25,0.33,0.0081,1.1,1.4,0.044,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.6e-05,4.2e-06,0.00038,4.5e-06,4.4e-06,0.00037,1,1,0.21 +36490000,0.64,-0.0068,0.02,0.77,-3.6,-3.7,-0.059,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00013,0.067,0.0049,-0.11,-0.2,-0.047,0.46,0.00053,-0.0015,-0.025,0,0,-3.7e+02,0.00012,9e-05,0.0011,0.27,0.35,0.0078,1.2,1.5,0.043,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.3e-06,0.00037,1,1,0.24 +36590000,0.64,-0.0068,0.02,0.77,-3.6,-3.8,-0.049,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.067,0.0049,-0.11,-0.2,-0.047,0.46,0.00057,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.28,0.37,0.0076,1.3,1.6,0.042,2.9e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.6e-05,4.2e-06,0.00038,4.4e-06,4.2e-06,0.00037,1,1,0.27 +36690000,0.64,-0.0069,0.02,0.77,-3.6,-3.9,-0.04,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00012,0.067,0.0051,-0.11,-0.2,-0.047,0.46,0.00059,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.3,0.39,0.0075,1.4,1.7,0.042,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4.1e-06,0.00037,1,1,0.31 +36790000,0.64,-0.0069,0.02,0.77,-3.6,-3.9,-0.031,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.067,0.005,-0.11,-0.2,-0.047,0.46,0.00063,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.31,0.41,0.0073,1.5,1.9,0.041,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.3e-06,4e-06,0.00037,1,1,0.34 +36890000,0.64,-0.0069,0.02,0.77,-3.7,-4,-0.023,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.00011,0.067,0.0052,-0.11,-0.2,-0.047,0.46,0.00066,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.9e-05,0.0011,0.33,0.43,0.0072,1.6,2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.37 +36990000,0.64,-0.0069,0.02,0.77,-3.7,-4.1,-0.015,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.067,0.0053,-0.11,-0.2,-0.047,0.46,0.00067,-0.0015,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.35,0.45,0.0071,1.8,2.2,0.04,3e-07,4.4e-07,1.9e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.2e-06,3.9e-06,0.00037,1,1,0.4 +37090000,0.64,-0.0069,0.02,0.77,-3.7,-4.2,-0.0065,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.068,0.0054,-0.11,-0.2,-0.047,0.46,0.00067,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.37,0.47,0.007,1.9,2.4,0.04,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.8e-06,0.00037,1,1,0.43 +37190000,0.64,-0.0069,0.021,0.77,-3.7,-4.2,0.0015,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,0.0001,0.068,0.0054,-0.11,-0.2,-0.047,0.46,0.00067,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.38,0.49,0.0069,2,2.6,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.1e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.47 +37290000,0.64,-0.007,0.021,0.77,-3.8,-4.3,0.0093,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.8e-05,0.068,0.0055,-0.11,-0.2,-0.047,0.46,0.00068,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.8e-05,0.0011,0.4,0.51,0.0068,2.2,2.8,0.039,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.5e-05,4.2e-06,0.00038,4.1e-06,3.7e-06,0.00037,1,1,0.5 +37390000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.016,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,9.4e-05,0.068,0.0054,-0.11,-0.2,-0.047,0.46,0.0007,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.0011,0.42,0.53,0.0068,2.4,3,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.6e-06,0.00037,1,1,0.53 +37490000,0.64,-0.0069,0.021,0.77,-3.8,-4.4,0.024,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,8.8e-05,0.068,0.0055,-0.11,-0.2,-0.047,0.46,0.00072,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.44,0.56,0.0067,2.5,3.2,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.57 +37590000,0.64,-0.0069,0.021,0.77,-3.9,-4.5,0.033,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0057,8.5e-05,0.068,0.0055,-0.11,-0.2,-0.047,0.46,0.00073,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.46,0.58,0.0067,2.7,3.5,0.038,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.5e-05,4.2e-06,0.00038,4e-06,3.5e-06,0.00037,1,1,0.6 +37690000,0.64,-0.007,0.021,0.77,-3.9,-4.6,0.042,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.9e-05,0.069,0.0056,-0.11,-0.2,-0.047,0.46,0.00074,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.48,0.6,0.0066,2.9,3.7,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.5e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.64 +37790000,0.64,-0.0071,0.021,0.77,-3.9,-4.7,0.051,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.7e-05,0.069,0.0054,-0.11,-0.2,-0.047,0.46,0.00075,-0.0014,-0.025,0,0,-3.7e+02,0.00012,8.7e-05,0.001,0.5,0.63,0.0066,3.1,4,0.037,3e-07,4.4e-07,1.8e-06,0.0047,0.005,9.2e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.4e-06,0.00037,1,1,0.67 +37890000,0.64,-0.0071,0.021,0.77,-3.9,-4.7,0.059,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.5e-05,0.069,0.0057,-0.11,-0.2,-0.047,0.46,0.00075,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.52,0.65,0.0066,3.4,4.3,0.036,3e-07,4.4e-07,1.8e-06,0.0047,0.0049,9.2e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.7 +37990000,0.64,-0.0071,0.021,0.77,-4,-4.8,0.068,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7.5e-05,0.069,0.0055,-0.11,-0.2,-0.047,0.46,0.00075,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.54,0.67,0.0066,3.6,4.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.3e-06,0.00037,1,1,0.74 +38090000,0.64,-0.0072,0.021,0.77,-4,-4.9,0.078,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7e-05,0.069,0.0054,-0.11,-0.2,-0.047,0.46,0.00077,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.57,0.7,0.0065,3.9,4.9,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.2e-06,0.00038,3.9e-06,3.2e-06,0.00037,1,1,0.77 +38190000,0.64,-0.0071,0.021,0.77,-4,-5,0.086,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.8e-05,0.069,0.0055,-0.11,-0.2,-0.047,0.46,0.00077,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.59,0.72,0.0065,4.1,5.3,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.2e-06,0.00037,1,1,0.81 +38290000,0.64,-0.0071,0.021,0.77,-4,-5,0.094,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.8e-05,0.069,0.0055,-0.11,-0.2,-0.047,0.46,0.00077,-0.0014,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.61,0.75,0.0065,4.4,5.6,0.036,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.2e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.84 +38390000,0.64,-0.0071,0.021,0.77,-4.1,-5.1,0.1,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,7e-05,0.069,0.0053,-0.11,-0.2,-0.047,0.46,0.00077,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.6e-05,0.001,0.64,0.77,0.0065,4.7,6,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3.1e-06,0.00037,1,1,0.88 +38490000,0.64,-0.0071,0.021,0.77,-4.1,-5.2,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.8e-05,0.069,0.0054,-0.11,-0.2,-0.047,0.46,0.00077,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.66,0.8,0.0065,5.1,6.4,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.91 +38590000,0.64,-0.007,0.021,0.77,-4.1,-5.2,0.11,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.9e-05,0.069,0.0056,-0.11,-0.2,-0.047,0.46,0.00077,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.68,0.82,0.0066,5.4,6.8,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.95 +38690000,0.64,-0.007,0.021,0.77,-4.1,-5.3,0.12,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.5e-05,0.068,0.0058,-0.11,-0.2,-0.047,0.46,0.00078,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.71,0.85,0.0066,5.8,7.3,0.035,3.1e-07,4.4e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,3e-06,0.00037,1,1,0.99 +38790000,0.64,-0.007,0.021,0.77,-4.2,-5.4,0.13,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.4e-05,0.068,0.0056,-0.11,-0.2,-0.047,0.46,0.00079,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.73,0.87,0.0066,6.1,7.7,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1 +38890000,0.64,-0.0071,0.021,0.77,-4.2,-5.4,0.63,-1e+06,1.2e+04,-3.7e+02,-0.0012,-0.0056,6.4e-05,0.068,0.0057,-0.11,-0.2,-0.047,0.46,0.00079,-0.0013,-0.025,0,0,-3.7e+02,0.00011,8.5e-05,0.001,0.75,0.89,0.0066,6.5,8.2,0.035,3.1e-07,4.3e-07,1.8e-06,0.0046,0.0049,9.2e-05,1.4e-05,4.1e-06,0.00038,3.8e-06,2.9e-06,0.00037,1,1,1.1 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 6b71f5f6d749..d1b4885f8f03 100644 --- a/src/modules/ekf2/test/change_indication/iris_gps.csv +++ b/src/modules/ekf2/test/change_indication/iris_gps.csv @@ -1,351 +1,351 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23] -10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-2.1e-11,0,0,-1.1e-06,0,0,0,0,0,0,0,0,0.011,0.011,0.00094,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-2.1e-10,0,0,-1e-05,0,0,0,0,0,0,0,0,0.012,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,1.5e-11,0,0,2.2e-06,0,0,0,0,0,0,0,0,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.4e-07,4.1e-08,0,0,-1e-06,0,0,0,0,0,0,0,0,0.013,0.013,0.00073,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.7e-07,4.5e-08,0,0,7.8e-05,0,0,0,0,0,0,0,0,0.015,0.015,0.001,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0024,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.016,0.016,0.00063,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,0.018,0.018,0.0008,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.0012,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1e-06,4.9e-07,0,0,-8.1e-05,0,0,0,0,0,0,0,0,0.019,0.019,0.00054,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1e-06,4.9e-07,0,0,-2.6e-05,0,0,0,0,0,0,0,0,0.021,0.021,0.00066,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00068,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,9.9e-07,0,0,1.1e-05,0,0,0,0,0,0,0,0,0.023,0.023,0.00047,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00043,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,9.7e-07,0,0,-0.00056,0,0,0,0,0,0,0,0,0.025,0.025,0.00056,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00043,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1290000,1,-0.012,-0.014,0.00042,0.019,-0.017,-0.11,0.0019,-0.0024,-0.048,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00083,0,0,0,0,0,0,0,0,0.026,0.026,0.00042,0.88,0.88,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.097,0.0043,-0.0044,-0.038,-0.00016,-9.2e-05,1.5e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.028,0.028,0.00049,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00029,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.12,0.0034,-0.0032,-0.053,-0.00039,-0.00033,1.3e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,0.027,0.027,0.00038,0.95,0.95,0.27,0.14,0.14,0.15,0.0089,0.0089,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1590000,1,-0.012,-0.014,0.00039,0.031,-0.024,-0.13,0.0061,-0.0055,-0.063,-0.00039,-0.00033,1.3e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.03,0.03,0.00043,1.3,1.3,0.23,0.21,0.21,0.14,0.0089,0.0089,0.0002,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1690000,1,-0.012,-0.014,0.00044,0.028,-0.019,-0.13,0.0043,-0.0036,-0.068,-0.00073,-0.00073,-8.2e-08,0,0,-0.0019,0,0,0,0,0,0,0,0,0.026,0.026,0.00034,1,1,0.19,0.14,0.14,0.13,0.0079,0.0079,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0.0075,-0.0059,-0.067,-0.00072,-0.00072,-3.8e-08,0,0,-0.0029,0,0,0,0,0,0,0,0,0.028,0.028,0.00039,1.3,1.3,0.17,0.21,0.21,0.12,0.0079,0.0079,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0.011,-0.0083,-0.075,-0.00072,-0.00072,-1.2e-08,0,0,-0.0033,0,0,0,0,0,0,0,0,0.031,0.031,0.00043,1.7,1.7,0.15,0.31,0.31,0.12,0.0079,0.0079,0.00015,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0.0081,-0.0053,-0.074,-0.0011,-0.0012,-3.1e-06,0,0,-0.0047,0,0,0,0,0,0,0,0,0.025,0.025,0.00035,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,0.00011,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0.012,-0.0073,-0.071,-0.0011,-0.0012,-3e-06,0,0,-0.0066,0,0,0,0,0,0,0,0,0.027,0.027,0.00039,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,0.00011,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2190000,1,-0.011,-0.014,0.00039,0.033,-0.013,-0.14,0.0081,-0.0043,-0.077,-0.0014,-0.0018,-7.8e-06,0,0,-0.0076,0,0,0,0,0,0,0,0,0.02,0.021,0.00031,1.2,1.2,0.11,0.2,0.2,0.11,0.0056,0.0056,9e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2290000,1,-0.011,-0.014,0.00038,0.038,-0.014,-0.14,0.012,-0.0057,-0.075,-0.0014,-0.0018,-7.6e-06,0,0,-0.0099,0,0,0,0,0,0,0,0,0.022,0.022,0.00034,1.5,1.5,0.11,0.3,0.3,0.1,0.0056,0.0056,9e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2390000,1,-0.011,-0.013,0.0004,0.029,-0.0098,-0.14,0.0075,-0.0033,-0.072,-0.0017,-0.0023,-1.3e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.017,0.017,0.00028,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,7.1e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2490000,1,-0.011,-0.014,0.00047,0.033,-0.0088,-0.14,0.011,-0.0042,-0.079,-0.0017,-0.0023,-1.3e-05,0,0,-0.014,0,0,0,0,0,0,0,0,0.018,0.018,0.00031,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,7.1e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2590000,1,-0.01,-0.013,0.00039,0.023,-0.006,-0.15,0.0066,-0.0023,-0.084,-0.0018,-0.0027,-1.9e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.014,0.014,0.00026,0.88,0.88,0.099,0.18,0.18,0.094,0.0038,0.0038,5.8e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2690000,1,-0.01,-0.013,0.00043,0.027,-0.0051,-0.15,0.0091,-0.0029,-0.084,-0.0018,-0.0027,-1.8e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.015,0.015,0.00028,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,5.8e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2790000,1,-0.01,-0.013,0.00037,0.022,-0.0029,-0.14,0.0059,-0.0016,-0.081,-0.0019,-0.003,-2.3e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.012,0.012,0.00024,0.75,0.75,0.095,0.16,0.16,0.089,0.0032,0.0032,4.8e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2890000,1,-0.01,-0.013,0.0003,0.026,-0.0046,-0.14,0.0082,-0.002,-0.081,-0.0019,-0.003,-2.3e-05,0,0,-0.026,0,0,0,0,0,0,0,0,0.013,0.013,0.00026,0.93,0.93,0.096,0.23,0.23,0.089,0.0032,0.0032,4.8e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -2990000,1,-0.01,-0.013,0.00031,0.02,-0.0035,-0.15,0.0054,-0.0012,-0.086,-0.002,-0.0033,-2.8e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.01,0.01,0.00022,0.66,0.66,0.095,0.15,0.15,0.088,0.0027,0.0027,4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3090000,1,-0.01,-0.013,0.00052,0.025,-0.0063,-0.15,0.0077,-0.0018,-0.087,-0.002,-0.0033,-2.8e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.011,0.011,0.00024,0.81,0.81,0.095,0.22,0.22,0.086,0.0027,0.0027,4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3190000,1,-0.01,-0.013,0.00056,0.02,-0.0061,-0.15,0.0051,-0.0013,-0.097,-0.002,-0.0036,-3.2e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.0089,0.0089,0.00021,0.58,0.58,0.096,0.14,0.14,0.087,0.0023,0.0023,3.4e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3290000,1,-0.01,-0.013,0.00059,0.023,-0.0062,-0.15,0.0073,-0.002,-0.11,-0.002,-0.0036,-3.2e-05,0,0,-0.035,0,0,0,0,0,0,0,0,0.0097,0.0097,0.00022,0.71,0.71,0.095,0.2,0.2,0.086,0.0023,0.0023,3.4e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3390000,1,-0.0098,-0.013,0.0006,0.019,-0.0032,-0.15,0.005,-0.0013,-0.1,-0.0021,-0.0038,-3.5e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.0079,0.0079,0.00019,0.52,0.52,0.095,0.14,0.14,0.085,0.002,0.002,2.9e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3490000,1,-0.0097,-0.013,0.00058,0.025,-0.0018,-0.15,0.0072,-0.0015,-0.1,-0.0021,-0.0038,-3.5e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.0086,0.0086,0.00021,0.64,0.64,0.095,0.19,0.19,0.086,0.002,0.002,2.9e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3590000,1,-0.0095,-0.012,0.00054,0.021,-0.0013,-0.15,0.0051,-0.0009,-0.11,-0.0022,-0.004,-3.9e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.0071,0.0071,0.00018,0.48,0.48,0.094,0.13,0.13,0.086,0.0017,0.0017,2.5e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3690000,1,-0.0095,-0.013,0.00052,0.024,-0.0006,-0.15,0.0074,-0.0011,-0.11,-0.0021,-0.004,-3.9e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.0077,0.0077,0.00019,0.58,0.58,0.093,0.17,0.17,0.085,0.0017,0.0017,2.5e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3790000,1,-0.0094,-0.012,0.00055,0.02,0.0038,-0.15,0.0051,-0.00045,-0.11,-0.0022,-0.0043,-4.4e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.0063,0.0063,0.00017,0.44,0.44,0.093,0.12,0.12,0.086,0.0014,0.0014,2.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3890000,1,-0.0094,-0.012,0.00063,0.021,0.0051,-0.14,0.0072,3.2e-06,-0.11,-0.0022,-0.0043,-4.3e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.0069,0.0069,0.00018,0.54,0.54,0.091,0.16,0.16,0.086,0.0014,0.0014,2.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -3990000,1,-0.0094,-0.013,0.0007,0.026,0.0049,-0.14,0.0096,0.00045,-0.11,-0.0022,-0.0043,-4.3e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.0075,0.0075,0.00019,0.65,0.65,0.089,0.22,0.22,0.085,0.0014,0.0014,2.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4090000,1,-0.0093,-0.012,0.00076,0.022,0.0042,-0.12,0.0071,0.00064,-0.098,-0.0022,-0.0045,-4.8e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.0061,0.0061,0.00017,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1.9e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4190000,1,-0.0094,-0.012,0.00073,0.024,0.004,-0.12,0.0094,0.0011,-0.1,-0.0022,-0.0045,-4.8e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.0066,0.0066,0.00018,0.6,0.6,0.086,0.21,0.21,0.086,0.0012,0.0012,1.9e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4290000,1,-0.0095,-0.012,0.00074,0.021,0.0038,-0.12,0.0068,0.00087,-0.11,-0.0021,-0.0047,-5.3e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.0054,0.0054,0.00016,0.46,0.46,0.084,0.15,0.15,0.085,0.00095,0.00095,1.6e-05,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4390000,1,-0.0094,-0.012,0.0007,0.025,0.0023,-0.11,0.0091,0.0011,-0.094,-0.0021,-0.0047,-5.3e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.0058,0.0058,0.00017,0.55,0.55,0.081,0.2,0.2,0.084,0.00095,0.00095,1.6e-05,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4490000,1,-0.0094,-0.012,0.00076,0.02,0.004,-0.11,0.0067,0.00089,-0.095,-0.0021,-0.0048,-5.7e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.0047,0.0047,0.00016,0.43,0.43,0.08,0.14,0.14,0.085,0.00077,0.00077,1.5e-05,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4590000,1,-0.0094,-0.012,0.00083,0.023,0.0029,-0.11,0.0089,0.0012,-0.098,-0.0021,-0.0048,-5.7e-05,0,0,-0.088,0,0,0,0,0,0,0,0,0.005,0.005,0.00016,0.51,0.51,0.077,0.19,0.19,0.084,0.00077,0.00077,1.5e-05,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4690000,1,-0.0094,-0.012,0.00076,0.017,0.003,-0.1,0.0065,0.0009,-0.09,-0.0021,-0.005,-6.2e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.0041,0.0041,0.00015,0.39,0.39,0.074,0.14,0.14,0.083,0.00062,0.00062,1.3e-05,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4790000,1,-0.0093,-0.012,0.00086,0.015,0.0052,-0.099,0.008,0.0014,-0.092,-0.0021,-0.005,-6.2e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.0044,0.0044,0.00016,0.47,0.47,0.073,0.18,0.18,0.084,0.00062,0.00062,1.3e-05,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4890000,1,-0.0093,-0.012,0.0009,0.0099,0.0026,-0.093,0.0053,0.001,-0.088,-0.0021,-0.0052,-6.5e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.0035,0.0035,0.00014,0.36,0.36,0.07,0.13,0.13,0.083,0.00049,0.00049,1.1e-05,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -4990000,1,-0.0092,-0.012,0.00089,0.013,0.0033,-0.085,0.0065,0.0014,-0.083,-0.0021,-0.0051,-6.5e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.0037,0.0037,0.00015,0.43,0.43,0.067,0.17,0.17,0.082,0.00049,0.00049,1.1e-05,0.04,0.04,0.01,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5090000,1,-0.0091,-0.011,0.00096,0.01,0.0035,-0.082,0.0045,0.00098,-0.081,-0.002,-0.0053,-6.8e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.003,0.003,0.00014,0.34,0.34,0.065,0.12,0.12,0.082,0.00039,0.00039,1e-05,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5190000,1,-0.009,-0.012,0.001,0.0096,0.0071,-0.08,0.0055,0.0015,-0.079,-0.002,-0.0053,-6.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0032,0.0032,0.00014,0.4,0.4,0.063,0.16,0.16,0.081,0.00039,0.00039,1e-05,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5290000,1,-0.0089,-0.011,0.0011,0.0079,0.0071,-0.068,0.0038,0.0013,-0.072,-0.002,-0.0053,-7.1e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0026,0.0026,0.00013,0.31,0.31,0.06,0.12,0.12,0.08,0.00031,0.00031,9.3e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5390000,1,-0.0088,-0.011,0.0011,0.0074,0.011,-0.065,0.0046,0.0022,-0.067,-0.002,-0.0053,-7.1e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0027,0.0027,0.00014,0.36,0.36,0.057,0.15,0.15,0.079,0.00031,0.00031,9.3e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5490000,1,-0.0089,-0.011,0.0011,0.0069,0.012,-0.06,0.0031,0.002,-0.065,-0.002,-0.0054,-7.3e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.0022,0.0022,0.00013,0.28,0.28,0.056,0.11,0.11,0.079,0.00025,0.00025,8.4e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5590000,1,-0.0089,-0.011,0.00099,0.008,0.015,-0.053,0.0039,0.0033,-0.058,-0.002,-0.0054,-7.3e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0023,0.0023,0.00013,0.33,0.33,0.053,0.15,0.15,0.078,0.00025,0.00025,8.4e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5690000,1,-0.0089,-0.011,0.0009,0.0074,0.015,-0.052,0.0028,0.0029,-0.055,-0.0019,-0.0054,-7.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0019,0.0019,0.00012,0.26,0.26,0.051,0.11,0.11,0.076,0.00019,0.00019,7.6e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5790000,1,-0.0088,-0.011,0.00085,0.0087,0.017,-0.049,0.0036,0.0045,-0.053,-0.0019,-0.0054,-7.5e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.002,0.002,0.00013,0.3,0.3,0.05,0.14,0.14,0.077,0.00019,0.00019,7.6e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5890000,1,-0.0088,-0.011,0.00089,0.0092,0.015,-0.048,0.0027,0.0037,-0.056,-0.0018,-0.0055,-7.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00012,0.24,0.24,0.047,0.1,0.1,0.075,0.00015,0.00015,6.9e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -5990000,1,-0.0088,-0.011,0.00086,0.011,0.016,-0.041,0.0037,0.0052,-0.05,-0.0018,-0.0055,-7.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0017,0.0017,0.00012,0.27,0.27,0.045,0.13,0.13,0.074,0.00015,0.00015,6.9e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6090000,1,-0.0088,-0.011,0.00067,0.011,0.018,-0.039,0.0048,0.0069,-0.047,-0.0018,-0.0055,-7.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0018,0.0018,0.00013,0.32,0.32,0.044,0.17,0.17,0.074,0.00015,0.00015,6.9e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6190000,1,-0.0089,-0.011,0.00068,0.0085,0.016,-0.038,0.0037,0.0056,-0.047,-0.0018,-0.0055,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0015,0.0015,0.00012,0.25,0.25,0.042,0.13,0.13,0.073,0.00012,0.00012,6.3e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6290000,1,-0.0089,-0.011,0.00071,0.0078,0.019,-0.041,0.0046,0.0073,-0.053,-0.0018,-0.0055,-8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.0016,0.0016,0.00012,0.29,0.29,0.04,0.16,0.16,0.072,0.00012,0.00012,6.3e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1 -6390000,-0.29,0.025,-0.0062,0.96,-0.0089,0.0093,-0.042,0.0041,0.0051,-0.056,-0.0017,-0.0055,-8.2e-05,0,0,-0.12,-0.095,-0.021,0.51,0.072,-0.027,-0.063,0,0,0.0012,0.0012,0.066,0.2,0.2,0.039,0.12,0.12,0.072,9.9e-05,9.9e-05,5.8e-06,0.04,0.04,0.0038,0.0014,0.00037,0.0014,0.0014,0.0011,0.0014,1,1 -6490000,-0.29,0.026,-0.0062,0.96,-0.027,0.0035,-0.039,0.0048,0.0045,-0.053,-0.0017,-0.0054,-8.1e-05,0,0,-0.13,-0.1,-0.022,0.51,0.076,-0.028,-0.067,0,0,0.0012,0.0012,0.056,0.2,0.2,0.038,0.15,0.15,0.07,9.9e-05,9.9e-05,5.8e-06,0.04,0.04,0.0036,0.0013,0.00021,0.0013,0.0013,0.00096,0.0013,1,1 -6590000,-0.29,0.026,-0.0062,0.96,-0.047,-0.0063,-0.041,0.004,0.0029,-0.056,-0.0016,-0.0053,-8e-05,-0.0003,0.00015,-0.13,-0.1,-0.022,0.5,0.077,-0.029,-0.068,0,0,0.0012,0.0012,0.054,0.21,0.21,0.036,0.18,0.18,0.069,9.9e-05,9.9e-05,5.8e-06,0.04,0.04,0.0033,0.0013,0.00016,0.0013,0.0013,0.00093,0.0013,1,1 -6690000,-0.29,0.026,-0.0061,0.96,-0.066,-0.015,-0.043,0.00029,0.00029,-0.057,-0.0015,-0.0052,-8e-05,-0.00055,0.00035,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0012,0.0012,0.052,0.21,0.21,0.035,0.22,0.22,0.068,9.9e-05,9.9e-05,5.8e-06,0.04,0.04,0.0031,0.0013,0.00014,0.0013,0.0013,0.00092,0.0013,1,1 -6790000,-0.29,0.026,-0.0062,0.96,-0.087,-0.026,-0.041,-0.0024,-0.0042,-0.057,-0.0015,-0.0051,-7.9e-05,-0.0011,0.00062,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0012,0.051,0.22,0.22,0.034,0.26,0.26,0.068,9.9e-05,9.8e-05,5.8e-06,0.04,0.04,0.003,0.0013,0.00013,0.0013,0.0013,0.00091,0.0013,1,1 -6890000,-0.29,0.026,-0.006,0.96,-0.11,-0.032,-0.037,-0.0093,-0.0085,-0.055,-0.0014,-0.005,-7.8e-05,-0.0014,0.00077,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0012,0.051,0.23,0.23,0.032,0.3,0.3,0.067,9.8e-05,9.8e-05,5.8e-06,0.04,0.04,0.0028,0.0013,0.00012,0.0013,0.0013,0.00091,0.0013,1,1 -6990000,-0.29,0.026,-0.0059,0.96,-0.13,-0.04,-0.035,-0.018,-0.013,-0.054,-0.0014,-0.0049,-7.7e-05,-0.0017,0.00088,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0013,0.05,0.24,0.24,0.031,0.35,0.35,0.066,9.7e-05,9.7e-05,5.8e-06,0.04,0.04,0.0026,0.0013,0.00011,0.0013,0.0013,0.00091,0.0013,1,1 -7090000,-0.29,0.026,-0.0058,0.96,-0.15,-0.051,-0.035,-0.031,-0.018,-0.055,-0.0014,-0.0049,-7.6e-05,-0.0018,0.00089,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0013,0.05,0.25,0.25,0.03,0.4,0.4,0.066,9.6e-05,9.6e-05,5.8e-06,0.04,0.04,0.0024,0.0013,0.0001,0.0013,0.0013,0.0009,0.0013,1,1 -7190000,-0.29,0.026,-0.0058,0.96,-0.18,-0.061,-0.034,-0.045,-0.027,-0.058,-0.0014,-0.0048,-7.7e-05,-0.0021,0.0011,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0013,0.0013,0.05,0.27,0.27,0.029,0.46,0.46,0.065,9.5e-05,9.5e-05,5.8e-06,0.04,0.04,0.0023,0.0013,9.8e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7290000,-0.29,0.026,-0.0057,0.96,-0.2,-0.07,-0.031,-0.065,-0.032,-0.053,-0.0014,-0.0049,-7.7e-05,-0.002,0.0011,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0013,0.05,0.29,0.29,0.028,0.51,0.51,0.064,9.4e-05,9.3e-05,5.8e-06,0.04,0.04,0.0022,0.0013,9.4e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7390000,-0.29,0.026,-0.0056,0.96,-0.23,-0.077,-0.029,-0.089,-0.037,-0.051,-0.0014,-0.0049,-7.7e-05,-0.0019,0.00095,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0013,0.0013,0.049,0.31,0.31,0.027,0.58,0.58,0.064,9.2e-05,9.2e-05,5.8e-06,0.04,0.04,0.002,0.0013,9.1e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7490000,-0.29,0.026,-0.0056,0.96,-0.25,-0.086,-0.023,-0.1,-0.044,-0.044,-0.0014,-0.0047,-7.1e-05,-0.0026,0.00089,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0014,0.0013,0.049,0.34,0.33,0.026,0.64,0.64,0.063,9e-05,9e-05,5.8e-06,0.04,0.04,0.0019,0.0013,8.9e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7590000,-0.29,0.026,-0.0056,0.96,-0.27,-0.096,-0.019,-0.12,-0.055,-0.039,-0.0014,-0.0047,-6.9e-05,-0.003,0.001,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0014,0.0013,0.049,0.37,0.36,0.025,0.71,0.71,0.062,8.8e-05,8.7e-05,5.8e-06,0.04,0.04,0.0018,0.0013,8.7e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7690000,-0.29,0.027,-0.0056,0.96,-0.3,-0.11,-0.018,-0.15,-0.068,-0.035,-0.0014,-0.0046,-6.9e-05,-0.0034,0.0012,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0014,0.0014,0.049,0.4,0.39,0.025,0.79,0.79,0.062,8.6e-05,8.5e-05,5.8e-06,0.04,0.04,0.0017,0.0013,8.5e-05,0.0013,0.0013,0.0009,0.0013,1,1 -7790000,-0.29,0.027,-0.0056,0.96,-0.32,-0.12,-0.02,-0.16,-0.091,-0.039,-0.0012,-0.0043,-6.8e-05,-0.0043,0.0017,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.43,0.43,0.024,0.87,0.87,0.061,8.3e-05,8.2e-05,5.8e-06,0.04,0.04,0.0016,0.0013,8.4e-05,0.0013,0.0013,0.00089,0.0013,1,1 -7890000,-0.29,0.027,-0.0056,0.96,-0.34,-0.13,-0.021,-0.19,-0.11,-0.042,-0.0012,-0.0043,-6.9e-05,-0.0043,0.0018,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0014,0.0014,0.049,0.47,0.46,0.023,0.96,0.96,0.06,8e-05,7.9e-05,5.8e-06,0.04,0.04,0.0015,0.0013,8.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 -7990000,-0.29,0.027,-0.0055,0.96,-0.37,-0.14,-0.017,-0.24,-0.11,-0.039,-0.0013,-0.0045,-7.1e-05,-0.0038,0.0016,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,0.51,0.5,0.022,1.1,1.1,0.059,7.7e-05,7.5e-05,5.8e-06,0.04,0.04,0.0015,0.0013,8.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8090000,-0.29,0.027,-0.0055,0.96,-0.4,-0.15,-0.017,-0.28,-0.14,-0.041,-0.0011,-0.0045,-7.7e-05,-0.0036,0.0021,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,0.56,0.55,0.022,1.2,1.2,0.059,7.4e-05,7.2e-05,5.8e-06,0.04,0.04,0.0014,0.0013,8e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8190000,-0.29,0.027,-0.0057,0.96,-0.024,-0.01,-0.012,-0.29,-0.14,-0.035,-0.001,-0.0045,-8.1e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0014,0.049,25,25,0.021,1e+02,1e+02,0.058,7e-05,6.9e-05,5.7e-06,0.04,0.04,0.0013,0.0013,7.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8290000,-0.29,0.027,-0.0057,0.96,-0.051,-0.021,-0.011,-0.29,-0.14,-0.035,-0.0011,-0.0048,-8.6e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.03,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.02,1e+02,1e+02,0.057,6.7e-05,6.5e-05,5.7e-06,0.04,0.04,0.0013,0.0013,7.8e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8390000,-0.29,0.027,-0.0056,0.96,-0.075,-0.028,-0.01,-0.3,-0.14,-0.033,-0.0011,-0.0049,-8.8e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.02,51,51,0.057,6.4e-05,6.2e-05,5.7e-06,0.04,0.04,0.0012,0.0013,7.7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8490000,-0.29,0.027,-0.0054,0.96,-0.099,-0.038,-0.011,-0.3,-0.15,-0.038,-0.0013,-0.005,-8.6e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0015,0.0015,0.049,25,25,0.019,52,52,0.056,6.1e-05,5.9e-05,5.7e-06,0.04,0.04,0.0011,0.0013,7.7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8590000,-0.29,0.027,-0.0054,0.96,-0.13,-0.047,-0.006,-0.31,-0.15,-0.032,-0.0011,-0.0049,-8.7e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0016,0.0015,0.049,25,25,0.019,35,35,0.055,5.7e-05,5.6e-05,5.7e-06,0.04,0.04,0.0011,0.0013,7.6e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8690000,-0.29,0.027,-0.0054,0.96,-0.15,-0.055,-0.0078,-0.32,-0.15,-0.034,-0.0012,-0.005,-8.7e-05,-0.0036,0.0022,-0.13,-0.1,-0.022,0.5,0.078,-0.029,-0.068,0,0,0.0016,0.0015,0.049,25,25,0.018,37,37,0.055,5.4e-05,5.3e-05,5.7e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8790000,-0.29,0.027,-0.0055,0.96,-0.18,-0.062,-0.0075,-0.33,-0.15,-0.031,-0.0012,-0.0051,-9.2e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,24,24,0.018,28,28,0.055,5.1e-05,5e-05,5.7e-06,0.04,0.04,0.001,0.0013,7.5e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8890000,-0.29,0.027,-0.0054,0.96,-0.2,-0.074,-0.0029,-0.35,-0.16,-0.025,-0.0012,-0.005,-8.7e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,24,24,0.017,30,30,0.054,4.8e-05,4.7e-05,5.7e-06,0.04,0.04,0.00095,0.0013,7.4e-05,0.0013,0.0013,0.00089,0.0013,1,1 -8990000,-0.29,0.027,-0.0052,0.96,-0.22,-0.085,-0.0018,-0.35,-0.16,-0.027,-0.0013,-0.0048,-8e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,23,23,0.017,25,25,0.054,4.6e-05,4.4e-05,5.7e-06,0.04,0.04,0.00092,0.0013,7.4e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9090000,-0.29,0.027,-0.005,0.96,-0.24,-0.098,-0.003,-0.37,-0.17,-0.027,-0.0014,-0.005,-7.8e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,23,23,0.017,27,27,0.053,4.3e-05,4.1e-05,5.7e-06,0.04,0.04,0.00088,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9190000,-0.29,0.027,-0.0049,0.96,-0.26,-0.11,-0.0023,-0.38,-0.18,-0.028,-0.0015,-0.005,-7.5e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.03,-0.069,0,0,0.0016,0.0015,0.049,21,21,0.016,23,23,0.053,4e-05,3.9e-05,5.7e-06,0.04,0.04,0.00084,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9290000,-0.29,0.027,-0.0047,0.96,-0.29,-0.12,-0.0004,-0.41,-0.19,-0.025,-0.0015,-0.0049,-7.3e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,21,21,0.016,26,26,0.052,3.8e-05,3.6e-05,5.7e-06,0.04,0.04,0.00081,0.0013,7.3e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9390000,-0.29,0.027,-0.0048,0.96,-0.3,-0.12,0.00091,-0.41,-0.19,-0.025,-0.0014,-0.0049,-7.8e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,19,19,0.015,23,23,0.052,3.6e-05,3.4e-05,5.7e-06,0.04,0.04,0.00078,0.0013,7.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9490000,-0.29,0.027,-0.0048,0.96,-0.33,-0.13,0.0026,-0.44,-0.2,-0.022,-0.0014,-0.005,-7.9e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,19,19,0.015,25,25,0.051,3.3e-05,3.2e-05,5.7e-06,0.04,0.04,0.00075,0.0013,7.2e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9590000,-0.29,0.027,-0.0051,0.96,-0.33,-0.12,0.0027,-0.43,-0.19,-0.023,-0.0013,-0.0051,-8.8e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,17,17,0.015,22,22,0.051,3.1e-05,3e-05,5.7e-06,0.04,0.04,0.00072,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9690000,-0.29,0.027,-0.0052,0.96,-0.36,-0.13,0.0057,-0.47,-0.21,-0.022,-0.0012,-0.0051,-9e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,17,17,0.014,25,25,0.051,3e-05,2.8e-05,5.7e-06,0.04,0.04,0.0007,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9790000,-0.29,0.027,-0.0054,0.96,-0.36,-0.12,0.0043,-0.46,-0.2,-0.023,-0.0011,-0.0053,-9.8e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,15,15,0.014,22,22,0.05,2.8e-05,2.7e-05,5.7e-06,0.04,0.04,0.00068,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9890000,-0.29,0.027,-0.0054,0.96,-0.39,-0.13,0.0058,-0.49,-0.21,-0.023,-0.0011,-0.0052,-9.6e-05,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,15,15,0.014,25,25,0.049,2.6e-05,2.5e-05,5.7e-06,0.04,0.04,0.00065,0.0013,7.1e-05,0.0013,0.0013,0.00089,0.0013,1,1 -9990000,-0.29,0.027,-0.0056,0.96,-0.38,-0.12,0.0065,-0.48,-0.2,-0.025,-0.001,-0.0053,-0.0001,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,13,13,0.014,22,22,0.049,2.5e-05,2.3e-05,5.7e-06,0.04,0.04,0.00063,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10090000,-0.29,0.027,-0.0057,0.96,-0.41,-0.12,0.0077,-0.52,-0.21,-0.023,-0.00092,-0.0054,-0.00011,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,14,14,0.013,24,24,0.049,2.3e-05,2.2e-05,5.7e-06,0.04,0.04,0.00061,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10190000,-0.29,0.027,-0.006,0.96,-0.44,-0.12,0.0086,-0.57,-0.22,-0.024,-0.00077,-0.0053,-0.00011,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,14,14,0.013,27,27,0.048,2.2e-05,2.1e-05,5.7e-06,0.04,0.04,0.00059,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10290000,-0.29,0.027,-0.006,0.96,-0.43,-0.12,0.0077,-0.55,-0.22,-0.023,-0.00081,-0.0053,-0.00011,-0.0036,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,12,12,0.013,24,24,0.048,2.1e-05,2e-05,5.7e-06,0.04,0.04,0.00058,0.0013,7e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10390000,-0.29,0.027,-0.0059,0.96,-0.011,-0.0087,-0.0026,-6e-05,-0.0003,-0.022,-0.00084,-0.0053,-0.00011,-0.0038,0.002,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,0.25,0.25,0.56,0.25,0.25,0.049,1.9e-05,1.8e-05,5.6e-06,0.04,0.04,0.00056,0.0013,6.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10490000,-0.29,0.027,-0.006,0.96,-0.04,-0.015,0.0064,-0.0026,-0.0014,-0.018,-0.00075,-0.0053,-0.00011,-0.0038,0.0024,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0016,0.0015,0.049,0.26,0.26,0.55,0.26,0.26,0.058,1.8e-05,1.7e-05,5.6e-06,0.04,0.04,0.00055,0.0013,6.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10590000,-0.29,0.027,-0.0059,0.96,-0.051,-0.014,0.012,-0.0032,-0.001,-0.016,-0.00082,-0.0053,-0.00011,-0.0032,0.002,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0015,0.0015,0.049,0.13,0.13,0.27,0.26,0.26,0.056,1.7e-05,1.6e-05,5.6e-06,0.039,0.039,0.00054,0.0013,6.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10690000,-0.29,0.027,-0.0059,0.96,-0.08,-0.02,0.015,-0.0097,-0.0027,-0.013,-0.0008,-0.0053,-0.00011,-0.0032,0.0022,-0.14,-0.1,-0.022,0.5,0.078,-0.029,-0.069,0,0,0.0015,0.0015,0.049,0.14,0.14,0.26,0.27,0.27,0.065,1.6e-05,1.6e-05,5.6e-06,0.039,0.039,0.00053,0.0013,6.9e-05,0.0013,0.0013,0.00089,0.0013,1,1 -10790000,-0.29,0.027,-0.0058,0.96,-0.078,-0.023,0.013,3.4e-06,-0.0018,-0.012,-0.00081,-0.0053,-0.00011,-0.00092,0.001,-0.14,-0.1,-0.022,0.5,0.079,-0.03,-0.069,0,0,0.0015,0.0014,0.049,0.096,0.096,0.17,0.13,0.13,0.062,1.5e-05,1.5e-05,5.6e-06,0.039,0.039,0.00052,0.0013,6.9e-05,0.0013,0.0013,0.00088,0.0013,1,1 -10890000,-0.29,0.027,-0.0056,0.96,-0.1,-0.033,0.009,-0.009,-0.0047,-0.015,-0.00091,-0.0053,-0.00011,-0.00093,0.0007,-0.14,-0.1,-0.022,0.5,0.079,-0.03,-0.069,0,0,0.0015,0.0014,0.049,0.11,0.11,0.16,0.14,0.14,0.068,1.4e-05,1.4e-05,5.6e-06,0.039,0.039,0.00051,0.0013,6.8e-05,0.0013,0.0013,0.00088,0.0013,1,1 -10990000,-0.29,0.026,-0.0056,0.96,-0.093,-0.034,0.015,-0.0033,-0.0023,-0.0093,-0.00093,-0.0055,-0.00011,0.0039,-0.00059,-0.14,-0.1,-0.023,0.5,0.079,-0.03,-0.069,0,0,0.0014,0.0013,0.049,0.084,0.084,0.12,0.091,0.091,0.065,1.4e-05,1.3e-05,5.6e-06,0.038,0.038,0.00051,0.0013,6.8e-05,0.0013,0.0013,0.00087,0.0013,1,1 -11090000,-0.29,0.026,-0.006,0.96,-0.12,-0.041,0.019,-0.014,-0.0058,-0.0055,-0.00082,-0.0054,-0.00011,0.0038,-0.0003,-0.14,-0.1,-0.023,0.5,0.079,-0.03,-0.069,0,0,0.0014,0.0013,0.048,0.097,0.098,0.11,0.097,0.097,0.069,1.3e-05,1.2e-05,5.6e-06,0.038,0.038,0.0005,0.0013,6.8e-05,0.0013,0.0013,0.00087,0.0013,1,1 -11190000,-0.29,0.025,-0.0062,0.96,-0.1,-0.036,0.026,-0.0063,-0.0032,0.00088,-0.00081,-0.0056,-0.00012,0.011,-0.0022,-0.14,-0.1,-0.023,0.5,0.079,-0.03,-0.069,0,0,0.0013,0.0012,0.048,0.08,0.08,0.083,0.072,0.072,0.066,1.2e-05,1.1e-05,5.6e-06,0.037,0.037,0.0005,0.0013,6.7e-05,0.0013,0.0012,0.00086,0.0013,1,1 -11290000,-0.29,0.025,-0.0062,0.96,-0.12,-0.04,0.025,-0.017,-0.0069,0.00092,-0.00081,-0.0056,-0.00012,0.011,-0.0022,-0.14,-0.1,-0.023,0.5,0.079,-0.03,-0.069,0,0,0.0013,0.0012,0.048,0.095,0.095,0.077,0.078,0.078,0.069,1.2e-05,1.1e-05,5.6e-06,0.037,0.037,0.0005,0.0013,6.7e-05,0.0013,0.0012,0.00086,0.0013,1,1 -11390000,-0.29,0.023,-0.0061,0.96,-0.1,-0.034,0.016,-0.0096,-0.0041,-0.008,-0.00084,-0.0057,-0.00012,0.018,-0.0041,-0.14,-0.1,-0.023,0.5,0.08,-0.03,-0.069,0,0,0.0011,0.0011,0.048,0.079,0.079,0.062,0.062,0.062,0.066,1.1e-05,1e-05,5.6e-06,0.036,0.036,0.00049,0.0012,6.7e-05,0.0013,0.0012,0.00085,0.0013,1,1 -11490000,-0.29,0.023,-0.0061,0.96,-0.12,-0.038,0.021,-0.021,-0.0078,-0.002,-0.00084,-0.0057,-0.00012,0.018,-0.0041,-0.14,-0.1,-0.023,0.5,0.08,-0.03,-0.069,0,0,0.0011,0.0011,0.048,0.094,0.094,0.057,0.069,0.069,0.067,1e-05,9.8e-06,5.6e-06,0.036,0.036,0.00049,0.0012,6.7e-05,0.0013,0.0012,0.00085,0.0013,1,1 -11590000,-0.29,0.022,-0.0062,0.96,-0.1,-0.031,0.019,-0.012,-0.0047,-0.0033,-0.00087,-0.0058,-0.00012,0.025,-0.0061,-0.14,-0.1,-0.023,0.5,0.081,-0.03,-0.069,0,0,0.001,0.00096,0.048,0.078,0.078,0.048,0.056,0.056,0.065,9.7e-06,9.2e-06,5.6e-06,0.035,0.035,0.00048,0.0012,6.6e-05,0.0013,0.0012,0.00084,0.0013,1,1 -11690000,-0.29,0.022,-0.0062,0.96,-0.12,-0.037,0.019,-0.023,-0.0081,-0.0047,-0.00089,-0.0059,-0.00012,0.025,-0.0061,-0.14,-0.1,-0.023,0.5,0.081,-0.03,-0.069,0,0,0.001,0.00096,0.048,0.092,0.093,0.044,0.063,0.063,0.066,9.2e-06,8.8e-06,5.6e-06,0.035,0.035,0.00048,0.0012,6.6e-05,0.0013,0.0012,0.00084,0.0013,1,1 -11790000,-0.29,0.021,-0.006,0.96,-0.094,-0.035,0.02,-0.013,-0.0067,-0.0019,-0.00097,-0.0059,-0.00012,0.031,-0.0081,-0.14,-0.1,-0.023,0.5,0.081,-0.03,-0.07,0,0,0.00088,0.00085,0.048,0.076,0.076,0.037,0.053,0.053,0.063,8.7e-06,8.2e-06,5.6e-06,0.034,0.034,0.00048,0.0012,6.6e-05,0.0013,0.0012,0.00083,0.0013,1,1 -11890000,-0.29,0.021,-0.0062,0.96,-0.11,-0.039,0.018,-0.023,-0.01,-0.0012,-0.00095,-0.006,-0.00012,0.031,-0.0082,-0.14,-0.1,-0.023,0.5,0.081,-0.03,-0.07,0,0,0.00088,0.00085,0.047,0.089,0.089,0.034,0.06,0.06,0.063,8.3e-06,7.9e-06,5.6e-06,0.034,0.034,0.00048,0.0012,6.6e-05,0.0013,0.0012,0.00082,0.0013,1,1 -11990000,-0.29,0.02,-0.0064,0.96,-0.087,-0.029,0.015,-0.016,-0.0067,-0.0049,-0.00096,-0.006,-0.00012,0.037,-0.0093,-0.14,-0.11,-0.023,0.5,0.082,-0.03,-0.07,0,0,0.00078,0.00076,0.047,0.077,0.077,0.03,0.062,0.062,0.061,7.9e-06,7.5e-06,5.6e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0013,0.0012,0.00081,0.0013,1,1 -12090000,-0.29,0.02,-0.0063,0.96,-0.1,-0.031,0.019,-0.026,-0.0096,0.0012,-0.00093,-0.006,-0.00012,0.037,-0.0095,-0.14,-0.11,-0.023,0.5,0.082,-0.03,-0.07,0,0,0.00078,0.00076,0.047,0.089,0.089,0.027,0.071,0.071,0.06,7.6e-06,7.1e-06,5.6e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.00081,0.0013,1,1 -12190000,-0.29,0.019,-0.0064,0.96,-0.081,-0.019,0.018,-0.014,-0.0034,0.0031,-0.00089,-0.006,-0.00012,0.043,-0.011,-0.14,-0.11,-0.023,0.5,0.082,-0.03,-0.07,0,0,0.0007,0.00068,0.047,0.071,0.071,0.024,0.057,0.057,0.058,7.2e-06,6.8e-06,5.6e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.0008,0.0012,1,1 -12290000,-0.29,0.019,-0.0065,0.96,-0.087,-0.018,0.017,-0.022,-0.0051,0.0041,-0.00086,-0.006,-0.00012,0.043,-0.011,-0.14,-0.11,-0.023,0.5,0.082,-0.03,-0.07,0,0,0.0007,0.00068,0.047,0.081,0.081,0.022,0.065,0.065,0.058,6.9e-06,6.5e-06,5.6e-06,0.033,0.033,0.00047,0.0012,6.5e-05,0.0012,0.0012,0.0008,0.0012,1,1 -12390000,-0.29,0.018,-0.0065,0.96,-0.07,-0.013,0.015,-0.012,-0.0029,-0.0019,-0.00086,-0.006,-0.00012,0.046,-0.013,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00063,0.00061,0.047,0.066,0.066,0.02,0.054,0.054,0.056,6.6e-06,6.2e-06,5.6e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00079,0.0012,1,1 -12490000,-0.29,0.019,-0.0066,0.96,-0.077,-0.015,0.02,-0.02,-0.0042,0.00028,-0.00083,-0.006,-0.00012,0.046,-0.013,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00063,0.00061,0.047,0.075,0.075,0.018,0.062,0.062,0.055,6.3e-06,5.9e-06,5.6e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00079,0.0012,1,1 -12590000,-0.29,0.018,-0.0065,0.96,-0.063,-0.012,0.021,-0.0099,-0.004,0.0021,-0.00084,-0.0061,-0.00013,0.049,-0.014,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00058,0.00056,0.047,0.061,0.061,0.017,0.052,0.052,0.054,6.1e-06,5.7e-06,5.6e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00078,0.0012,1,1 -12690000,-0.29,0.018,-0.0065,0.96,-0.069,-0.011,0.021,-0.016,-0.0051,0.0038,-0.00083,-0.0061,-0.00013,0.049,-0.015,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00058,0.00057,0.047,0.07,0.07,0.015,0.059,0.059,0.053,5.9e-06,5.5e-06,5.6e-06,0.032,0.032,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00078,0.0012,1,1 -12790000,-0.29,0.017,-0.0063,0.96,-0.054,-0.016,0.022,-0.0096,-0.008,0.006,-0.00088,-0.006,-0.00013,0.052,-0.016,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00054,0.00053,0.047,0.061,0.061,0.014,0.061,0.061,0.051,5.6e-06,5.2e-06,5.6e-06,0.031,0.031,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00077,0.0012,1,1 -12890000,-0.29,0.017,-0.0062,0.96,-0.058,-0.017,0.023,-0.015,-0.0099,0.0091,-0.0009,-0.006,-0.00012,0.052,-0.016,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00054,0.00053,0.047,0.069,0.069,0.013,0.07,0.07,0.051,5.4e-06,5.1e-06,5.6e-06,0.031,0.031,0.00047,0.0012,6.4e-05,0.0012,0.0012,0.00077,0.0012,1,1 -12990000,-0.29,0.017,-0.0062,0.96,-0.047,-0.014,0.024,-0.0076,-0.0068,0.01,-0.00093,-0.006,-0.00012,0.054,-0.016,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00051,0.0005,0.046,0.055,0.055,0.012,0.057,0.056,0.05,5.2e-06,4.8e-06,5.6e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 -13090000,-0.29,0.017,-0.0062,0.96,-0.052,-0.016,0.021,-0.013,-0.0086,0.0093,-0.00096,-0.006,-0.00012,0.054,-0.016,-0.14,-0.11,-0.023,0.5,0.083,-0.031,-0.07,0,0,0.00051,0.0005,0.046,0.062,0.062,0.011,0.065,0.065,0.049,5e-06,4.7e-06,5.6e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 -13190000,-0.29,0.017,-0.0061,0.96,-0.043,-0.015,0.021,-0.0093,-0.0093,0.01,-0.00098,-0.006,-0.00012,0.055,-0.016,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00048,0.00047,0.046,0.055,0.055,0.011,0.066,0.066,0.047,4.9e-06,4.5e-06,5.6e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 -13290000,-0.29,0.017,-0.0062,0.96,-0.047,-0.016,0.018,-0.014,-0.011,0.0094,-0.00096,-0.006,-0.00012,0.055,-0.016,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00049,0.00048,0.046,0.061,0.061,0.01,0.075,0.075,0.047,4.7e-06,4.3e-06,5.6e-06,0.031,0.031,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00076,0.0012,1,1 -13390000,-0.29,0.017,-0.0062,0.96,-0.038,-0.012,0.018,-0.0071,-0.0072,0.01,-0.00095,-0.006,-0.00012,0.056,-0.017,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00046,0.00045,0.046,0.049,0.049,0.0094,0.06,0.06,0.046,4.5e-06,4.2e-06,5.6e-06,0.031,0.03,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 -13490000,-0.29,0.017,-0.0062,0.96,-0.041,-0.014,0.018,-0.011,-0.0086,0.0064,-0.00094,-0.006,-0.00012,0.057,-0.017,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00046,0.00045,0.046,0.054,0.054,0.009,0.068,0.068,0.045,4.4e-06,4e-06,5.6e-06,0.031,0.03,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 -13590000,-0.29,0.017,-0.0062,0.96,-0.033,-0.01,0.019,-0.0033,-0.0058,0.005,-0.00092,-0.006,-0.00012,0.058,-0.018,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00045,0.00044,0.046,0.044,0.044,0.0086,0.055,0.055,0.044,4.3e-06,3.9e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.3e-05,0.0012,0.0012,0.00075,0.0012,1,1 -13690000,-0.29,0.016,-0.0061,0.96,-0.035,-0.01,0.019,-0.0067,-0.007,0.0078,-0.00094,-0.006,-0.00012,0.058,-0.017,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00045,0.00044,0.046,0.049,0.049,0.0082,0.063,0.063,0.044,4.1e-06,3.8e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00075,0.0012,1,1 -13790000,-0.29,0.016,-0.0062,0.96,-0.027,-0.0076,0.019,0.00083,-0.0036,0.0074,-0.00093,-0.006,-0.00012,0.059,-0.018,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00043,0.00042,0.046,0.041,0.041,0.0078,0.052,0.052,0.042,4e-06,3.6e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -13890000,-0.29,0.016,-0.0061,0.96,-0.029,-0.009,0.02,-0.0018,-0.0046,0.0097,-0.00096,-0.006,-0.00012,0.059,-0.018,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00043,0.00042,0.046,0.045,0.045,0.0076,0.059,0.059,0.042,3.9e-06,3.5e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -13990000,-0.29,0.016,-0.006,0.96,-0.028,-0.012,0.019,-0.00049,-0.005,0.0087,-0.00098,-0.006,-0.00012,0.06,-0.018,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00042,0.00041,0.046,0.038,0.038,0.0073,0.05,0.05,0.041,3.7e-06,3.4e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -14090000,-0.29,0.016,-0.0062,0.96,-0.028,-0.0066,0.021,-0.0036,-0.0055,0.0053,-0.00091,-0.006,-0.00012,0.06,-0.018,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00042,0.00041,0.046,0.042,0.042,0.0072,0.057,0.057,0.041,3.6e-06,3.3e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -14190000,-0.29,0.016,-0.0062,0.96,-0.023,-0.0051,0.021,-0.00069,-0.0042,0.0056,-0.00087,-0.006,-0.00012,0.06,-0.019,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00041,0.00041,0.046,0.036,0.036,0.007,0.049,0.049,0.04,3.5e-06,3.2e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -14290000,-0.29,0.016,-0.0062,0.96,-0.026,-0.0055,0.019,-0.0031,-0.0046,0.0099,-0.00087,-0.006,-0.00012,0.06,-0.019,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00041,0.00041,0.046,0.039,0.04,0.0069,0.055,0.055,0.039,3.4e-06,3.1e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00074,0.0012,1,1 -14390000,-0.29,0.016,-0.0062,0.96,-0.024,-0.0049,0.02,-0.00066,-0.005,0.014,-0.00088,-0.006,-0.00012,0.061,-0.019,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00041,0.0004,0.046,0.034,0.034,0.0067,0.048,0.048,0.039,3.3e-06,3e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.2e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14490000,-0.29,0.016,-0.0064,0.96,-0.024,-0.0043,0.024,-0.0032,-0.0052,0.017,-0.00084,-0.006,-0.00013,0.061,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.00041,0.0004,0.046,0.037,0.037,0.0066,0.054,0.054,0.038,3.2e-06,2.9e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14590000,-0.29,0.016,-0.0064,0.96,-0.026,-0.0058,0.022,-0.0035,-0.0055,0.013,-0.00083,-0.0059,-0.00013,0.062,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.0004,0.00039,0.046,0.032,0.032,0.0065,0.047,0.047,0.038,3.1e-06,2.8e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14690000,-0.29,0.016,-0.0064,0.96,-0.027,-0.0061,0.022,-0.0062,-0.0062,0.013,-0.00083,-0.0059,-0.00013,0.062,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.0004,0.0004,0.046,0.035,0.035,0.0065,0.053,0.053,0.037,3.1e-06,2.7e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14790000,-0.29,0.016,-0.0066,0.96,-0.028,-0.0033,0.022,-0.0048,-0.0016,0.016,-0.00085,-0.0059,-0.00012,0.063,-0.019,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.0004,0.00039,0.046,0.03,0.031,0.0064,0.046,0.046,0.036,3e-06,2.6e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14890000,-0.29,0.016,-0.0064,0.96,-0.03,-0.0014,0.027,-0.008,-0.0021,0.017,-0.00085,-0.0058,-0.00012,0.063,-0.019,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.07,0,0,0.0004,0.00039,0.046,0.033,0.033,0.0064,0.052,0.052,0.036,2.9e-06,2.6e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -14990000,-0.29,0.016,-0.0065,0.96,-0.028,-0.0031,0.029,-0.0064,-0.0032,0.02,-0.00086,-0.0058,-0.00012,0.064,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.07,0,0,0.00039,0.00039,0.046,0.029,0.029,0.0064,0.045,0.045,0.036,2.8e-06,2.5e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -15090000,-0.29,0.016,-0.0064,0.96,-0.03,-0.0043,0.033,-0.0093,-0.0035,0.022,-0.00085,-0.0058,-0.00012,0.064,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.07,0,0,0.00039,0.00039,0.046,0.031,0.032,0.0064,0.051,0.051,0.035,2.7e-06,2.4e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -15190000,-0.29,0.016,-0.0066,0.96,-0.028,-0.0023,0.034,-0.0075,-0.0027,0.024,-0.00084,-0.0058,-0.00012,0.064,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00039,0.00038,0.046,0.027,0.028,0.0064,0.045,0.045,0.035,2.7e-06,2.3e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -15290000,-0.29,0.016,-0.0066,0.96,-0.031,-0.0024,0.034,-0.011,-0.0033,0.021,-0.00085,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00039,0.00038,0.046,0.03,0.03,0.0065,0.05,0.05,0.035,2.6e-06,2.3e-06,5.6e-06,0.03,0.03,0.00046,0.0012,6.1e-05,0.0012,0.0012,0.00073,0.0012,1,1 -15390000,-0.29,0.016,-0.0067,0.96,-0.03,-0.0042,0.033,-0.0086,-0.0027,0.022,-0.00085,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00039,0.00038,0.046,0.026,0.026,0.0065,0.044,0.044,0.034,2.5e-06,2.2e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15490000,-0.29,0.016,-0.0068,0.96,-0.031,-0.0018,0.033,-0.012,-0.0031,0.023,-0.00085,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00039,0.00038,0.046,0.028,0.028,0.0065,0.05,0.05,0.034,2.5e-06,2.2e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15590000,-0.29,0.016,-0.0067,0.96,-0.028,-0.006,0.033,-0.0071,-0.0062,0.022,-0.00088,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00038,0.046,0.025,0.025,0.0065,0.044,0.044,0.034,2.4e-06,2.1e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6.1e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15690000,-0.29,0.016,-0.0066,0.96,-0.03,-0.0041,0.034,-0.0093,-0.0067,0.023,-0.00091,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00039,0.00038,0.046,0.027,0.027,0.0066,0.049,0.049,0.034,2.4e-06,2e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15790000,-0.29,0.016,-0.0066,0.96,-0.026,-0.0026,0.033,-0.0072,-0.0058,0.024,-0.00095,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00038,0.046,0.024,0.024,0.0066,0.043,0.043,0.033,2.3e-06,2e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15890000,-0.29,0.016,-0.0067,0.96,-0.026,-0.004,0.034,-0.01,-0.0059,0.024,-0.00092,-0.0058,-0.00012,0.065,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00038,0.046,0.026,0.026,0.0068,0.049,0.049,0.034,2.2e-06,1.9e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -15990000,-0.29,0.016,-0.0066,0.96,-0.024,-0.0033,0.031,-0.0084,-0.0049,0.024,-0.00091,-0.0058,-0.00012,0.066,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.023,0.023,0.0068,0.043,0.043,0.033,2.2e-06,1.9e-06,5.6e-06,0.03,0.03,0.00045,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16090000,-0.29,0.016,-0.0066,0.96,-0.026,-0.0021,0.029,-0.011,-0.0051,0.024,-0.0009,-0.0058,-0.00012,0.066,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00038,0.046,0.024,0.025,0.0069,0.048,0.048,0.033,2.1e-06,1.8e-06,5.6e-06,0.03,0.03,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16190000,-0.29,0.016,-0.0066,0.96,-0.024,-0.0018,0.028,-0.01,-0.0042,0.021,-0.00088,-0.0058,-0.00012,0.066,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.022,0.022,0.0069,0.043,0.043,0.033,2.1e-06,1.8e-06,5.6e-06,0.03,0.029,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16290000,-0.29,0.016,-0.0066,0.96,-0.026,-0.00094,0.028,-0.013,-0.0044,0.022,-0.00089,-0.0058,-0.00012,0.066,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.023,0.024,0.007,0.048,0.048,0.033,2e-06,1.7e-06,5.6e-06,0.03,0.029,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16390000,-0.29,0.016,-0.0066,0.96,-0.025,-0.0013,0.028,-0.01,-0.0041,0.022,-0.0009,-0.0058,-0.00012,0.067,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.021,0.021,0.007,0.042,0.042,0.033,2e-06,1.7e-06,5.6e-06,0.03,0.029,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16490000,-0.29,0.016,-0.0067,0.96,-0.03,-0.00031,0.031,-0.013,-0.0041,0.027,-0.00089,-0.0058,-0.00012,0.067,-0.021,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.022,0.023,0.0072,0.047,0.047,0.033,1.9e-06,1.6e-06,5.6e-06,0.03,0.029,0.00044,0.0012,6e-05,0.0012,0.0012,0.00072,0.0012,1,1 -16590000,-0.29,0.016,-0.0067,0.96,-0.033,0.00016,0.034,-0.011,-0.0035,0.027,-0.0009,-0.0058,-0.00012,0.067,-0.021,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.02,0.02,0.0072,0.042,0.042,0.033,1.9e-06,1.6e-06,5.6e-06,0.03,0.029,0.00043,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 -16690000,-0.29,0.016,-0.0067,0.96,-0.036,0.0038,0.034,-0.015,-0.0035,0.028,-0.00091,-0.0058,-0.00012,0.067,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.021,0.022,0.0073,0.047,0.047,0.033,1.9e-06,1.6e-06,5.6e-06,0.03,0.029,0.00043,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 -16790000,-0.29,0.016,-0.0066,0.96,-0.036,0.0036,0.033,-0.012,-0.0031,0.028,-0.00093,-0.0058,-0.00012,0.067,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.019,0.02,0.0073,0.042,0.042,0.033,1.8e-06,1.5e-06,5.6e-06,0.03,0.029,0.00043,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 -16890000,-0.29,0.016,-0.0065,0.96,-0.036,0.0032,0.034,-0.016,-0.0032,0.027,-0.00095,-0.0058,-0.00012,0.067,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.021,0.021,0.0074,0.046,0.046,0.033,1.8e-06,1.5e-06,5.6e-06,0.03,0.029,0.00042,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 -16990000,-0.29,0.016,-0.0066,0.96,-0.034,0.0037,0.034,-0.014,-0.0033,0.025,-0.00096,-0.0058,-0.00012,0.067,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.031,-0.069,0,0,0.00038,0.00037,0.046,0.02,0.021,0.0074,0.049,0.049,0.033,1.7e-06,1.5e-06,5.6e-06,0.03,0.029,0.00042,0.0012,6e-05,0.0012,0.0012,0.00071,0.0012,1,1 -17090000,-0.29,0.016,-0.0067,0.96,-0.038,0.0056,0.034,-0.018,-0.0029,0.025,-0.00095,-0.0058,-0.00012,0.068,-0.02,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.022,0.022,0.0075,0.054,0.054,0.033,1.7e-06,1.4e-06,5.6e-06,0.03,0.029,0.00042,0.0012,6e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17190000,-0.29,0.016,-0.0068,0.96,-0.037,0.0074,0.035,-0.017,-0.0046,0.028,-0.00095,-0.0058,-0.00013,0.067,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.021,0.022,0.0076,0.056,0.057,0.033,1.7e-06,1.4e-06,5.6e-06,0.03,0.029,0.00041,0.0012,6e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17290000,-0.29,0.016,-0.0068,0.96,-0.04,0.0079,0.035,-0.021,-0.0035,0.028,-0.00093,-0.0058,-0.00013,0.067,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.023,0.023,0.0077,0.062,0.063,0.033,1.6e-06,1.4e-06,5.6e-06,0.03,0.029,0.00041,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17390000,-0.29,0.016,-0.0067,0.96,-0.03,0.013,0.034,-0.013,-0.002,0.028,-0.00096,-0.0058,-0.00013,0.067,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.019,0.02,0.0076,0.052,0.052,0.033,1.6e-06,1.3e-06,5.6e-06,0.03,0.029,0.00041,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17490000,-0.29,0.016,-0.0067,0.96,-0.03,0.014,0.034,-0.016,-0.00051,0.03,-0.00096,-0.0058,-0.00013,0.067,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.021,0.021,0.0078,0.058,0.058,0.033,1.6e-06,1.3e-06,5.6e-06,0.03,0.029,0.0004,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17590000,-0.29,0.016,-0.0067,0.96,-0.03,0.012,0.033,-0.015,-0.00075,0.027,-0.00096,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.018,0.019,0.0077,0.049,0.049,0.033,1.5e-06,1.3e-06,5.5e-06,0.03,0.029,0.0004,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17690000,-0.29,0.016,-0.0068,0.96,-0.031,0.013,0.034,-0.018,0.00034,0.03,-0.00097,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.019,0.02,0.0078,0.054,0.054,0.033,1.5e-06,1.2e-06,5.6e-06,0.03,0.029,0.00039,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17790000,-0.29,0.016,-0.0069,0.96,-0.031,0.013,0.035,-0.017,0.0011,0.035,-0.00098,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.019,0.02,0.0078,0.057,0.057,0.033,1.5e-06,1.2e-06,5.5e-06,0.03,0.029,0.00039,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17890000,-0.29,0.016,-0.0068,0.96,-0.035,0.014,0.034,-0.02,0.0023,0.039,-0.00099,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00038,0.00037,0.046,0.02,0.021,0.0079,0.062,0.062,0.033,1.4e-06,1.2e-06,5.5e-06,0.03,0.029,0.00039,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -17990000,-0.29,0.016,-0.0068,0.96,-0.033,0.015,0.034,-0.016,0.0047,0.04,-0.00099,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.018,0.018,0.0079,0.052,0.052,0.033,1.4e-06,1.2e-06,5.5e-06,0.03,0.029,0.00038,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -18090000,-0.29,0.016,-0.0069,0.96,-0.035,0.016,0.033,-0.02,0.0062,0.038,-0.00099,-0.0058,-0.00013,0.068,-0.021,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.019,0.019,0.008,0.057,0.057,0.034,1.4e-06,1.1e-06,5.5e-06,0.03,0.029,0.00038,0.0012,5.9e-05,0.0012,0.0011,0.00071,0.0012,1,1 -18190000,-0.28,0.016,-0.0068,0.96,-0.032,0.014,0.034,-0.015,0.0041,0.036,-0.001,-0.0058,-0.00013,0.069,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.017,0.0079,0.049,0.049,0.034,1.3e-06,1.1e-06,5.5e-06,0.03,0.029,0.00037,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18290000,-0.28,0.016,-0.0068,0.96,-0.035,0.014,0.033,-0.019,0.0052,0.035,-0.001,-0.0058,-0.00013,0.069,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00037,0.046,0.018,0.018,0.008,0.053,0.054,0.034,1.3e-06,1.1e-06,5.5e-06,0.03,0.029,0.00037,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18390000,-0.28,0.016,-0.0068,0.96,-0.031,0.013,0.032,-0.014,0.0044,0.034,-0.001,-0.0058,-0.00014,0.069,-0.021,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.016,0.0079,0.046,0.046,0.034,1.3e-06,1.1e-06,5.5e-06,0.03,0.029,0.00036,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18490000,-0.28,0.016,-0.0068,0.96,-0.035,0.012,0.031,-0.017,0.0053,0.036,-0.001,-0.0058,-0.00013,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.017,0.018,0.008,0.05,0.051,0.034,1.3e-06,1e-06,5.5e-06,0.03,0.029,0.00036,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18590000,-0.28,0.015,-0.0067,0.96,-0.033,0.013,0.031,-0.014,0.0047,0.038,-0.001,-0.0058,-0.00014,0.069,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0079,0.044,0.045,0.034,1.2e-06,1e-06,5.5e-06,0.03,0.029,0.00035,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18690000,-0.28,0.015,-0.0066,0.96,-0.033,0.011,0.029,-0.017,0.0056,0.037,-0.0011,-0.0058,-0.00014,0.069,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.017,0.008,0.048,0.049,0.034,1.2e-06,1e-06,5.5e-06,0.03,0.029,0.00035,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18790000,-0.28,0.015,-0.0066,0.96,-0.03,0.011,0.029,-0.013,0.0048,0.035,-0.0011,-0.0058,-0.00014,0.069,-0.02,-0.14,-0.11,-0.024,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0079,0.043,0.043,0.034,1.2e-06,9.7e-07,5.5e-06,0.03,0.029,0.00034,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18890000,-0.28,0.015,-0.0066,0.96,-0.03,0.012,0.027,-0.016,0.006,0.031,-0.0011,-0.0058,-0.00014,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.017,0.008,0.047,0.047,0.034,1.2e-06,9.6e-07,5.4e-06,0.03,0.029,0.00034,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -18990000,-0.28,0.015,-0.0065,0.96,-0.027,0.012,0.028,-0.014,0.0052,0.034,-0.0011,-0.0058,-0.00015,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.014,0.015,0.0079,0.042,0.042,0.034,1.1e-06,9.3e-07,5.4e-06,0.03,0.029,0.00033,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19090000,-0.28,0.015,-0.0066,0.96,-0.027,0.012,0.028,-0.017,0.0059,0.031,-0.0011,-0.0058,-0.00014,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.008,0.045,0.046,0.035,1.1e-06,9.2e-07,5.4e-06,0.03,0.029,0.00033,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19190000,-0.28,0.015,-0.0065,0.96,-0.024,0.013,0.028,-0.014,0.0058,0.03,-0.0011,-0.0058,-0.00015,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.014,0.015,0.0079,0.041,0.041,0.034,1.1e-06,9e-07,5.4e-06,0.03,0.029,0.00032,0.0012,5.9e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19290000,-0.28,0.015,-0.0065,0.96,-0.025,0.013,0.028,-0.017,0.007,0.029,-0.0011,-0.0058,-0.00015,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0079,0.044,0.045,0.034,1.1e-06,8.9e-07,5.4e-06,0.03,0.029,0.00032,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19390000,-0.28,0.015,-0.0066,0.96,-0.024,0.012,0.029,-0.015,0.0069,0.028,-0.0011,-0.0058,-0.00016,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.014,0.015,0.0078,0.04,0.04,0.035,1.1e-06,8.6e-07,5.4e-06,0.03,0.029,0.00031,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19490000,-0.28,0.015,-0.0067,0.96,-0.026,0.013,0.029,-0.018,0.0083,0.027,-0.0011,-0.0058,-0.00016,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0078,0.043,0.044,0.035,1e-06,8.5e-07,5.4e-06,0.03,0.029,0.00031,0.0012,5.8e-05,0.0012,0.0011,0.0007,0.0012,1,1 -19590000,-0.28,0.015,-0.0066,0.96,-0.023,0.014,0.031,-0.015,0.0065,0.028,-0.0011,-0.0058,-0.00017,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.014,0.015,0.0078,0.039,0.039,0.035,1e-06,8.3e-07,5.4e-06,0.03,0.029,0.00031,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -19690000,-0.28,0.015,-0.0066,0.96,-0.023,0.012,0.029,-0.018,0.0073,0.027,-0.0011,-0.0058,-0.00017,0.069,-0.02,-0.14,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0078,0.043,0.043,0.035,1e-06,8.2e-07,5.4e-06,0.03,0.029,0.0003,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -19790000,-0.28,0.015,-0.0067,0.96,-0.02,0.012,0.027,-0.018,0.0079,0.023,-0.0011,-0.0058,-0.00017,0.069,-0.02,-0.13,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0077,0.045,0.046,0.035,9.9e-07,8e-07,5.3e-06,0.03,0.029,0.0003,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -19890000,-0.28,0.015,-0.0067,0.96,-0.021,0.012,0.028,-0.02,0.0091,0.022,-0.0011,-0.0058,-0.00017,0.069,-0.02,-0.13,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.017,0.0077,0.049,0.05,0.035,9.8e-07,7.9e-07,5.3e-06,0.03,0.029,0.00029,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -19990000,-0.28,0.016,-0.0067,0.96,-0.018,0.013,0.025,-0.015,0.0084,0.019,-0.0011,-0.0058,-0.00018,0.069,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.014,0.015,0.0076,0.043,0.044,0.035,9.5e-07,7.7e-07,5.3e-06,0.03,0.029,0.00029,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20090000,-0.28,0.016,-0.0067,0.96,-0.021,0.016,0.025,-0.017,0.0097,0.022,-0.0011,-0.0058,-0.00018,0.069,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.032,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0076,0.047,0.048,0.035,9.4e-07,7.6e-07,5.3e-06,0.029,0.029,0.00028,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20190000,-0.28,0.016,-0.0067,0.96,-0.021,0.013,0.026,-0.019,0.01,0.021,-0.0011,-0.0058,-0.00018,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00037,0.00036,0.046,0.015,0.016,0.0075,0.049,0.05,0.035,9.2e-07,7.5e-07,5.3e-06,0.029,0.029,0.00028,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20290000,-0.28,0.016,-0.0067,0.96,-0.02,0.016,0.026,-0.021,0.011,0.022,-0.0011,-0.0058,-0.00018,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.016,0.018,0.0075,0.054,0.055,0.035,9.1e-07,7.4e-07,5.3e-06,0.029,0.029,0.00028,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20390000,-0.28,0.016,-0.0066,0.96,-0.018,0.014,0.026,-0.021,0.011,0.023,-0.0011,-0.0058,-0.00019,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00037,0.00036,0.046,0.016,0.018,0.0075,0.056,0.057,0.035,8.9e-07,7.2e-07,5.2e-06,0.029,0.029,0.00027,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20490000,-0.28,0.016,-0.0066,0.96,-0.015,0.016,0.026,-0.023,0.012,0.021,-0.0011,-0.0058,-0.00018,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.017,0.019,0.0075,0.061,0.063,0.035,8.8e-07,7.1e-07,5.2e-06,0.029,0.029,0.00027,0.0012,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20590000,-0.28,0.016,-0.0065,0.96,-0.015,0.015,0.025,-0.023,0.011,0.019,-0.0011,-0.0058,-0.00018,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.084,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.017,0.018,0.0074,0.064,0.065,0.035,8.6e-07,7e-07,5.2e-06,0.029,0.029,0.00026,0.0011,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20690000,-0.28,0.016,-0.0065,0.96,-0.014,0.016,0.026,-0.024,0.013,0.02,-0.0011,-0.0058,-0.00019,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00038,0.00036,0.046,0.018,0.02,0.0074,0.069,0.071,0.035,8.5e-07,6.9e-07,5.2e-06,0.029,0.029,0.00026,0.0011,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20790000,-0.28,0.015,-0.0059,0.96,-0.0088,0.011,0.011,-0.018,0.0093,0.019,-0.0012,-0.0058,-0.00019,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.085,-0.033,-0.069,0,0,0.00037,0.00035,0.046,0.015,0.017,0.0073,0.056,0.057,0.035,8.2e-07,6.7e-07,5.2e-06,0.029,0.029,0.00026,0.0011,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20890000,-0.28,0.011,0.0028,0.96,-0.0041,0.001,-0.11,-0.02,0.0098,0.012,-0.0012,-0.0058,-0.00019,0.07,-0.02,-0.13,-0.11,-0.023,0.5,0.084,-0.032,-0.069,0,0,0.00036,0.00034,0.046,0.016,0.018,0.0073,0.061,0.062,0.035,8.2e-07,6.6e-07,5.2e-06,0.029,0.029,0.00025,0.0011,5.8e-05,0.0012,0.0011,0.00069,0.0012,1,1 -20990000,-0.28,0.0069,0.0058,0.96,0.01,-0.015,-0.25,-0.016,0.0075,-0.0029,-0.0011,-0.0058,-0.0002,0.07,-0.021,-0.13,-0.11,-0.023,0.5,0.084,-0.032,-0.068,0,0,0.00035,0.00034,0.046,0.014,0.016,0.0072,0.051,0.052,0.034,7.9e-07,6.4e-07,5.1e-06,0.029,0.029,0.00025,0.0011,5.8e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21090000,-0.28,0.0074,0.0042,0.96,0.024,-0.028,-0.37,-0.015,0.0055,-0.033,-0.0011,-0.0058,-0.00021,0.07,-0.021,-0.13,-0.11,-0.023,0.5,0.084,-0.032,-0.068,0,0,0.00035,0.00034,0.046,0.015,0.017,0.0072,0.056,0.057,0.035,7.9e-07,6.4e-07,5.1e-06,0.029,0.029,0.00025,0.0011,5.8e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21190000,-0.28,0.0093,0.0016,0.96,0.03,-0.033,-0.49,-0.012,0.0042,-0.07,-0.0011,-0.0058,-0.0002,0.07,-0.021,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00033,0.046,0.014,0.016,0.0071,0.048,0.049,0.035,7.6e-07,6.2e-07,5.1e-06,0.029,0.029,0.00024,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21290000,-0.28,0.011,-0.00045,0.96,0.028,-0.035,-0.62,-0.0097,0.0016,-0.13,-0.0011,-0.0058,-0.00021,0.07,-0.021,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.015,0.017,0.0071,0.052,0.053,0.035,7.6e-07,6.2e-07,5.1e-06,0.029,0.029,0.00024,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21390000,-0.29,0.012,-0.0019,0.96,0.022,-0.028,-0.75,-0.011,0.005,-0.19,-0.0011,-0.0058,-0.00018,0.07,-0.022,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.015,0.017,0.007,0.054,0.055,0.035,7.4e-07,6e-07,5e-06,0.029,0.029,0.00024,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21490000,-0.29,0.012,-0.0027,0.96,0.015,-0.026,-0.88,-0.0091,0.0021,-0.28,-0.0011,-0.0058,-0.00018,0.07,-0.022,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.016,0.018,0.007,0.059,0.06,0.035,7.4e-07,6e-07,5e-06,0.029,0.029,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00068,0.0012,1,1 -21590000,-0.29,0.012,-0.0033,0.96,0.0031,-0.02,-1,-0.013,0.0071,-0.37,-0.0011,-0.0058,-0.00015,0.07,-0.023,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.016,0.018,0.0069,0.061,0.063,0.034,7.2e-07,5.9e-07,5e-06,0.029,0.029,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -21690000,-0.29,0.012,-0.0036,0.96,-0.0023,-0.016,-1.1,-0.013,0.0047,-0.48,-0.0011,-0.0058,-0.00014,0.07,-0.023,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.017,0.02,0.0069,0.066,0.068,0.035,7.2e-07,5.8e-07,5e-06,0.029,0.029,0.00023,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -21790000,-0.29,0.012,-0.004,0.96,-0.0083,-0.0076,-1.3,-0.015,0.011,-0.6,-0.0011,-0.0058,-0.00011,0.07,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00036,0.00034,0.046,0.017,0.02,0.0069,0.068,0.07,0.034,7e-07,5.7e-07,4.9e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -21890000,-0.29,0.012,-0.0043,0.96,-0.014,-0.0031,-1.4,-0.016,0.011,-0.74,-0.0011,-0.0058,-0.00011,0.07,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.046,0.019,0.021,0.0068,0.074,0.076,0.034,7e-07,5.7e-07,4.9e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -21990000,-0.29,0.013,-0.0051,0.96,-0.02,0.0053,-1.4,-0.023,0.018,-0.88,-0.0011,-0.0058,-8.3e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.046,0.018,0.02,0.0068,0.076,0.079,0.034,6.8e-07,5.5e-07,4.9e-06,0.029,0.029,0.00022,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22090000,-0.29,0.014,-0.0057,0.96,-0.023,0.0086,-1.4,-0.024,0.019,-1,-0.0011,-0.0058,-7.6e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.046,0.019,0.021,0.0068,0.082,0.085,0.034,6.7e-07,5.5e-07,4.9e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22190000,-0.29,0.014,-0.0062,0.96,-0.03,0.015,-1.4,-0.028,0.026,-1.2,-0.0011,-0.0058,-4.8e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.045,0.019,0.021,0.0067,0.085,0.087,0.034,6.6e-07,5.4e-07,4.9e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22290000,-0.29,0.014,-0.0069,0.96,-0.038,0.02,-1.4,-0.032,0.028,-1.3,-0.0011,-0.0058,-4.9e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00035,0.045,0.02,0.022,0.0067,0.091,0.094,0.034,6.5e-07,5.3e-07,4.8e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22390000,-0.29,0.015,-0.0072,0.96,-0.045,0.027,-1.4,-0.038,0.032,-1.5,-0.0011,-0.0058,-5e-05,0.07,-0.027,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.045,0.019,0.021,0.0066,0.093,0.096,0.034,6.4e-07,5.2e-07,4.8e-06,0.029,0.029,0.00021,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22490000,-0.29,0.015,-0.0073,0.96,-0.051,0.033,-1.4,-0.043,0.036,-1.6,-0.0011,-0.0058,-5.3e-05,0.07,-0.027,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00035,0.045,0.02,0.022,0.0066,0.1,0.1,0.034,6.4e-07,5.2e-07,4.8e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22590000,-0.29,0.016,-0.0071,0.96,-0.056,0.038,-1.4,-0.044,0.039,-1.7,-0.0011,-0.0058,-4.5e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.045,0.019,0.022,0.0065,0.1,0.11,0.034,6.2e-07,5.1e-07,4.8e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22690000,-0.29,0.016,-0.0071,0.96,-0.061,0.043,-1.4,-0.051,0.043,-1.9,-0.0011,-0.0058,-4.7e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.045,0.02,0.023,0.0065,0.11,0.11,0.034,6.2e-07,5e-07,4.7e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22790000,-0.29,0.016,-0.007,0.96,-0.067,0.048,-1.4,-0.057,0.046,-2,-0.0011,-0.0058,-5.6e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00036,0.00034,0.045,0.02,0.022,0.0065,0.11,0.11,0.034,6e-07,4.9e-07,4.7e-06,0.029,0.029,0.0002,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22890000,-0.29,0.017,-0.0071,0.96,-0.072,0.052,-1.4,-0.063,0.05,-2.2,-0.0011,-0.0058,-4.7e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0065,0.12,0.12,0.034,6e-07,4.9e-07,4.7e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00067,0.0012,1,1 -22990000,-0.29,0.017,-0.0069,0.96,-0.075,0.052,-1.4,-0.066,0.048,-2.3,-0.0011,-0.0058,-5.3e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00037,0.00034,0.045,0.02,0.022,0.0064,0.12,0.12,0.034,5.8e-07,4.8e-07,4.6e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23090000,-0.28,0.017,-0.0069,0.96,-0.081,0.056,-1.4,-0.073,0.054,-2.5,-0.0011,-0.0058,-5.1e-05,0.07,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.024,0.0064,0.13,0.13,0.034,5.8e-07,4.8e-07,4.6e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23190000,-0.28,0.017,-0.0068,0.96,-0.082,0.051,-1.4,-0.073,0.05,-2.6,-0.0011,-0.0058,-7.9e-05,0.07,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00037,0.00034,0.045,0.02,0.023,0.0063,0.13,0.13,0.033,5.7e-07,4.7e-07,4.6e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23290000,-0.28,0.018,-0.0072,0.96,-0.089,0.054,-1.4,-0.081,0.054,-2.7,-0.0011,-0.0058,-7.4e-05,0.07,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.031,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.024,0.0063,0.14,0.14,0.034,5.7e-07,4.7e-07,4.6e-06,0.029,0.029,0.00019,0.0011,5.7e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23390000,-0.28,0.018,-0.0071,0.96,-0.088,0.057,-1.4,-0.076,0.056,-2.9,-0.0011,-0.0058,-9.6e-05,0.069,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0063,0.14,0.14,0.033,5.5e-07,4.6e-07,4.5e-06,0.029,0.029,0.00018,0.0011,5.7e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23490000,-0.28,0.018,-0.0072,0.96,-0.095,0.057,-1.4,-0.086,0.06,-3,-0.0011,-0.0058,-8.6e-05,0.069,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.024,0.0063,0.15,0.15,0.033,5.5e-07,4.5e-07,4.5e-06,0.029,0.029,0.00018,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23590000,-0.28,0.018,-0.0073,0.96,-0.093,0.051,-1.4,-0.081,0.05,-3.2,-0.0011,-0.0058,-0.00012,0.069,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0062,0.15,0.15,0.033,5.4e-07,4.4e-07,4.5e-06,0.029,0.029,0.00018,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23690000,-0.28,0.018,-0.0079,0.96,-0.092,0.053,-1.3,-0.091,0.055,-3.3,-0.0011,-0.0058,-0.00011,0.069,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00037,0.00035,0.045,0.021,0.024,0.0062,0.16,0.16,0.033,5.4e-07,4.4e-07,4.5e-06,0.029,0.029,0.00018,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23790000,-0.28,0.021,-0.0094,0.96,-0.077,0.05,-0.95,-0.081,0.05,-3.4,-0.0012,-0.0058,-0.00012,0.069,-0.023,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.068,0,0,0.00039,0.00036,0.045,0.02,0.023,0.0061,0.16,0.16,0.033,5.3e-07,4.3e-07,4.4e-06,0.029,0.029,0.00018,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23890000,-0.28,0.026,-0.012,0.96,-0.07,0.051,-0.52,-0.088,0.054,-3.5,-0.0012,-0.0058,-0.00012,0.069,-0.023,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.067,0,0,0.00042,0.00038,0.045,0.021,0.023,0.0061,0.17,0.17,0.033,5.2e-07,4.3e-07,4.4e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -23990000,-0.28,0.028,-0.014,0.96,-0.061,0.049,-0.13,-0.076,0.049,-3.6,-0.0012,-0.0058,-0.00014,0.07,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.067,0,0,0.00043,0.0004,0.045,0.02,0.022,0.0061,0.17,0.17,0.033,5.1e-07,4.3e-07,4.4e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24090000,-0.28,0.027,-0.014,0.96,-0.067,0.056,0.1,-0.081,0.054,-3.6,-0.0012,-0.0058,-0.00013,0.07,-0.024,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.067,0,0,0.00042,0.0004,0.045,0.021,0.023,0.0061,0.18,0.18,0.033,5.1e-07,4.2e-07,4.4e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24190000,-0.28,0.023,-0.011,0.96,-0.071,0.053,0.09,-0.068,0.041,-3.6,-0.0012,-0.0058,-0.00016,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.0004,0.00037,0.045,0.02,0.022,0.006,0.18,0.18,0.033,5e-07,4.2e-07,4.3e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24290000,-0.28,0.019,-0.0092,0.96,-0.075,0.055,0.068,-0.075,0.046,-3.6,-0.0012,-0.0058,-0.00016,0.071,-0.025,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00038,0.00035,0.045,0.021,0.023,0.006,0.19,0.19,0.033,5e-07,4.1e-07,4.3e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24390000,-0.28,0.018,-0.0084,0.96,-0.059,0.048,0.084,-0.057,0.037,-3.6,-0.0013,-0.0058,-0.00017,0.071,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00035,0.045,0.02,0.022,0.006,0.19,0.19,0.033,4.9e-07,4.1e-07,4.3e-06,0.029,0.029,0.00017,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24490000,-0.28,0.018,-0.0086,0.96,-0.054,0.045,0.082,-0.062,0.04,-3.6,-0.0013,-0.0058,-0.00016,0.071,-0.026,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.024,0.006,0.2,0.2,0.033,4.9e-07,4.1e-07,4.2e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24590000,-0.28,0.019,-0.0093,0.96,-0.043,0.043,0.077,-0.044,0.034,-3.6,-0.0013,-0.0059,-0.00018,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.02,0.023,0.0059,0.2,0.2,0.033,4.8e-07,4e-07,4.2e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24690000,-0.28,0.018,-0.0098,0.96,-0.04,0.042,0.077,-0.048,0.038,-3.5,-0.0013,-0.0059,-0.00017,0.072,-0.027,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.024,0.0059,0.21,0.21,0.033,4.8e-07,4e-07,4.2e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24790000,-0.28,0.018,-0.0099,0.96,-0.034,0.04,0.068,-0.036,0.03,-3.5,-0.0013,-0.0059,-0.00019,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.02,0.023,0.0059,0.21,0.21,0.032,4.7e-07,3.9e-07,4.1e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24890000,-0.28,0.017,-0.0098,0.96,-0.033,0.043,0.058,-0.039,0.033,-3.5,-0.0013,-0.0059,-0.00018,0.072,-0.028,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.024,0.0059,0.22,0.22,0.032,4.7e-07,3.9e-07,4.1e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -24990000,-0.28,0.017,-0.0096,0.96,-0.021,0.043,0.051,-0.025,0.028,-3.5,-0.0013,-0.0059,-0.0002,0.073,-0.029,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0058,0.22,0.22,0.032,4.6e-07,3.9e-07,4.1e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25090000,-0.28,0.017,-0.0099,0.96,-0.016,0.043,0.048,-0.026,0.032,-3.5,-0.0013,-0.0059,-0.0002,0.073,-0.029,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.024,0.0058,0.23,0.23,0.032,4.6e-07,3.8e-07,4.1e-06,0.029,0.029,0.00016,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25190000,-0.28,0.017,-0.01,0.96,-0.006,0.039,0.048,-0.011,0.022,-3.5,-0.0013,-0.0059,-0.00023,0.073,-0.029,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00037,0.00036,0.045,0.021,0.023,0.0058,0.23,0.23,0.032,4.5e-07,3.8e-07,4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25290000,-0.28,0.016,-0.01,0.96,-0.0012,0.042,0.043,-0.011,0.026,-3.5,-0.0013,-0.0059,-0.00023,0.073,-0.029,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0058,0.24,0.24,0.032,4.5e-07,3.8e-07,4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25390000,-0.28,0.016,-0.011,0.96,0.0075,0.04,0.042,-0.0015,0.021,-3.5,-0.0013,-0.0059,-0.00025,0.073,-0.03,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0058,0.24,0.24,0.032,4.5e-07,3.7e-07,4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25490000,-0.28,0.016,-0.011,0.96,0.012,0.041,0.041,-0.0014,0.024,-3.5,-0.0013,-0.0059,-0.00025,0.073,-0.03,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0058,0.25,0.25,0.032,4.4e-07,3.7e-07,4e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25590000,-0.28,0.016,-0.011,0.96,0.017,0.036,0.042,0.0056,0.0097,-3.5,-0.0014,-0.0059,-0.00028,0.073,-0.03,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.25,0.25,0.032,4.4e-07,3.7e-07,3.9e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25690000,-0.28,0.015,-0.01,0.96,0.018,0.035,0.031,0.0074,0.013,-3.5,-0.0014,-0.0059,-0.00027,0.073,-0.03,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.26,0.26,0.032,4.4e-07,3.7e-07,3.9e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25790000,-0.28,0.015,-0.01,0.96,0.028,0.03,0.031,0.014,0.0034,-3.5,-0.0014,-0.0058,-0.00029,0.074,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.26,0.26,0.032,4.3e-07,3.6e-07,3.9e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25890000,-0.28,0.015,-0.01,0.96,0.034,0.03,0.033,0.018,0.007,-3.5,-0.0014,-0.0059,-0.0003,0.074,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.27,0.27,0.032,4.3e-07,3.6e-07,3.9e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -25990000,-0.28,0.015,-0.01,0.96,0.036,0.024,0.027,0.014,-0.0039,-3.5,-0.0014,-0.0058,-0.00032,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0057,0.26,0.27,0.032,4.2e-07,3.6e-07,3.8e-06,0.029,0.029,0.00015,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -26090000,-0.28,0.015,-0.0099,0.96,0.041,0.025,0.026,0.018,-0.0021,-3.5,-0.0014,-0.0058,-0.00031,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0057,0.28,0.28,0.032,4.2e-07,3.5e-07,3.8e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -26190000,-0.28,0.015,-0.0097,0.96,0.046,0.015,0.021,0.021,-0.018,-3.5,-0.0014,-0.0058,-0.00032,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00036,0.00036,0.045,0.021,0.023,0.0056,0.27,0.28,0.032,4.1e-07,3.5e-07,3.8e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -26290000,-0.28,0.016,-0.0097,0.96,0.046,0.015,0.015,0.024,-0.016,-3.5,-0.0014,-0.0058,-0.00033,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.032,-0.066,0,0,0.00036,0.00036,0.045,0.022,0.024,0.0056,0.29,0.29,0.032,4.1e-07,3.5e-07,3.8e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00066,0.0012,1,1 -26390000,-0.28,0.016,-0.0091,0.96,0.043,0.0057,0.019,0.016,-0.03,-3.5,-0.0014,-0.0058,-0.00035,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00036,0.00035,0.045,0.021,0.023,0.0056,0.28,0.29,0.032,4.1e-07,3.4e-07,3.7e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26490000,-0.28,0.016,-0.0089,0.96,0.047,0.0034,0.029,0.021,-0.03,-3.5,-0.0014,-0.0058,-0.00035,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00036,0.00035,0.045,0.022,0.024,0.0056,0.3,0.3,0.032,4.1e-07,3.4e-07,3.7e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26590000,-0.28,0.016,-0.0083,0.96,0.045,-0.0066,0.029,0.02,-0.042,-3.5,-0.0014,-0.0058,-0.00037,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00035,0.045,0.021,0.023,0.0056,0.29,0.3,0.032,4e-07,3.4e-07,3.7e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26690000,-0.28,0.015,-0.0082,0.96,0.047,-0.01,0.027,0.024,-0.043,-3.5,-0.0014,-0.0058,-0.00038,0.073,-0.031,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0056,0.31,0.31,0.032,4e-07,3.4e-07,3.7e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26790000,-0.28,0.015,-0.008,0.96,0.05,-0.017,0.027,0.021,-0.056,-3.5,-0.0014,-0.0058,-0.0004,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00036,0.00035,0.045,0.021,0.023,0.0055,0.3,0.31,0.031,3.9e-07,3.3e-07,3.7e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26890000,-0.28,0.015,-0.0074,0.96,0.056,-0.019,0.022,0.027,-0.058,-3.5,-0.0014,-0.0058,-0.00039,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00035,0.045,0.022,0.024,0.0056,0.32,0.32,0.032,3.9e-07,3.3e-07,3.6e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -26990000,-0.28,0.015,-0.0068,0.96,0.056,-0.026,0.022,0.019,-0.064,-3.5,-0.0014,-0.0058,-0.0004,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0055,0.31,0.32,0.031,3.9e-07,3.3e-07,3.6e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27090000,-0.28,0.016,-0.0066,0.96,0.059,-0.032,0.025,0.025,-0.067,-3.5,-0.0014,-0.0058,-0.0004,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00034,0.045,0.022,0.024,0.0055,0.32,0.33,0.031,3.9e-07,3.3e-07,3.6e-06,0.029,0.029,0.00014,0.0011,5.6e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27190000,-0.28,0.016,-0.0067,0.96,0.059,-0.036,0.027,0.015,-0.07,-3.5,-0.0014,-0.0058,-0.0004,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00037,0.00034,0.045,0.021,0.023,0.0055,0.32,0.33,0.031,3.8e-07,3.3e-07,3.6e-06,0.029,0.029,0.00014,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27290000,-0.28,0.017,-0.0068,0.96,0.067,-0.04,0.14,0.021,-0.074,-3.5,-0.0014,-0.0058,-0.0004,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.00038,0.00035,0.045,0.022,0.024,0.0055,0.33,0.34,0.031,3.8e-07,3.3e-07,3.6e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27390000,-0.28,0.019,-0.008,0.96,0.07,-0.033,0.46,0.0052,-0.026,-3.5,-0.0014,-0.0058,-0.00037,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00035,0.045,0.015,0.017,0.0055,0.15,0.15,0.031,3.7e-07,3.2e-07,3.5e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27490000,-0.28,0.021,-0.0092,0.96,0.076,-0.036,0.78,0.013,-0.03,-3.5,-0.0014,-0.0058,-0.00038,0.073,-0.032,-0.13,-0.11,-0.024,0.5,0.084,-0.033,-0.066,0,0,0.0004,0.00036,0.045,0.015,0.018,0.0055,0.15,0.15,0.031,3.7e-07,3.2e-07,3.5e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27590000,-0.28,0.02,-0.0092,0.96,0.068,-0.038,0.87,0.0067,-0.02,-3.4,-0.0014,-0.0058,-0.00036,0.073,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00036,0.045,0.014,0.015,0.0055,0.096,0.097,0.031,3.7e-07,3.2e-07,3.5e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27690000,-0.28,0.017,-0.0083,0.96,0.062,-0.036,0.78,0.013,-0.024,-3.3,-0.0014,-0.0058,-0.00036,0.073,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00035,0.045,0.014,0.016,0.0055,0.1,0.1,0.031,3.7e-07,3.2e-07,3.4e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27790000,-0.28,0.015,-0.0071,0.96,0.058,-0.033,0.77,0.011,-0.019,-3.2,-0.0014,-0.0058,-0.00034,0.072,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00034,0.045,0.013,0.015,0.0054,0.073,0.074,0.031,3.6e-07,3.1e-07,3.4e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27890000,-0.28,0.016,-0.0067,0.96,0.065,-0.039,0.81,0.016,-0.023,-3.2,-0.0014,-0.0058,-0.00034,0.073,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00034,0.045,0.014,0.016,0.0055,0.076,0.077,0.031,3.7e-07,3.1e-07,3.4e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -27990000,-0.28,0.016,-0.0071,0.96,0.065,-0.042,0.8,0.019,-0.025,-3.1,-0.0014,-0.0058,-0.00034,0.072,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00035,0.045,0.014,0.016,0.0054,0.079,0.079,0.031,3.6e-07,3.1e-07,3.4e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28090000,-0.28,0.016,-0.0074,0.96,0.068,-0.042,0.8,0.026,-0.03,-3,-0.0014,-0.0058,-0.00033,0.072,-0.033,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00037,0.00035,0.045,0.015,0.017,0.0054,0.082,0.083,0.031,3.6e-07,3.1e-07,3.4e-06,0.029,0.029,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28190000,-0.28,0.016,-0.0068,0.96,0.065,-0.04,0.81,0.027,-0.032,-2.9,-0.0013,-0.0058,-0.00031,0.072,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.015,0.017,0.0054,0.084,0.086,0.031,3.6e-07,3.1e-07,3.3e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28290000,-0.28,0.016,-0.0063,0.96,0.069,-0.043,0.81,0.033,-0.037,-2.9,-0.0013,-0.0058,-0.00031,0.072,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.015,0.018,0.0054,0.088,0.09,0.031,3.6e-07,3.1e-07,3.3e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28390000,-0.28,0.017,-0.0062,0.96,0.071,-0.045,0.81,0.036,-0.037,-2.8,-0.0013,-0.0058,-0.00028,0.072,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.015,0.018,0.0054,0.091,0.092,0.031,3.5e-07,3e-07,3.3e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28490000,-0.28,0.018,-0.0065,0.96,0.073,-0.048,0.81,0.044,-0.042,-2.7,-0.0013,-0.0058,-0.00028,0.072,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00035,0.045,0.016,0.019,0.0054,0.095,0.097,0.031,3.5e-07,3e-07,3.3e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28590000,-0.28,0.017,-0.0065,0.96,0.066,-0.049,0.81,0.044,-0.045,-2.6,-0.0013,-0.0058,-0.00027,0.071,-0.032,-0.13,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00035,0.045,0.016,0.018,0.0054,0.098,0.1,0.031,3.5e-07,3e-07,3.3e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28690000,-0.28,0.017,-0.0063,0.96,0.065,-0.049,0.81,0.05,-0.05,-2.6,-0.0013,-0.0058,-0.00027,0.072,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.019,0.0054,0.1,0.1,0.031,3.5e-07,3e-07,3.2e-06,0.029,0.028,0.00013,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28790000,-0.28,0.017,-0.0057,0.96,0.063,-0.048,0.81,0.052,-0.048,-2.5,-0.0013,-0.0058,-0.00024,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.019,0.0053,0.11,0.11,0.031,3.4e-07,3e-07,3.2e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28890000,-0.28,0.016,-0.0055,0.96,0.066,-0.05,0.81,0.058,-0.054,-2.4,-0.0013,-0.0058,-0.00023,0.072,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.02,0.0054,0.11,0.11,0.031,3.4e-07,3e-07,3.2e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00065,0.0012,1,1 -28990000,-0.28,0.016,-0.0053,0.96,0.064,-0.047,0.81,0.059,-0.053,-2.3,-0.0013,-0.0058,-0.00022,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.017,0.019,0.0053,0.11,0.12,0.031,3.4e-07,2.9e-07,3.2e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29090000,-0.28,0.016,-0.0051,0.96,0.067,-0.049,0.81,0.066,-0.058,-2.3,-0.0013,-0.0058,-0.00022,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.02,0.0053,0.12,0.12,0.031,3.4e-07,2.9e-07,3.2e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29190000,-0.28,0.017,-0.005,0.96,0.068,-0.048,0.8,0.068,-0.057,-2.2,-0.0013,-0.0058,-0.00019,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.02,0.0053,0.12,0.12,0.031,3.4e-07,2.9e-07,3.1e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29290000,-0.28,0.017,-0.0053,0.96,0.072,-0.053,0.81,0.077,-0.061,-2.1,-0.0013,-0.0058,-0.00019,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00039,0.00034,0.045,0.018,0.021,0.0053,0.13,0.13,0.031,3.4e-07,2.9e-07,3.1e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29390000,-0.28,0.016,-0.0058,0.96,0.068,-0.05,0.81,0.075,-0.058,-2,-0.0013,-0.0058,-0.00016,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.018,0.02,0.0053,0.13,0.13,0.031,3.3e-07,2.9e-07,3.1e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29490000,-0.28,0.016,-0.0058,0.96,0.071,-0.051,0.81,0.082,-0.064,-2,-0.0013,-0.0058,-0.00015,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.083,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.019,0.021,0.0053,0.14,0.14,0.031,3.3e-07,2.9e-07,3.1e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29590000,-0.28,0.016,-0.0057,0.96,0.068,-0.049,0.81,0.08,-0.062,-1.9,-0.0013,-0.0058,-0.00012,0.071,-0.032,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.018,0.021,0.0053,0.14,0.14,0.031,3.3e-07,2.8e-07,3e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29690000,-0.28,0.016,-0.0058,0.96,0.073,-0.047,0.81,0.088,-0.067,-1.8,-0.0013,-0.0058,-0.00011,0.07,-0.032,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.019,0.022,0.0053,0.15,0.15,0.031,3.3e-07,2.8e-07,3e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29790000,-0.27,0.016,-0.0055,0.96,0.07,-0.041,0.81,0.085,-0.063,-1.7,-0.0013,-0.0058,-7.9e-05,0.07,-0.032,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00038,0.00034,0.045,0.019,0.021,0.0053,0.15,0.15,0.031,3.2e-07,2.8e-07,3e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29890000,-0.27,0.016,-0.005,0.96,0.071,-0.042,0.8,0.093,-0.067,-1.7,-0.0013,-0.0058,-7.2e-05,0.07,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.045,0.019,0.022,0.0053,0.15,0.16,0.031,3.2e-07,2.8e-07,3e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -29990000,-0.27,0.016,-0.0051,0.96,0.066,-0.04,0.8,0.087,-0.066,-1.6,-0.0013,-0.0058,-5.5e-05,0.07,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.045,0.019,0.021,0.0052,0.16,0.16,0.03,3.2e-07,2.8e-07,2.9e-06,0.029,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30090000,-0.27,0.016,-0.0053,0.96,0.067,-0.04,0.8,0.095,-0.069,-1.5,-0.0013,-0.0058,-7.2e-05,0.07,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.16,0.17,0.03,3.2e-07,2.8e-07,2.9e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30190000,-0.27,0.016,-0.0053,0.96,0.063,-0.033,0.8,0.09,-0.06,-1.5,-0.0013,-0.0058,-5.7e-05,0.07,-0.033,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.022,0.0052,0.17,0.17,0.031,3.1e-07,2.7e-07,2.9e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30290000,-0.27,0.016,-0.0053,0.96,0.063,-0.033,0.8,0.097,-0.063,-1.4,-0.0013,-0.0058,-5.6e-05,0.07,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.023,0.0052,0.17,0.18,0.031,3.2e-07,2.7e-07,2.9e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30390000,-0.27,0.016,-0.0053,0.96,0.06,-0.027,0.8,0.096,-0.057,-1.3,-0.0013,-0.0058,-2.2e-05,0.07,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.044,0.019,0.022,0.0052,0.17,0.18,0.03,3.1e-07,2.7e-07,2.9e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30490000,-0.27,0.016,-0.0053,0.96,0.063,-0.027,0.8,0.1,-0.06,-1.2,-0.0013,-0.0058,-1.5e-05,0.07,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.066,0,0,0.00039,0.00034,0.044,0.02,0.023,0.0052,0.18,0.19,0.031,3.1e-07,2.7e-07,2.9e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30590000,-0.27,0.017,-0.0056,0.96,0.061,-0.025,0.8,0.098,-0.056,-1.2,-0.0012,-0.0058,1.2e-05,0.07,-0.034,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.18,0.19,0.03,3.1e-07,2.7e-07,2.8e-06,0.028,0.028,0.00012,0.0011,5.5e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30690000,-0.27,0.017,-0.006,0.96,0.059,-0.024,0.8,0.1,-0.059,-1.1,-0.0012,-0.0058,1e-05,0.07,-0.035,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.023,0.0052,0.19,0.2,0.03,3.1e-07,2.7e-07,2.8e-06,0.028,0.028,0.00012,0.0011,5.4e-05,0.0012,0.0011,0.00064,0.0012,1,1 -30790000,-0.27,0.016,-0.0058,0.96,0.052,-0.014,0.8,0.096,-0.046,-1,-0.0012,-0.0058,3.9e-05,0.07,-0.035,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.19,0.2,0.03,3e-07,2.7e-07,2.8e-06,0.028,0.028,0.00012,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -30890000,-0.27,0.016,-0.0052,0.96,0.051,-0.01,0.79,0.1,-0.047,-0.95,-0.0012,-0.0058,2.7e-05,0.071,-0.035,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.023,0.0052,0.2,0.21,0.03,3e-07,2.7e-07,2.8e-06,0.028,0.028,0.00012,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -30990000,-0.27,0.016,-0.0053,0.96,0.047,-0.0086,0.79,0.096,-0.046,-0.88,-0.0012,-0.0058,3.2e-05,0.071,-0.035,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.2,0.21,0.03,3e-07,2.6e-07,2.7e-06,0.028,0.028,0.00012,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31090000,-0.27,0.016,-0.0055,0.96,0.046,-0.0072,0.79,0.1,-0.046,-0.81,-0.0012,-0.0058,2.4e-05,0.071,-0.036,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.21,0.22,0.03,3e-07,2.6e-07,2.7e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31190000,-0.27,0.016,-0.0056,0.96,0.043,-0.0037,0.8,0.094,-0.042,-0.74,-0.0012,-0.0058,5e-05,0.071,-0.036,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0052,0.21,0.22,0.03,3e-07,2.6e-07,2.7e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31290000,-0.27,0.017,-0.0058,0.96,0.039,-0.0018,0.8,0.097,-0.043,-0.67,-0.0012,-0.0058,5.6e-05,0.071,-0.036,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.22,0.23,0.03,3e-07,2.6e-07,2.7e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31390000,-0.27,0.016,-0.0056,0.96,0.035,0.0031,0.8,0.091,-0.039,-0.59,-0.0012,-0.0058,5.5e-05,0.071,-0.036,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0051,0.22,0.23,0.03,2.9e-07,2.6e-07,2.6e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31490000,-0.27,0.017,-0.0053,0.96,0.036,0.0065,0.8,0.096,-0.038,-0.52,-0.0012,-0.0058,5.2e-05,0.071,-0.036,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.021,0.023,0.0052,0.23,0.24,0.03,2.9e-07,2.6e-07,2.6e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31590000,-0.27,0.017,-0.0052,0.96,0.037,0.0084,0.8,0.093,-0.035,-0.45,-0.0012,-0.0058,6.6e-05,0.071,-0.037,-0.12,-0.11,-0.024,0.5,0.082,-0.033,-0.067,0,0,0.00039,0.00034,0.044,0.02,0.022,0.0051,0.23,0.24,0.03,2.9e-07,2.6e-07,2.6e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31690000,-0.27,0.017,-0.0051,0.96,0.04,0.0097,0.8,0.098,-0.034,-0.38,-0.0012,-0.0058,7.6e-05,0.071,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.0004,0.00034,0.044,0.021,0.023,0.0051,0.24,0.25,0.03,2.9e-07,2.6e-07,2.6e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31790000,-0.27,0.018,-0.0054,0.96,0.034,0.015,0.8,0.094,-0.025,-0.3,-0.0012,-0.0058,9.7e-05,0.071,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.0004,0.00034,0.044,0.02,0.022,0.0051,0.24,0.25,0.03,2.9e-07,2.5e-07,2.6e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31890000,-0.27,0.018,-0.0051,0.96,0.033,0.017,0.8,0.098,-0.023,-0.23,-0.0012,-0.0058,0.0001,0.071,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.0004,0.00034,0.044,0.021,0.023,0.0051,0.25,0.26,0.03,2.9e-07,2.5e-07,2.5e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -31990000,-0.27,0.017,-0.0055,0.96,0.029,0.019,0.79,0.095,-0.018,-0.17,-0.0012,-0.0058,0.0001,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00039,0.00034,0.043,0.02,0.022,0.0051,0.25,0.26,0.03,2.9e-07,2.5e-07,2.5e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32090000,-0.27,0.017,-0.0059,0.96,0.031,0.023,0.8,0.099,-0.015,-0.095,-0.0012,-0.0058,9.9e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00039,0.00034,0.043,0.021,0.023,0.0051,0.26,0.27,0.03,2.9e-07,2.5e-07,2.5e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32190000,-0.27,0.017,-0.0061,0.96,0.027,0.03,0.8,0.094,-0.0067,-0.027,-0.0012,-0.0058,0.0001,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00039,0.00034,0.043,0.02,0.022,0.0051,0.26,0.27,0.03,2.8e-07,2.5e-07,2.5e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32290000,-0.27,0.017,-0.006,0.96,0.027,0.033,0.8,0.097,-0.0036,0.042,-0.0012,-0.0058,0.00011,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00039,0.00034,0.043,0.021,0.023,0.0051,0.27,0.28,0.03,2.8e-07,2.5e-07,2.5e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32390000,-0.27,0.017,-0.0061,0.96,0.024,0.035,0.8,0.093,-0.00038,0.12,-0.0012,-0.0058,0.00011,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00039,0.00034,0.043,0.02,0.022,0.0051,0.27,0.28,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32490000,-0.27,0.016,-0.0092,0.96,-0.017,0.093,-0.077,0.092,0.0079,0.12,-0.0012,-0.0058,0.0001,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00038,0.00035,0.043,0.022,0.025,0.0051,0.28,0.29,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32590000,-0.27,0.016,-0.0091,0.96,-0.014,0.091,-0.079,0.092,-0.00015,0.1,-0.0012,-0.0058,9.1e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00038,0.00035,0.043,0.021,0.024,0.0051,0.28,0.29,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32690000,-0.27,0.015,-0.0091,0.96,-0.01,0.098,-0.081,0.091,0.0093,0.088,-0.0012,-0.0058,9e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00038,0.00035,0.043,0.022,0.025,0.0051,0.29,0.3,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32790000,-0.27,0.016,-0.009,0.96,-0.0061,0.096,-0.082,0.092,0.00074,0.073,-0.0012,-0.0058,7.9e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.023,0.0051,0.29,0.3,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32890000,-0.27,0.016,-0.0089,0.96,-0.0065,0.1,-0.083,0.091,0.01,0.058,-0.0012,-0.0058,8.5e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00035,0.043,0.021,0.024,0.0051,0.3,0.31,0.03,2.8e-07,2.5e-07,2.4e-06,0.028,0.028,0.00011,0.0011,5.4e-05,0.0012,0.0011,0.00063,0.0012,1,1 -32990000,-0.27,0.016,-0.0088,0.96,-0.0026,0.096,-0.083,0.092,-0.0038,0.044,-0.0012,-0.0058,7e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.023,0.0051,0.3,0.31,0.03,2.7e-07,2.4e-07,2.3e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33090000,-0.27,0.016,-0.0087,0.96,0.0013,0.1,-0.08,0.092,0.0061,0.037,-0.0012,-0.0058,7.1e-05,0.072,-0.038,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.31,0.32,0.03,2.7e-07,2.4e-07,2.3e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33190000,-0.27,0.016,-0.0086,0.96,0.0057,0.097,-0.079,0.093,-0.0098,0.029,-0.0012,-0.0058,4e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.0051,0.31,0.32,0.03,2.7e-07,2.4e-07,2.3e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33290000,-0.27,0.016,-0.0086,0.96,0.0098,0.1,-0.079,0.094,-0.00037,0.021,-0.0012,-0.0058,5.7e-05,0.073,-0.037,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.32,0.33,0.03,2.7e-07,2.4e-07,2.3e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33390000,-0.27,0.016,-0.0086,0.96,0.014,0.096,-0.077,0.093,-0.0094,0.013,-0.0013,-0.0058,4.5e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.033,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.0051,0.32,0.33,0.03,2.7e-07,2.4e-07,2.3e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33490000,-0.27,0.016,-0.0085,0.96,0.02,0.1,-0.076,0.096,0.0004,0.0031,-0.0013,-0.0058,5.4e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.33,0.34,0.03,2.7e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00063,0.0012,1,1 -33590000,-0.27,0.016,-0.0084,0.96,0.023,0.097,-0.073,0.095,-0.013,-0.0048,-0.0013,-0.0058,4.6e-05,0.073,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.33,0.34,0.03,2.7e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -33690000,-0.27,0.016,-0.0084,0.96,0.026,0.1,-0.074,0.096,-0.0038,-0.013,-0.0013,-0.0058,5.1e-05,0.074,-0.036,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.34,0.35,0.03,2.7e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -33790000,-0.27,0.016,-0.0083,0.96,0.029,0.097,-0.068,0.093,-0.018,-0.02,-0.0013,-0.0058,2.9e-05,0.074,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.34,0.35,0.03,2.7e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -33890000,-0.27,0.016,-0.0083,0.96,0.033,0.099,-0.068,0.096,-0.0081,-0.026,-0.0013,-0.0058,4.5e-05,0.074,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.35,0.36,0.03,2.7e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -33990000,-0.27,0.016,-0.0082,0.96,0.036,0.096,-0.064,0.095,-0.017,-0.03,-0.0013,-0.0058,2.8e-05,0.075,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.02,0.023,0.005,0.35,0.36,0.03,2.6e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34090000,-0.27,0.016,-0.0081,0.96,0.039,0.1,-0.063,0.098,-0.0072,-0.034,-0.0013,-0.0058,3.2e-05,0.075,-0.035,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.043,0.021,0.024,0.0051,0.36,0.37,0.03,2.6e-07,2.4e-07,2.2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34190000,-0.27,0.016,-0.0081,0.96,0.04,0.097,-0.06,0.093,-0.019,-0.038,-0.0013,-0.0057,2.4e-05,0.075,-0.034,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00034,0.042,0.02,0.022,0.005,0.36,0.37,0.03,2.6e-07,2.4e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34290000,-0.27,0.016,-0.0079,0.96,0.041,0.1,-0.059,0.097,-0.0092,-0.044,-0.0013,-0.0057,3.6e-05,0.075,-0.034,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00037,0.00034,0.042,0.021,0.023,0.005,0.37,0.38,0.03,2.6e-07,2.4e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34390000,-0.27,0.016,-0.0078,0.96,0.043,0.096,-0.054,0.092,-0.021,-0.048,-0.0013,-0.0057,2.4e-05,0.076,-0.034,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00034,0.042,0.02,0.022,0.005,0.37,0.37,0.03,2.6e-07,2.4e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34490000,-0.27,0.016,-0.0079,0.96,0.045,0.1,-0.052,0.096,-0.011,-0.05,-0.0013,-0.0057,3.6e-05,0.076,-0.034,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00034,0.042,0.021,0.023,0.005,0.38,0.39,0.03,2.6e-07,2.4e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34590000,-0.27,0.016,-0.0078,0.96,0.049,0.093,0.74,0.091,-0.026,-0.022,-0.0013,-0.0057,2.3e-05,0.076,-0.033,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00033,0.042,0.019,0.021,0.005,0.38,0.38,0.03,2.6e-07,2.3e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34690000,-0.27,0.015,-0.0078,0.96,0.058,0.094,1.7,0.096,-0.016,0.097,-0.0013,-0.0057,2.9e-05,0.076,-0.033,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00033,0.042,0.02,0.022,0.0051,0.39,0.4,0.03,2.6e-07,2.3e-07,2.1e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34790000,-0.27,0.015,-0.0077,0.96,0.062,0.087,2.7,0.089,-0.03,0.28,-0.0013,-0.0057,1.9e-05,0.077,-0.033,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00033,0.042,0.02,0.021,0.005,0.39,0.39,0.03,2.6e-07,2.3e-07,2e-06,0.028,0.028,0.00011,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 -34890000,-0.27,0.015,-0.0077,0.96,0.071,0.088,3.7,0.096,-0.021,0.57,-0.0013,-0.0057,2.5e-05,0.077,-0.033,-0.12,-0.11,-0.024,0.5,0.081,-0.034,-0.067,0,0,0.00036,0.00033,0.042,0.021,0.023,0.005,0.4,0.41,0.03,2.6e-07,2.3e-07,2e-06,0.028,0.028,0.0001,0.0011,5.3e-05,0.0012,0.0011,0.00062,0.0012,1,1 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,8.1e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.032 +90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.1,0.01,0.01,0.00036,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.058 +190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-2.1e-11,0,0,-1.1e-06,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00084,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.082 +290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-2.1e-10,0,0,-1e-05,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.00075,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.11 +390000,1,-0.012,-0.011,0.00049,-0.0025,-0.0059,-0.063,-0.00056,-0.00071,-0.013,-7.1e-09,-5.8e-09,1.5e-11,0,0,2.2e-06,0,0,0,0,0,0,0,0,0.095,0.012,0.012,0.0012,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0049,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.13 +490000,1,-0.012,-0.012,0.00055,-0.0007,-0.0062,-0.069,-0.00015,-0.00046,-0.011,-1.2e-06,7.4e-07,4.1e-08,0,0,-1e-06,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00072,7.8,7.8,5.9,0.34,0.34,0.31,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.16 +590000,1,-0.012,-0.012,0.00057,-0.002,-0.009,-0.12,-0.00028,-0.0012,-0.029,-1.3e-06,7.7e-07,4.5e-08,0,0,7.8e-05,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00099,7.9,7.9,4.2,0.67,0.67,0.32,0.01,0.01,0.0021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.18 +690000,1,-0.012,-0.012,0.0006,5.4e-05,-0.0088,-0.05,-8e-05,-0.00078,-0.0088,-5.6e-06,1.6e-06,1.6e-07,0,0,-0.00016,0,0,0,0,0,0,0,0,0.095,0.016,0.016,0.00061,2.7,2.7,2.8,0.26,0.26,0.29,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.21 +790000,1,-0.012,-0.012,0.0006,0.0022,-0.01,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.6e-06,1.5e-07,0,0,-0.0002,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00077,2.8,2.8,1.9,0.42,0.42,0.27,0.01,0.01,0.00098,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.23 +890000,1,-0.012,-0.013,0.00061,0.0031,-0.0084,-0.093,0.00015,-0.0011,-0.031,-2.1e-05,1e-06,4.9e-07,0,0,-8.1e-05,0,0,0,0,0,0,0,0,0.095,0.019,0.019,0.00051,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.26 +990000,1,-0.012,-0.013,0.00058,0.006,-0.0097,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1e-06,4.9e-07,0,0,-2.6e-05,0,0,0,0,0,0,0,0,0.095,0.021,0.021,0.00062,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00053,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.28 +1090000,1,-0.012,-0.013,0.00054,0.011,-0.013,-0.13,0.00077,-0.0014,-0.062,-6e-05,-1.5e-05,9.8e-07,0,0,1.1e-05,0,0,0,0,0,0,0,0,0.095,0.023,0.023,0.00043,0.93,0.93,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.31 +1190000,1,-0.012,-0.013,0.00047,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,9.6e-07,0,0,-0.00056,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00051,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00032,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.33 +1290000,1,-0.012,-0.014,0.00042,0.019,-0.018,-0.11,0.0019,-0.0024,-0.048,-0.00017,-9.7e-05,1.5e-06,0,0,-0.00083,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.00038,0.89,0.89,0.42,0.15,0.15,0.18,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.36 +1390000,1,-0.012,-0.014,0.00038,0.026,-0.023,-0.097,0.0043,-0.0044,-0.038,-0.00016,-9.2e-05,1.5e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00043,1.2,1.2,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00021,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.38 +1490000,1,-0.012,-0.014,0.00038,0.024,-0.02,-0.12,0.0034,-0.0032,-0.053,-0.00039,-0.00033,1.2e-06,0,0,-0.0013,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00033,0.96,0.96,0.27,0.14,0.14,0.15,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.41 +1590000,1,-0.012,-0.014,0.00039,0.031,-0.024,-0.13,0.0061,-0.0055,-0.063,-0.00039,-0.00033,1.2e-06,0,0,-0.0015,0,0,0,0,0,0,0,0,0.095,0.03,0.03,0.00037,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.00014,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.43 +1690000,1,-0.012,-0.014,0.00044,0.028,-0.019,-0.13,0.0043,-0.0037,-0.068,-0.00073,-0.00074,-3.4e-07,0,0,-0.0019,0,0,0,0,0,0,0,0,0.095,0.026,0.026,0.0003,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.46 +1790000,1,-0.012,-0.014,0.0004,0.035,-0.024,-0.13,0.0076,-0.0059,-0.067,-0.00073,-0.00073,-2.9e-07,0,0,-0.0029,0,0,0,0,0,0,0,0,0.095,0.028,0.028,0.00033,1.3,1.3,0.17,0.2,0.2,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.48 +1890000,1,-0.012,-0.014,0.00039,0.043,-0.025,-0.14,0.011,-0.0083,-0.075,-0.00072,-0.00072,-2.6e-07,0,0,-0.0033,0,0,0,0,0,0,0,0,0.095,0.031,0.031,0.00037,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.0001,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.51 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0.0082,-0.0054,-0.074,-0.0011,-0.0013,-3.6e-06,0,0,-0.0047,0,0,0,0,0,0,0,0,0.095,0.025,0.025,0.00029,1.3,1.3,0.13,0.2,0.2,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.53 +2090000,1,-0.011,-0.014,0.00043,0.042,-0.02,-0.14,0.012,-0.0073,-0.071,-0.0011,-0.0012,-3.5e-06,0,0,-0.0066,0,0,0,0,0,0,0,0,0.095,0.027,0.027,0.00032,1.7,1.7,0.12,0.31,0.31,0.11,0.0067,0.0067,7.6e-05,0.04,0.04,0.039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.56 +2190000,1,-0.011,-0.014,0.00039,0.033,-0.014,-0.14,0.0081,-0.0043,-0.077,-0.0014,-0.0018,-8.7e-06,0,0,-0.0076,0,0,0,0,0,0,0,0,0.095,0.02,0.02,0.00027,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.58 +2290000,1,-0.011,-0.014,0.00038,0.038,-0.014,-0.14,0.012,-0.0057,-0.075,-0.0014,-0.0018,-8.5e-06,0,0,-0.0099,0,0,0,0,0,0,0,0,0.095,0.022,0.022,0.00029,1.5,1.5,0.11,0.3,0.3,0.1,0.0055,0.0055,5.8e-05,0.04,0.04,0.038,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.61 +2390000,1,-0.011,-0.013,0.0004,0.029,-0.01,-0.14,0.0075,-0.0033,-0.072,-0.0017,-0.0023,-1.4e-05,0,0,-0.013,0,0,0,0,0,0,0,0,0.095,0.017,0.017,0.00024,1,1,0.1,0.19,0.19,0.098,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.63 +2490000,1,-0.011,-0.014,0.00047,0.033,-0.0091,-0.14,0.011,-0.0043,-0.079,-0.0017,-0.0023,-1.4e-05,0,0,-0.014,0,0,0,0,0,0,0,0,0.095,0.018,0.018,0.00026,1.3,1.3,0.1,0.28,0.28,0.097,0.0046,0.0046,4.5e-05,0.04,0.04,0.037,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.66 +2590000,1,-0.01,-0.013,0.00039,0.023,-0.0062,-0.15,0.0066,-0.0024,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.015,0,0,0,0,0,0,0,0,0.095,0.014,0.014,0.00022,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.68 +2690000,1,-0.01,-0.013,0.00043,0.027,-0.0055,-0.15,0.0091,-0.003,-0.084,-0.0018,-0.0027,-2e-05,0,0,-0.018,0,0,0,0,0,0,0,0,0.095,0.015,0.015,0.00024,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,3.6e-05,0.04,0.04,0.036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.71 +2790000,1,-0.01,-0.013,0.00037,0.022,-0.0032,-0.14,0.0059,-0.0017,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.022,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.77,0.77,0.095,0.16,0.16,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.035,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.73 +2890000,1,-0.01,-0.013,0.0003,0.026,-0.005,-0.14,0.0082,-0.0021,-0.081,-0.0019,-0.003,-2.5e-05,0,0,-0.026,0,0,0,0,0,0,0,0,0.095,0.013,0.013,0.00022,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,2.9e-05,0.04,0.04,0.034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.76 +2990000,1,-0.01,-0.013,0.00031,0.02,-0.0039,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,-3e-05,0,0,-0.028,0,0,0,0,0,0,0,0,0.095,0.0099,0.0099,0.00019,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,2.4e-05,0.04,0.04,0.033,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.78 +3090000,1,-0.01,-0.013,0.00052,0.025,-0.0068,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,-3e-05,0,0,-0.031,0,0,0,0,0,0,0,0,0.095,0.011,0.011,0.00021,0.83,0.83,0.095,0.22,0.22,0.086,0.0027,0.0027,2.4e-05,0.04,0.04,0.032,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.81 +3190000,1,-0.01,-0.013,0.00056,0.02,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,-3.5e-05,0,0,-0.033,0,0,0,0,0,0,0,0,0.095,0.0088,0.0088,0.00018,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,2e-05,0.04,0.04,0.031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.83 +3290000,1,-0.01,-0.013,0.00058,0.023,-0.0067,-0.15,0.0073,-0.0021,-0.11,-0.0021,-0.0036,-3.4e-05,0,0,-0.035,0,0,0,0,0,0,0,0,0.095,0.0096,0.0096,0.00019,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,2e-05,0.04,0.04,0.03,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.86 +3390000,1,-0.0098,-0.013,0.0006,0.019,-0.0036,-0.15,0.0049,-0.0014,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.04,0,0,0,0,0,0,0,0,0.095,0.0078,0.0078,0.00017,0.53,0.53,0.095,0.14,0.14,0.085,0.002,0.002,1.7e-05,0.04,0.04,0.029,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.88 +3490000,1,-0.0097,-0.013,0.00058,0.025,-0.0023,-0.15,0.0072,-0.0017,-0.1,-0.0021,-0.0038,-3.8e-05,0,0,-0.044,0,0,0,0,0,0,0,0,0.095,0.0086,0.0086,0.00018,0.66,0.66,0.095,0.19,0.19,0.086,0.002,0.002,1.7e-05,0.04,0.04,0.027,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.91 +3590000,1,-0.0095,-0.012,0.00054,0.021,-0.0018,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.047,0,0,0,0,0,0,0,0,0.095,0.0071,0.0071,0.00016,0.49,0.49,0.094,0.13,0.13,0.086,0.0017,0.0017,1.4e-05,0.04,0.04,0.026,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.93 +3690000,1,-0.0095,-0.013,0.00052,0.024,-0.0011,-0.15,0.0074,-0.0012,-0.11,-0.0022,-0.004,-4.2e-05,0,0,-0.052,0,0,0,0,0,0,0,0,0.095,0.0077,0.0077,0.00017,0.6,0.6,0.093,0.18,0.18,0.085,0.0017,0.0017,1.4e-05,0.04,0.04,0.025,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.96 +3790000,1,-0.0094,-0.012,0.00055,0.019,0.0033,-0.15,0.0051,-0.00056,-0.11,-0.0022,-0.0043,-4.7e-05,0,0,-0.055,0,0,0,0,0,0,0,0,0.095,0.0064,0.0064,0.00015,0.45,0.45,0.093,0.12,0.12,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.024,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,0.98 +3890000,1,-0.0094,-0.013,0.00063,0.021,0.0045,-0.14,0.0072,-0.00017,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.059,0,0,0,0,0,0,0,0,0.095,0.0069,0.0069,0.00016,0.55,0.55,0.091,0.17,0.17,0.086,0.0014,0.0014,1.2e-05,0.04,0.04,0.022,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +3990000,1,-0.0094,-0.013,0.00069,0.026,0.0042,-0.14,0.0096,0.00021,-0.11,-0.0022,-0.0042,-4.7e-05,0,0,-0.064,0,0,0,0,0,0,0,0,0.095,0.0075,0.0075,0.00017,0.66,0.66,0.089,0.23,0.23,0.085,0.0014,0.0014,1.2e-05,0.04,0.04,0.021,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1 +4090000,1,-0.0093,-0.012,0.00076,0.022,0.0037,-0.12,0.0071,0.00045,-0.098,-0.0022,-0.0044,-5.1e-05,0,0,-0.072,0,0,0,0,0,0,0,0,0.095,0.0062,0.0062,0.00015,0.5,0.5,0.087,0.16,0.16,0.085,0.0012,0.0012,1e-05,0.04,0.04,0.02,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4190000,1,-0.0094,-0.012,0.00072,0.024,0.0034,-0.12,0.0094,0.0008,-0.1,-0.0022,-0.0044,-5.1e-05,0,0,-0.074,0,0,0,0,0,0,0,0,0.095,0.0068,0.0068,0.00016,0.61,0.61,0.086,0.21,0.21,0.086,0.0012,0.0012,1e-05,0.04,0.04,0.019,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4290000,1,-0.0095,-0.012,0.00074,0.021,0.0033,-0.12,0.0068,0.00068,-0.11,-0.0022,-0.0046,-5.6e-05,0,0,-0.077,0,0,0,0,0,0,0,0,0.095,0.0056,0.0056,0.00014,0.47,0.47,0.084,0.15,0.15,0.085,0.00097,0.00097,9.1e-06,0.04,0.04,0.017,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4390000,1,-0.0094,-0.012,0.00069,0.025,0.0018,-0.11,0.0091,0.00086,-0.094,-0.0022,-0.0046,-5.6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,0.095,0.006,0.006,0.00015,0.56,0.56,0.081,0.2,0.2,0.084,0.00097,0.00097,9.1e-06,0.04,0.04,0.016,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.1 +4490000,1,-0.0094,-0.012,0.00076,0.021,0.0036,-0.11,0.0067,0.00072,-0.095,-0.0022,-0.0048,-6.1e-05,0,0,-0.086,0,0,0,0,0,0,0,0,0.095,0.005,0.005,0.00014,0.43,0.43,0.08,0.14,0.14,0.085,0.0008,0.0008,7.9e-06,0.04,0.04,0.015,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4590000,1,-0.0094,-0.012,0.00082,0.023,0.0024,-0.11,0.0089,0.001,-0.098,-0.0022,-0.0048,-6.1e-05,0,0,-0.088,0,0,0,0,0,0,0,0,0.095,0.0054,0.0054,0.00014,0.52,0.52,0.077,0.19,0.19,0.084,0.0008,0.0008,7.9e-06,0.04,0.04,0.014,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4690000,1,-0.0094,-0.012,0.00076,0.017,0.0027,-0.1,0.0064,0.00074,-0.09,-0.0021,-0.005,-6.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,0.095,0.0044,0.0044,0.00013,0.4,0.4,0.074,0.14,0.14,0.083,0.00065,0.00065,6.9e-06,0.04,0.04,0.013,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4790000,1,-0.0093,-0.012,0.00086,0.015,0.0048,-0.099,0.008,0.0012,-0.092,-0.0021,-0.005,-6.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,0.095,0.0047,0.0047,0.00014,0.47,0.47,0.073,0.18,0.18,0.084,0.00065,0.00065,6.9e-06,0.04,0.04,0.012,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.2 +4890000,1,-0.0093,-0.012,0.0009,0.01,0.0024,-0.093,0.0053,0.0009,-0.088,-0.0021,-0.0051,-6.9e-05,0,0,-0.099,0,0,0,0,0,0,0,0,0.095,0.0039,0.0039,0.00012,0.36,0.36,0.07,0.13,0.13,0.083,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +4990000,1,-0.0092,-0.012,0.00088,0.013,0.0031,-0.085,0.0065,0.0012,-0.083,-0.0021,-0.0051,-6.9e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0042,0.0042,0.00013,0.43,0.43,0.067,0.17,0.17,0.082,0.00053,0.00053,6.1e-06,0.04,0.04,0.011,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5090000,1,-0.0091,-0.011,0.00096,0.01,0.0034,-0.082,0.0045,0.00087,-0.082,-0.0021,-0.0052,-7.2e-05,0,0,-0.1,0,0,0,0,0,0,0,0,0.095,0.0034,0.0034,0.00012,0.33,0.33,0.065,0.12,0.12,0.082,0.00043,0.00043,5.4e-06,0.04,0.04,0.0098,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5190000,1,-0.0089,-0.012,0.001,0.0099,0.007,-0.08,0.0055,0.0014,-0.079,-0.0021,-0.0052,-7.2e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0037,0.0037,0.00012,0.39,0.39,0.063,0.16,0.16,0.081,0.00043,0.00043,5.4e-06,0.04,0.04,0.0091,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.3 +5290000,1,-0.0089,-0.011,0.0011,0.0082,0.0071,-0.068,0.0038,0.0013,-0.072,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.003,0.003,0.00011,0.3,0.3,0.06,0.12,0.12,0.08,0.00035,0.00035,4.8e-06,0.04,0.04,0.0084,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5390000,1,-0.0088,-0.011,0.0011,0.0077,0.011,-0.065,0.0046,0.0021,-0.067,-0.0021,-0.0053,-7.4e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0032,0.0032,0.00012,0.36,0.36,0.057,0.16,0.16,0.079,0.00035,0.00035,4.8e-06,0.04,0.04,0.0078,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5490000,1,-0.0088,-0.011,0.0011,0.0072,0.012,-0.06,0.0031,0.002,-0.065,-0.002,-0.0054,-7.7e-05,0,0,-0.11,0,0,0,0,0,0,0,0,0.095,0.0027,0.0027,0.00011,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,4.3e-06,0.04,0.04,0.0073,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5590000,1,-0.0089,-0.012,0.00099,0.0083,0.016,-0.053,0.004,0.0033,-0.058,-0.002,-0.0054,-7.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0028,0.0028,0.00011,0.33,0.33,0.053,0.15,0.15,0.078,0.00028,0.00028,4.3e-06,0.04,0.04,0.0067,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.4 +5690000,1,-0.0089,-0.011,0.00089,0.0077,0.016,-0.052,0.0028,0.0029,-0.055,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0024,0.0024,0.00011,0.25,0.25,0.051,0.11,0.11,0.076,0.00023,0.00023,3.8e-06,0.04,0.04,0.0063,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5790000,1,-0.0088,-0.011,0.00085,0.0089,0.018,-0.049,0.0036,0.0046,-0.053,-0.002,-0.0054,-7.9e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0025,0.0025,0.00011,0.3,0.3,0.05,0.14,0.14,0.077,0.00023,0.00023,3.8e-06,0.04,0.04,0.0059,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5890000,1,-0.0088,-0.011,0.00088,0.0095,0.015,-0.048,0.0027,0.0037,-0.056,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.0001,0.23,0.23,0.047,0.1,0.1,0.075,0.00018,0.00018,3.5e-06,0.04,0.04,0.0054,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +5990000,1,-0.0088,-0.012,0.00085,0.011,0.017,-0.041,0.0038,0.0053,-0.05,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0022,0.0022,0.00011,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.005,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.5 +6090000,1,-0.0088,-0.011,0.00067,0.011,0.018,-0.039,0.0049,0.0071,-0.047,-0.0019,-0.0055,-8.2e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0023,0.0023,0.00011,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,3.5e-06,0.04,0.04,0.0047,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6190000,1,-0.0089,-0.011,0.00067,0.0087,0.017,-0.038,0.0038,0.0057,-0.047,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.002,0.002,0.0001,0.24,0.24,0.042,0.13,0.13,0.073,0.00015,0.00015,3.1e-06,0.04,0.04,0.0044,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6290000,1,-0.0089,-0.011,0.0007,0.008,0.019,-0.041,0.0046,0.0075,-0.053,-0.0019,-0.0055,-8.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0021,0.0021,0.00011,0.28,0.28,0.04,0.16,0.16,0.072,0.00015,0.00015,3.1e-06,0.04,0.04,0.0041,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6390000,1,-0.0089,-0.011,0.00071,0.0082,0.016,-0.042,0.0034,0.006,-0.056,-0.0018,-0.0056,-8.7e-05,0,0,-0.12,0,0,0,0,0,0,0,0,0.095,0.0017,0.0017,9.9e-05,0.22,0.22,0.039,0.12,0.12,0.072,0.00012,0.00012,2.9e-06,0.04,0.04,0.0039,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.6 +6490000,1,-0.0089,-0.011,0.00062,0.0057,0.016,-0.039,0.0041,0.0076,-0.053,-0.0018,-0.0056,-8.7e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0018,0.0018,0.0001,0.25,0.25,0.038,0.15,0.15,0.07,0.00012,0.00012,2.9e-06,0.04,0.04,0.0036,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6590000,1,-0.0089,-0.011,0.00055,0.0039,0.015,-0.042,0.0029,0.0058,-0.056,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.6e-05,0.2,0.2,0.036,0.12,0.12,0.069,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0034,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6690000,1,-0.0088,-0.011,0.0005,0.0022,0.018,-0.044,0.0032,0.0075,-0.057,-0.0017,-0.0056,-8.9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6790000,1,-0.0089,-0.011,0.00047,0.003,0.015,-0.042,0.0021,0.006,-0.058,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7 +6890000,1,-0.0087,-0.011,0.00039,0.0023,0.015,-0.039,0.0024,0.0075,-0.055,-0.0017,-0.0056,-9.1e-05,0,0,-0.13,0,0,0,0,0,0,0,0,0.095,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8 +6990000,-0.29,0.024,-0.0061,0.96,-0.2,-0.032,-0.037,-0.11,-0.019,-0.055,-0.00077,-0.0097,-0.0002,0,0,-0.13,-0.091,-0.021,0.51,0.069,-0.028,-0.058,0,0,0.095,0.0011,0.0012,0.076,0.16,0.16,0.031,0.1,0.1,0.066,6.3e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0014,0.00049,0.0014,0.0014,0.0012,0.0014,1,1,1.8 +7090000,-0.29,0.025,-0.0062,0.96,-0.24,-0.048,-0.038,-0.15,-0.027,-0.056,-0.00064,-0.01,-0.00021,0,0,-0.13,-0.099,-0.023,0.51,0.075,-0.029,-0.065,0,0,0.095,0.0011,0.0011,0.063,0.19,0.18,0.03,0.13,0.13,0.066,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.0024,0.0013,0.00025,0.0013,0.0013,0.00095,0.0013,1,1,1.8 +7190000,-0.29,0.025,-0.0062,0.96,-0.26,-0.057,-0.037,-0.17,-0.034,-0.059,-0.0006,-0.01,-0.00021,-0.00035,-7.7e-05,-0.13,-0.1,-0.023,0.51,0.077,-0.03,-0.067,0,0,0.095,0.0011,0.0011,0.06,0.21,0.21,0.029,0.16,0.15,0.065,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00018,0.0013,0.0013,0.0009,0.0013,1,1,1.8 +7290000,-0.29,0.025,-0.0063,0.96,-0.28,-0.066,-0.034,-0.2,-0.04,-0.055,-0.00059,-0.01,-0.00021,-0.00088,-0.00019,-0.13,-0.1,-0.023,0.51,0.077,-0.03,-0.067,0,0,0.095,0.001,0.0011,0.058,0.25,0.24,0.028,0.19,0.19,0.064,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00015,0.0013,0.0013,0.00087,0.0013,1,1,1.9 +7390000,-0.29,0.024,-0.0062,0.96,-0.3,-0.072,-0.032,-0.23,-0.047,-0.052,-0.00058,-0.01,-0.00021,-0.0011,-0.00023,-0.13,-0.1,-0.024,0.5,0.078,-0.03,-0.068,0,0,0.095,0.001,0.001,0.057,0.28,0.28,0.027,0.23,0.23,0.064,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00014,0.0013,0.0013,0.00086,0.0013,1,1,1.9 +7490000,-0.29,0.024,-0.0063,0.96,-0.32,-0.08,-0.026,-0.25,-0.056,-0.046,-0.00057,-0.0099,-0.00021,-0.0015,-0.00027,-0.13,-0.1,-0.024,0.5,0.078,-0.03,-0.068,0,0,0.095,0.00097,0.001,0.056,0.32,0.31,0.026,0.28,0.28,0.063,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00012,0.0013,0.0013,0.00085,0.0013,1,1,1.9 +7590000,-0.29,0.024,-0.0064,0.96,-0.34,-0.088,-0.022,-0.28,-0.065,-0.041,-0.00055,-0.0098,-0.00021,-0.0016,-0.00024,-0.13,-0.1,-0.024,0.5,0.078,-0.03,-0.068,0,0,0.095,0.00094,0.00097,0.056,0.36,0.36,0.025,0.34,0.34,0.062,6.2e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00012,0.0013,0.0013,0.00085,0.0013,1,1,1.9 +7690000,-0.29,0.024,-0.0064,0.96,-0.35,-0.096,-0.022,-0.31,-0.076,-0.036,-0.00053,-0.0097,-0.00021,-0.0017,-0.00022,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00091,0.00094,0.055,0.4,0.4,0.025,0.41,0.4,0.062,6.1e-05,6.2e-05,2.2e-06,0.04,0.04,0.0017,0.0013,0.00011,0.0013,0.0013,0.00084,0.0013,1,1,2 +7790000,-0.29,0.023,-0.0062,0.96,-0.36,-0.11,-0.024,-0.34,-0.09,-0.041,-0.00046,-0.0096,-0.0002,-0.0017,-0.00025,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00088,0.00091,0.055,0.45,0.45,0.024,0.48,0.48,0.061,6.1e-05,6.2e-05,2.2e-06,0.04,0.04,0.0016,0.0013,0.00011,0.0013,0.0013,0.00084,0.0013,1,1,2 +7890000,-0.29,0.023,-0.0063,0.96,-0.37,-0.11,-0.025,-0.37,-0.1,-0.045,-0.00044,-0.0095,-0.0002,-0.0016,-0.00025,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00086,0.00088,0.055,0.5,0.49,0.023,0.57,0.56,0.06,6e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,0.0001,0.0013,0.0013,0.00083,0.0013,1,1,2 +7990000,-0.29,0.023,-0.0063,0.96,-0.39,-0.12,-0.021,-0.4,-0.11,-0.041,-0.00047,-0.0094,-0.0002,-0.0016,-0.0002,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00083,0.00085,0.054,0.55,0.54,0.022,0.67,0.66,0.059,6e-05,6.1e-05,2.2e-06,0.04,0.04,0.0015,0.0013,9.8e-05,0.0013,0.0012,0.00083,0.0013,1,1,2 +8090000,-0.29,0.023,-0.0062,0.96,-0.4,-0.13,-0.022,-0.43,-0.13,-0.044,-0.0004,-0.0094,-0.0002,-0.0016,-0.00025,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.0008,0.00083,0.054,0.6,0.6,0.022,0.79,0.77,0.059,5.9e-05,6e-05,2.2e-06,0.04,0.04,0.0014,0.0013,9.6e-05,0.0013,0.0012,0.00083,0.0013,1,1,2.1 +8190000,-0.29,0.022,-0.0062,0.96,-0.41,-0.14,-0.017,-0.46,-0.15,-0.038,-0.00033,-0.0092,-0.0002,-0.0016,-0.00026,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00078,0.0008,0.054,0.66,0.65,0.021,0.91,0.9,0.058,5.8e-05,5.9e-05,2.2e-06,0.04,0.04,0.0013,0.0012,9.4e-05,0.0013,0.0012,0.00082,0.0013,1,1,2.1 +8290000,-0.29,0.022,-0.0064,0.96,-0.44,-0.15,-0.016,-0.51,-0.16,-0.038,-0.00038,-0.0093,-0.0002,-0.0017,-0.00023,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00075,0.00078,0.054,0.71,0.7,0.02,1.1,1,0.057,5.7e-05,5.8e-05,2.2e-06,0.04,0.04,0.0013,0.0012,9.2e-05,0.0013,0.0012,0.00082,0.0013,1,1,2.1 +8390000,-0.29,0.022,-0.0064,0.96,-0.45,-0.15,-0.015,-0.55,-0.17,-0.036,-0.00039,-0.0092,-0.0002,-0.0017,-0.00021,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00073,0.00075,0.054,0.77,0.76,0.02,1.2,1.2,0.057,5.6e-05,5.7e-05,2.2e-06,0.04,0.04,0.0012,0.0012,9e-05,0.0013,0.0012,0.00082,0.0013,1,1,2.1 +8490000,-0.29,0.022,-0.0064,0.96,-0.45,-0.15,-0.016,-0.58,-0.18,-0.041,-0.00048,-0.0091,-0.00019,-0.0016,-0.00013,-0.13,-0.1,-0.024,0.5,0.079,-0.031,-0.068,0,0,0.095,0.00071,0.00073,0.053,0.82,0.81,0.019,1.4,1.4,0.056,5.5e-05,5.5e-05,2.2e-06,0.04,0.04,0.0011,0.0012,8.9e-05,0.0013,0.0012,0.00082,0.0013,1,1,2.2 +8590000,-0.29,0.022,-0.0062,0.96,-0.45,-0.16,-0.012,-0.6,-0.2,-0.036,-0.00042,-0.0089,-0.00019,-0.0016,-0.00013,-0.13,-0.1,-0.024,0.5,0.08,-0.031,-0.069,0,0,0.095,0.00069,0.00071,0.053,0.88,0.87,0.019,1.6,1.5,0.055,5.3e-05,5.4e-05,2.2e-06,0.04,0.04,0.0011,0.0012,8.7e-05,0.0013,0.0012,0.00081,0.0013,1,1,2.2 +8690000,-0.29,0.021,-0.0063,0.96,-0.46,-0.16,-0.014,-0.63,-0.21,-0.037,-0.00048,-0.0088,-0.00019,-0.0016,-9e-05,-0.13,-0.1,-0.024,0.5,0.08,-0.031,-0.069,0,0,0.095,0.00067,0.00069,0.053,0.94,0.93,0.018,1.8,1.7,0.055,5.2e-05,5.2e-05,2.2e-06,0.04,0.04,0.001,0.0012,8.6e-05,0.0013,0.0012,0.00081,0.0013,1,1,2.2 +8790000,-0.29,0.021,-0.0064,0.96,-0.48,-0.17,-0.013,-0.67,-0.23,-0.035,-0.00046,-0.0088,-0.00018,-0.0016,-8.7e-05,-0.13,-0.1,-0.024,0.5,0.08,-0.031,-0.069,0,0,0.095,0.00065,0.00067,0.053,1,0.99,0.018,2,2,0.055,5e-05,5.1e-05,2.2e-06,0.04,0.04,0.00099,0.0012,8.5e-05,0.0013,0.0012,0.00081,0.0013,1,1,2.2 +8890000,-0.29,0.021,-0.0063,0.96,-0.47,-0.17,-0.0093,-0.68,-0.24,-0.029,-0.00051,-0.0085,-0.00018,-0.0016,1.9e-05,-0.13,-0.1,-0.024,0.5,0.08,-0.031,-0.069,0,0,0.095,0.00063,0.00065,0.053,1.1,1,0.017,2.2,2.2,0.054,4.8e-05,4.9e-05,2.2e-06,0.04,0.04,0.00095,0.0012,8.4e-05,0.0013,0.0012,0.0008,0.0013,1,1,2.3 +8990000,-0.29,0.02,-0.0061,0.96,-0.45,-0.17,-0.0088,-0.67,-0.25,-0.032,-0.00055,-0.0082,-0.00017,-0.0013,0.00012,-0.13,-0.1,-0.024,0.5,0.08,-0.032,-0.069,0,0,0.095,0.00062,0.00063,0.053,1.1,1.1,0.017,2.5,2.4,0.054,4.7e-05,4.7e-05,2.2e-06,0.04,0.04,0.00091,0.0012,8.3e-05,0.0013,0.0012,0.0008,0.0013,1,1,2.3 +9090000,-0.29,0.02,-0.0063,0.96,-0.46,-0.16,-0.0099,-0.72,-0.24,-0.032,-0.00071,-0.0082,-0.00016,-0.0012,0.00029,-0.13,-0.1,-0.024,0.5,0.08,-0.031,-0.069,0,0,0.095,0.0006,0.00062,0.053,1.2,1.2,0.016,2.7,2.7,0.053,4.5e-05,4.5e-05,2.2e-06,0.04,0.04,0.00087,0.0012,8.2e-05,0.0013,0.0012,0.0008,0.0013,1,1,2.3 +9190000,-0.29,0.02,-0.0063,0.96,-0.46,-0.16,-0.0095,-0.73,-0.24,-0.033,-0.00081,-0.008,-0.00016,-0.0011,0.00042,-0.13,-0.1,-0.024,0.5,0.081,-0.031,-0.069,0,0,0.095,0.00059,0.0006,0.053,1.2,1.2,0.016,3,3,0.052,4.3e-05,4.3e-05,2.2e-06,0.04,0.04,0.00084,0.0012,8.2e-05,0.0013,0.0012,0.00079,0.0013,1,1,2.3 +9290000,-0.29,0.02,-0.006,0.96,-0.45,-0.16,-0.0081,-0.73,-0.25,-0.03,-0.00081,-0.0078,-0.00015,-0.001,0.00048,-0.13,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00058,0.00059,0.053,1.3,1.3,0.015,3.3,3.3,0.052,4.1e-05,4.1e-05,2.2e-06,0.04,0.04,0.0008,0.0012,8.1e-05,0.0013,0.0012,0.00079,0.0013,1,1,2.4 +9390000,-0.29,0.02,-0.0058,0.96,-0.45,-0.18,-0.0071,-0.75,-0.29,-0.031,-0.00073,-0.0077,-0.00015,-0.00084,0.00046,-0.13,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00057,0.00058,0.053,1.4,1.4,0.015,3.7,3.6,0.052,4e-05,4e-05,2.2e-06,0.04,0.04,0.00077,0.0012,8e-05,0.0013,0.0012,0.00079,0.0013,1,1,2.4 +9490000,-0.29,0.019,-0.0059,0.96,-0.46,-0.17,-0.0055,-0.78,-0.29,-0.028,-0.0008,-0.0076,-0.00015,-0.00092,0.00055,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00056,0.00057,0.053,1.4,1.4,0.015,4,4,0.051,3.8e-05,3.8e-05,2.2e-06,0.04,0.04,0.00074,0.0012,8e-05,0.0013,0.0012,0.00079,0.0013,1,1,2.4 +9590000,-0.29,0.019,-0.0059,0.96,-0.46,-0.19,-0.0054,-0.81,-0.34,-0.029,-0.00068,-0.0075,-0.00015,-0.00085,0.00042,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00055,0.00056,0.053,1.5,1.5,0.014,4.4,4.3,0.05,3.6e-05,3.6e-05,2.2e-06,0.04,0.04,0.00072,0.0012,7.9e-05,0.0013,0.0012,0.00078,0.0013,1,1,2.4 +9690000,-0.29,0.019,-0.0058,0.96,-0.46,-0.2,-0.0026,-0.83,-0.37,-0.028,-0.00064,-0.0074,-0.00015,-0.00077,0.00041,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00054,0.00056,0.053,1.6,1.6,0.014,4.8,4.8,0.05,3.5e-05,3.4e-05,2.2e-06,0.04,0.04,0.00069,0.0012,7.9e-05,0.0013,0.0012,0.00078,0.0013,1,1,2.5 +9790000,-0.29,0.019,-0.0058,0.96,-0.48,-0.22,-0.0039,-0.88,-0.41,-0.029,-0.00055,-0.0074,-0.00015,-0.00075,0.00033,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00054,0.00055,0.053,1.6,1.6,0.014,5.2,5.2,0.05,3.3e-05,3.3e-05,2.2e-06,0.04,0.04,0.00067,0.0012,7.8e-05,0.0013,0.0012,0.00078,0.0013,1,1,2.5 +9890000,-0.29,0.019,-0.0057,0.96,-0.47,-0.22,-0.0027,-0.87,-0.43,-0.03,-0.00058,-0.0072,-0.00014,-0.00051,0.00039,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00053,0.00054,0.053,1.7,1.7,0.013,5.7,5.6,0.049,3.1e-05,3.1e-05,2.2e-06,0.04,0.04,0.00064,0.0012,7.8e-05,0.0013,0.0012,0.00078,0.0013,1,1,2.5 +9990000,-0.29,0.019,-0.0057,0.96,-0.48,-0.23,-0.002,-0.92,-0.47,-0.032,-0.00052,-0.0072,-0.00015,-0.00043,0.00033,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00053,0.00054,0.053,1.8,1.8,0.013,6.2,6.2,0.049,3e-05,2.9e-05,2.2e-06,0.04,0.04,0.00062,0.0012,7.7e-05,0.0013,0.0012,0.00077,0.0013,1,1,2.5 +10090000,-0.29,0.019,-0.0056,0.96,-0.49,-0.25,-0.00079,-0.95,-0.53,-0.03,-0.00041,-0.0071,-0.00015,-0.00043,0.00025,-0.14,-0.1,-0.024,0.5,0.081,-0.032,-0.069,0,0,0.095,0.00052,0.00053,0.053,1.9,1.9,0.013,6.8,6.7,0.049,2.8e-05,2.8e-05,2.2e-06,0.04,0.04,0.0006,0.0012,7.7e-05,0.0013,0.0012,0.00077,0.0013,1,1,2.6 +10190000,-0.29,0.019,-0.0055,0.96,-0.49,-0.27,5.1e-05,-0.97,-0.6,-0.031,-0.00028,-0.007,-0.00015,-0.00027,0.00012,-0.14,-0.1,-0.024,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00052,0.00053,0.053,2,1.9,0.013,7.3,7.3,0.048,2.7e-05,2.6e-05,2.2e-06,0.04,0.04,0.00058,0.0012,7.7e-05,0.0013,0.0012,0.00077,0.0013,1,1,2.6 +10290000,-0.29,0.019,-0.0055,0.96,-0.48,-0.27,-0.0011,-0.97,-0.61,-0.03,-0.00034,-0.0069,-0.00014,-0.00015,0.00018,-0.14,-0.1,-0.024,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00052,0.00052,0.053,2,2,0.012,8,7.9,0.048,2.6e-05,2.5e-05,2.2e-06,0.04,0.04,0.00057,0.0012,7.6e-05,0.0013,0.0012,0.00077,0.0013,1,1,2.6 +10390000,-0.29,0.019,-0.0054,0.96,0.0032,-0.0054,-0.0025,0.00059,-0.00016,-0.029,-0.00039,-0.0067,-0.00014,9.3e-06,0.00025,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00051,0.00052,0.053,0.25,0.25,0.56,0.25,0.25,0.048,2.4e-05,2.4e-05,2.2e-06,0.04,0.04,0.00055,0.0012,7.6e-05,0.0013,0.0012,0.00077,0.0013,1,1,2.6 +10490000,-0.29,0.019,-0.0053,0.96,-0.0099,-0.0093,0.0071,0.00026,-0.00086,-0.024,-0.00032,-0.0067,-0.00014,-0.00013,0.00019,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00051,0.00052,0.053,0.26,0.26,0.55,0.26,0.26,0.057,2.3e-05,2.2e-05,2.2e-06,0.04,0.04,0.00054,0.0012,7.6e-05,0.0013,0.0012,0.00076,0.0013,1,1,2.7 +10590000,-0.29,0.019,-0.0052,0.96,-0.02,-0.0079,0.013,-0.0011,-0.00061,-0.022,-0.00041,-0.0066,-0.00013,0.00012,0.00055,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.0005,0.00051,0.053,0.13,0.13,0.27,0.26,0.26,0.055,2.2e-05,2.1e-05,2.2e-06,0.039,0.039,0.00052,0.0012,7.6e-05,0.0013,0.0012,0.00076,0.0013,1,1,2.7 +10690000,-0.29,0.019,-0.0051,0.96,-0.033,-0.011,0.016,-0.0038,-0.0016,-0.018,-0.00039,-0.0065,-0.00013,0.00015,0.00054,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.0005,0.00051,0.053,0.14,0.14,0.26,0.27,0.27,0.065,2.1e-05,2e-05,2.2e-06,0.039,0.039,0.00052,0.0012,7.6e-05,0.0013,0.0012,0.00076,0.0013,1,1,2.7 +10790000,-0.29,0.019,-0.005,0.96,-0.033,-0.015,0.014,0.00012,-0.0018,-0.016,-0.00042,-0.0064,-0.00013,0.002,5.9e-05,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00048,0.00049,0.052,0.094,0.094,0.17,0.13,0.13,0.062,2e-05,1.9e-05,2.2e-06,0.038,0.038,0.00051,0.0012,7.6e-05,0.0013,0.0012,0.00076,0.0013,1,1,2.7 +10890000,-0.29,0.019,-0.0049,0.96,-0.043,-0.02,0.01,-0.0036,-0.0035,-0.019,-0.00054,-0.0063,-0.00012,0.0021,0.0002,-0.14,-0.11,-0.025,0.5,0.081,-0.033,-0.069,0,0,0.095,0.00048,0.00049,0.052,0.1,0.1,0.16,0.14,0.14,0.068,1.9e-05,1.8e-05,2.2e-06,0.038,0.038,0.0005,0.0012,7.6e-05,0.0013,0.0012,0.00076,0.0013,1,1,2.8 +10990000,-0.29,0.019,-0.005,0.96,-0.039,-0.022,0.016,-0.0014,-0.0019,-0.012,-0.00058,-0.0064,-0.00012,0.0058,0.00072,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00045,0.00046,0.052,0.081,0.081,0.12,0.091,0.091,0.065,1.8e-05,1.7e-05,2.2e-06,0.036,0.036,0.00049,0.0012,7.6e-05,0.0013,0.0012,0.00075,0.0013,1,1,2.8 +11090000,-0.29,0.019,-0.0051,0.96,-0.05,-0.029,0.02,-0.0059,-0.0046,-0.0081,-0.00046,-0.0063,-0.00013,0.006,0.00072,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00045,0.00046,0.052,0.093,0.093,0.11,0.097,0.097,0.069,1.7e-05,1.6e-05,2.2e-06,0.036,0.036,0.00049,0.0012,7.6e-05,0.0013,0.0012,0.00075,0.0013,1,1,2.8 +11190000,-0.29,0.018,-0.0051,0.96,-0.041,-0.027,0.027,-0.002,-0.0028,-0.0011,-0.00047,-0.0063,-0.00013,0.013,0.0018,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00041,0.00042,0.052,0.076,0.076,0.083,0.072,0.072,0.066,1.6e-05,1.5e-05,2.2e-06,0.033,0.033,0.00048,0.0012,7.6e-05,0.0013,0.0012,0.00075,0.0013,1,1,2.8 +11290000,-0.29,0.018,-0.0052,0.96,-0.05,-0.029,0.026,-0.0065,-0.0056,-0.00072,-0.00048,-0.0063,-0.00013,0.013,0.0017,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00041,0.00042,0.052,0.09,0.09,0.077,0.078,0.078,0.069,1.5e-05,1.5e-05,2.2e-06,0.033,0.033,0.00048,0.0012,7.6e-05,0.0013,0.0012,0.00075,0.0013,1,1,2.9 +11390000,-0.29,0.018,-0.0051,0.96,-0.043,-0.025,0.017,-0.0037,-0.0035,-0.0091,-0.00053,-0.0063,-0.00013,0.019,0.0032,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00037,0.00037,0.052,0.075,0.075,0.062,0.062,0.062,0.066,1.4e-05,1.4e-05,2.2e-06,0.029,0.029,0.00047,0.0012,7.6e-05,0.0013,0.0012,0.00074,0.0013,1,1,2.9 +11490000,-0.29,0.018,-0.005,0.96,-0.05,-0.027,0.021,-0.0082,-0.0063,-0.003,-0.00054,-0.0063,-0.00012,0.019,0.0034,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00037,0.00037,0.052,0.089,0.089,0.057,0.068,0.068,0.067,1.4e-05,1.3e-05,2.2e-06,0.029,0.029,0.00047,0.0012,7.6e-05,0.0013,0.0012,0.00074,0.0013,1,1,2.9 +11590000,-0.29,0.018,-0.0051,0.96,-0.044,-0.023,0.019,-0.005,-0.004,-0.004,-0.00058,-0.0063,-0.00012,0.025,0.0047,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00032,0.00032,0.052,0.074,0.074,0.048,0.056,0.056,0.065,1.3e-05,1.2e-05,2.2e-06,0.025,0.025,0.00047,0.0012,7.5e-05,0.0013,0.0012,0.00074,0.0013,1,1,2.9 +11690000,-0.29,0.017,-0.0051,0.96,-0.05,-0.027,0.019,-0.0096,-0.0065,-0.0053,-0.00062,-0.0063,-0.00012,0.025,0.0047,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00032,0.00032,0.052,0.087,0.087,0.044,0.063,0.063,0.066,1.2e-05,1.2e-05,2.2e-06,0.025,0.025,0.00047,0.0012,7.5e-05,0.0013,0.0012,0.00074,0.0013,1,1,3 +11790000,-0.29,0.017,-0.005,0.96,-0.04,-0.026,0.02,-0.0055,-0.0056,-0.0023,-0.00072,-0.0063,-0.00012,0.03,0.0053,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00027,0.00027,0.052,0.073,0.073,0.037,0.053,0.053,0.063,1.2e-05,1.1e-05,2.2e-06,0.021,0.021,0.00046,0.0012,7.5e-05,0.0013,0.0012,0.00073,0.0013,1,1,3 +11890000,-0.29,0.017,-0.0052,0.96,-0.048,-0.029,0.018,-0.01,-0.0083,-0.0016,-0.0007,-0.0063,-0.00012,0.03,0.0052,-0.14,-0.11,-0.025,0.5,0.082,-0.033,-0.069,0,0,0.095,0.00027,0.00027,0.052,0.085,0.085,0.034,0.06,0.06,0.063,1.1e-05,1.1e-05,2.2e-06,0.021,0.021,0.00046,0.0012,7.5e-05,0.0013,0.0012,0.00073,0.0013,1,1,3 +11990000,-0.29,0.017,-0.0052,0.96,-0.039,-0.022,0.015,-0.0063,-0.0055,-0.0052,-0.00073,-0.0063,-0.00012,0.037,0.0074,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.069,0,0,0.095,0.00023,0.00023,0.052,0.074,0.074,0.03,0.062,0.062,0.061,1.1e-05,1e-05,2.2e-06,0.018,0.018,0.00046,0.0012,7.5e-05,0.0013,0.0012,0.00073,0.0013,1,1,3 +12090000,-0.29,0.017,-0.0051,0.96,-0.045,-0.024,0.018,-0.011,-0.008,0.00088,-0.00071,-0.0062,-0.00012,0.037,0.0077,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.069,0,0,0.095,0.00023,0.00023,0.052,0.085,0.085,0.027,0.07,0.07,0.06,1e-05,9.7e-06,2.2e-06,0.018,0.018,0.00046,0.0012,7.5e-05,0.0013,0.0012,0.00073,0.0013,1,1,3.1 +12190000,-0.29,0.017,-0.0051,0.96,-0.038,-0.014,0.017,-0.0054,-0.0031,0.0028,-0.00067,-0.0062,-0.00012,0.043,0.0095,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.069,0,0,0.095,0.0002,0.0002,0.052,0.068,0.068,0.024,0.057,0.057,0.058,9.7e-06,9.2e-06,2.2e-06,0.015,0.015,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.1 +12290000,-0.29,0.017,-0.0052,0.96,-0.04,-0.013,0.016,-0.0093,-0.0044,0.0038,-0.00064,-0.0062,-0.00012,0.043,0.0095,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.069,0,0,0.095,0.0002,0.0002,0.052,0.079,0.079,0.022,0.065,0.065,0.058,9.3e-06,8.8e-06,2.2e-06,0.015,0.015,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.1 +12390000,-0.29,0.016,-0.0052,0.96,-0.033,-0.0095,0.014,-0.0051,-0.0028,-0.0022,-0.00063,-0.0062,-0.00012,0.047,0.0098,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.069,0,0,0.095,0.00017,0.00017,0.051,0.064,0.064,0.02,0.054,0.054,0.056,8.9e-06,8.4e-06,2.2e-06,0.013,0.013,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.1 +12490000,-0.29,0.016,-0.0052,0.96,-0.036,-0.011,0.018,-0.0086,-0.0039,-0.00015,-0.00062,-0.0061,-0.00012,0.047,0.0099,-0.14,-0.11,-0.025,0.5,0.083,-0.033,-0.07,0,0,0.095,0.00017,0.00017,0.051,0.073,0.073,0.018,0.061,0.061,0.055,8.5e-06,8e-06,2.2e-06,0.013,0.013,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.2 +12590000,-0.29,0.016,-0.0051,0.96,-0.03,-0.0091,0.019,-0.0035,-0.0041,0.0017,-0.00062,-0.0061,-0.00012,0.05,0.0089,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00015,0.00015,0.051,0.06,0.06,0.017,0.051,0.051,0.054,8.1e-06,7.7e-06,2.2e-06,0.011,0.011,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.2 +12690000,-0.29,0.016,-0.0051,0.96,-0.033,-0.0073,0.019,-0.0066,-0.0049,0.0032,-0.00061,-0.0061,-0.00012,0.05,0.009,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00015,0.00015,0.051,0.068,0.068,0.015,0.059,0.059,0.053,7.8e-06,7.3e-06,2.2e-06,0.011,0.011,0.00045,0.0012,7.5e-05,0.0012,0.0012,0.00072,0.0013,1,1,3.2 +12790000,-0.29,0.016,-0.0049,0.96,-0.023,-0.013,0.02,-0.0015,-0.0079,0.0054,-0.00068,-0.0061,-0.00012,0.053,0.008,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00013,0.00013,0.051,0.06,0.06,0.014,0.061,0.061,0.051,7.4e-06,7e-06,2.2e-06,0.0093,0.0096,0.00044,0.0012,7.5e-05,0.0012,0.0012,0.00071,0.0013,1,1,3.2 +12890000,-0.29,0.016,-0.0049,0.96,-0.025,-0.013,0.021,-0.0039,-0.0093,0.0084,-0.00071,-0.006,-0.00012,0.053,0.0085,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00013,0.00013,0.051,0.068,0.068,0.013,0.07,0.07,0.051,7.2e-06,6.7e-06,2.2e-06,0.0093,0.0095,0.00044,0.0012,7.5e-05,0.0012,0.0012,0.00071,0.0013,1,1,3.3 +12990000,-0.29,0.016,-0.0049,0.96,-0.021,-0.011,0.022,-0.0011,-0.0065,0.0096,-0.00076,-0.0061,-0.00012,0.054,0.0091,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00012,0.00012,0.051,0.054,0.054,0.012,0.056,0.056,0.05,6.9e-06,6.4e-06,2.2e-06,0.008,0.0083,0.00044,0.0012,7.5e-05,0.0012,0.0012,0.00071,0.0013,1,1,3.3 +13090000,-0.29,0.016,-0.0049,0.96,-0.023,-0.012,0.019,-0.0032,-0.0078,0.0084,-0.0008,-0.006,-0.00011,0.054,0.0097,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00012,0.00012,0.051,0.061,0.061,0.011,0.064,0.064,0.049,6.6e-06,6.2e-06,2.2e-06,0.008,0.0082,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0013,1,1,3.3 +13190000,-0.29,0.016,-0.0048,0.96,-0.019,-0.012,0.018,-0.0017,-0.0088,0.009,-0.00083,-0.006,-0.00011,0.055,0.0096,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00011,0.00011,0.051,0.054,0.054,0.011,0.066,0.066,0.047,6.3e-06,5.9e-06,2.2e-06,0.0072,0.0074,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0013,1,1,3.3 +13290000,-0.29,0.016,-0.0048,0.96,-0.02,-0.012,0.016,-0.0037,-0.01,0.0083,-0.00081,-0.006,-0.00011,0.056,0.0099,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.00011,0.00011,0.051,0.06,0.06,0.01,0.075,0.075,0.047,6.1e-06,5.7e-06,2.2e-06,0.0071,0.0074,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0012,1,1,3.4 +13390000,-0.29,0.016,-0.0048,0.96,-0.017,-0.0089,0.015,-0.0013,-0.0069,0.0089,-0.00079,-0.006,-0.00011,0.057,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,0.0001,9.9e-05,0.051,0.048,0.048,0.0094,0.06,0.06,0.046,5.8e-06,5.4e-06,2.2e-06,0.0063,0.0065,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0012,1,1,3.4 +13490000,-0.29,0.016,-0.0048,0.96,-0.019,-0.011,0.015,-0.0032,-0.0081,0.0051,-0.0008,-0.006,-0.00011,0.057,0.01,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,0.0001,0.0001,0.051,0.053,0.053,0.009,0.067,0.067,0.045,5.6e-06,5.2e-06,2.2e-06,0.0063,0.0065,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0012,1,1,3.4 +13590000,-0.29,0.016,-0.0048,0.96,-0.014,-0.0079,0.016,0.0017,-0.0057,0.0036,-0.00078,-0.006,-0.00011,0.059,0.01,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,9.3e-05,9.2e-05,0.051,0.044,0.044,0.0085,0.055,0.055,0.044,5.4e-06,5e-06,2.2e-06,0.0056,0.0059,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.00071,0.0012,1,1,3.4 +13690000,-0.29,0.016,-0.0047,0.96,-0.015,-0.0071,0.016,0.00029,-0.0065,0.0062,-0.0008,-0.0059,-0.00011,0.059,0.011,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,9.4e-05,9.3e-05,0.051,0.049,0.049,0.0082,0.063,0.063,0.044,5.2e-06,4.8e-06,2.2e-06,0.0056,0.0058,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.5 +13790000,-0.29,0.015,-0.0047,0.96,-0.011,-0.0051,0.016,0.0054,-0.0035,0.0057,-0.00079,-0.006,-0.00011,0.061,0.011,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,8.7e-05,8.7e-05,0.051,0.041,0.041,0.0078,0.052,0.052,0.042,5e-06,4.6e-06,2.2e-06,0.0051,0.0053,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.5 +13890000,-0.29,0.015,-0.0046,0.96,-0.012,-0.0062,0.017,0.0044,-0.0041,0.0078,-0.00083,-0.006,-0.00011,0.06,0.011,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,8.8e-05,8.8e-05,0.051,0.045,0.045,0.0076,0.059,0.059,0.042,4.8e-06,4.5e-06,2.2e-06,0.0051,0.0053,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.5 +13990000,-0.29,0.015,-0.0046,0.96,-0.014,-0.01,0.016,0.0039,-0.0046,0.0067,-0.00086,-0.006,-0.00011,0.061,0.011,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,8.3e-05,8.3e-05,0.051,0.038,0.038,0.0073,0.05,0.05,0.041,4.7e-06,4.3e-06,2.2e-06,0.0047,0.0049,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.5 +14090000,-0.29,0.015,-0.0047,0.96,-0.013,-0.0041,0.017,0.0023,-0.0051,0.0031,-0.00078,-0.006,-0.00011,0.061,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,8.4e-05,8.3e-05,0.051,0.042,0.042,0.0072,0.057,0.057,0.041,4.5e-06,4.2e-06,2.2e-06,0.0046,0.0049,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.6 +14190000,-0.29,0.015,-0.0048,0.96,-0.01,-0.0029,0.017,0.0036,-0.004,0.0033,-0.00074,-0.006,-0.00012,0.063,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,8e-05,7.9e-05,0.051,0.036,0.036,0.007,0.049,0.049,0.04,4.3e-06,4e-06,2.2e-06,0.0043,0.0046,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.6 +14290000,-0.29,0.015,-0.0047,0.96,-0.013,-0.0032,0.015,0.0025,-0.0043,0.0075,-0.00074,-0.0059,-0.00011,0.063,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,8.1e-05,8e-05,0.051,0.039,0.039,0.0069,0.055,0.055,0.039,4.2e-06,3.9e-06,2.2e-06,0.0043,0.0045,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.6 +14390000,-0.29,0.015,-0.0047,0.96,-0.012,-0.0028,0.017,0.0036,-0.0048,0.012,-0.00075,-0.0059,-0.00011,0.063,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,7.7e-05,7.6e-05,0.051,0.034,0.034,0.0067,0.047,0.047,0.039,4e-06,3.7e-06,2.2e-06,0.004,0.0043,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.6 +14490000,-0.29,0.015,-0.0049,0.96,-0.012,-0.0021,0.02,0.0023,-0.0049,0.014,-0.00071,-0.0059,-0.00012,0.064,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,7.8e-05,7.7e-05,0.051,0.037,0.037,0.0066,0.054,0.054,0.038,3.9e-06,3.6e-06,2.2e-06,0.004,0.0042,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.7 +14590000,-0.29,0.015,-0.0049,0.96,-0.015,-0.0038,0.018,0.0007,-0.0053,0.01,-0.0007,-0.0059,-0.00012,0.064,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,7.5e-05,7.4e-05,0.051,0.032,0.032,0.0065,0.047,0.047,0.038,3.8e-06,3.5e-06,2.2e-06,0.0038,0.004,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.7 +14690000,-0.29,0.015,-0.0049,0.96,-0.016,-0.0041,0.018,-0.00088,-0.0058,0.01,-0.0007,-0.0059,-0.00011,0.064,0.01,-0.14,-0.11,-0.025,0.5,0.083,-0.034,-0.07,0,0,0.095,7.6e-05,7.4e-05,0.051,0.035,0.035,0.0065,0.052,0.052,0.037,3.7e-06,3.4e-06,2.2e-06,0.0037,0.004,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.7 +14790000,-0.29,0.015,-0.005,0.96,-0.019,-0.0015,0.018,-0.00058,-0.0013,0.013,-0.00073,-0.0058,-0.00011,0.065,0.012,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7.3e-05,7.1e-05,0.051,0.03,0.03,0.0064,0.046,0.046,0.036,3.5e-06,3.2e-06,2.2e-06,0.0036,0.0038,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.7 +14890000,-0.29,0.015,-0.0049,0.96,-0.02,0.00041,0.022,-0.0027,-0.0017,0.014,-0.00074,-0.0058,-0.00011,0.065,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7.4e-05,7.2e-05,0.051,0.033,0.033,0.0064,0.052,0.052,0.036,3.4e-06,3.1e-06,2.2e-06,0.0035,0.0038,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.8 +14990000,-0.29,0.015,-0.0049,0.96,-0.02,-0.0016,0.025,-0.0022,-0.0029,0.016,-0.00075,-0.0057,-0.00011,0.066,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7.1e-05,7e-05,0.051,0.029,0.029,0.0064,0.045,0.045,0.036,3.3e-06,3e-06,2.2e-06,0.0034,0.0036,0.00044,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.8 +15090000,-0.29,0.015,-0.0048,0.96,-0.021,-0.0027,0.029,-0.0043,-0.0031,0.018,-0.00075,-0.0057,-0.00011,0.066,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7.2e-05,7e-05,0.051,0.031,0.031,0.0064,0.051,0.051,0.035,3.2e-06,2.9e-06,2.2e-06,0.0033,0.0036,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.8 +15190000,-0.29,0.016,-0.005,0.96,-0.021,-0.00079,0.029,-0.0034,-0.0024,0.02,-0.00073,-0.0057,-0.00011,0.067,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7e-05,6.8e-05,0.051,0.027,0.028,0.0064,0.045,0.045,0.035,3.1e-06,2.8e-06,2.2e-06,0.0032,0.0035,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.0007,0.0012,1,1,3.8 +15290000,-0.29,0.016,-0.005,0.96,-0.024,-0.00091,0.029,-0.0058,-0.0029,0.017,-0.00075,-0.0057,-0.00011,0.067,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,7e-05,6.9e-05,0.051,0.03,0.03,0.0065,0.05,0.05,0.035,3e-06,2.7e-06,2.2e-06,0.0032,0.0035,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,3.9 +15390000,-0.29,0.016,-0.0051,0.96,-0.023,-0.0028,0.028,-0.0046,-0.0024,0.017,-0.00075,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.8e-05,6.7e-05,0.051,0.026,0.026,0.0064,0.044,0.044,0.034,2.9e-06,2.6e-06,2.2e-06,0.0031,0.0033,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,3.9 +15490000,-0.29,0.016,-0.0051,0.96,-0.025,-0.00034,0.028,-0.007,-0.0026,0.018,-0.00075,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.9e-05,6.7e-05,0.051,0.028,0.028,0.0065,0.05,0.05,0.034,2.8e-06,2.6e-06,2.2e-06,0.0031,0.0033,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,3.9 +15590000,-0.29,0.016,-0.0051,0.96,-0.022,-0.0047,0.028,-0.0032,-0.0058,0.017,-0.00079,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.7e-05,6.6e-05,0.051,0.025,0.025,0.0065,0.044,0.044,0.034,2.7e-06,2.5e-06,2.2e-06,0.003,0.0032,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,3.9 +15690000,-0.29,0.016,-0.005,0.96,-0.024,-0.0026,0.028,-0.0048,-0.0062,0.018,-0.00083,-0.0057,-0.00011,0.067,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.8e-05,6.6e-05,0.051,0.027,0.027,0.0066,0.049,0.049,0.034,2.7e-06,2.4e-06,2.2e-06,0.0029,0.0032,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4 +15790000,-0.29,0.015,-0.005,0.96,-0.021,-0.0013,0.028,-0.0035,-0.0053,0.019,-0.00087,-0.0058,-0.00011,0.066,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.6e-05,6.4e-05,0.051,0.024,0.024,0.0066,0.043,0.043,0.033,2.6e-06,2.3e-06,2.2e-06,0.0029,0.0031,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4 +15890000,-0.29,0.016,-0.0051,0.96,-0.021,-0.0026,0.029,-0.0059,-0.0053,0.019,-0.00084,-0.0057,-0.00011,0.067,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.7e-05,6.5e-05,0.051,0.026,0.026,0.0067,0.049,0.049,0.034,2.5e-06,2.3e-06,2.2e-06,0.0028,0.0031,0.00043,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4 +15990000,-0.29,0.016,-0.005,0.96,-0.02,-0.0021,0.026,-0.0049,-0.0045,0.018,-0.00083,-0.0057,-0.00011,0.067,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.6e-05,6.3e-05,0.051,0.023,0.023,0.0068,0.043,0.043,0.033,2.4e-06,2.2e-06,2.2e-06,0.0028,0.003,0.00042,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4 +16090000,-0.29,0.016,-0.005,0.96,-0.022,-0.00083,0.024,-0.0072,-0.0045,0.018,-0.00082,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.6e-05,6.4e-05,0.051,0.024,0.025,0.0069,0.048,0.048,0.033,2.4e-06,2.1e-06,2.2e-06,0.0027,0.003,0.00042,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.1 +16190000,-0.29,0.016,-0.005,0.96,-0.02,-0.00061,0.023,-0.0071,-0.0037,0.015,-0.0008,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.5e-05,6.3e-05,0.051,0.022,0.022,0.0069,0.043,0.043,0.033,2.3e-06,2.1e-06,2.2e-06,0.0027,0.0029,0.00042,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.1 +16290000,-0.29,0.015,-0.005,0.96,-0.022,0.00033,0.022,-0.009,-0.0038,0.017,-0.00081,-0.0057,-0.00011,0.068,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.6e-05,6.3e-05,0.051,0.023,0.024,0.007,0.048,0.048,0.033,2.2e-06,2e-06,2.2e-06,0.0027,0.0029,0.00042,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.1 +16390000,-0.29,0.016,-0.0049,0.96,-0.023,-0.00021,0.023,-0.007,-0.0037,0.017,-0.00083,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.4e-05,6.2e-05,0.051,0.021,0.021,0.007,0.042,0.042,0.033,2.2e-06,2e-06,2.2e-06,0.0026,0.0029,0.00041,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.1 +16490000,-0.29,0.016,-0.005,0.96,-0.027,0.00075,0.025,-0.0098,-0.0036,0.021,-0.00082,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.5e-05,6.2e-05,0.051,0.022,0.023,0.0072,0.047,0.047,0.033,2.1e-06,1.9e-06,2.2e-06,0.0026,0.0029,0.00041,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.2 +16590000,-0.29,0.016,-0.005,0.96,-0.031,0.0011,0.029,-0.0085,-0.0031,0.021,-0.00082,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.4e-05,6.1e-05,0.051,0.02,0.02,0.0072,0.042,0.042,0.033,2.1e-06,1.8e-06,2.2e-06,0.0026,0.0028,0.00041,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.2 +16690000,-0.29,0.015,-0.0051,0.96,-0.034,0.0047,0.029,-0.011,-0.0029,0.021,-0.00084,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.4e-05,6.1e-05,0.051,0.022,0.022,0.0073,0.047,0.047,0.033,2e-06,1.8e-06,2.2e-06,0.0025,0.0028,0.00041,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.2 +16790000,-0.29,0.015,-0.0049,0.96,-0.035,0.0045,0.028,-0.0095,-0.0026,0.021,-0.00086,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6e-05,0.051,0.019,0.02,0.0073,0.042,0.042,0.033,1.9e-06,1.7e-06,2.2e-06,0.0025,0.0028,0.0004,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.2 +16890000,-0.29,0.015,-0.0049,0.96,-0.035,0.004,0.029,-0.013,-0.0026,0.02,-0.00089,-0.0057,-0.00011,0.068,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6.1e-05,0.051,0.021,0.021,0.0074,0.046,0.046,0.033,1.9e-06,1.7e-06,2.2e-06,0.0025,0.0027,0.0004,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.3 +16990000,-0.29,0.015,-0.0049,0.96,-0.032,0.0044,0.029,-0.011,-0.0028,0.019,-0.0009,-0.0057,-0.00011,0.069,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6e-05,0.051,0.021,0.021,0.0074,0.049,0.049,0.033,1.8e-06,1.7e-06,2.2e-06,0.0025,0.0027,0.0004,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.3 +17090000,-0.29,0.015,-0.005,0.96,-0.037,0.0063,0.028,-0.015,-0.0023,0.018,-0.00089,-0.0057,-0.00011,0.069,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6e-05,0.051,0.022,0.022,0.0075,0.054,0.054,0.033,1.8e-06,1.6e-06,2.2e-06,0.0024,0.0027,0.00039,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.3 +17190000,-0.29,0.015,-0.0051,0.96,-0.036,0.0082,0.03,-0.014,-0.0039,0.021,-0.00089,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6e-05,0.051,0.021,0.022,0.0076,0.056,0.057,0.033,1.8e-06,1.6e-06,2.2e-06,0.0024,0.0027,0.00039,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.3 +17290000,-0.29,0.015,-0.0051,0.96,-0.039,0.0089,0.029,-0.017,-0.0027,0.021,-0.00088,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.3e-05,6e-05,0.051,0.023,0.023,0.0077,0.062,0.062,0.033,1.7e-06,1.5e-06,2.2e-06,0.0024,0.0027,0.00039,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.4 +17390000,-0.29,0.015,-0.005,0.96,-0.029,0.014,0.029,-0.01,-0.0014,0.021,-0.00091,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.1e-05,5.8e-05,0.051,0.02,0.02,0.0076,0.052,0.052,0.033,1.7e-06,1.5e-06,2.2e-06,0.0024,0.0026,0.00038,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.4 +17490000,-0.29,0.015,-0.0051,0.96,-0.029,0.015,0.029,-0.013,0.00019,0.023,-0.0009,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.2e-05,5.9e-05,0.051,0.021,0.021,0.0078,0.058,0.058,0.033,1.6e-06,1.5e-06,2.2e-06,0.0024,0.0026,0.00038,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.4 +17590000,-0.29,0.015,-0.0051,0.96,-0.029,0.013,0.028,-0.012,-0.00019,0.02,-0.00091,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.1e-05,5.8e-05,0.051,0.018,0.019,0.0077,0.049,0.049,0.033,1.6e-06,1.4e-06,2.2e-06,0.0023,0.0026,0.00037,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.4 +17690000,-0.29,0.015,-0.0052,0.96,-0.03,0.014,0.029,-0.015,0.00095,0.023,-0.00092,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.1e-05,5.8e-05,0.051,0.019,0.02,0.0078,0.054,0.054,0.033,1.5e-06,1.4e-06,2.2e-06,0.0023,0.0026,0.00037,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.5 +17790000,-0.29,0.015,-0.0052,0.96,-0.031,0.014,0.029,-0.015,0.0017,0.028,-0.00093,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.07,0,0,0.095,6.1e-05,5.7e-05,0.051,0.019,0.02,0.0078,0.057,0.057,0.033,1.5e-06,1.3e-06,2.2e-06,0.0023,0.0026,0.00036,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.5 +17890000,-0.29,0.015,-0.0051,0.96,-0.035,0.015,0.029,-0.017,0.0029,0.032,-0.00094,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,6.1e-05,5.8e-05,0.051,0.02,0.021,0.0079,0.062,0.062,0.033,1.5e-06,1.3e-06,2.2e-06,0.0023,0.0025,0.00036,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.5 +17990000,-0.29,0.015,-0.0051,0.96,-0.034,0.016,0.029,-0.014,0.0052,0.033,-0.00094,-0.0057,-0.00011,0.069,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,6e-05,5.7e-05,0.051,0.018,0.018,0.0079,0.052,0.052,0.033,1.4e-06,1.3e-06,2.2e-06,0.0023,0.0025,0.00036,0.0012,7.4e-05,0.0012,0.0012,0.00069,0.0012,1,1,4.5 +18090000,-0.29,0.015,-0.0052,0.96,-0.036,0.017,0.028,-0.018,0.0067,0.031,-0.00094,-0.0057,-0.00011,0.07,0.013,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,6e-05,5.7e-05,0.051,0.019,0.019,0.008,0.057,0.057,0.034,1.4e-06,1.3e-06,2.2e-06,0.0023,0.0025,0.00035,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.6 +18190000,-0.29,0.015,-0.0051,0.96,-0.032,0.014,0.028,-0.013,0.0045,0.029,-0.00098,-0.0057,-0.00011,0.07,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.9e-05,5.6e-05,0.051,0.017,0.017,0.0079,0.049,0.049,0.034,1.4e-06,1.2e-06,2.2e-06,0.0022,0.0025,0.00035,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.6 +18290000,-0.29,0.015,-0.0051,0.96,-0.036,0.014,0.027,-0.017,0.0056,0.028,-0.00098,-0.0057,-0.00011,0.07,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,6e-05,5.6e-05,0.051,0.018,0.018,0.008,0.053,0.053,0.034,1.3e-06,1.2e-06,2.2e-06,0.0022,0.0025,0.00034,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.6 +18390000,-0.29,0.015,-0.005,0.96,-0.032,0.014,0.027,-0.012,0.0047,0.027,-0.00099,-0.0057,-0.00011,0.07,0.014,-0.14,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.5e-05,0.051,0.016,0.016,0.0079,0.046,0.046,0.034,1.3e-06,1.2e-06,2.2e-06,0.0022,0.0025,0.00034,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.6 +18490000,-0.29,0.015,-0.005,0.96,-0.036,0.013,0.026,-0.015,0.0056,0.029,-0.001,-0.0057,-0.00011,0.07,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.9e-05,5.6e-05,0.051,0.017,0.017,0.008,0.051,0.051,0.034,1.3e-06,1.1e-06,2.2e-06,0.0022,0.0025,0.00033,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.7 +18590000,-0.29,0.015,-0.0049,0.96,-0.034,0.013,0.026,-0.013,0.005,0.031,-0.001,-0.0057,-0.00011,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.5e-05,0.051,0.015,0.016,0.0079,0.044,0.044,0.034,1.2e-06,1.1e-06,2.2e-06,0.0022,0.0024,0.00033,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.7 +18690000,-0.29,0.015,-0.0049,0.96,-0.034,0.012,0.025,-0.015,0.0059,0.03,-0.001,-0.0057,-0.00011,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.5e-05,0.051,0.016,0.017,0.008,0.048,0.049,0.034,1.2e-06,1.1e-06,2.2e-06,0.0022,0.0024,0.00032,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.7 +18790000,-0.29,0.015,-0.0049,0.96,-0.031,0.011,0.024,-0.012,0.0051,0.028,-0.001,-0.0058,-0.00012,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.4e-05,0.051,0.015,0.015,0.0079,0.043,0.043,0.034,1.2e-06,1.1e-06,2.2e-06,0.0022,0.0024,0.00032,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.7 +18890000,-0.29,0.015,-0.0048,0.96,-0.031,0.012,0.022,-0.015,0.0064,0.024,-0.001,-0.0058,-0.00012,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.4e-05,0.051,0.016,0.016,0.008,0.047,0.047,0.034,1.2e-06,1e-06,2.2e-06,0.0021,0.0024,0.00031,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.8 +18990000,-0.29,0.015,-0.0048,0.96,-0.029,0.012,0.023,-0.013,0.0056,0.028,-0.0011,-0.0058,-0.00012,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.7e-05,5.4e-05,0.051,0.015,0.015,0.0079,0.042,0.042,0.034,1.1e-06,1e-06,2.2e-06,0.0021,0.0024,0.00031,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.8 +19090000,-0.29,0.015,-0.0048,0.96,-0.028,0.013,0.024,-0.016,0.0063,0.024,-0.0011,-0.0058,-0.00012,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.8e-05,5.4e-05,0.051,0.016,0.016,0.0079,0.045,0.046,0.035,1.1e-06,9.9e-07,2.2e-06,0.0021,0.0024,0.0003,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.8 +19190000,-0.29,0.015,-0.0048,0.96,-0.026,0.014,0.023,-0.014,0.0062,0.023,-0.0011,-0.0058,-0.00012,0.068,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.7e-05,5.3e-05,0.05,0.014,0.015,0.0079,0.041,0.041,0.034,1.1e-06,9.6e-07,2.2e-06,0.0021,0.0024,0.0003,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.8 +19290000,-0.29,0.015,-0.0048,0.96,-0.027,0.014,0.024,-0.017,0.0074,0.022,-0.0011,-0.0058,-0.00012,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.7e-05,5.3e-05,0.05,0.015,0.016,0.0079,0.044,0.044,0.034,1.1e-06,9.5e-07,2.2e-06,0.0021,0.0024,0.00029,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.9 +19390000,-0.29,0.015,-0.0049,0.96,-0.026,0.012,0.025,-0.015,0.0073,0.021,-0.0011,-0.0058,-0.00012,0.068,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.6e-05,5.3e-05,0.05,0.014,0.015,0.0078,0.04,0.04,0.035,1e-06,9.2e-07,2.2e-06,0.0021,0.0023,0.00029,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.9 +19490000,-0.29,0.015,-0.005,0.96,-0.028,0.013,0.024,-0.018,0.0088,0.021,-0.0011,-0.0058,-0.00012,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.7e-05,5.3e-05,0.05,0.015,0.016,0.0078,0.043,0.044,0.035,1e-06,9.1e-07,2.2e-06,0.0021,0.0023,0.00029,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.9 +19590000,-0.29,0.015,-0.0049,0.96,-0.025,0.014,0.026,-0.015,0.007,0.021,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.6e-05,5.2e-05,0.05,0.014,0.014,0.0077,0.039,0.039,0.035,9.9e-07,8.8e-07,2.2e-06,0.0021,0.0023,0.00028,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,4.9 +19690000,-0.29,0.015,-0.0048,0.96,-0.025,0.013,0.025,-0.018,0.0078,0.021,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.6e-05,5.2e-05,0.05,0.015,0.015,0.0078,0.043,0.043,0.035,9.8e-07,8.7e-07,2.2e-06,0.0021,0.0023,0.00028,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,5 +19790000,-0.29,0.015,-0.0049,0.96,-0.022,0.013,0.023,-0.018,0.0084,0.017,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.6e-05,5.2e-05,0.05,0.015,0.016,0.0077,0.045,0.045,0.035,9.6e-07,8.5e-07,2.2e-06,0.0021,0.0023,0.00027,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,5 +19890000,-0.29,0.015,-0.005,0.96,-0.023,0.013,0.023,-0.02,0.0097,0.015,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.6e-05,5.2e-05,0.05,0.016,0.017,0.0077,0.049,0.049,0.035,9.5e-07,8.4e-07,2.2e-06,0.0021,0.0023,0.00027,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,5 +19990000,-0.29,0.015,-0.005,0.96,-0.02,0.014,0.021,-0.016,0.009,0.012,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.5e-05,5.1e-05,0.05,0.014,0.015,0.0076,0.043,0.044,0.035,9.2e-07,8.1e-07,2.2e-06,0.002,0.0023,0.00026,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,5 +20090000,-0.29,0.015,-0.005,0.96,-0.023,0.016,0.021,-0.018,0.01,0.016,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.095,5.5e-05,5.1e-05,0.05,0.015,0.016,0.0076,0.047,0.048,0.035,9.1e-07,8e-07,2.2e-06,0.002,0.0023,0.00026,0.0012,7.4e-05,0.0012,0.0012,0.00068,0.0012,1,1,5.1 +20190000,-0.29,0.015,-0.005,0.96,-0.024,0.014,0.022,-0.019,0.011,0.015,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.12,5.5e-05,5.1e-05,0.05,0.015,0.016,0.0075,0.05,0.05,0.035,8.8e-07,7.8e-07,2.2e-06,0.002,0.0023,0.00025,0.0012,7.3e-05,0.0012,0.0012,0.00068,0.0012,1,1,0.01 +20290000,-0.29,0.015,-0.005,0.96,-0.022,0.016,0.022,-0.021,0.012,0.016,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.12,5.5e-05,5.1e-05,0.05,0.016,0.017,0.0075,0.054,0.054,0.035,8.7e-07,7.7e-07,2.2e-06,0.002,0.0023,0.00025,0.0012,7.3e-05,0.0012,0.0012,0.00068,0.0012,1,1,0.01 +20390000,-0.29,0.015,-0.0049,0.96,-0.02,0.015,0.022,-0.021,0.012,0.017,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.12,5.5e-05,5.1e-05,0.05,0.016,0.017,0.0075,0.056,0.057,0.035,8.5e-07,7.5e-07,2.2e-06,0.002,0.0023,0.00025,0.0012,7.3e-05,0.0012,0.0012,0.00068,0.0012,1,1,0.01 +20490000,-0.29,0.015,-0.0049,0.96,-0.018,0.017,0.022,-0.023,0.013,0.015,-0.0011,-0.0058,-0.00013,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.12,5.5e-05,5.1e-05,0.05,0.017,0.018,0.0075,0.061,0.062,0.035,8.4e-07,7.4e-07,2.2e-06,0.002,0.0023,0.00024,0.0012,7.3e-05,0.0012,0.0012,0.00068,0.0012,1,1,0.01 +20590000,-0.29,0.015,-0.0048,0.96,-0.018,0.016,0.022,-0.023,0.012,0.014,-0.0011,-0.0058,-0.00013,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.11,5.4e-05,5e-05,0.05,0.017,0.018,0.0074,0.064,0.064,0.035,8.2e-07,7.2e-07,2.2e-06,0.002,0.0023,0.00024,0.0012,7.2e-05,0.0012,0.0011,0.00068,0.0012,1,1,0.01 +20690000,-0.29,0.015,-0.0047,0.96,-0.017,0.016,0.023,-0.025,0.014,0.014,-0.0011,-0.0058,-0.00013,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.11,5.5e-05,5.1e-05,0.05,0.018,0.019,0.0074,0.069,0.07,0.035,8.1e-07,7.2e-07,2.2e-06,0.002,0.0022,0.00023,0.0012,7.2e-05,0.0012,0.0011,0.00068,0.0012,1,1,0.01 +20790000,-0.29,0.015,-0.0041,0.96,-0.011,0.012,0.0077,-0.019,0.01,0.013,-0.0011,-0.0058,-0.00014,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.11,5.3e-05,4.9e-05,0.05,0.015,0.016,0.0073,0.056,0.057,0.035,7.8e-07,6.9e-07,2.2e-06,0.002,0.0022,0.00023,0.0012,7.2e-05,0.0012,0.0011,0.00068,0.0012,1,1,0.01 +20890000,-0.29,0.01,0.0045,0.96,-0.0062,0.0015,-0.11,-0.021,0.01,0.0066,-0.0011,-0.0058,-0.00014,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.034,-0.069,0,0,0.11,5.3e-05,5e-05,0.05,0.016,0.017,0.0073,0.061,0.062,0.035,7.8e-07,6.9e-07,2.2e-06,0.002,0.0022,0.00023,0.0012,7.2e-05,0.0012,0.0011,0.00068,0.0012,1,1,0.01 +20990000,-0.29,0.0063,0.0075,0.96,0.0089,-0.015,-0.25,-0.017,0.008,-0.0084,-0.0011,-0.0058,-0.00014,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,0.092,5.2e-05,4.8e-05,0.05,0.015,0.015,0.0072,0.051,0.052,0.034,7.5e-07,6.7e-07,2.2e-06,0.002,0.0022,0.00022,0.0012,7.2e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21090000,-0.29,0.0069,0.0059,0.96,0.023,-0.028,-0.37,-0.015,0.0061,-0.039,-0.0011,-0.0058,-0.00014,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,0.061,5.3e-05,4.9e-05,0.05,0.016,0.016,0.0072,0.056,0.056,0.035,7.5e-07,6.6e-07,2.2e-06,0.002,0.0022,0.00022,0.0012,7.1e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21190000,-0.29,0.0088,0.0033,0.96,0.029,-0.033,-0.5,-0.013,0.0046,-0.075,-0.0011,-0.0058,-0.00014,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,0.025,5.1e-05,4.8e-05,0.05,0.014,0.015,0.0071,0.048,0.048,0.035,7.2e-07,6.4e-07,2.1e-06,0.002,0.0022,0.00022,0.0012,7.1e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21290000,-0.29,0.01,0.0013,0.96,0.027,-0.036,-0.63,-0.01,0.002,-0.13,-0.0011,-0.0058,-0.00014,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.034,5.2e-05,4.8e-05,0.05,0.015,0.016,0.0071,0.052,0.052,0.035,7.2e-07,6.4e-07,2.1e-06,0.002,0.0022,0.00021,0.0012,7.1e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21390000,-0.29,0.011,-0.00017,0.96,0.021,-0.029,-0.75,-0.012,0.0052,-0.2,-0.0011,-0.0058,-0.00013,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.099,5.1e-05,4.7e-05,0.05,0.015,0.016,0.007,0.054,0.055,0.035,7e-07,6.2e-07,2.1e-06,0.002,0.0022,0.00021,0.0012,7.1e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21490000,-0.29,0.012,-0.00092,0.96,0.014,-0.028,-0.89,-0.0096,0.0021,-0.28,-0.0011,-0.0058,-0.00013,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.18,5.2e-05,4.8e-05,0.05,0.016,0.017,0.007,0.059,0.059,0.035,7e-07,6.2e-07,2.1e-06,0.002,0.0022,0.00021,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21590000,-0.29,0.012,-0.0014,0.96,0.0029,-0.022,-1,-0.014,0.0067,-0.37,-0.0011,-0.0058,-0.00012,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.27,5.1e-05,4.7e-05,0.05,0.016,0.017,0.0069,0.061,0.062,0.034,6.8e-07,6e-07,2.1e-06,0.002,0.0022,0.0002,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21690000,-0.29,0.012,-0.0018,0.96,-0.0024,-0.019,-1.1,-0.014,0.0039,-0.49,-0.0011,-0.0058,-0.00011,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.39,5.2e-05,4.7e-05,0.05,0.017,0.018,0.0069,0.066,0.067,0.035,6.8e-07,6e-07,2.1e-06,0.0019,0.0022,0.0002,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21790000,-0.29,0.012,-0.0021,0.96,-0.0082,-0.011,-1.3,-0.015,0.01,-0.61,-0.0011,-0.0058,-0.0001,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-0.51,5.1e-05,4.7e-05,0.05,0.017,0.018,0.0069,0.069,0.069,0.034,6.6e-07,5.8e-07,2.1e-06,0.0019,0.0022,0.0002,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21890000,-0.29,0.012,-0.0025,0.96,-0.014,-0.0073,-1.4,-0.016,0.0096,-0.75,-0.0011,-0.0058,-0.0001,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-0.65,5.1e-05,4.7e-05,0.05,0.018,0.019,0.0068,0.074,0.075,0.034,6.6e-07,5.8e-07,2.1e-06,0.0019,0.0022,0.00019,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +21990000,-0.29,0.012,-0.0032,0.96,-0.02,0.00088,-1.4,-0.022,0.017,-0.89,-0.001,-0.0058,-8.7e-05,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-0.79,5.1e-05,4.6e-05,0.05,0.018,0.019,0.0068,0.076,0.077,0.034,6.4e-07,5.7e-07,2.1e-06,0.0019,0.0022,0.00019,0.0012,7e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22090000,-0.29,0.013,-0.0038,0.96,-0.023,0.0041,-1.4,-0.023,0.016,-1,-0.0011,-0.0058,-8.4e-05,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-0.93,5.1e-05,4.7e-05,0.05,0.019,0.02,0.0068,0.082,0.084,0.034,6.4e-07,5.6e-07,2.1e-06,0.0019,0.0022,0.00019,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22190000,-0.29,0.013,-0.0042,0.96,-0.03,0.011,-1.4,-0.027,0.024,-1.2,-0.0011,-0.0058,-7.1e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.1,5e-05,4.6e-05,0.05,0.018,0.02,0.0067,0.085,0.086,0.034,6.2e-07,5.5e-07,2.1e-06,0.0019,0.0022,0.00019,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22290000,-0.29,0.014,-0.0049,0.96,-0.038,0.015,-1.4,-0.031,0.025,-1.3,-0.0011,-0.0058,-7.1e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.2,5.1e-05,4.6e-05,0.05,0.019,0.021,0.0067,0.091,0.093,0.034,6.2e-07,5.4e-07,2.1e-06,0.0019,0.0022,0.00018,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22390000,-0.29,0.014,-0.0052,0.96,-0.045,0.022,-1.4,-0.038,0.029,-1.5,-0.001,-0.0058,-7.1e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.4,5e-05,4.5e-05,0.05,0.019,0.02,0.0066,0.093,0.095,0.034,6e-07,5.3e-07,2.1e-06,0.0019,0.0022,0.00018,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22490000,-0.29,0.014,-0.0054,0.96,-0.052,0.028,-1.4,-0.043,0.032,-1.6,-0.001,-0.0058,-7.2e-05,0.07,0.012,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.5,5e-05,4.6e-05,0.05,0.02,0.021,0.0066,0.1,0.1,0.034,6e-07,5.3e-07,2.1e-06,0.0019,0.0022,0.00018,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22590000,-0.29,0.015,-0.0052,0.96,-0.057,0.034,-1.4,-0.044,0.036,-1.7,-0.0011,-0.0058,-6.8e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.6,4.9e-05,4.5e-05,0.05,0.019,0.021,0.0065,0.1,0.1,0.034,5.8e-07,5.2e-07,2.1e-06,0.0019,0.0021,0.00018,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22690000,-0.29,0.015,-0.0051,0.96,-0.062,0.039,-1.4,-0.05,0.04,-1.9,-0.001,-0.0058,-6.9e-05,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.8,5e-05,4.5e-05,0.05,0.02,0.022,0.0065,0.11,0.11,0.034,5.8e-07,5.1e-07,2.1e-06,0.0019,0.0021,0.00017,0.0012,6.9e-05,0.0012,0.0011,0.00067,0.0012,1,1,0.01 +22790000,-0.29,0.016,-0.005,0.96,-0.068,0.044,-1.4,-0.056,0.043,-2,-0.001,-0.0058,-7.2e-05,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-1.9,4.9e-05,4.4e-05,0.05,0.02,0.021,0.0065,0.11,0.11,0.034,5.6e-07,5e-07,2.1e-06,0.0019,0.0021,0.00017,0.0012,6.9e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +22890000,-0.29,0.016,-0.0051,0.96,-0.073,0.048,-1.4,-0.062,0.046,-2.2,-0.0011,-0.0058,-6.8e-05,0.07,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-2.1,4.9e-05,4.5e-05,0.05,0.021,0.022,0.0064,0.12,0.12,0.034,5.6e-07,5e-07,2.1e-06,0.0019,0.0021,0.00017,0.0012,6.9e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +22990000,-0.29,0.016,-0.0049,0.96,-0.076,0.048,-1.4,-0.065,0.045,-2.3,-0.0011,-0.0058,-7e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.069,0,0,-2.2,4.8e-05,4.4e-05,0.049,0.02,0.022,0.0064,0.12,0.12,0.034,5.5e-07,4.9e-07,2.1e-06,0.0019,0.0021,0.00017,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23090000,-0.29,0.017,-0.0049,0.96,-0.082,0.052,-1.4,-0.072,0.05,-2.5,-0.0011,-0.0058,-6.9e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-2.4,4.9e-05,4.4e-05,0.05,0.021,0.023,0.0064,0.13,0.13,0.034,5.5e-07,4.8e-07,2.1e-06,0.0019,0.0021,0.00016,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23190000,-0.29,0.017,-0.0048,0.96,-0.084,0.047,-1.4,-0.072,0.046,-2.6,-0.0011,-0.0058,-8.1e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-2.5,4.8e-05,4.4e-05,0.049,0.02,0.022,0.0063,0.13,0.13,0.033,5.3e-07,4.7e-07,2.1e-06,0.0019,0.0021,0.00016,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23290000,-0.29,0.017,-0.0053,0.96,-0.09,0.051,-1.4,-0.08,0.051,-2.8,-0.0011,-0.0058,-7.9e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-2.7,4.8e-05,4.4e-05,0.049,0.021,0.023,0.0063,0.14,0.14,0.034,5.3e-07,4.7e-07,2.1e-06,0.0019,0.0021,0.00016,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23390000,-0.29,0.017,-0.0052,0.96,-0.09,0.054,-1.4,-0.075,0.053,-2.9,-0.0011,-0.0058,-8.8e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-2.8,4.7e-05,4.3e-05,0.049,0.021,0.022,0.0063,0.14,0.14,0.033,5.2e-07,4.6e-07,2.1e-06,0.0019,0.0021,0.00016,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23490000,-0.29,0.017,-0.0053,0.96,-0.096,0.054,-1.4,-0.085,0.057,-3,-0.0011,-0.0058,-8.3e-05,0.069,0.013,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-2.9,4.8e-05,4.4e-05,0.049,0.021,0.023,0.0063,0.15,0.15,0.033,5.2e-07,4.6e-07,2.1e-06,0.0019,0.0021,0.00016,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23590000,-0.29,0.017,-0.0054,0.96,-0.095,0.048,-1.4,-0.081,0.048,-3.2,-0.0011,-0.0058,-9.6e-05,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-3.1,4.7e-05,4.3e-05,0.049,0.021,0.022,0.0062,0.15,0.15,0.033,5e-07,4.5e-07,2.1e-06,0.0019,0.0021,0.00015,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23690000,-0.29,0.018,-0.006,0.96,-0.094,0.051,-1.3,-0.09,0.051,-3.3,-0.0011,-0.0058,-9.2e-05,0.069,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-3.2,4.7e-05,4.3e-05,0.049,0.022,0.023,0.0062,0.16,0.16,0.033,5e-07,4.5e-07,2.1e-06,0.0019,0.0021,0.00015,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23790000,-0.29,0.021,-0.0074,0.96,-0.079,0.048,-0.95,-0.08,0.047,-3.4,-0.0012,-0.0058,-9.7e-05,0.068,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-3.3,4.6e-05,4.2e-05,0.049,0.02,0.022,0.0061,0.16,0.16,0.033,4.9e-07,4.4e-07,2e-06,0.0019,0.0021,0.00015,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23890000,-0.29,0.025,-0.01,0.96,-0.074,0.051,-0.52,-0.087,0.051,-3.5,-0.0012,-0.0058,-9.4e-05,0.068,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.068,0,0,-3.4,4.7e-05,4.3e-05,0.049,0.021,0.022,0.0061,0.17,0.17,0.033,4.9e-07,4.3e-07,2e-06,0.0019,0.0021,0.00015,0.0012,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +23990000,-0.29,0.028,-0.012,0.96,-0.065,0.05,-0.13,-0.074,0.046,-3.6,-0.0012,-0.0058,-0.0001,0.068,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.6e-05,4.2e-05,0.049,0.02,0.021,0.0061,0.17,0.17,0.033,4.8e-07,4.3e-07,2e-06,0.0019,0.0021,0.00015,0.0011,6.8e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +24090000,-0.29,0.027,-0.012,0.96,-0.072,0.058,0.098,-0.08,0.052,-3.6,-0.0012,-0.0058,-0.0001,0.068,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.7e-05,4.3e-05,0.049,0.021,0.022,0.0061,0.18,0.18,0.033,4.8e-07,4.3e-07,2e-06,0.0019,0.0021,0.00015,0.0011,6.7e-05,0.0012,0.0011,0.00066,0.0012,1,1,0.01 +24190000,-0.29,0.022,-0.0092,0.96,-0.076,0.055,0.088,-0.066,0.039,-3.6,-0.0012,-0.0058,-0.00012,0.068,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.6e-05,4.2e-05,0.049,0.02,0.021,0.006,0.18,0.18,0.033,4.7e-07,4.2e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24290000,-0.29,0.019,-0.0071,0.96,-0.081,0.057,0.066,-0.074,0.044,-3.6,-0.0012,-0.0058,-0.00011,0.067,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.7e-05,4.2e-05,0.049,0.021,0.023,0.006,0.19,0.19,0.033,4.7e-07,4.2e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24390000,-0.29,0.018,-0.0063,0.96,-0.064,0.05,0.082,-0.055,0.035,-3.6,-0.0012,-0.0058,-0.00012,0.067,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.6e-05,4.2e-05,0.049,0.02,0.022,0.006,0.19,0.19,0.033,4.6e-07,4.1e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24490000,-0.29,0.018,-0.0065,0.96,-0.059,0.047,0.08,-0.061,0.038,-3.6,-0.0012,-0.0058,-0.00011,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.6e-05,4.2e-05,0.049,0.021,0.023,0.006,0.2,0.2,0.033,4.6e-07,4.1e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24590000,-0.29,0.018,-0.0071,0.96,-0.047,0.045,0.076,-0.042,0.033,-3.6,-0.0013,-0.0058,-0.00012,0.067,0.014,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.5,4.6e-05,4.2e-05,0.049,0.02,0.022,0.0059,0.2,0.2,0.033,4.5e-07,4e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24690000,-0.29,0.018,-0.0076,0.96,-0.045,0.044,0.075,-0.046,0.036,-3.5,-0.0013,-0.0058,-0.00012,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.4,4.6e-05,4.2e-05,0.049,0.021,0.023,0.0059,0.21,0.21,0.033,4.5e-07,4e-07,2e-06,0.0019,0.0021,0.00014,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24790000,-0.29,0.017,-0.0077,0.96,-0.039,0.043,0.067,-0.034,0.028,-3.5,-0.0013,-0.0058,-0.00013,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.4,4.6e-05,4.2e-05,0.049,0.02,0.022,0.0059,0.21,0.21,0.032,4.4e-07,3.9e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24890000,-0.29,0.017,-0.0076,0.96,-0.038,0.045,0.056,-0.037,0.032,-3.5,-0.0013,-0.0058,-0.00013,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.4,4.6e-05,4.2e-05,0.049,0.021,0.023,0.0059,0.22,0.22,0.032,4.4e-07,3.9e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +24990000,-0.29,0.016,-0.0074,0.96,-0.025,0.046,0.049,-0.022,0.026,-3.5,-0.0013,-0.0058,-0.00013,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-3.4,4.6e-05,4.2e-05,0.048,0.021,0.022,0.0058,0.22,0.22,0.032,4.3e-07,3.9e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25090000,-0.29,0.016,-0.0077,0.96,-0.021,0.046,0.047,-0.023,0.031,-3.5,-0.0013,-0.0058,-0.00013,0.067,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.6e-05,4.2e-05,0.048,0.021,0.023,0.0058,0.23,0.23,0.032,4.3e-07,3.8e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25190000,-0.29,0.016,-0.0079,0.96,-0.011,0.042,0.047,-0.0078,0.021,-3.5,-0.0013,-0.0059,-0.00015,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0058,0.23,0.23,0.032,4.2e-07,3.8e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25290000,-0.3,0.015,-0.0081,0.96,-0.006,0.045,0.042,-0.0086,0.026,-3.5,-0.0013,-0.0059,-0.00015,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.6e-05,4.2e-05,0.048,0.022,0.023,0.0058,0.24,0.24,0.032,4.2e-07,3.8e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.7e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25390000,-0.3,0.015,-0.0082,0.96,0.003,0.043,0.04,0.0015,0.02,-3.5,-0.0013,-0.0059,-0.00016,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0057,0.24,0.24,0.032,4.2e-07,3.7e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25490000,-0.3,0.015,-0.0083,0.96,0.0074,0.044,0.04,0.0012,0.024,-3.5,-0.0013,-0.0059,-0.00016,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.6e-05,4.2e-05,0.048,0.022,0.023,0.0058,0.25,0.25,0.032,4.2e-07,3.7e-07,2e-06,0.0018,0.0021,0.00013,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25590000,-0.3,0.015,-0.0085,0.96,0.012,0.04,0.041,0.0087,0.0095,-3.5,-0.0013,-0.0058,-0.00017,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0057,0.25,0.25,0.032,4.1e-07,3.6e-07,2e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25690000,-0.3,0.014,-0.0079,0.96,0.013,0.039,0.03,0.01,0.013,-3.5,-0.0013,-0.0058,-0.00017,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.6e-05,4.1e-05,0.048,0.022,0.023,0.0057,0.26,0.26,0.032,4.1e-07,3.6e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25790000,-0.3,0.014,-0.0078,0.96,0.024,0.034,0.03,0.018,0.0034,-3.5,-0.0013,-0.0058,-0.00018,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0057,0.25,0.26,0.032,4e-07,3.6e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25890000,-0.3,0.014,-0.0078,0.96,0.03,0.034,0.033,0.02,0.0076,-3.5,-0.0013,-0.0058,-0.00019,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.6e-05,4.1e-05,0.048,0.022,0.023,0.0057,0.27,0.27,0.032,4e-07,3.6e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +25990000,-0.3,0.014,-0.0078,0.96,0.032,0.029,0.026,0.017,-0.0036,-3.5,-0.0014,-0.0058,-0.0002,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0056,0.26,0.27,0.032,4e-07,3.5e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26090000,-0.3,0.014,-0.0075,0.96,0.037,0.029,0.025,0.021,-0.0015,-3.5,-0.0014,-0.0058,-0.00019,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.022,0.023,0.0056,0.28,0.28,0.032,3.9e-07,3.5e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26190000,-0.3,0.014,-0.0073,0.96,0.042,0.02,0.02,0.024,-0.018,-3.5,-0.0014,-0.0058,-0.0002,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0056,0.27,0.28,0.032,3.9e-07,3.5e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26290000,-0.3,0.015,-0.0073,0.96,0.042,0.02,0.015,0.027,-0.015,-3.5,-0.0014,-0.0058,-0.0002,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.066,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.022,0.023,0.0056,0.29,0.29,0.032,3.9e-07,3.5e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26390000,-0.3,0.015,-0.0067,0.96,0.04,0.011,0.019,0.019,-0.03,-3.5,-0.0014,-0.0058,-0.00021,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.021,0.022,0.0056,0.28,0.29,0.032,3.8e-07,3.4e-07,1.9e-06,0.0018,0.0021,0.00012,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26490000,-0.3,0.015,-0.0065,0.96,0.043,0.0088,0.028,0.024,-0.029,-3.5,-0.0014,-0.0058,-0.00022,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.022,0.023,0.0056,0.3,0.3,0.032,3.8e-07,3.4e-07,1.9e-06,0.0018,0.0021,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26590000,-0.3,0.015,-0.0059,0.95,0.042,-0.0012,0.028,0.023,-0.042,-3.5,-0.0014,-0.0058,-0.00023,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.021,0.022,0.0056,0.29,0.3,0.032,3.8e-07,3.4e-07,1.9e-06,0.0018,0.0021,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26690000,-0.3,0.015,-0.0058,0.95,0.044,-0.0048,0.027,0.027,-0.041,-3.5,-0.0014,-0.0058,-0.00023,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.022,0.023,0.0056,0.31,0.31,0.032,3.8e-07,3.4e-07,1.9e-06,0.0018,0.0021,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26790000,-0.3,0.014,-0.0056,0.95,0.047,-0.011,0.026,0.025,-0.054,-3.5,-0.0014,-0.0058,-0.00024,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.021,0.022,0.0055,0.3,0.31,0.031,3.7e-07,3.3e-07,1.9e-06,0.0018,0.0021,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26890000,-0.3,0.014,-0.005,0.96,0.053,-0.013,0.022,0.03,-0.056,-3.5,-0.0014,-0.0058,-0.00024,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-3.4,4.5e-05,4.1e-05,0.048,0.022,0.023,0.0055,0.31,0.32,0.032,3.7e-07,3.3e-07,1.9e-06,0.0018,0.0021,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +26990000,-0.3,0.015,-0.0044,0.95,0.054,-0.02,0.021,0.023,-0.064,-3.5,-0.0014,-0.0058,-0.00024,0.066,0.016,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.021,0.022,0.0055,0.31,0.31,0.031,3.7e-07,3.3e-07,1.9e-06,0.0018,0.002,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +27090000,-0.3,0.015,-0.0042,0.95,0.056,-0.026,0.025,0.028,-0.066,-3.5,-0.0014,-0.0058,-0.00025,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.022,0.023,0.0055,0.32,0.33,0.031,3.7e-07,3.3e-07,1.9e-06,0.0018,0.002,0.00011,0.0011,6.6e-05,0.0012,0.0011,0.00065,0.0012,1,1,0.01 +27190000,-0.3,0.015,-0.0043,0.96,0.057,-0.03,0.027,0.019,-0.069,-3.5,-0.0014,-0.0058,-0.00025,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.021,0.022,0.0055,0.32,0.32,0.031,3.6e-07,3.2e-07,1.9e-06,0.0018,0.002,0.00011,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27290000,-0.3,0.016,-0.0043,0.96,0.064,-0.034,0.14,0.025,-0.072,-3.5,-0.0014,-0.0058,-0.00025,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.5e-05,4e-05,0.048,0.022,0.023,0.0055,0.33,0.34,0.031,3.6e-07,3.2e-07,1.9e-06,0.0018,0.002,0.00011,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27390000,-0.3,0.018,-0.0055,0.96,0.067,-0.025,0.46,0.0071,-0.027,-3.5,-0.0014,-0.0058,-0.00023,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.4e-05,3.9e-05,0.048,0.015,0.016,0.0054,0.15,0.15,0.031,3.5e-07,3.2e-07,1.8e-06,0.0018,0.002,0.00011,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27490000,-0.3,0.02,-0.0066,0.95,0.072,-0.028,0.78,0.014,-0.029,-3.5,-0.0014,-0.0058,-0.00024,0.066,0.015,-0.13,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.4,4.4e-05,4e-05,0.048,0.015,0.016,0.0055,0.15,0.15,0.031,3.5e-07,3.2e-07,1.8e-06,0.0018,0.002,0.00011,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27590000,-0.3,0.019,-0.0067,0.96,0.063,-0.03,0.87,0.0077,-0.02,-3.4,-0.0014,-0.0058,-0.00023,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.3,4.4e-05,3.9e-05,0.048,0.013,0.014,0.0054,0.096,0.096,0.031,3.5e-07,3.1e-07,1.8e-06,0.0018,0.002,0.00011,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27690000,-0.3,0.016,-0.0058,0.96,0.058,-0.028,0.78,0.014,-0.023,-3.3,-0.0014,-0.0058,-0.00023,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.2,4.5e-05,4e-05,0.048,0.014,0.015,0.0054,0.1,0.1,0.031,3.5e-07,3.1e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27790000,-0.3,0.015,-0.0046,0.96,0.054,-0.026,0.77,0.011,-0.019,-3.2,-0.0013,-0.0058,-0.00023,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.1,4.4e-05,3.9e-05,0.048,0.013,0.014,0.0054,0.073,0.074,0.031,3.5e-07,3.1e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27890000,-0.3,0.015,-0.0043,0.96,0.061,-0.031,0.81,0.016,-0.021,-3.2,-0.0013,-0.0058,-0.00023,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3.1,4.5e-05,4e-05,0.048,0.014,0.015,0.0054,0.076,0.077,0.031,3.5e-07,3.1e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +27990000,-0.3,0.015,-0.0047,0.96,0.061,-0.034,0.8,0.019,-0.024,-3.1,-0.0013,-0.0058,-0.00022,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-3,4.4e-05,3.9e-05,0.048,0.014,0.015,0.0054,0.079,0.079,0.031,3.4e-07,3.1e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28090000,-0.3,0.015,-0.0049,0.96,0.065,-0.034,0.8,0.026,-0.027,-3,-0.0013,-0.0058,-0.00022,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.9,4.5e-05,4e-05,0.048,0.014,0.016,0.0054,0.082,0.083,0.031,3.4e-07,3.1e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28190000,-0.3,0.015,-0.0043,0.96,0.062,-0.033,0.81,0.026,-0.03,-2.9,-0.0013,-0.0058,-0.00021,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.8,4.5e-05,3.9e-05,0.048,0.014,0.015,0.0054,0.084,0.085,0.031,3.4e-07,3e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28290000,-0.3,0.016,-0.0037,0.96,0.066,-0.036,0.81,0.032,-0.034,-2.9,-0.0013,-0.0058,-0.00021,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.8,4.5e-05,4e-05,0.048,0.015,0.016,0.0054,0.088,0.089,0.031,3.4e-07,3e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28390000,-0.3,0.016,-0.0037,0.96,0.068,-0.038,0.81,0.035,-0.034,-2.8,-0.0013,-0.0058,-0.0002,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.7,4.5e-05,3.9e-05,0.048,0.015,0.016,0.0053,0.091,0.092,0.031,3.3e-07,3e-07,1.8e-06,0.0018,0.002,0.0001,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28490000,-0.3,0.017,-0.0039,0.96,0.07,-0.041,0.81,0.043,-0.038,-2.7,-0.0013,-0.0058,-0.0002,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.6,4.5e-05,4e-05,0.048,0.016,0.017,0.0054,0.095,0.096,0.031,3.4e-07,3e-07,1.8e-06,0.0018,0.002,9.9e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28590000,-0.29,0.017,-0.0039,0.96,0.063,-0.042,0.81,0.043,-0.041,-2.6,-0.0013,-0.0058,-0.00019,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.5,4.5e-05,3.9e-05,0.048,0.016,0.017,0.0053,0.098,0.099,0.031,3.3e-07,3e-07,1.8e-06,0.0018,0.002,9.8e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28690000,-0.29,0.016,-0.0038,0.96,0.062,-0.043,0.81,0.049,-0.045,-2.6,-0.0013,-0.0058,-0.00019,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.083,-0.032,-0.067,0,0,-2.5,4.5e-05,4e-05,0.048,0.017,0.018,0.0053,0.1,0.1,0.031,3.3e-07,3e-07,1.8e-06,0.0018,0.002,9.8e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28790000,-0.29,0.016,-0.0032,0.96,0.06,-0.042,0.81,0.05,-0.044,-2.5,-0.0013,-0.0058,-0.00018,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2.4,4.5e-05,3.9e-05,0.048,0.016,0.018,0.0053,0.11,0.11,0.031,3.3e-07,2.9e-07,1.8e-06,0.0018,0.002,9.7e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28890000,-0.29,0.015,-0.003,0.96,0.064,-0.044,0.81,0.056,-0.049,-2.4,-0.0013,-0.0058,-0.00018,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2.3,4.5e-05,4e-05,0.048,0.017,0.018,0.0053,0.11,0.11,0.031,3.3e-07,2.9e-07,1.8e-06,0.0018,0.002,9.7e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +28990000,-0.29,0.016,-0.0027,0.96,0.062,-0.042,0.81,0.058,-0.049,-2.3,-0.0013,-0.0058,-0.00017,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2.2,4.5e-05,3.9e-05,0.048,0.017,0.018,0.0053,0.11,0.11,0.031,3.2e-07,2.9e-07,1.8e-06,0.0018,0.002,9.6e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29090000,-0.29,0.016,-0.0026,0.96,0.065,-0.043,0.81,0.065,-0.053,-2.3,-0.0013,-0.0058,-0.00017,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2.2,4.5e-05,3.9e-05,0.048,0.018,0.019,0.0053,0.12,0.12,0.031,3.2e-07,2.9e-07,1.8e-06,0.0018,0.002,9.5e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29190000,-0.29,0.016,-0.0025,0.96,0.066,-0.043,0.8,0.067,-0.052,-2.2,-0.0013,-0.0058,-0.00016,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2.1,4.5e-05,3.9e-05,0.048,0.017,0.019,0.0053,0.12,0.12,0.031,3.2e-07,2.9e-07,1.7e-06,0.0018,0.002,9.5e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29290000,-0.29,0.016,-0.0027,0.96,0.071,-0.048,0.81,0.075,-0.056,-2.1,-0.0013,-0.0058,-0.00016,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-2,4.5e-05,3.9e-05,0.048,0.018,0.02,0.0053,0.13,0.13,0.031,3.2e-07,2.9e-07,1.7e-06,0.0018,0.002,9.4e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29390000,-0.29,0.015,-0.0033,0.96,0.067,-0.045,0.81,0.074,-0.053,-2,-0.0013,-0.0058,-0.00014,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.9,4.5e-05,3.9e-05,0.048,0.018,0.019,0.0053,0.13,0.13,0.031,3.2e-07,2.8e-07,1.7e-06,0.0018,0.002,9.4e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29490000,-0.29,0.015,-0.0032,0.96,0.07,-0.046,0.81,0.081,-0.059,-2,-0.0013,-0.0058,-0.00014,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.9,4.5e-05,3.9e-05,0.048,0.019,0.02,0.0053,0.14,0.14,0.031,3.2e-07,2.8e-07,1.7e-06,0.0018,0.002,9.3e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29590000,-0.29,0.015,-0.0031,0.96,0.068,-0.045,0.81,0.079,-0.057,-1.9,-0.0013,-0.0058,-0.00012,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.8,4.5e-05,3.9e-05,0.048,0.018,0.019,0.0052,0.14,0.14,0.031,3.1e-07,2.8e-07,1.7e-06,0.0018,0.002,9.3e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29690000,-0.29,0.015,-0.0032,0.96,0.072,-0.044,0.81,0.087,-0.062,-1.8,-0.0013,-0.0058,-0.00012,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.7,4.5e-05,3.9e-05,0.048,0.019,0.02,0.0053,0.14,0.15,0.031,3.1e-07,2.8e-07,1.7e-06,0.0018,0.002,9.2e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29790000,-0.29,0.015,-0.0029,0.96,0.069,-0.038,0.8,0.084,-0.059,-1.7,-0.0013,-0.0058,-9.8e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.6,4.5e-05,3.9e-05,0.048,0.018,0.02,0.0052,0.15,0.15,0.031,3.1e-07,2.8e-07,1.7e-06,0.0018,0.002,9.1e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29890000,-0.29,0.015,-0.0023,0.96,0.071,-0.039,0.8,0.092,-0.063,-1.7,-0.0013,-0.0058,-9.4e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.6,4.5e-05,3.9e-05,0.048,0.019,0.021,0.0053,0.15,0.16,0.031,3.1e-07,2.8e-07,1.7e-06,0.0018,0.002,9.1e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +29990000,-0.29,0.016,-0.0025,0.96,0.066,-0.038,0.8,0.087,-0.062,-1.6,-0.0013,-0.0058,-8.5e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.5,4.4e-05,3.8e-05,0.048,0.019,0.02,0.0052,0.16,0.16,0.03,3e-07,2.8e-07,1.7e-06,0.0018,0.002,9e-05,0.0011,6.5e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30090000,-0.29,0.016,-0.0026,0.96,0.067,-0.037,0.8,0.094,-0.064,-1.5,-0.0013,-0.0058,-9.4e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,-1.4,4.5e-05,3.9e-05,0.048,0.02,0.021,0.0052,0.16,0.17,0.03,3e-07,2.8e-07,1.7e-06,0.0018,0.002,9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30190000,-0.29,0.016,-0.0026,0.96,0.062,-0.031,0.8,0.089,-0.056,-1.5,-0.0013,-0.0058,-8.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1.4,4.4e-05,3.8e-05,0.048,0.019,0.02,0.0052,0.17,0.17,0.03,3e-07,2.7e-07,1.7e-06,0.0018,0.002,8.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30290000,-0.29,0.016,-0.0026,0.96,0.062,-0.031,0.8,0.096,-0.059,-1.4,-0.0013,-0.0058,-8.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1.3,4.5e-05,3.9e-05,0.048,0.02,0.021,0.0052,0.17,0.18,0.03,3e-07,2.7e-07,1.7e-06,0.0018,0.002,8.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30390000,-0.29,0.015,-0.0026,0.96,0.06,-0.026,0.8,0.095,-0.053,-1.3,-0.0013,-0.0058,-6.8e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1.2,4.4e-05,3.8e-05,0.048,0.019,0.021,0.0052,0.17,0.18,0.03,3e-07,2.7e-07,1.7e-06,0.0018,0.002,8.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30490000,-0.29,0.015,-0.0026,0.96,0.063,-0.025,0.8,0.1,-0.056,-1.2,-0.0013,-0.0058,-6.4e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1.1,4.5e-05,3.8e-05,0.048,0.02,0.022,0.0052,0.18,0.19,0.031,3e-07,2.7e-07,1.7e-06,0.0018,0.002,8.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30590000,-0.29,0.016,-0.0029,0.96,0.062,-0.024,0.8,0.098,-0.053,-1.2,-0.0013,-0.0058,-5e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1.1,4.4e-05,3.8e-05,0.048,0.019,0.021,0.0052,0.18,0.19,0.03,2.9e-07,2.7e-07,1.6e-06,0.0018,0.002,8.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30690000,-0.29,0.016,-0.0032,0.96,0.059,-0.023,0.8,0.1,-0.055,-1.1,-0.0013,-0.0058,-5e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-1,4.4e-05,3.8e-05,0.048,0.02,0.022,0.0052,0.19,0.2,0.03,2.9e-07,2.7e-07,1.6e-06,0.0018,0.002,8.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30790000,-0.29,0.016,-0.003,0.96,0.053,-0.014,0.79,0.096,-0.044,-1,-0.0013,-0.0058,-3.4e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.92,4.4e-05,3.8e-05,0.048,0.019,0.021,0.0052,0.19,0.2,0.03,2.9e-07,2.7e-07,1.6e-06,0.0018,0.002,8.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30890000,-0.29,0.015,-0.0024,0.96,0.051,-0.01,0.79,0.1,-0.044,-0.95,-0.0013,-0.0058,-4e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.85,4.4e-05,3.8e-05,0.048,0.02,0.022,0.0052,0.2,0.21,0.03,2.9e-07,2.7e-07,1.6e-06,0.0018,0.002,8.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +30990000,-0.29,0.015,-0.0025,0.96,0.047,-0.0086,0.79,0.095,-0.043,-0.88,-0.0012,-0.0058,-3.8e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.78,4.4e-05,3.8e-05,0.048,0.02,0.021,0.0052,0.2,0.21,0.03,2.9e-07,2.6e-07,1.6e-06,0.0018,0.002,8.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31090000,-0.29,0.015,-0.0027,0.96,0.046,-0.0074,0.79,0.099,-0.043,-0.81,-0.0012,-0.0058,-4.3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.71,4.4e-05,3.8e-05,0.048,0.02,0.022,0.0052,0.21,0.22,0.03,2.9e-07,2.6e-07,1.6e-06,0.0018,0.002,8.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31190000,-0.29,0.016,-0.0028,0.96,0.043,-0.0042,0.8,0.094,-0.039,-0.74,-0.0012,-0.0058,-2.7e-05,0.067,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.64,4.4e-05,3.8e-05,0.048,0.02,0.021,0.0052,0.21,0.22,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31290000,-0.29,0.016,-0.003,0.96,0.04,-0.0027,0.8,0.097,-0.041,-0.67,-0.0012,-0.0058,-2.3e-05,0.067,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.57,4.4e-05,3.8e-05,0.048,0.02,0.022,0.0052,0.22,0.23,0.03,2.9e-07,2.6e-07,1.6e-06,0.0018,0.002,8.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31390000,-0.29,0.015,-0.0028,0.96,0.035,0.0022,0.8,0.091,-0.037,-0.59,-0.0012,-0.0058,-2.5e-05,0.067,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.49,4.3e-05,3.7e-05,0.048,0.02,0.021,0.0051,0.22,0.23,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.4e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31490000,-0.29,0.016,-0.0025,0.96,0.036,0.0056,0.8,0.096,-0.036,-0.52,-0.0012,-0.0058,-2.8e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.42,4.4e-05,3.8e-05,0.048,0.021,0.022,0.0052,0.23,0.24,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.4e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31590000,-0.29,0.016,-0.0023,0.96,0.037,0.0074,0.8,0.093,-0.032,-0.45,-0.0012,-0.0058,-2e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.35,4.3e-05,3.7e-05,0.048,0.02,0.021,0.0051,0.23,0.24,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.3e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31690000,-0.29,0.016,-0.0023,0.96,0.041,0.0084,0.8,0.098,-0.032,-0.38,-0.0012,-0.0058,-1.3e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.28,4.4e-05,3.8e-05,0.048,0.021,0.022,0.0051,0.24,0.25,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.3e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31790000,-0.29,0.017,-0.0025,0.96,0.035,0.014,0.8,0.094,-0.023,-0.3,-0.0012,-0.0058,-1e-06,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.2,4.3e-05,3.7e-05,0.048,0.02,0.021,0.0051,0.24,0.25,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.2e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31890000,-0.29,0.017,-0.0022,0.96,0.033,0.015,0.8,0.099,-0.021,-0.24,-0.0012,-0.0058,1.4e-06,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.14,4.3e-05,3.7e-05,0.048,0.021,0.022,0.0051,0.25,0.26,0.03,2.8e-07,2.6e-07,1.6e-06,0.0018,0.002,8.2e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +31990000,-0.29,0.016,-0.0025,0.96,0.03,0.017,0.79,0.096,-0.016,-0.17,-0.0012,-0.0058,5.2e-07,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,-0.068,4.3e-05,3.7e-05,0.048,0.02,0.022,0.0051,0.25,0.26,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.2e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +32090000,-0.29,0.016,-0.0029,0.96,0.031,0.021,0.8,0.099,-0.014,-0.096,-0.0012,-0.0058,8.8e-08,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.0038,4.3e-05,3.7e-05,0.048,0.021,0.023,0.0051,0.26,0.27,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +32190000,-0.29,0.016,-0.0032,0.96,0.028,0.028,0.8,0.094,-0.0046,-0.028,-0.0012,-0.0058,3.2e-06,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.072,4.3e-05,3.7e-05,0.048,0.02,0.022,0.0051,0.26,0.27,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +32290000,-0.29,0.016,-0.003,0.96,0.027,0.03,0.79,0.098,-0.0019,0.042,-0.0012,-0.0058,7e-06,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.14,4.3e-05,3.7e-05,0.048,0.021,0.023,0.0051,0.27,0.28,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00064,0.0012,1,1,0.01 +32390000,-0.29,0.016,-0.0032,0.96,0.024,0.032,0.79,0.094,0.0018,0.12,-0.0012,-0.0058,5.2e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.22,4.3e-05,3.7e-05,0.048,0.02,0.022,0.0051,0.27,0.28,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32490000,-0.29,0.015,-0.0063,0.96,-0.016,0.085,-0.078,0.092,0.0094,0.12,-0.0012,-0.0058,2.9e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.22,4.3e-05,3.7e-05,0.048,0.022,0.024,0.0051,0.28,0.29,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32590000,-0.29,0.015,-0.0063,0.96,-0.014,0.084,-0.081,0.093,0.0025,0.1,-0.0012,-0.0058,-2e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.2,4.2e-05,3.7e-05,0.047,0.021,0.023,0.0051,0.28,0.29,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32690000,-0.29,0.015,-0.0063,0.96,-0.0097,0.091,-0.082,0.091,0.011,0.087,-0.0012,-0.0058,-2e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.19,4.3e-05,3.7e-05,0.047,0.022,0.024,0.0051,0.29,0.3,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32790000,-0.29,0.015,-0.0061,0.96,-0.0057,0.09,-0.083,0.093,0.0037,0.072,-0.0012,-0.0058,-7.5e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.17,4.2e-05,3.7e-05,0.047,0.021,0.023,0.0051,0.29,0.3,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32890000,-0.29,0.015,-0.0061,0.96,-0.0062,0.096,-0.085,0.092,0.012,0.057,-0.0012,-0.0058,-3e-06,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.033,-0.067,0,0,0.16,4.2e-05,3.7e-05,0.047,0.022,0.023,0.0051,0.3,0.31,0.03,2.7e-07,2.5e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +32990000,-0.29,0.015,-0.006,0.96,-0.0023,0.091,-0.084,0.092,-0.00052,0.043,-0.0013,-0.0058,-1.1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.14,4.2e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.3,0.31,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +33090000,-0.29,0.015,-0.0059,0.96,0.0014,0.097,-0.081,0.092,0.0087,0.036,-0.0013,-0.0058,-1e-05,0.066,0.013,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.14,4.2e-05,3.7e-05,0.047,0.022,0.023,0.0051,0.31,0.32,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8.1e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +33190000,-0.29,0.015,-0.0059,0.96,0.0056,0.093,-0.08,0.093,-0.0059,0.028,-0.0013,-0.0058,-2.9e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.13,4.1e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.31,0.31,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +33290000,-0.29,0.015,-0.0059,0.96,0.0098,0.096,-0.08,0.094,0.0029,0.02,-0.0013,-0.0058,-1.8e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.2e-05,3.7e-05,0.047,0.021,0.023,0.0051,0.32,0.33,0.03,2.6e-07,2.4e-07,1.5e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.01 +33390000,-0.3,0.015,-0.0058,0.96,0.014,0.093,-0.078,0.094,-0.0054,0.011,-0.0013,-0.0058,-2.6e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.0051,0.32,0.32,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.033 +33490000,-0.3,0.015,-0.0058,0.96,0.02,0.097,-0.077,0.097,0.004,0.0016,-0.0013,-0.0058,-2.1e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.2e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.33,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.058 +33590000,-0.3,0.015,-0.0056,0.95,0.023,0.094,-0.074,0.096,-0.0089,-0.0062,-0.0013,-0.0058,-2.7e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.33,0.33,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.083 +33690000,-0.3,0.015,-0.0056,0.95,0.026,0.098,-0.075,0.097,9.1e-05,-0.014,-0.0013,-0.0058,-2.2e-05,0.066,0.014,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.34,0.35,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.9e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.11 +33790000,-0.3,0.015,-0.0056,0.95,0.028,0.096,-0.069,0.094,-0.013,-0.021,-0.0013,-0.0058,-3.5e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.34,0.34,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.13 +33890000,-0.3,0.015,-0.0055,0.95,0.033,0.097,-0.069,0.097,-0.004,-0.028,-0.0013,-0.0058,-2.5e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.35,0.36,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.16 +33990000,-0.3,0.015,-0.0054,0.95,0.035,0.096,-0.065,0.096,-0.012,-0.032,-0.0013,-0.0058,-3.6e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.35,0.35,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.18 +34090000,-0.3,0.015,-0.0054,0.95,0.038,0.1,-0.064,0.099,-0.0027,-0.036,-0.0013,-0.0057,-3.2e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.0051,0.36,0.37,0.03,2.6e-07,2.4e-07,1.4e-06,0.0018,0.002,7.8e-05,0.0011,6.4e-05,0.0012,0.0011,0.00063,0.0012,1,1,0.21 +34190000,-0.3,0.015,-0.0054,0.95,0.039,0.097,-0.061,0.094,-0.014,-0.039,-0.0013,-0.0057,-3.7e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.36,0.36,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.23 +34290000,-0.3,0.015,-0.0052,0.95,0.041,0.1,-0.06,0.098,-0.0047,-0.045,-0.0013,-0.0057,-3e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.023,0.005,0.37,0.37,0.03,2.5e-07,2.4e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.26 +34390000,-0.3,0.015,-0.0051,0.95,0.042,0.097,-0.055,0.093,-0.016,-0.049,-0.0013,-0.0057,-3.9e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.084,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.02,0.022,0.005,0.37,0.37,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.7e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.28 +34490000,-0.3,0.015,-0.0051,0.95,0.045,0.1,-0.053,0.097,-0.0068,-0.052,-0.0013,-0.0057,-3e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.085,-0.032,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.047,0.021,0.022,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.31 +34590000,-0.3,0.014,-0.005,0.95,0.045,0.098,0.74,0.092,-0.021,-0.024,-0.0013,-0.0057,-4e-05,0.066,0.015,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.38,0.38,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.33 +34690000,-0.3,0.014,-0.0051,0.95,0.05,0.1,1.7,0.097,-0.011,0.095,-0.0013,-0.0057,-3.7e-05,0.066,0.016,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.02,0.021,0.005,0.39,0.39,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.6e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.36 +34790000,-0.3,0.014,-0.005,0.95,0.051,0.099,2.7,0.091,-0.025,0.27,-0.0013,-0.0057,-4.7e-05,0.066,0.016,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.019,0.019,0.005,0.39,0.39,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.4e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.38 +34890000,-0.3,0.014,-0.005,0.95,0.056,0.1,3.7,0.096,-0.015,0.56,-0.0013,-0.0057,-4.3e-05,0.066,0.016,-0.12,-0.11,-0.025,0.5,0.085,-0.031,-0.067,0,0,0.12,4.1e-05,3.6e-05,0.046,0.019,0.019,0.005,0.4,0.4,0.03,2.5e-07,2.3e-07,1.4e-06,0.0018,0.002,7.5e-05,0.0011,6.3e-05,0.0012,0.0011,0.00062,0.0012,1,1,0.41 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 513e6d057b3d..9fd62885e36b 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -132,7 +132,7 @@ void EkfWrapper::disableGpsHeadingFusion() bool EkfWrapper::isIntendingGpsHeadingFusion() const { - return _ekf->control_status_flags().gps_yaw; + return _ekf->control_status_flags().gnss_yaw; } void EkfWrapper::enableFlowFusion() @@ -240,38 +240,14 @@ bool EkfWrapper::isWindVelocityEstimated() const return _ekf->control_status_flags().wind; } -void EkfWrapper::enableTerrainRngFusion() -{ - _ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseRangeFinder; -} - -void EkfWrapper::disableTerrainRngFusion() -{ - _ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseRangeFinder; -} - bool EkfWrapper::isIntendingTerrainRngFusion() const { - terrain_fusion_status_u terrain_status; - terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); - return terrain_status.flags.range_finder; -} - -void EkfWrapper::enableTerrainFlowFusion() -{ - _ekf_params->terrain_fusion_mode |= TerrainFusionMask::TerrainFuseOpticalFlow; -} - -void EkfWrapper::disableTerrainFlowFusion() -{ - _ekf_params->terrain_fusion_mode &= ~TerrainFusionMask::TerrainFuseOpticalFlow; + return _ekf->control_status_flags().rng_terrain; } bool EkfWrapper::isIntendingTerrainFlowFusion() const { - terrain_fusion_status_u terrain_status; - terrain_status.value = _ekf->getTerrainEstimateSensorBitfield(); - return terrain_status.flags.flow; + return _ekf->control_status_flags().opt_flow_terrain; } Eulerf EkfWrapper::getEulerAngles() const @@ -314,3 +290,13 @@ float EkfWrapper::getMagHeadingNoise() const { return _ekf_params->mag_heading_noise; } + +void EkfWrapper::enableGyroBiasEstimation() +{ + _ekf_params->imu_ctrl |= static_cast(ImuCtrl::GyroBias); +} + +void EkfWrapper::disableGyroBiasEstimation() +{ + _ekf_params->imu_ctrl &= ~static_cast(ImuCtrl::GyroBias); +} diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 43753e9bc548..69b72475dca9 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -110,12 +110,8 @@ class EkfWrapper bool isWindVelocityEstimated() const; - void enableTerrainRngFusion(); - void disableTerrainRngFusion(); bool isIntendingTerrainRngFusion() const; - void enableTerrainFlowFusion(); - void disableTerrainFlowFusion(); bool isIntendingTerrainFlowFusion() const; Eulerf getEulerAngles() const; @@ -128,6 +124,9 @@ class EkfWrapper float getMagHeadingNoise() const; + void enableGyroBiasEstimation(); + void disableGyroBiasEstimation(); + private: std::shared_ptr _ekf; diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index f962cc9bced7..e8ff206916f7 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -66,6 +66,7 @@ class Gps: public Sensor void setPdop(const float pdop); gnssSample getDefaultGpsData(); + const gnssSample &getData() const { return _gps_data; } private: void send(uint64_t time) override; diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.h b/src/modules/ekf2/test/sensor_simulator/range_finder.h index 0dcd068232e7..c4910ed57c43 100644 --- a/src/modules/ekf2/test/sensor_simulator/range_finder.h +++ b/src/modules/ekf2/test/sensor_simulator/range_finder.h @@ -55,7 +55,7 @@ class RangeFinder: public Sensor void setLimits(float min_distance_m, float max_distance_m); private: - rangeSample _range_sample{}; + estimator::sensor::rangeSample _range_sample{}; float _min_distance{0.2f}; float _max_distance{20.0f}; diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index c7a8ffa83643..e95443344235 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -123,7 +123,7 @@ TEST_F(EkfBasicsTest, initialControlMode) EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); - EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); @@ -178,7 +178,7 @@ TEST_F(EkfBasicsTest, gpsFusion) EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault); EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect); EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck); - EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw); + EXPECT_EQ(0, (int) _ekf->control_status_flags().gnss_yaw); EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight); EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel); EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z); diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 734d485fbcd3..5b1439edd9f2 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -259,37 +259,24 @@ TEST_F(EkfExternalVisionTest, visionAlignment) TEST_F(EkfExternalVisionTest, velocityFrameBody) { - // GIVEN: Drone is turned 90 degrees - const Quatf quat_sim(Eulerf(0.0f, 0.0f, math::radians(90.0f))); - _sensor_simulator.simulateOrientation(quat_sim); + _ekf_wrapper.setMagFuseTypeNone(); _sensor_simulator.runSeconds(_tilt_align_time); - _ekf->set_vehicle_at_rest(false); - // Without any measurement x and y velocity variance are close - const Vector3f velVar_init = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001); - - // WHEN: measurement is given in BODY-FRAME and - // x variance is bigger than y variance - _sensor_simulator._vio.setVelocityFrameToBody(); + float yaw_var0 = _ekf->getYawVar(); const Vector3f vel_cov_body(2.0f, 0.01f, 0.01f); const Vector3f vel_body(1.0f, 0.0f, 0.0f); + _sensor_simulator._vio.setVelocityFrameToBody(); _sensor_simulator._vio.setVelocityVariance(vel_cov_body); _sensor_simulator._vio.setVelocity(vel_body); _ekf_wrapper.enableExternalVisionVelocityFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(4); - // THEN: As the drone is turned 90 degrees, velocity variance - // along local y axis is expected to be bigger - const Vector3f velVar_new = _ekf->getVelocityVariance(); - EXPECT_NEAR(velVar_new(1) / velVar_new(0), 30.f, 15.f); - - const Vector3f vel_earth_est = _ekf->getVelocity(); - EXPECT_NEAR(vel_earth_est(0), 0.0f, 0.1f); - EXPECT_NEAR(vel_earth_est(1), 1.0f, 0.1f); + const Vector3f vel_var = _ekf->getVelocityVariance(); + EXPECT_TRUE(yaw_var0 < _ekf->getYawVar()); + EXPECT_TRUE(vel_var(1) < vel_var(0)); } TEST_F(EkfExternalVisionTest, velocityFrameLocal) diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 9d3df6cbd8d3..787560ce1bf6 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -120,7 +120,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) _sensor_simulator.runSeconds(5.f); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + const float estimated_distance_to_ground = _ekf->getHagl(); EXPECT_FLOAT_EQ(estimated_distance_to_ground, simulated_distance_to_ground); reset_logging_checker.capturePreResetState(); @@ -133,10 +133,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); - // Let it reset but not fuse more measurements. We actually need to send 2 - // samples to get a reset because the first one cannot be used as the gyro - // compensation needs to be accumulated between two samples. - _sensor_simulator.runTrajectorySeconds(0.14); + _sensor_simulator.runTrajectorySeconds(1); // THEN: estimated velocity should match simulated velocity const Vector3f estimated_velocity = _ekf->getVelocity(); @@ -144,7 +141,11 @@ TEST_F(EkfFlowTest, resetToFlowVelocityInAir) simulated_velocity.print(); EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity)) << "estimated vel = " << estimated_velocity(0) << ", " - << estimated_velocity(1); + << estimated_velocity(1) << "\n" + << "simulated vel = " << simulated_velocity(0) << ", " + << simulated_velocity(1); + + EXPECT_NEAR(simulated_distance_to_ground, _ekf->getHagl(), 0.1f); // AND: the reset in velocity should be saved correctly reset_logging_checker.capturePostResetState(); @@ -158,7 +159,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) ResetLoggingChecker reset_logging_checker(_ekf); // WHEN: being on ground - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + const float estimated_distance_to_ground = _ekf->getHagl(); EXPECT_LT(estimated_distance_to_ground, 0.3f); reset_logging_checker.capturePreResetState(); @@ -177,11 +178,10 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) EXPECT_TRUE(isEqual(estimated_horz_velocity, Vector2f(0.f, 0.f))) << estimated_horz_velocity(0) << ", " << estimated_horz_velocity(1); - // AND: the reset in velocity should be saved correctly + // AND: the horizontal velocity is reset to the flow value reset_logging_checker.capturePostResetState(); EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); - EXPECT_TRUE(reset_logging_checker.isVelocityDeltaLoggedCorrectly(1e-9f)); } TEST_F(EkfFlowTest, inAirConvergence) @@ -222,9 +222,9 @@ TEST_F(EkfFlowTest, inAirConvergence) // THEN: estimated velocity should converge to the simulated velocity // This takes a bit of time because the data is inconsistent with IMU measurements estimated_velocity = _ekf->getVelocity(); - EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.05f) + EXPECT_NEAR(estimated_velocity(0), simulated_velocity(0), 0.01f) << "estimated vel = " << estimated_velocity(0); - EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.05f) + EXPECT_NEAR(estimated_velocity(1), simulated_velocity(1), 0.01f) << estimated_velocity(1); } @@ -258,10 +258,11 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithAutopilotGyroData) // THEN: the flow due to the yaw rotation and the offsets is canceled // and the velocity estimate stays 0 + // FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(0); - EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(1); } @@ -296,10 +297,11 @@ TEST_F(EkfFlowTest, yawMotionCorrectionWithFlowGyroData) // THEN: the flow due to the yaw rotation and the offsets is canceled // and the velocity estimate stays 0 + // FIXME: the estimate isn't perfect 0 mainly because the mag simulated measurement isn't rotating const Vector2f estimated_horz_velocity = Vector2f(_ekf->getVelocity()); - EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(0), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(0); - EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.01f) + EXPECT_NEAR(estimated_horz_velocity(1), 0.f, 0.02f) << "estimated vel = " << estimated_horz_velocity(1); _ekf->state().vector().print(); _ekf->covariances().print(); diff --git a/src/modules/ekf2/test/test_EKF_flow_generated.cpp b/src/modules/ekf2/test/test_EKF_flow_generated.cpp new file mode 100644 index 000000000000..58a0eabdeb6a --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_flow_generated.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2024 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 "EKF/ekf.h" +#include "test_helper/comparison_helper.h" + +#include "../EKF/python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h" +#include "../EKF/python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h" + +using namespace matrix; + +TEST(FlowGenerated, distBottom0xy) +{ + // GIVEN: 0 distance to the ground (singularity) + StateSample state{}; + state.quat_nominal = Quatf(); + + const float R = sq(radians(sq(0.5f))); + SquareMatrixState P = createRandomCovarianceMatrix(); + + VectorState H; + Vector2f innov_var; + sym::ComputeFlowXyInnovVarAndHx(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); + EXPECT_GT(innov_var(0), 1e12); + EXPECT_GT(innov_var(1), 1e12); +} + +TEST(FlowGenerated, distBottom0y) +{ + // GIVEN: 0 distance to the ground (singularity) + StateSample state{}; + state.quat_nominal = Quatf(); + + const float R = sq(radians(sq(0.5f))); + SquareMatrixState P = createRandomCovarianceMatrix(); + + VectorState H; + float innov_var; + sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H); + EXPECT_GT(innov_var, 1e12); +} diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp similarity index 92% rename from src/modules/ekf2/test/test_EKF_gps_yaw.cpp rename to src/modules/ekf2/test/test_EKF_gnss_yaw.cpp index 42737306eae3..93dd222375f9 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gnss_yaw.cpp @@ -81,9 +81,10 @@ class EkfGpsHeadingTest : public ::testing::Test void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float antenna_offset_rad) { // GIVEN: an initial GPS yaw, not aligned with the current one - float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + yaw_offset_rad); + // The yaw antenna offset has already been corrected in the driver + float gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle()); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYaw(gps_heading); // used to remove the correction to fuse the real measurement _sensor_simulator._gps.setYawOffset(antenna_offset_rad); // WHEN: the GPS yaw fusion is activated @@ -91,7 +92,7 @@ void EkfGpsHeadingTest::runConvergenceScenario(float yaw_offset_rad, float anten _sensor_simulator.runSeconds(5); // THEN: the estimate is reset and stays close to the measurement - checkConvergence(gps_heading, 0.05f); + checkConvergence(gps_heading, 0.01f); } void EkfGpsHeadingTest::checkConvergence(float truth, float tolerance_deg) @@ -203,7 +204,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); - const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + //const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); // BUT WHEN: the GPS yaw is suddenly invalid gps_heading = NAN; @@ -214,7 +215,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag) // the estimator should fall back to mag fusion EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + //EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); } TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator) @@ -274,9 +275,15 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(8); - // THEN: the fusion should reset - EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + // THEN: the fusion should stop, reset to mag + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); + + // AND THEN: restart GNSS yaw fusion + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion()); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f)); } @@ -284,7 +291,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) { // GIVEN: the GPS yaw fusion activated float gps_heading = _ekf_wrapper.getYawAngle(); - _sensor_simulator._gps.setYaw(gps_heading); + _sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f)); _sensor_simulator.runSeconds(5); _ekf->set_in_air_status(true); @@ -294,20 +301,14 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) _sensor_simulator._gps.setYaw(gps_heading); _sensor_simulator.runSeconds(7.5); - // THEN: the fusion should reset - EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); - - // BUT WHEN: the measurement jumps a 2nd time - gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f)); - _sensor_simulator._gps.setYaw(gps_heading); - _sensor_simulator.runSeconds(7.5); + // THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); // THEN: after a few seconds, the fusion should stop and // the estimator doesn't fall back to mag fusion because it has // been declared inconsistent with the filter states EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent()); - //TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop? EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); } @@ -327,7 +328,7 @@ TEST_F(EkfGpsHeadingTest, stopOnGround) // THEN: the fusion should stop and the GPS pos/vel aiding // should stop as well because the yaw is not aligned anymore EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion()); - EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + //EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); // AND IF: the mag fusion type is set to NONE _ekf_wrapper.setMagFuseTypeNone(); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 57e277240faf..b03a8701d52a 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -105,7 +105,7 @@ TEST_F(EkfGpsTest, gpsFixLoss) _sensor_simulator._gps.setFixType(0); // THEN: after dead-reconing for a couple of seconds, the local position gets invalidated - _sensor_simulator.runSeconds(5); + _sensor_simulator.runSeconds(6); EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); EXPECT_FALSE(_ekf->local_position_is_valid()); @@ -136,7 +136,7 @@ TEST_F(EkfGpsTest, resetToGpsVelocity) _ekf->set_in_air_status(true); _ekf->set_vehicle_at_rest(false); - _sensor_simulator.runSeconds(5.2); // required to pass the checks + _sensor_simulator.runSeconds(1.2); // required to pass the checks _sensor_simulator.runMicroseconds(dt_us); // THEN: a reset to GPS velocity should be done diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 329015dc4a40..b322a2818245 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -60,6 +60,7 @@ class EkfHeightFusionTest : public ::testing::Test { _ekf->init(0); _ekf_wrapper.disableBaroHeightFusion(); + _ekf_wrapper.disableRangeHeightFusion(); _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); @@ -111,7 +112,7 @@ TEST_F(EkfHeightFusionTest, baroRef) EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); // AND WHEN: the baro data increases - const float baro_increment = 5.f; + const float baro_increment = 4.f; _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_increment); _sensor_simulator.runSeconds(60); @@ -124,8 +125,8 @@ TEST_F(EkfHeightFusionTest, baroRef) const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus(); EXPECT_NEAR(gps_status.bias, -baro_increment, 0.2f); - const BiasEstimator::status &rng_status = _ekf->getRngHgtBiasEstimatorStatus(); - EXPECT_NEAR(rng_status.bias, -baro_increment, 1.2f); + const float terrain = _ekf->getTerrainVertPos(); + EXPECT_NEAR(terrain, -baro_increment, 1.2f); const BiasEstimator::status &ev_status = _ekf->getEvHgtBiasEstimatorStatus(); EXPECT_EQ(ev_status.bias, 0.f); @@ -150,8 +151,8 @@ TEST_F(EkfHeightFusionTest, baroRef) // the estimated height follows the GPS height EXPECT_NEAR(_ekf->getPosition()(2), -(baro_increment + gps_increment), 0.3f); // and the range finder bias is adjusted to follow the new reference - const BiasEstimator::status &rng_status_2 = _ekf->getRngHgtBiasEstimatorStatus(); - EXPECT_NEAR(rng_status_2.bias, -(baro_increment + gps_increment), 1.3f); + const float terrain2 = _ekf->getTerrainVertPos(); + EXPECT_NEAR(terrain2, -(baro_increment + gps_increment), 1.3f); } TEST_F(EkfHeightFusionTest, gpsRef) @@ -181,8 +182,8 @@ TEST_F(EkfHeightFusionTest, gpsRef) const BiasEstimator::status &baro_status = _ekf->getBaroBiasEstimatorStatus(); EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f); - const BiasEstimator::status &rng_status = _ekf->getRngHgtBiasEstimatorStatus(); - EXPECT_NEAR(rng_status.bias, 0.f, 1.1f); // TODO: why? + const float terrain = _ekf->getTerrainVertPos(); + EXPECT_NEAR(terrain, 0.f, 1.1f); // TODO: why? // BUT WHEN: the GPS jumps by a lot const float gps_step = 100.f; @@ -260,6 +261,93 @@ TEST_F(EkfHeightFusionTest, gpsRefFailOver) EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::UNKNOWN); } +TEST_F(EkfHeightFusionTest, gpsRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS (reference) and baro + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setGpsHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::GNSS); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(15); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (GNSS) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - gnss_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); +} + +TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) +{ + // GIVEN: EKF that fuses GNSS and baro (reference) + _sensor_simulator.startBaro(); + _sensor_simulator.startGps(); + _ekf_wrapper.setBaroHeightRef(); + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + + _sensor_simulator.runSeconds(11); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::BARO); + + const Vector3f previous_position = _ekf->getPosition(); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: + const float gnss_height_step = 10.f; + _sensor_simulator._gps.stepHeightByMeters(gnss_height_step); + + const float baro_height_step = 5.f; + _sensor_simulator._baro.setData(_sensor_simulator._baro.getData() + baro_height_step); + _sensor_simulator.runSeconds(20); + + // THEN: then the fusion of both sensors starts to fail and the height is reset to the + // reference sensor (baro) + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + + const Vector3f new_position = _ekf->getPosition(); + EXPECT_NEAR(new_position(2), previous_position(2) - baro_height_step, 0.2f); + + // Also check the reset counters to make sure the reset logic triggered + reset_logging_checker.capturePostResetState(); + + // The velocity does not reset as baro only provides height measurement + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + + // The height resets twice in a row as the baro innovation is not corrected after a height + // reset and triggers a new reset at the next iteration + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); +} + TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) { _sensor_simulator.startBaro(); @@ -290,7 +378,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) reset_logging_checker.capturePostResetState(); EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() + alt_increment, 0.2f); - EXPECT_NEAR(_ekf->getRngHgtBiasEstimatorStatus().bias, alt_increment, 1.f); + EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); } diff --git a/src/modules/ekf2/test/test_EKF_imuSampling.cpp b/src/modules/ekf2/test/test_EKF_imuSampling.cpp index bd300c538557..5ba380d2c079 100644 --- a/src/modules/ekf2/test/test_EKF_imuSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_imuSampling.cpp @@ -34,7 +34,7 @@ #include #include #include "EKF/ekf.h" -#include "EKF/imu_down_sampler.hpp" +#include "EKF/imu_down_sampler/imu_down_sampler.hpp" class EkfImuSamplingTest : public ::testing::TestWithParam> { diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index 6e2dae1b9eba..fb1990f90b1f 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -78,7 +78,7 @@ class EkfInitializationTest : public ::testing::Test void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f) { - const matrix::Vector3f quat_variance = _ekf->getQuaternionVariance(); + const matrix::Vector3f quat_variance = _ekf->getRotVarBody(); EXPECT_TRUE(quat_variance(0) > quat_variance_limit) << "quat_variance(3): " << quat_variance(0); EXPECT_TRUE(quat_variance(1) > quat_variance_limit) << "quat_variance(1): " << quat_variance(1); EXPECT_TRUE(quat_variance(2) > quat_variance_limit) << "quat_variance(2): " << quat_variance(2); @@ -191,6 +191,32 @@ TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest) learningCorrectAccelBias(); } +TEST_F(EkfInitializationTest, initializeWithTiltNoGyroBiasEstimate) +{ + const float pitch = math::radians(30.0f); + const float roll = math::radians(-20.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _ekf_wrapper.disableGyroBiasEstimation(); + _sensor_simulator.simulateOrientation(quat_sim); + + _sensor_simulator.runSeconds(_init_tilt_period); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); // Fake position fusion obs var when at rest sq(0.5f) + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); +} + TEST_F(EkfInitializationTest, gyroBias) { // GIVEN: a healthy filter @@ -320,7 +346,7 @@ TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest) _ekf->set_vehicle_at_rest(false); _sensor_simulator.simulateOrientation(quat_sim); //_sensor_simulator.runSeconds(_init_tilt_period); - _sensor_simulator.runSeconds(7); + _sensor_simulator.runSeconds(10); EXPECT_TRUE(_ekf->control_status_flags().tilt_align); diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 6641b4fe6dc1..ca4f440c624d 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -68,6 +68,7 @@ class EkfMagTest : public ::testing::Test TEST_F(EkfMagTest, fusionStartWithReset) { + _ekf->set_min_required_gps_health_time(5e6); // GIVEN: some meaningful mag data const float mag_heading = M_PI_F / 3.f; const float incl = 63.1f; @@ -84,11 +85,10 @@ TEST_F(EkfMagTest, fusionStartWithReset) EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); - // AND WHEN: GNSS fusion starts _ekf_wrapper.enableGpsFusion(); _sensor_simulator.startGps(); - _sensor_simulator.runSeconds(11); + _sensor_simulator.runSeconds(6); // THEN: the earth mag field is reset to the WMM EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain.cpp similarity index 84% rename from src/modules/ekf2/test/test_EKF_terrain_estimator.cpp rename to src/modules/ekf2/test/test_EKF_terrain.cpp index ccd7dbc76a6b..5549eb8030d0 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain.cpp @@ -32,8 +32,7 @@ ****************************************************************************/ /** - * Test the terrain estimator - * @author Mathieu Bresciani + * Test the terrain estimate */ #include @@ -116,18 +115,22 @@ class EkfTerrainTest : public ::testing::Test TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) { + // GIVEN: flow and range are enabled + _ekf_wrapper.enableFlowFusion(); + _ekf_wrapper.enableRangeHeightFusion(); + // WHEN: simulate being 5m above ground const float simulated_distance_to_ground = 1.f; runFlowAndRngScenario(simulated_distance_to_ground, simulated_distance_to_ground); - // THEN: By default, both rng and flow aiding are active + // THEN: both should start terrain aiding EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); - EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); - EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 0.01); + EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + const float estimated_distance_to_ground = _ekf->getHagl(); + EXPECT_NEAR(estimated_distance_to_ground, simulated_distance_to_ground, 0.02); // WHEN: rng fusion is disabled - _ekf_wrapper.disableTerrainRngFusion(); + _ekf_wrapper.disableRangeHeightFusion(); _sensor_simulator.runSeconds(5.1); // THEN: rng fusion should be disabled and flow fusion should take over @@ -135,7 +138,7 @@ TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); // WHEN: flow is now diabled - _ekf_wrapper.disableTerrainFlowFusion(); + _ekf_wrapper.disableFlowFusion(); _sensor_simulator.runSeconds(0.2); // THEN: flow is now also disabled @@ -146,54 +149,56 @@ TEST_F(EkfTerrainTest, setFlowAndRangeTerrainFusion) TEST_F(EkfTerrainTest, testFlowForTerrainFusion) { // GIVEN: flow for terrain enabled but not range finder - _ekf_wrapper.enableTerrainFlowFusion(); - _ekf_wrapper.disableTerrainRngFusion(); + _ekf_wrapper.enableFlowFusion(); + _ekf_wrapper.disableRangeHeightFusion(); // WHEN: the sensors do not agree const float rng_height = 1.f; - const float flow_height = 5.f; + const float flow_height = 8.f; runFlowAndRngScenario(rng_height, flow_height); // THEN: the estimator should use flow for terrain and the estimated terrain height // should converge to the simulated height EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainRngFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + EXPECT_TRUE(_ekf->isTerrainEstimateValid()); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); - EXPECT_NEAR(estimated_distance_to_ground, flow_height, 0.5f); + const float estimated_distance_to_ground = _ekf->getHagl(); + EXPECT_NEAR(estimated_distance_to_ground, flow_height, 0.9f); } TEST_F(EkfTerrainTest, testRngForTerrainFusion) { // GIVEN: rng for terrain but not flow - _ekf_wrapper.disableTerrainFlowFusion(); - _ekf_wrapper.enableTerrainRngFusion(); + _ekf_wrapper.disableFlowFusion(); + _ekf_wrapper.enableRangeHeightFusion(); // WHEN: the sensors do not agree const float rng_height = 1.f; - const float flow_height = 5.f; + const float flow_height = 8.f; runFlowAndRngScenario(rng_height, flow_height); // THEN: the estimator should use rng for terrain and the estimated terrain height // should converge to the simulated height EXPECT_TRUE(_ekf_wrapper.isIntendingTerrainRngFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingTerrainFlowFusion()); + EXPECT_TRUE(_ekf->isTerrainEstimateValid()); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); + const float estimated_distance_to_ground = _ekf->getHagl(); EXPECT_NEAR(estimated_distance_to_ground, rng_height, 0.01f); } TEST_F(EkfTerrainTest, testHeightReset) { // GIVEN: rng for terrain but not flow - _ekf_wrapper.disableTerrainFlowFusion(); - _ekf_wrapper.enableTerrainRngFusion(); + _ekf_wrapper.disableFlowFusion(); + _ekf_wrapper.enableRangeHeightFusion(); const float rng_height = 1.f; const float flow_height = 1.f; runFlowAndRngScenario(rng_height, flow_height); - const float estimated_distance_to_ground = _ekf->getTerrainVertPos() - _ekf->getPosition()(2); + const float estimated_distance_to_ground = _ekf->getHagl(); ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); @@ -202,10 +207,10 @@ TEST_F(EkfTerrainTest, testHeightReset) const float new_baro_height = _sensor_simulator._baro.getData() + 50.f; _sensor_simulator._baro.setData(new_baro_height); _sensor_simulator.stopGps(); // prevent from switching to GNSS height - _sensor_simulator.runSeconds(6); + _sensor_simulator.runSeconds(10); - // THEN: a height reset occured and the estimated distance to the ground remains constant + // THEN: a height reset occurred and the estimated distance to the ground remains constant reset_logging_checker.capturePostResetState(); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); - EXPECT_NEAR(estimated_distance_to_ground, _ekf->getTerrainVertPos() - _ekf->getPosition()(2), 1e-3f); + EXPECT_NEAR(estimated_distance_to_ground, _ekf->getHagl(), 1e-3f); } diff --git a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp index 7e536e62bb79..77d1f10687d4 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_fusion_generated.cpp @@ -42,9 +42,8 @@ using namespace matrix; Vector3f getRotVarNed(const Quatf &q, const SquareMatrixState &P) { constexpr auto S = State::quat_nominal; - matrix::SquareMatrix3f rot_cov_body = P.slice(S.idx, S.idx); - auto R_to_earth = Dcmf(q); - return matrix::SquareMatrix(R_to_earth * rot_cov_body * R_to_earth.T()).diag(); + matrix::SquareMatrix3f rot_cov_ned = P.slice(S.idx, S.idx); + return rot_cov_ned.diag(); } TEST(YawFusionGenerated, positiveVarianceAllOrientations) diff --git a/src/modules/ekf2/test/test_SensorRangeFinder.cpp b/src/modules/ekf2/test/test_SensorRangeFinder.cpp index 4a3b3c2b5255..fc515a875768 100644 --- a/src/modules/ekf2/test/test_SensorRangeFinder.cpp +++ b/src/modules/ekf2/test/test_SensorRangeFinder.cpp @@ -34,10 +34,10 @@ #include #include #include "EKF/common.h" -#include "EKF/sensor_range_finder.hpp" +#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp" #include -using estimator::rangeSample; +using estimator::sensor::rangeSample; using matrix::Dcmf; using matrix::Eulerf; using namespace estimator::sensor; diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 4c5fbc8be785..b2bb5632b7bf 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -170,7 +170,8 @@ bool FlightTaskAuto::update() waypoints[2] = _position_setpoint; } - const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned; + const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first) + && !_yaw_sp_aligned; const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active; _updateTrajConstraints(); PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints; @@ -451,7 +452,7 @@ bool FlightTaskAuto::_evaluateTriplets() _triplet_prev_wp(2) = -(_sub_triplet_setpoint.get().previous.alt - _reference_altitude); } else { - _triplet_prev_wp = _position; + _triplet_prev_wp = _triplet_target; } _prev_was_valid = _sub_triplet_setpoint.get().previous.valid; @@ -532,32 +533,37 @@ void FlightTaskAuto::_set_heading_from_mode() Vector2f v; // Vector that points towards desired location - switch (_param_mpc_yaw_mode.get()) { + switch (yaw_mode(_param_mpc_yaw_mode.get())) { - case 0: // Heading points towards the current waypoint. - case 4: // Same as 0 but yaw first and then go + case yaw_mode::towards_waypoint: // Heading points towards the current waypoint. + case yaw_mode::towards_waypoint_yaw_first: // Same as 0 but yaw first and then go v = Vector2f(_target) - Vector2f(_position); break; - case 1: // Heading points towards home. + case yaw_mode::towards_home: // Heading points towards home. if (_sub_home_position.get().valid_lpos) { v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position); } break; - case 2: // Heading point away from home. + case yaw_mode::away_from_home: // Heading point away from home. if (_sub_home_position.get().valid_lpos) { v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x); } break; - case 3: // Along trajectory. + case yaw_mode::along_trajectory: // Along trajectory. // The heading depends on the kind of setpoint generation. This needs to be implemented // in the subclasses where the velocity setpoints are generated. v.setAll(NAN); break; + + case yaw_mode::yaw_fixed: // Yaw fixed. + // Yaw is operated via manual control or MAVLINK messages. + break; + } if (v.isAllFinite()) { @@ -626,29 +632,27 @@ bool FlightTaskAuto::_evaluateGlobalReference() State FlightTaskAuto::_getCurrentState() { // Calculate the vehicle current state based on the Navigator triplets and the current position. - const Vector2f u_prev_to_target_xy = Vector2f(_triplet_target - _triplet_prev_wp).unit_or_zero(); - const Vector2f pos_to_target_xy = Vector2f(_triplet_target - _position); - const Vector2f prev_to_pos_xy = Vector2f(_position - _triplet_prev_wp); + const Vector3f u_prev_to_target = (_triplet_target - _triplet_prev_wp).unit_or_zero(); + const Vector3f prev_to_pos = _position - _triplet_prev_wp; + const Vector3f pos_to_target = _triplet_target - _position; // Calculate the closest point to the vehicle position on the line prev_wp - target - const Vector2f closest_pt_xy = Vector2f(_triplet_prev_wp) + u_prev_to_target_xy * (prev_to_pos_xy * - u_prev_to_target_xy); - _closest_pt = Vector3f(closest_pt_xy(0), closest_pt_xy(1), _triplet_target(2)); + _closest_pt = _triplet_prev_wp + u_prev_to_target * (prev_to_pos * u_prev_to_target); State return_state = State::none; - if (u_prev_to_target_xy.length() < FLT_EPSILON) { + if (!u_prev_to_target.longerThan(FLT_EPSILON)) { // Previous and target are the same point, so we better don't try to do any special line following return_state = State::none; - } else if (u_prev_to_target_xy * pos_to_target_xy < 0.0f) { + } else if (u_prev_to_target * pos_to_target < 0.0f) { // Target is behind return_state = State::target_behind; - } else if (u_prev_to_target_xy * prev_to_pos_xy < 0.0f && prev_to_pos_xy.longerThan(_target_acceptance_radius)) { + } else if (u_prev_to_target * prev_to_pos < 0.0f && prev_to_pos.longerThan(_target_acceptance_radius)) { // Previous is in front return_state = State::previous_infront; - } else if (Vector2f(_position - _closest_pt).longerThan(_target_acceptance_radius)) { + } else if ((_position - _closest_pt).longerThan(_target_acceptance_radius)) { // Vehicle too far from the track return_state = State::offtrack; diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index fc671cd5e5d6..90a98cac23ad 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -81,6 +81,15 @@ enum class State { none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */ }; +enum class yaw_mode : int32_t { + towards_waypoint = 0, + towards_home = 1, + away_from_home = 2, + along_trajectory = 3, + towards_waypoint_yaw_first = 4, + yaw_fixed = 5, +}; + class FlightTaskAuto : public FlightTask { public: diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index 063b78dcf6c6..55b587d270d9 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -63,6 +63,11 @@ void FlightTask::_checkEkfResetCounters() _reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter; } + if (_sub_vehicle_local_position.get().dist_bottom_reset_counter != _reset_counters.hagl) { + _ekfResetHandlerHagl(_sub_vehicle_local_position.get().delta_dist_bottom); + _reset_counters.hagl = _sub_vehicle_local_position.get().dist_bottom_reset_counter; + } + if (_sub_vehicle_local_position.get().vz_reset_counter != _reset_counters.vz) { _ekfResetHandlerVelocityZ(_sub_vehicle_local_position.get().delta_vz); _reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter; @@ -138,7 +143,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // distance to bottom if (_sub_vehicle_local_position.get().dist_bottom_valid && PX4_ISFINITE(_sub_vehicle_local_position.get().dist_bottom)) { - _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; + _dist_to_bottom = _sub_vehicle_local_position.get().dist_bottom; } // global frame reference coordinates to enable conversions diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 3b9ddbd46660..6ee861f3cda1 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include @@ -62,6 +61,7 @@ struct ekf_reset_counters_s { uint8_t z; uint8_t vz; uint8_t heading; + uint8_t hagl; }; class FlightTask : public ModuleParams @@ -132,12 +132,6 @@ class FlightTask : public ModuleParams */ const landing_gear_s &getGear() { return _gear; } - /** - * Get avoidance desired waypoint - * @return desired waypoints - */ - const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; } - /** * All setpoints are set to NAN (uncontrolled), timestamp to zero */ @@ -198,6 +192,7 @@ class FlightTask : public ModuleParams virtual void _ekfResetHandlerPositionXY(const matrix::Vector2f &delta_xy) {}; virtual void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) {}; virtual void _ekfResetHandlerPositionZ(float delta_z) {}; + virtual void _ekfResetHandlerHagl(float delta_hagl) {}; virtual void _ekfResetHandlerVelocityZ(float delta_vz) {}; virtual void _ekfResetHandlerHeading(float delta_psi) {}; @@ -251,12 +246,6 @@ class FlightTask : public ModuleParams landing_gear_s _gear{}; - /** - * Desired waypoints. - * Goals set by the FCU to be sent to the obstacle avoidance system. - */ - vehicle_trajectory_waypoint_s _desired_waypoint{}; - DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams, (ParamFloat) _param_mpc_xy_vel_max, (ParamFloat) _param_mpc_z_vel_max_dn, diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index f639e492493d..144c6c7ba158 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -90,7 +90,14 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command, b // commanded heading behaviour if (PX4_ISFINITE(command.param3)) { - _yaw_behaviour = command.param3; + if (static_cast(command.param3 + .5f) == vehicle_command_s::ORBIT_YAW_BEHAVIOUR_UNCHANGED) { + if (!_currently_orbiting) { // only change the yaw behaviour if we are not actively orbiting + _yaw_behaviour = _param_mc_orbit_yaw_mod.get(); + } + + } else { + _yaw_behaviour = command.param3; + } } // save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING @@ -133,9 +140,11 @@ bool FlightTaskOrbit::sendTelemetry() orbit_status.yaw_behaviour = _yaw_behaviour; if (_geo_projection.isInitialized()) { + // While chainging altitude by stick _position_setpoint(2) is not set (NAN) + float local_altitude = PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2); // local -> global _geo_projection.reproject(_center(0), _center(1), orbit_status.x, orbit_status.y); - orbit_status.z = _global_local_alt0 - _position_setpoint(2); + orbit_status.z = _global_local_alt0 - local_altitude; } else { return false; // don't send the message if the transformation failed @@ -165,6 +174,7 @@ void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) { bool ret = FlightTaskManualAltitude::activate(last_setpoint); + _currently_orbiting = false; _orbit_radius = _radius_min; _orbit_velocity = 1.f; _center = _position; @@ -199,6 +209,7 @@ bool FlightTaskOrbit::activate(const trajectory_setpoint_s &last_setpoint) bool FlightTaskOrbit::update() { bool ret = true; + _currently_orbiting = true; _updateTrajectoryBoundaries(); _adjustParametersByStick(); diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 187a47de4119..2c55bbc15ba7 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -124,6 +124,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER; bool _started_clockwise{true}; + bool _currently_orbiting{false}; float _initial_heading = 0.f; /**< the heading of the drone when the orbit command was issued */ SlewRateYaw _slew_rate_yaw; @@ -132,6 +133,7 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel DEFINE_PARAMETERS( (ParamFloat) _param_mc_orbit_rad_max, + (ParamInt) _param_mc_orbit_yaw_mod, (ParamFloat) _param_mpc_xy_cruise, /**< cruise speed for circle approach */ (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mpc_xy_traj_p, diff --git a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c index 3e655c80e2ed..c8f66eb9f15d 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c +++ b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c @@ -39,6 +39,18 @@ * @max 10000.0 * @increment 0.5 * @decimal 1 - * @group FlightTaskOrbit + * @group Flight Task Orbit */ PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f); + +/** + * Yaw behaviour during orbit flight. + * + * @value 0 Front to Circle Center + * @value 1 Hold Initial Heading + * @value 2 Uncontrolled + * @value 3 Hold Front Tangent to Circle + * @value 4 RC Controlled + * @group Flight Task Orbit + */ +PARAM_DEFINE_INT32(MC_ORBIT_YAW_MOD, 0); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 7de353c8f977..988853fdeaaf 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -92,21 +92,20 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) // Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { - if (!_vcontrol_mode.flag_control_climb_rate_enabled & _vcontrol_mode.flag_control_attitude_enabled) { + if (!_vcontrol_mode.flag_control_climb_rate_enabled && _vcontrol_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); + const float roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get()); - _att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) - + radians(_param_fw_psp_off.get()); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, - -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); + float pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get()) + + radians(_param_fw_psp_off.get()); + pitch_body = constrain(pitch_body, + -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.reset_integral = false; @@ -325,16 +324,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { - _roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + const Eulerf setpoint(Quatf(_att_sp.q_d)); + const float roll_body = setpoint.phi(); + const float pitch_body = setpoint.theta(); + + if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + + _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - _wheel_ctrl.control_attitude(_att_sp.yaw_body, euler_angles.psi()); + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); 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 7f6e5b9c10c8..5e63924d3b57 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 @@ -620,22 +620,52 @@ void FwAutotuneAttitudeControl::saveGainsToParams() const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() { - if (_steps_counter > _max_steps) { - _signal_sign = (_signal_sign == 1) ? 0 : 1; - _steps_counter = 0; - if (_max_steps > 1) { - _max_steps--; - } else { - _max_steps = 5; + const hrt_abstime now = hrt_absolute_time(); + const float t = static_cast(now - _state_start_time) * 1e-6f; + float signal = 0.0f; + + switch (_param_fw_sysid_signal_type.get()) { + case static_cast(SignalType::kStep): { + if (_steps_counter > _max_steps) { + _signal_sign = (_signal_sign == 1) ? 0 : 1; + _steps_counter = 0; + + if (_max_steps > 1) { + _max_steps--; + + } else { + _max_steps = 5; + } + } + + _steps_counter++; + signal = float(_signal_sign); } - } + break; + + case static_cast(SignalType::kLinearSineSweep): { - _steps_counter++; + signal = signal_generator::getLinearSineSweep(_param_fw_at_sysid_f0.get(), + _param_fw_at_sysid_f1.get(), + _param_fw_sysid_time.get(), t); + } + break; + + case static_cast(SignalType::kLogSineSweep): { + signal = signal_generator::getLogSineSweep(_param_fw_at_sysid_f0.get(), _param_fw_at_sysid_f1.get(), + _param_fw_sysid_time.get(), t); + } + break; + + default: + signal = 0.f; + break; + } - const float signal = float(_signal_sign) * _param_fw_at_sysid_amp.get(); + signal *= _param_fw_at_sysid_amp.get(); Vector3f rate_sp{}; float signal_scaled = 0.f; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp index 9be5f5559664..4350724eed43 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include #include @@ -64,6 +65,12 @@ using namespace time_literals; +enum class SignalType : uint8_t { + kStep = 0, + kLinearSineSweep, + kLogSineSweep +}; + class FwAutotuneAttitudeControl : public ModuleBase, public ModuleParams, public px4::WorkItem { @@ -204,7 +211,12 @@ class FwAutotuneAttitudeControl : public ModuleBase, (ParamFloat) _param_fw_yr_p, (ParamFloat) _param_fw_yr_i, (ParamFloat) _param_fw_yr_ff, - (ParamFloat) _param_fw_y_rmax + (ParamFloat) _param_fw_y_rmax, + + (ParamFloat) _param_fw_at_sysid_f0, + (ParamFloat) _param_fw_at_sysid_f1, + (ParamFloat) _param_fw_sysid_time, + (ParamInt) _param_fw_sysid_signal_type ) static constexpr float _publishing_dt_s = 100e-3f; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c index 13609dbbb187..eb10036631b6 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c +++ b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control_params.c @@ -121,3 +121,54 @@ PARAM_DEFINE_INT32(FW_AT_AXES, 3); * @group Autotune */ PARAM_DEFINE_INT32(FW_AT_MAN_AUX, 0); + +/** + * Start frequency of the injected signal + * + * Can be set lower or higher than the end frequency + * + * @min 0.1 + * @max 30.0 + * @decimal 1 + * @unit Hz + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_F0, 1.f); + +/** + * End frequency of the injected signal + * + * Can be set lower or higher than the start frequency + * + * @min 0.1 + * @max 30.0 + * @decimal 1 + * @unit Hz + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_F1, 20.f); + +/** + * Maneuver time for each axis + * + * Duration of the input signal sent on each axis during system identification + * + * @min 5 + * @max 120 + * @decimal 0 + * @unit s + * @group Autotune + */ +PARAM_DEFINE_FLOAT(FW_AT_SYSID_TIME, 10.f); + +/** + * Input signal type + * + * Type of signal used during system identification to excite the system. + * + * @value 0 Step + * @value 1 Linear sine sweep + * @value 2 Logarithmic sine sweep + * @group Autotune + */ +PARAM_DEFINE_INT32(FW_AT_SYSID_TYPE, 0); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 4bb1d5ee3188..7858fe2c1c5f 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -129,6 +129,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); + _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); @@ -137,6 +138,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); + _tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get()); _tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); @@ -210,8 +212,7 @@ FixedwingPositionControl::airspeed_poll() _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) - && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s) - && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) { + && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) { airspeed_valid = true; @@ -449,9 +450,6 @@ FixedwingPositionControl::status_publish() { position_controller_status_s pos_ctrl_status = {}; - pos_ctrl_status.nav_roll = _att_sp.roll_body; - pos_ctrl_status.nav_pitch = _att_sp.pitch_body; - npfg_status_s npfg_status = {}; npfg_status.wind_est_valid = _wind_valid; @@ -789,8 +787,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) /* reset setpoints from other modes (auto) otherwise we won't * level out without new manual input */ - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const Eulerf current_setpoint(Quatf(_att_sp.q_d)); + const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body)); + setpoint.copyTo(_att_sp.q_d); } _control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION; @@ -875,11 +876,16 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto } switch (position_sp_type) { - case position_setpoint_s::SETPOINT_TYPE_IDLE: - _att_sp.thrust_body[0] = 0.0f; - _att_sp.roll_body = 0.0f; - _att_sp.pitch_body = radians(_param_fw_psp_off.get()); - break; + case position_setpoint_s::SETPOINT_TYPE_IDLE: { + _att_sp.thrust_body[0] = 0.0f; + const float roll_body = 0.0f; + const float pitch_body = radians(_param_fw_psp_off.get()); + const float yaw_body = 0.0f; + + const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + setpoint.copyTo(_att_sp.q_d); + break; + } case position_setpoint_s::SETPOINT_TYPE_POSITION: control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); @@ -925,9 +931,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); } - /* Copy thrust and pitch values from tecs */ - _att_sp.pitch_body = get_tecs_pitch(); - if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(current_sp); } @@ -948,8 +951,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _param_sinkrate_target.get(), _param_climbrate_target.get()); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; if (_landed) { _att_sp.thrust_body[0] = _param_fw_thr_min.get(); @@ -958,7 +961,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i _att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get()); } - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } @@ -968,6 +973,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) // Hard-code descend rate to 0.5m/s. This is a compromise to give the system to recover, // but not letting it drift too far away. const float descend_rate = -0.5f; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, _current_altitude, @@ -978,14 +984,16 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _param_fw_thr_max.get(), _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, descend_rate); - _att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle - _att_sp.yaw_body = 0.f; + const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle + const float yaw_body = 0.f; _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } uint8_t @@ -1107,10 +1115,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1121,6 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + const float pitch_body = get_tecs_pitch(); + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1156,10 +1167,11 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; + const bool disable_underspeed_handling = false; tecs_update_pitch_throttle(control_interval, position_sp_alt, @@ -1170,8 +1182,12 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, pos_sp_curr.vz); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1251,10 +1267,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw float alt_sp = pos_sp_curr.alt; @@ -1265,7 +1281,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons } else { // continue straight until vehicle has sufficient altitude - _att_sp.roll_body = 0.0f; + roll_body = 0.0f; } _tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get()); @@ -1280,6 +1296,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } #ifdef CONFIG_FIGURE_OF_EIGHT @@ -1304,7 +1325,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c // Apply control _figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed); - _att_sp.roll_body = _figure_eight.getRollSetpoint(); + float roll_body = _figure_eight.getRollSetpoint(); target_airspeed = _figure_eight.getAirspeedSetpoint(); _target_bearing = _figure_eight.getTargetBearing(); _closest_point_on_path = _figure_eight.getClosestPoint(); @@ -1335,7 +1356,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _param_climbrate_target.get()); // Yaw - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp) @@ -1390,10 +1415,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const 0.0f; navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -1404,6 +1429,12 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const tecs_fw_thr_max, _param_sinkrate_target.get(), _param_climbrate_target.get()); + + _att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -1489,7 +1520,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); + float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint()); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1497,7 +1528,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const float bearing = _npfg.getBearing(); // heading hold mode will override this bearing setpoint - _att_sp.yaw_body = _runway_takeoff.getYaw(bearing); + float yaw_body = _runway_takeoff.getYaw(bearing); // update tecs const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get())); @@ -1512,6 +1543,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _tecs.resetIntegrals(); } + const bool disable_underspeed_handling = true; + tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, target_airspeed, @@ -1520,13 +1553,19 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), _param_fw_thr_max.get(), _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + disable_underspeed_handling); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); + const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch()); _att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_to_scl.get(); // retract ladning gear once passed the climbout state @@ -1551,8 +1590,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launchDetector.forceSetFlyState(); } - if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH - && _param_fw_laun_detcn_on.get()) { + if (!_launch_detected && _launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { _launch_detected = true; _launch_global_position = global_position; _takeoff_ground_alt = _current_altitude; @@ -1577,8 +1615,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo } /* Set control values depending on the detection state */ - if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH - && _param_fw_laun_detcn_on.get()) { + if (_launchDetector.getLaunchDetected() > launch_detection_status_s::STATE_WAITING_FOR_LAUNCH) { /* Launch has been detected, hence we have to control the plane. */ float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, @@ -1587,12 +1624,13 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint_amsl, @@ -1602,27 +1640,38 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_min.get(), max_takeoff_throttle, _param_sinkrate_target.get(), - _performance_model.getMaximumClimbRate(_air_density)); + _performance_model.getMaximumClimbRate(_air_density), + disable_underspeed_handling); if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) { // explicitly set idle throttle until motors are enabled _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); } else { - _att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust(); + _att_sp.thrust_body[0] = get_tecs_thrust(); } - _att_sp.pitch_body = get_tecs_pitch(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float pitch_body = get_tecs_pitch(); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } else { /* Tell the attitude controller to stop integrating while we are waiting for the launch */ _att_sp.reset_integral = true; /* Set default roll and pitch setpoints during detection phase */ - _att_sp.roll_body = 0.0f; + float roll_body = 0.0f; + float yaw_body = _yaw; _att_sp.thrust_body[0] = _param_fw_thr_idle.get(); - _att_sp.pitch_body = radians(_takeoff_pitch_min.get()); + float pitch_body = radians(_takeoff_pitch_min.get()); + roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt); + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + } launch_detection_status_s launch_detection_status; @@ -1631,7 +1680,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_detection_status_pub.publish(launch_detection_status); } - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt); + _flaps_setpoint = _param_fw_flaps_to_scl.get(); if (!_vehicle_status.in_transition_to_fw) { publishLocalPositionSetpoint(pos_sp_curr); @@ -1736,10 +1785,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); // use npfg's bearing to commanded course, controlled via yaw angle while on runway - _att_sp.yaw_body = _npfg.getBearing(); + float yaw_body = _npfg.getBearing(); /* longitudinal guidance */ @@ -1769,6 +1818,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, altitude_setpoint, @@ -1779,13 +1829,19 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -1815,7 +1871,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -1837,10 +1893,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + float pitch_body = get_tecs_pitch(); + + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + float yaw_body = _yaw; + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -1850,8 +1911,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); - _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -1909,6 +1968,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, } // the terrain estimate (if enabled) is always used to determine the flaring altitude + float roll_body; + float yaw_body; + float pitch_body; + if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) { // flare and land with minimal speed @@ -1941,9 +2004,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw /* longitudinal guidance */ @@ -1972,6 +2035,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float throttle_max = flare_ramp_interpolator_sqrt * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator_sqrt) * _param_fw_thr_max.get(); + const bool disable_underspeed_handling = true; tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate @@ -1982,13 +2046,13 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - true, + disable_underspeed_handling, height_rate_setpoint); /* set the attitude and throttle commands */ // TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = true; @@ -2018,7 +2082,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, pos_sp_curr.loiter_direction_counter_clockwise, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + roll_body = getCorrectedNpfgRollSetpoint(); /* longitudinal guidance */ @@ -2028,6 +2092,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f); const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()), _param_fw_t_sink_max.get()); + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // is not controlled, control descend rate target_airspeed, @@ -2037,15 +2103,15 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(), desired_max_sinkrate, _param_climbrate_target.get(), - false, + disable_underspeed_handling, -glide_slope_sink_rate); // heightrate = -sinkrate /* set the attitude and throttle commands */ - _att_sp.pitch_body = get_tecs_pitch(); + pitch_body = get_tecs_pitch(); // yaw is not controlled in nominal flight - _att_sp.yaw_body = _yaw; + yaw_body = _yaw; // enable direct yaw control using rudder/wheel _att_sp.fw_control_yaw_wheel = false; @@ -2055,7 +2121,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation - _att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt); + roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); + _flaps_setpoint = _param_fw_flaps_lnd_scl.get(); _spoilers_setpoint = _param_fw_spoilers_lnd.get(); @@ -2090,6 +2160,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max = 0.0f; } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, calibrated_airspeed_sp, @@ -2099,14 +2171,17 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void @@ -2131,6 +2206,15 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max = 0.0f; } + if (_local_pos.xy_reset_counter != _xy_reset_counter) { + _time_last_xy_reset = _local_pos.timestamp; + } + + Eulerf current_setpoint(Quatf(_att_sp.q_d)); + float yaw_body = current_setpoint.psi(); + float roll_body = current_setpoint.phi(); + float pitch_body = current_setpoint.theta(); + /* heading control */ // TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here) if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH && @@ -2152,28 +2236,34 @@ FixedwingPositionControl::control_manual_position(const float control_interval, if (_yaw_lock_engaged) { - /* just switched back from non heading-hold to heading hold */ + Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; + if (!_hdg_hold_enabled) { + // just switched back from non heading-hold to heading hold _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - _hdg_hold_position.lat = _current_latitude; - _hdg_hold_position.lon = _current_longitude; + _hdg_hold_position = curr_pos_local; } - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); + // if there's a reset-by-fusion, the ekf needs some time to converge, + // therefore we go into track holiding for 2 seconds + if (_local_pos.timestamp - _time_last_xy_reset < 2_s) { + _hdg_hold_position = curr_pos_local; + } _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas); - navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); + roll_body = getCorrectedNpfgRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } } + const bool disable_underspeed_handling = false; + tecs_update_pitch_throttle(control_interval, _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, @@ -2183,7 +2273,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, throttle_max, _param_sinkrate_target.get(), _param_climbrate_target.get(), - false, + disable_underspeed_handling, height_rate_sp); if (!_yaw_lock_engaged || fabsf(_manual_control_setpoint.roll) >= HDG_HOLD_MAN_INPUT_THRESH || @@ -2192,12 +2282,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = false; _yaw_lock_engaged = false; - _att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get()); + yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw } _att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max); - _att_sp.pitch_body = get_tecs_pitch(); + + pitch_body = get_tecs_pitch(); + + Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed, @@ -2220,10 +2314,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); - _att_sp.roll_body = getCorrectedNpfgRollSetpoint(); + float roll_body = getCorrectedNpfgRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; - _att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw + float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw tecs_update_pitch_throttle(control_interval, pos_sp_curr.alt, @@ -2236,7 +2330,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv _param_climbrate_target.get()); _att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get()); - _att_sp.pitch_body = get_tecs_pitch(); + const float pitch_body = get_tecs_pitch(); + + const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body)); + attitude_setpoint.copyTo(_att_sp.q_d); } float FixedwingPositionControl::get_tecs_pitch() @@ -2303,32 +2400,28 @@ FixedwingPositionControl::Run() _reference_altitude = 0.f; } - _current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters + _current_altitude = -_local_pos.z + _reference_altitude; // Altitude AMSL in meters // handle estimator reset events. we only adjust setpoins for manual modes if (_control_mode.flag_control_manual_enabled) { - if (_control_mode.flag_control_altitude_enabled && _local_pos.vz_reset_counter != _alt_reset_counter) { + if (_control_mode.flag_control_altitude_enabled && _local_pos.z_reset_counter != _z_reset_counter) { // make TECS accept step in altitude and demanded altitude _tecs.handle_alt_step(_current_altitude, -_local_pos.vz); } // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled - && _local_pos.vxy_reset_counter != _pos_reset_counter) { + && _local_pos.xy_reset_counter != _xy_reset_counter) { // reset heading hold flag, which will re-initialise position control _hdg_hold_enabled = false; } } - // update the reset counters in any case - _alt_reset_counter = _local_pos.vz_reset_counter; - _pos_reset_counter = _local_pos.vxy_reset_counter; - // Convert Local setpoints to global setpoints if (!_global_local_proj_ref.isInitialized() || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) - || (_local_pos.vxy_reset_counter != _pos_reset_counter)) { + || (_local_pos.xy_reset_counter != _xy_reset_counter)) { double reference_latitude = 0.; double reference_longitude = 0.; @@ -2550,12 +2643,16 @@ FixedwingPositionControl::Run() if (_control_mode_current != FW_POSCTRL_MODE_OTHER) { + Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); + float roll_body = attitude_setpoint.phi(); + float pitch_body = attitude_setpoint.theta(); + float yaw_body = attitude_setpoint.psi(); if (_control_mode.flag_control_manual_enabled) { - _att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()), - radians(_param_fw_r_lim.get())); - _att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()), - radians(_param_fw_p_lim_max.get())); + roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()), + radians(_param_fw_r_lim.get())); + pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get())); } if (_control_mode.flag_control_position_enabled || @@ -2565,9 +2662,9 @@ FixedwingPositionControl::Run() _control_mode.flag_control_climb_rate_enabled) { // roll slew rate - _att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval); + roll_body = _roll_slew_rate.update(roll_body, control_interval); - const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); @@ -2613,6 +2710,9 @@ FixedwingPositionControl::Run() _spoilers_setpoint_pub.publish(spoilers_setpoint); } + _z_reset_counter = _local_pos.z_reset_counter; + _xy_reset_counter = _local_pos.xy_reset_counter; + perf_end(_loop_perf); } } @@ -3153,8 +3253,10 @@ float FixedwingPositionControl::getLoadFactor() { float load_factor_from_bank_angle = 1.0f; - if (PX4_ISFINITE(_att_sp.roll_body)) { - load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON); + float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi(); + + if (PX4_ISFINITE(roll_body)) { + load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON); } return load_factor_from_bank_angle; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 5089021685da..84300ae1b409 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -298,7 +298,7 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_t_hrate_ff, (ParamFloat) _param_fw_t_h_error_tc, + (ParamFloat) _param_fw_t_fast_alt_err, (ParamFloat) _param_fw_t_thr_integ, (ParamFloat) _param_fw_t_I_gain_pit, (ParamFloat) _param_fw_t_ptch_damp, @@ -989,7 +987,6 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_flaps_lnd_scl, (ParamFloat) _param_fw_flaps_to_scl, (ParamFloat) _param_fw_spoilers_lnd, - (ParamFloat) _param_fw_spoilers_desc, (ParamInt) _param_fw_pos_stk_conf, diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index a636d8d75768..ac2ab276af41 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -35,6 +35,7 @@ * Path navigation roll slew rate limit. * * The maximum change in roll angle setpoint per second. + * This limit is applied in all Auto modes, plus manual Position and Altitude modes. * * @unit deg/s * @min 0 @@ -192,9 +193,9 @@ PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f); PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); /** - * Minimum pitch angle + * Minimum pitch angle setpoint * - * The minimum pitch angle setpoint for a height-rate or altitude controlled mode. + * Applies in any altitude controlled flight mode. * * @unit deg * @min -60.0 @@ -206,9 +207,9 @@ PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f); PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f); /** - * Maximum pitch angle + * Maximum pitch angle setpoint * - * The maximum pitch angle setpoint setpoint for a height-rate or altitude controlled mode. + * Applies in any altitude controlled flight mode. * * @unit deg * @min 0.0 @@ -220,9 +221,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f); PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f); /** - * Maximum roll angle + * Maximum roll angle setpoint * - * The maximum roll angle setpoint for setpoint for a height-rate or altitude controlled mode. + * Applies in any altitude controlled flight mode. * * @unit deg * @min 35.0 @@ -236,7 +237,7 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max * - * Maximum throttle limit in altitude controlled modes. + * Applies in any altitude controlled flight mode. * Should be set accordingly to achieve FW_T_CLMB_MAX. * * @unit norm @@ -251,13 +252,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * Minimum throttle limit in altitude controlled modes. + * Applies in any altitude controlled flight mode. * Usually set to 0 but can be increased to prevent the motor from stopping when * descending, which can increase achievable descent rates. * - * For aircraft with internal combustion engine this parameter should be set - * for desired idle rpm. - * * @unit norm * @min 0.0 * @max 1.0 @@ -270,13 +268,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Idle throttle * - * This is the minimum throttle while on the ground - * - * For aircraft with internal combustion engines, this parameter should be set - * above the desired idle rpm. For electric motors, idle should typically be set - * to zero. - * - * Note that in automatic modes, "landed" conditions will engage idle throttle. + * This is the minimum throttle while on the ground ("landed") in auto modes. * * @unit norm * @min 0.0 @@ -317,9 +309,9 @@ PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f); /** * Takeoff Airspeed * - * The calibrated airspeed setpoint TECS will stabilize to during the takeoff climbout. + * The calibrated airspeed setpoint during the takeoff climbout. * - * If set <= 0.0, FW_AIRSPD_MIN will be set by default. + * If set <= 0, FW_AIRSPD_MIN will be set by default. * * @unit m/s * @min -1.0 @@ -343,7 +335,9 @@ PARAM_DEFINE_FLOAT(FW_TKO_AIRSPD, -1.0f); PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f); /** - * Use terrain estimation during landing. This is critical for detecting when to flare, and should be enabled if possible. + * Use terrain estimation during landing. + * + * This is critical for detecting when to flare, and should be enabled if possible. * * NOTE: terrain estimate is currently solely derived from a distance sensor. * @@ -364,9 +358,9 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 1); /** * Early landing configuration deployment * - * When disabled, the landing configuration (flaps, landing airspeed, etc.) is only activated - * on the final approach to landing. When enabled, it is already activated when entering the - * final loiter-down (loiter-to-alt) waypoint before the landing approach. + * Allows to deploy the landing configuration (flaps, landing airspeed, etc.) already in + * the loiter-down waypoint before the final approach. + * Otherwise is enabled only in the final approach. * * @boolean * @@ -377,8 +371,7 @@ PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0); /** * Flare, minimum pitch * - * Minimum pitch during flare, a positive sign means nose up - * Applied once flaring is triggered + * Minimum pitch during landing flare. * * @unit deg * @min -5 @@ -392,8 +385,7 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f); /** * Flare, maximum pitch * - * Maximum pitch during flare, a positive sign means nose up - * Applied once flaring is triggered + * Maximum pitch during landing flare. * * @unit deg * @min 0 @@ -409,7 +401,7 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f); * * The calibrated airspeed setpoint during landing. * - * If set <= 0.0, landing airspeed = FW_AIRSPD_MIN by default. + * If set <= 0, landing airspeed = FW_AIRSPD_MIN by default. * * @unit m/s * @min -1.0 @@ -422,9 +414,8 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f); /** * Altitude time constant factor for landing * - * Set this parameter to less than 1.0 to make TECS react faster to altitude errors during - * landing than during normal flight. During landing, the TECS - * altitude time constant (FW_T_ALT_TC) is multiplied by this value. + * During landing, the TECS altitude time constant (FW_T_ALT_TC) + * is multiplied by this value. * * @unit * @min 0.2 @@ -444,10 +435,6 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); * Maximum descent rate * * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding - * the aircraft. * * @unit m/s * @min 1.0 @@ -462,7 +449,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); * Throttle damping factor * * This is the damping gain for the throttle demand loop. - * Increase to add damping to correct for oscillations in speed and height. * * @min 0.0 * @max 1.0 @@ -475,7 +461,6 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMPING, 0.05f); /** * Integrator gain throttle * - * Integrator gain on the throttle part of the control loop. * Increase it to trim out speed and height offsets faster, * with the downside of possible overshoots and oscillations. * @@ -490,7 +475,6 @@ PARAM_DEFINE_FLOAT(FW_T_THR_INTEG, 0.02f); /** * Integrator gain pitch * - * Integrator gain on the pitch part of the control loop. * Increase it to trim out speed and height offsets faster, * with the downside of possible overshoots and oscillations. * @@ -505,11 +489,9 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); /** * Maximum vertical acceleration * - * This is the maximum vertical acceleration (in m/s/s) + * This is the maximum vertical acceleration * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover - * from under-speed conditions. + * or height errors. * * @unit m/s^2 * @min 1.0 @@ -521,9 +503,9 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f); PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** - * Airspeed measurement standard deviation for airspeed filter. + * Airspeed measurement standard deviation * - * This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS. + * For the airspeed filter in TECS. * * @unit m/s * @min 0.01 @@ -532,12 +514,12 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); * @increment 0.1 * @group FW TECS */ -PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f); +PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.07f); /** - * Airspeed rate measurement standard deviation for airspeed filter. + * Airspeed rate measurement standard deviation * - * This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS. + * For the airspeed filter in TECS. * * @unit m/s^2 * @min 0.01 @@ -549,12 +531,10 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f); PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.2f); /** - * Process noise standard deviation for the airspeed rate in the airspeed filter. + * Process noise standard deviation for the airspeed rate * - * This is the process noise standard deviation in the airspeed filter filter defining the noise in the - * airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and - * the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the - * drawback for delays. + * This is defining the noise in the airspeed rate for the constant airspeed rate model + * of the TECS airspeed filter. * * @unit m/s^2 * @min 0.01 @@ -569,14 +549,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas - * inefficient low aspect-ratio models (eg delta wings) can use a higher value. + * Is used to compensate for the additional drag created by turning. + * Increase this gain if the aircraft initially loses energy in turns + * and reduce if the aircraft initially gains energy in turns. * * @min 0.0 * @max 20.0 @@ -589,13 +564,11 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control + * Adjusts the amount of weighting that the pitch control * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. + * pitch control to control height and ignore speed errors. * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch + * and ignore height errors. The default value of 1.0 allows the pitch * control to simultaneously control height and speed. * Set to 2 for gliders. * @@ -608,12 +581,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 15.0f); PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** - * Pitch damping factor - * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned - * properly. + * Pitch damping gain * * @min 0.0 * @max 2.0 @@ -633,6 +601,18 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f); */ PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); +/** + * Fast descend: minimum altitude error + * + * Minimum altitude error needed to descend with max airspeed and minimal throttle. + * A negative value disables fast descend. + * + * @min -1.0 + * @decimal 0 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f); + /** * Height rate feed forward * @@ -670,9 +650,9 @@ PARAM_DEFINE_FLOAT(FW_T_TAS_TC, 5.0f); PARAM_DEFINE_FLOAT(FW_GND_SPD_MIN, 5.0f); /** - * RC stick configuration fixed-wing. + * Custom stick configuration * - * Set RC/joystick configuration for fixed-wing manual position and altitude controlled flight. + * Applies in manual Position and Altitude flight modes. * * @min 0 * @max 3 @@ -710,9 +690,8 @@ PARAM_DEFINE_FLOAT(FW_T_SEB_R_FF, 1.0f); /** * Default target climbrate. * - * The default rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints. - * In manual modes this defines the maximum rate at which the altitude setpoint can be increased. - * + * In auto modes: default climb rate output by controller to achieve altitude setpoints. + * In manual modes: maximum climb rate setpoint. * * @unit m/s * @min 0.5 @@ -726,9 +705,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); /** * Default target sinkrate. * - * - * The default rate at which the vehicle will sink in autonomous modes to achieve altitude setpoints. - * In manual modes this defines the maximum rate at which the altitude setpoint can be decreased. + * In auto modes: default sink rate output by controller to achieve altitude setpoints. + * In manual modes: maximum sink rate setpoint. * * @unit m/s * @min 0.5 @@ -742,7 +720,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); /** * GPS failure loiter time * - * The time in seconds the system should do open loop loiter and wait for GPS recovery + * The time the system should do open loop loiter and wait for GPS recovery * before it starts descending. Set to 0 to disable. Roll angle is set to FW_GPSF_R. * Does only apply for fixed-wing vehicles or VTOLs with NAV_FORCE_VT set to 0. * @@ -756,7 +734,7 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30); /** * GPS failure fixed roll angle * - * Roll in degrees during the loiter after the vehicle has lost GPS in an auto mode (e.g. mission or loiter). + * Roll angle in GPS failure loiter mode. * * @unit deg * @min 0.0 @@ -865,7 +843,7 @@ PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0); * Approach path nudging: shifts the touchdown point laterally along with the entire approach path * * This is useful for manually adjusting the landing point in real time when map or GNSS errors cause an offset from the - * desired landing vector. Nuding is done with yaw stick, constrained to FW_LND_TD_OFF (in meters) and the direction is + * desired landing vector. Nudging is done with yaw stick, constrained to FW_LND_TD_OFF (in meters) and the direction is * relative to the vehicle heading (stick deflection to the right = land point moves to the right as seen by the vehicle). * * @min 0 @@ -904,7 +882,6 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); * Multiplying this factor with the current absolute wind estimate gives the airspeed offset * added to the minimum airspeed setpoint limit. This helps to make the * system more robust against disturbances (turbulence) in high wind. - * Only applies to AUTO flight mode. * * @min 0 * @decimal 2 @@ -965,15 +942,3 @@ PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); - -/** - * Spoiler descend setting - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group FW Attitude Control - */ -PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); diff --git a/src/modules/gimbal/CMakeLists.txt b/src/modules/gimbal/CMakeLists.txt index a99123b08903..859b519718fa 100644 --- a/src/modules/gimbal/CMakeLists.txt +++ b/src/modules/gimbal/CMakeLists.txt @@ -46,4 +46,3 @@ px4_add_module( DEPENDS geo ) - diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 5893d29738f2..804f792fc65e 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -207,16 +207,6 @@ static int gimbal_thread_main(int argc, char *argv[]) update_params(param_handles, params); } - if (thread_data.last_input_active == -1) { - // Reset control as no one is active anymore, or yet. - thread_data.control_data.sysid_primary_control = 0; - thread_data.control_data.compid_primary_control = 0; - // If the output is set to AUX we still want the input to be able to control the gimbal - // via mavlink, so we set the device_compid to 1. This follows the mavlink spec which states - // that the gimbal_device_id should be between 1 and 6. - thread_data.control_data.device_compid = params.mnt_mode_out == 0 ? 1 : 0; - } - InputBase::UpdateResult update_result = InputBase::UpdateResult::NoUpdate; if (thread_data.input_objs_len > 0) { diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp index ac7cbc4e7fd3..ebfde30d2a73 100644 --- a/src/modules/gimbal/input_mavlink.cpp +++ b/src/modules/gimbal/input_mavlink.cpp @@ -518,7 +518,7 @@ InputMavlinkGimbalV2::update(unsigned int timeout_ms, ControlData &control_data, // We can't return early instead because we need to copy all topics that triggered poll. bool exit_loop = false; - UpdateResult update_result = already_active ? UpdateResult::UpdatedActive : UpdateResult::NoUpdate; + UpdateResult update_result = UpdateResult::NoUpdate; while (!exit_loop && poll_timeout >= 0) { @@ -856,7 +856,9 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ control_data.compid_primary_control = new_compid_primary_control; } - return UpdateResult::UpdatedActive; + // Just doing the configuration doesn't mean there is actually an update to use yet. + // After that we still need to have an actual setpoint. + return UpdateResult::NoUpdate; // TODO: support secondary control // TODO: support gimbal device id for multiple gimbals @@ -880,7 +882,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_ _ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); - return UpdateResult::UpdatedActive; + return UpdateResult::UpdatedActiveOnce; } else { PX4_INFO("GIMBAL_MANAGER_PITCHYAW from %d/%d denied, in control: %d/%d", diff --git a/src/modules/gimbal/input_rc.cpp b/src/modules/gimbal/input_rc.cpp index c512cda254ee..c15f916ab9a4 100644 --- a/src/modules/gimbal/input_rc.cpp +++ b/src/modules/gimbal/input_rc.cpp @@ -120,9 +120,12 @@ InputRC::UpdateResult InputRC::_read_control_data_from_subscription(ControlData return false; }(); - if (already_active || major_movement) { + if (major_movement) { control_data.sysid_primary_control = _parameters.mav_sysid; control_data.compid_primary_control = _parameters.mav_compid; + } + + if (already_active || major_movement) { if (_parameters.mnt_rc_in_mode == 0) { // We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees. diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index c43c3dfaa8d5..61486f3f7722 100644 --- a/src/modules/gimbal/output.cpp +++ b/src/modules/gimbal/output.cpp @@ -260,4 +260,3 @@ void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool y } } /* namespace gimbal */ - diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index a5878b3801aa..4b48349a4dfb 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -61,7 +61,8 @@ void OutputRC::update(const ControlData &control_data, bool new_setpoints, uint8 _stream_device_attitude_status(); - // If the output is RC, then it means we are also the gimbal device. gimbal_device_id = (uint8_t)_parameters.mnt_mav_compid_v1; + // If the output is RC, then we signal this by referring to compid 1. + gimbal_device_id = 1; // _angle_outputs are in radians, gimbal_controls are in [-1, 1] gimbal_controls_s gimbal_controls{}; diff --git a/src/modules/gyro_fft/CMSIS_5/CMSIS/Core/Include/cmsis_compiler.h b/src/modules/gyro_fft/CMSIS_5/CMSIS/Core/Include/cmsis_compiler.h index adbf296f15a4..ff806745a4c6 100644 --- a/src/modules/gyro_fft/CMSIS_5/CMSIS/Core/Include/cmsis_compiler.h +++ b/src/modules/gyro_fft/CMSIS_5/CMSIS/Core/Include/cmsis_compiler.h @@ -280,4 +280,3 @@ #endif /* __CMSIS_COMPILER_H */ - diff --git a/src/modules/gyro_fft/CMSIS_5/CMSIS/DSP/Include/arm_common_tables.h b/src/modules/gyro_fft/CMSIS_5/CMSIS/DSP/Include/arm_common_tables.h index 721b18dd2d43..6d6d432042c1 100644 --- a/src/modules/gyro_fft/CMSIS_5/CMSIS/DSP/Include/arm_common_tables.h +++ b/src/modules/gyro_fft/CMSIS_5/CMSIS/DSP/Include/arm_common_tables.h @@ -514,4 +514,3 @@ extern const unsigned char hwLUT[256]; #endif /* (defined(ARM_MATH_MVEI) || defined(ARM_MATH_HELIUM)) */ #endif /* ARM_COMMON_TABLES_H */ - diff --git a/src/modules/land_detector/CMakeLists.txt b/src/modules/land_detector/CMakeLists.txt index 632c90d5cf8d..6823df9f8b0f 100644 --- a/src/modules/land_detector/CMakeLists.txt +++ b/src/modules/land_detector/CMakeLists.txt @@ -45,4 +45,3 @@ px4_add_module( DEPENDS hysteresis ) - diff --git a/src/modules/landing_target_estimator/CMakeLists.txt b/src/modules/landing_target_estimator/CMakeLists.txt index 146869aa3fe4..32308d3c9301 100644 --- a/src/modules/landing_target_estimator/CMakeLists.txt +++ b/src/modules/landing_target_estimator/CMakeLists.txt @@ -41,4 +41,3 @@ px4_add_module( KalmanFilter.cpp DEPENDS ) - diff --git a/src/modules/load_mon/LoadMon.cpp b/src/modules/load_mon/LoadMon.cpp index aa400391dc52..937d02c03e12 100644 --- a/src/modules/load_mon/LoadMon.cpp +++ b/src/modules/load_mon/LoadMon.cpp @@ -202,7 +202,7 @@ void LoadMon::cpuload() } } - fseek(_proc_fd, 0, SEEK_END); + fseek(_proc_fd, 0, SEEK_SET); if (parsedCount == 5) { int32_t kb_main_cached = kb_page_cache + kb_slab_reclaimable; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 5538965f0ed1..8517ff03b0eb 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -137,6 +137,14 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : bool BlockLocalPositionEstimator::init() { + uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; + vehicle_local_position_sub.update(); + + if (vehicle_local_position_sub.advertised() && (hrt_elapsed_time(&vehicle_local_position_sub.get().timestamp) < 1_s)) { + PX4_ERR("init failed, vehicle_local_position already advertised"); + return false; + } + if (!_sensors_sub.registerCallback()) { PX4_ERR("callback registration failed"); return false; diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index da199ca24675..636f4289b04d 100644 --- a/src/modules/local_position_estimator/params.c +++ b/src/modules/local_position_estimator/params.c @@ -1,6 +1,12 @@ #include -// 16 is max name length +/** + * Local position estimator enable (unsupported) + * + * @group Local Position Estimator + * @boolean + */ +PARAM_DEFINE_INT32(LPE_EN, 0); /** * Optical flow z offset from center diff --git a/src/modules/logger/Kconfig b/src/modules/logger/Kconfig index db2c5a75460e..985fd3d56ed1 100644 --- a/src/modules/logger/Kconfig +++ b/src/modules/logger/Kconfig @@ -10,3 +10,11 @@ menuconfig USER_LOGGER depends on BOARD_PROTECTED && MODULES_LOGGER ---help--- Put logger in userspace memory + +menuconfig LOGGER_STACK_SIZE + int "stack size of logger task" + default 3700 + depends on MODULES_LOGGER + ---help--- + Stack size of the logger task. Some configurations require more stack + than the default. diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 68b3d27b42fd..5be39234e34f 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -53,12 +53,11 @@ void LoggedTopics::add_default_topics() add_optional_topic("autotune_attitude_control_status", 100); add_optional_topic("camera_capture"); add_optional_topic("camera_trigger"); + add_optional_topic("can_interface_status", 10); add_topic("cellular_status", 200); add_topic("commander_state"); add_topic("config_overrides"); add_topic("cpuload"); - add_optional_topic("differential_drive_control_output", 100); - add_optional_topic("differential_drive_setpoint", 100); add_optional_topic("external_ins_attitude"); add_optional_topic("external_ins_global_position"); add_optional_topic("external_ins_local_position"); @@ -70,6 +69,7 @@ void LoggedTopics::add_default_topics() add_optional_topic("follow_target_status", 400); add_optional_topic("flaps_setpoint", 1000); add_optional_topic("flight_phase_estimation", 1000); + add_optional_topic("fuel_tank_status", 10); add_topic("gimbal_manager_set_attitude", 500); add_optional_topic("generator_status"); add_optional_topic("gps_dump"); @@ -91,6 +91,7 @@ void LoggedTopics::add_default_topics() add_topic("manual_control_switches"); add_topic("mission_result"); add_topic("navigator_mission_item"); + add_topic("navigator_status"); add_topic("npfg_status", 100); add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); @@ -101,8 +102,12 @@ void LoggedTopics::add_default_topics() add_topic("position_setpoint_triplet", 200); add_optional_topic("px4io_status"); add_topic("radio_status"); + add_optional_topic("rover_ackermann_guidance_status", 100); + add_optional_topic("rover_ackermann_status", 100); + add_optional_topic("rover_differential_guidance_status", 100); + add_optional_topic("rover_differential_status", 100); add_topic("rtl_time_estimate", 1000); - add_optional_topic("rtl_status", 5000); + add_topic("rtl_status", 2000); add_optional_topic("sensor_airflow", 100); add_topic("sensor_combined"); add_optional_topic("sensor_correction"); @@ -144,71 +149,30 @@ void LoggedTopics::add_default_topics() add_optional_topic_multi("rate_ctrl_status", 200, 2); add_optional_topic_multi("sensor_hygrometer", 500, 4); add_optional_topic_multi("rpm", 200); + add_topic_multi("timesync_status", 1000, 3); add_optional_topic_multi("telemetry_status", 1000, 4); - // EKF multi topics (currently max 9 estimators) -#if CONSTRAINED_MEMORY - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1; -#else - static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed - add_optional_topic("estimator_selector_status"); - add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); -#endif - - // always add the first instance - add_topic("estimator_baro_bias", 500); - add_topic("estimator_gnss_hgt_bias", 500); - add_topic("estimator_rng_hgt_bias", 500); - add_topic("estimator_ev_pos_bias", 500); - add_topic("estimator_event_flags", 0); - add_topic("estimator_gps_status", 1000); - add_topic("estimator_innovation_test_ratios", 500); - add_topic("estimator_innovation_variances", 500); - add_topic("estimator_innovations", 500); - add_topic("estimator_optical_flow_vel", 200); - add_topic("estimator_sensor_bias", 0); - add_topic("estimator_states", 1000); - add_topic("estimator_status", 200); - add_topic("estimator_status_flags", 0); - add_topic("yaw_estimator_status", 1000); - - add_optional_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gnss_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_rng_hgt_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_ev_pos_bias", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_gps_status", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_innovations", 500, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_optical_flow_vel", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_sensor_bias", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_states", 1000, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); - - // add_optional_topic_multi("estimator_aid_src_airspeed", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_baro_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gravity", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_rng_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_hgt", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_fake_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_yaw", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_vel", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_gnss_pos", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag_heading", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_mag", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 100, MAX_ESTIMATOR_INSTANCES); - // add_optional_topic_multi("estimator_aid_src_ev_yaw", 100, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_aux_global_position", 100, MAX_ESTIMATOR_INSTANCES); + // EKF multi topics + { + // optionally log all estimator* topics at minimal rate + const uint16_t kEKFVerboseIntervalMilliseconds = 500; // 2 Hz + const struct orb_metadata *const *topic_list = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) { + add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds); + } + } + } + + // important EKF topics (higher rate) + add_optional_topic("estimator_selector_status", 10); + add_optional_topic_multi("estimator_event_flags", 10); + add_optional_topic_multi("estimator_optical_flow_vel", 200); + add_optional_topic_multi("estimator_sensor_bias", 1000); + add_optional_topic_multi("estimator_status", 200); + add_optional_topic_multi("estimator_status_flags", 10); + add_optional_topic_multi("yaw_estimator_status", 1000); // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 2); @@ -262,46 +226,23 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_local_position_groundtruth", 20); // EKF replay - add_topic("estimator_baro_bias"); - add_topic("estimator_gnss_hgt_bias"); - add_topic("estimator_rng_hgt_bias"); - add_topic("estimator_ev_pos_bias"); - add_topic("estimator_event_flags"); - add_topic("estimator_gps_status"); - add_topic("estimator_innovation_test_ratios"); - add_topic("estimator_innovation_variances"); - add_topic("estimator_innovations"); - add_topic("estimator_optical_flow_vel"); - add_topic("estimator_sensor_bias"); - add_topic("estimator_states"); - add_topic("estimator_status"); - add_topic("estimator_status_flags"); + { + // optionally log all estimator* topics at minimal rate + const uint16_t kEKFVerboseIntervalMilliseconds = 10; // 100 Hz + const struct orb_metadata *const *topic_list = orb_get_topics(); + + for (size_t i = 0; i < orb_topics_count(); i++) { + if (strncmp(topic_list[i]->o_name, "estimator", 9) == 0) { + add_optional_topic_multi(topic_list[i]->o_name, kEKFVerboseIntervalMilliseconds); + } + } + } + add_topic("vehicle_attitude"); add_topic("vehicle_global_position"); add_topic("vehicle_local_position"); add_topic("wind"); - add_topic("yaw_estimator_status"); - - add_optional_topic_multi("estimator_aid_src_airspeed", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_baro_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_rng_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_fake_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_vel", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_ev_yaw", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_hgt", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_pos", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_vel", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gnss_yaw", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_gravity", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_mag_heading", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_mag", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_terrain_optical_flow", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_terrain_range_finder", 0, MAX_ESTIMATOR_INSTANCES); - add_optional_topic_multi("estimator_aid_src_sideslip", 0, MAX_ESTIMATOR_INSTANCES); + add_optional_topic_multi("yaw_estimator_status"); #endif /* CONFIG_ARCH_BOARD_PX4_SITL */ } @@ -317,7 +258,9 @@ void LoggedTopics::add_high_rate_topics() add_topic("vehicle_attitude_setpoint"); add_topic("vehicle_rates_setpoint"); + add_topic("esc_status", 5); add_topic("actuator_motors"); + add_topic("actuator_outputs_debug"); add_topic("actuator_servos"); add_topic_multi("vehicle_thrust_setpoint", 0, 2); add_topic_multi("vehicle_torque_setpoint", 0, 2); @@ -397,6 +340,8 @@ void LoggedTopics::add_system_identification_topics() add_topic("sensor_combined"); add_topic("vehicle_angular_velocity"); add_topic("vehicle_torque_setpoint"); + add_topic("vehicle_acceleration"); + add_topic("actuator_motors"); } void LoggedTopics::add_mavlink_tunnel() diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index f9c3f66800c9..8d897a103a4d 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -176,7 +176,7 @@ int Logger::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("logger", SCHED_DEFAULT, SCHED_PRIORITY_LOG_CAPTURE, - PX4_STACK_ADJUSTED(3700), + PX4_STACK_ADJUSTED(CONFIG_LOGGER_STACK_SIZE), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -1037,6 +1037,12 @@ void Logger::publish_logger_status() if (hrt_elapsed_time(&_logger_status_last) >= 1_s) { for (int i = 0; i < (int)LogType::Count; ++i) { + logger_status_s status = {}; + status.type = i; + status.backend = _writer.backend(); + status.num_messages = _num_subscriptions; + status.timestamp = hrt_absolute_time(); + const LogType log_type = static_cast(i); if (_writer.is_started(log_type)) { @@ -1046,19 +1052,16 @@ void Logger::publish_logger_status() const float kb_written = _writer.get_total_written_file(log_type) / 1024.0f; const float seconds = hrt_elapsed_time(&_statistics[i].start_time_file) * 1e-6f; - logger_status_s status; - status.type = i; - status.backend = _writer.backend(); + status.is_logging = true; status.total_written_kb = kb_written; status.write_rate_kb_s = kb_written / seconds; status.dropouts = _statistics[i].write_dropouts; status.message_gaps = _message_gaps; status.buffer_used_bytes = buffer_fill_count_file; status.buffer_size_bytes = _writer.get_buffer_size_file(log_type); - status.num_messages = _num_subscriptions; - status.timestamp = hrt_absolute_time(); - _logger_status_pub[i].publish(status); } + + _logger_status_pub[i].publish(status); } _logger_status_last = hrt_absolute_time(); diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 9b468010997f..44499c71b503 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -158,8 +158,8 @@ class Logger : public ModuleBase, public ModuleParams static constexpr int MAX_MISSION_TOPICS_NUM = 5; /**< Maximum number of mission topics */ static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */ static constexpr const char *LOG_ROOT[(int)LogType::Count] = { - PX4_STORAGEDIR "/log", - PX4_STORAGEDIR "/mission_log" + CONFIG_BOARD_ROOT_PATH "/log", + CONFIG_BOARD_ROOT_PATH "/mission_log" }; struct LogFileName { diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index d295d43744bf..f42dd6fa9b7b 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -44,8 +44,6 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py --lang C --wire-protocol 2.0 - #--no-validate - #--strict-units --output ${MAVLINK_LIBRARY_DIR} ${MAVLINK_GIT_DIR}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${MAVLINK_DIALECT_UAVIONIX}.log DEPENDS @@ -64,8 +62,6 @@ add_custom_command( COMMAND ${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py --lang C --wire-protocol 2.0 - #--no-validate - #--strict-units --output ${MAVLINK_LIBRARY_DIR} ${MAVLINK_GIT_DIR}/message_definitions/v1.0/${CONFIG_MAVLINK_DIALECT}.xml > ${CMAKE_CURRENT_BINARY_DIR}/mavgen_${CONFIG_MAVLINK_DIALECT}.log DEPENDS diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index c4a5c497379c..bb52e30d2b92 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit c4a5c497379ca873f73abe691a033641a6a5a817 +Subproject commit bb52e30d2b924d5a250f2400144d33012271a19d diff --git a/src/modules/mavlink/mavlink_events.cpp b/src/modules/mavlink/mavlink_events.cpp index c1596530d45e..135cf154d2e7 100644 --- a/src/modules/mavlink/mavlink_events.cpp +++ b/src/modules/mavlink/mavlink_events.cpp @@ -208,17 +208,18 @@ void SendProtocol::handle_request_event(const mavlink_message_t &msg) const void SendProtocol::send_event(const Event &event) const { - mavlink_event_t event_msg{}; - event_msg.event_time_boot_ms = event.timestamp_ms; - event_msg.destination_component = MAV_COMP_ID_ALL; - event_msg.destination_system = 0; - event_msg.id = event.id; - event_msg.sequence = event.sequence; - event_msg.log_levels = event.log_levels; - static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small"); - memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments)); - mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg); - + if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) { + mavlink_event_t event_msg{}; + event_msg.event_time_boot_ms = event.timestamp_ms; + event_msg.destination_component = MAV_COMP_ID_ALL; + event_msg.destination_system = 0; + event_msg.id = event.id; + event_msg.sequence = event.sequence; + event_msg.log_levels = event.log_levels; + static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small"); + memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments)); + mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg); + } } void SendProtocol::on_gcs_connected() @@ -228,8 +229,9 @@ void SendProtocol::on_gcs_connected() void SendProtocol::send_current_sequence(const hrt_abstime &now, bool force_reset) { - // only send if enough tx buffer space available - if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) { + // only send if enough tx buffer space available or not MAVLINK_MODE_IRIDIUM + if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES || + _mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { return; } diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 9d078b16b090..058e9d95f1b3 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -45,17 +45,13 @@ #include "mavlink_ftp.h" #include "mavlink_tests/mavlink_ftp_test.h" -#ifndef MAVLINK_FTP_UNIT_TEST #include "mavlink_main.h" -#else -#include -#endif using namespace time_literals; constexpr const char MavlinkFTP::_root_dir[]; -MavlinkFTP::MavlinkFTP(Mavlink *mavlink) : +MavlinkFTP::MavlinkFTP(Mavlink &mavlink) : _mavlink(mavlink) { // initialize session @@ -96,7 +92,7 @@ MavlinkFTP::_getServerSystemId() return MavlinkFtpTest::serverSystemId; #else // Not unit testing, use the real thing - return _mavlink->get_system_id(); + return _mavlink.get_system_id(); #endif } @@ -108,7 +104,7 @@ MavlinkFTP::_getServerComponentId() return MavlinkFtpTest::serverComponentId; #else // Not unit testing, use the real thing - return _mavlink->get_component_id(); + return _mavlink.get_component_id(); #endif } @@ -120,7 +116,7 @@ MavlinkFTP::_getServerChannel() return MavlinkFtpTest::serverChannel; #else // Not unit testing, use the real thing - return _mavlink->get_channel(); + return _mavlink.get_channel(); #endif } @@ -180,7 +176,7 @@ MavlinkFTP::_process_request( #ifdef MAVLINK_FTP_UNIT_TEST _utRcvMsgFunc(last_reply, _worker_data); #else - mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), last_reply); + mavlink_msg_file_transfer_protocol_send_struct(_mavlink.get_channel(), last_reply); #endif return; } @@ -326,7 +322,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) // we can simply resend the response. // we only keep small responses to reduce RAM usage and avoid large memcpy's. The larger responses are all data // retrievals without side-effects, meaning it's ok to reexecute them if a response gets lost - if (payload->size <= sizeof(uint32_t)) { + if (payload->size <= sizeof(uint32_t) && payload->data[0] != kErrNoSessionsAvailable) { _last_reply_valid = true; memcpy(_last_reply, ftp_req, sizeof(_last_reply)); } @@ -340,19 +336,31 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t *ftp_req) // Unit test hook is set, call that instead _utRcvMsgFunc(ftp_req, _worker_data); #else - mavlink_msg_file_transfer_protocol_send_struct(_mavlink->get_channel(), ftp_req); + mavlink_msg_file_transfer_protocol_send_struct(_mavlink.get_channel(), ftp_req); #endif } +void MavlinkFTP::_constructPath(char *dst, int dst_len, const char *path) const +{ + strncpy(dst, _root_dir, dst_len); + int root_dir_len = _root_dir_len; + + // If neither the root ends nor the given path starts with a '/', add a separating '/' in between + if (dst[0] != '\0' && dst[strlen(dst) - 1] != '/' && path[0] != '/') { + strncat(dst, "/", dst_len); + ++root_dir_len; + } + + strncpy(dst + root_dir_len, path, dst_len - root_dir_len); + // Ensure termination + dst[dst_len - 1] = '\0'; +} /// @brief Responds to a List command MavlinkFTP::ErrorCode MavlinkFTP::_workList(PayloadHeader *payload) { - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); - // ensure termination - _work_buffer1[_work_buffer1_len - 1] = '\0'; + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); ErrorCode errorCode = kErrNone; unsigned offset = 0; @@ -502,12 +510,11 @@ MavlinkFTP::ErrorCode MavlinkFTP::_workOpen(PayloadHeader *payload, int oflag) { if (_session_info.fd >= 0) { - PX4_ERR("FTP: Open failed - out of sessions\n"); + PX4_ERR("FTP: Open failed - out of sessions"); return kErrNoSessionsAvailable; } - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); PX4_DEBUG("FTP: open '%s'", _work_buffer1); @@ -649,10 +656,7 @@ MavlinkFTP::_workWrite(PayloadHeader *payload) MavlinkFTP::ErrorCode MavlinkFTP::_workRemoveFile(PayloadHeader *payload) { - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); - // ensure termination - _work_buffer1[_work_buffer1_len - 1] = '\0'; + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); if (!_validatePathIsWritable(_work_buffer1)) { return kErrFailFileProtected; @@ -675,10 +679,7 @@ MavlinkFTP::_workRemoveFile(PayloadHeader *payload) MavlinkFTP::ErrorCode MavlinkFTP::_workTruncateFile(PayloadHeader *payload) { - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); - // ensure termination - _work_buffer1[_work_buffer1_len - 1] = '\0'; + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); payload->size = 0; if (!_validatePathIsWritable(_work_buffer1)) { @@ -833,19 +834,14 @@ MavlinkFTP::_workRename(PayloadHeader *payload) char *ptr = _data_as_cstring(payload); size_t oldpath_sz = strlen(ptr); - if (oldpath_sz == payload->size) { + if (oldpath_sz + 2 >= payload->size) { // no newpath errno = EINVAL; return kErrFailErrno; } - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, ptr, _work_buffer1_len - _root_dir_len); - _work_buffer1[_work_buffer1_len - 1] = '\0'; // ensure termination - - strncpy(_work_buffer2, _root_dir, _work_buffer2_len); - strncpy(_work_buffer2 + _root_dir_len, ptr + oldpath_sz + 1, _work_buffer2_len - _root_dir_len); - _work_buffer2[_work_buffer2_len - 1] = '\0'; // ensure termination + _constructPath(_work_buffer1, _work_buffer1_len, ptr); + _constructPath(_work_buffer2, _work_buffer2_len, ptr + oldpath_sz + 1); if (!_validatePathIsWritable(_work_buffer2)) { return kErrFailFileProtected; @@ -868,10 +864,7 @@ MavlinkFTP::_workRename(PayloadHeader *payload) MavlinkFTP::ErrorCode MavlinkFTP::_workRemoveDirectory(PayloadHeader *payload) { - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); - // ensure termination - _work_buffer1[_work_buffer1_len - 1] = '\0'; + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); if (!_validatePathIsWritable(_work_buffer1)) { return kErrFailFileProtected; @@ -894,10 +887,7 @@ MavlinkFTP::_workRemoveDirectory(PayloadHeader *payload) MavlinkFTP::ErrorCode MavlinkFTP::_workCreateDirectory(PayloadHeader *payload) { - strncpy(_work_buffer1, _root_dir, _work_buffer1_len); - strncpy(_work_buffer1 + _root_dir_len, _data_as_cstring(payload), _work_buffer1_len - _root_dir_len); - // ensure termination - _work_buffer1[_work_buffer1_len - 1] = '\0'; + _constructPath(_work_buffer1, _work_buffer1_len, _data_as_cstring(payload)); if (!_validatePathIsWritable(_work_buffer1)) { return kErrFailFileProtected; @@ -922,10 +912,7 @@ MavlinkFTP::_workCalcFileCRC32(PayloadHeader *payload) { uint32_t checksum = 0; ssize_t bytes_read; - strncpy(_work_buffer2, _root_dir, _work_buffer2_len); - strncpy(_work_buffer2 + _root_dir_len, _data_as_cstring(payload), _work_buffer2_len - _root_dir_len); - // ensure termination - _work_buffer2[_work_buffer2_len - 1] = '\0'; + _constructPath(_work_buffer2, _work_buffer2_len, _data_as_cstring(payload)); int fd = ::open(_work_buffer2, O_RDONLY); @@ -1064,8 +1051,8 @@ void MavlinkFTP::send() #ifndef MAVLINK_FTP_UNIT_TEST // Skip send if not enough room - unsigned max_bytes_to_send = _mavlink->get_free_tx_buf(); - PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf()); + unsigned max_bytes_to_send = _mavlink.get_free_tx_buf(); + PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink.get_free_tx_buf()); if (max_bytes_to_send < get_size()) { return; @@ -1173,7 +1160,7 @@ bool MavlinkFTP::_validatePathIsWritable(const char *path) // Don't allow writes to system paths as they are in RAM // Ideally we'd canonicalize the path (with 'realpath'), but it might not exist, so realpath() would fail. // The next simpler thing is to check there's no reference to a parent dir. - if (strncmp(path, "/fs/microsd/", 12) != 0 || strstr(path, "/../") != nullptr) { + if (strncmp(path, CONFIG_BOARD_ROOT_PATH "/", 12) != 0 || strstr(path, "/../") != nullptr) { PX4_ERR("Disallowing write to %s", path); return false; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 4372033043d0..d5b9ce294688 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -43,11 +43,7 @@ #include #include -#ifndef MAVLINK_FTP_UNIT_TEST #include "mavlink_bridge_header.h" -#else -#include -#endif class MavlinkFtpTest; class Mavlink; @@ -56,7 +52,7 @@ class Mavlink; class MavlinkFTP { public: - MavlinkFTP(Mavlink *mavlink); + MavlinkFTP(Mavlink &mavlink); ~MavlinkFTP(); /** @@ -155,6 +151,11 @@ class MavlinkFTP uint8_t _getServerComponentId(void); uint8_t _getServerChannel(void); + /** + * Construct local path by appending `path` to `_root_dir` and storing the result in `dst`. + */ + void _constructPath(char *dst, int dst_len, const char *path) const; + bool _validatePathIsWritable(const char *path); /** @@ -185,7 +186,7 @@ class MavlinkFTP ReceiveMessageFunc_t _utRcvMsgFunc{}; ///< Unit test override for mavlink message sending void *_worker_data{nullptr}; ///< Additional parameter to _utRcvMsgFunc; - Mavlink *_mavlink; + Mavlink &_mavlink; /* do not allow copying this class */ MavlinkFTP(const MavlinkFTP &); diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index d4ed61d4285c..1afa32382b19 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 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 @@ -31,558 +31,498 @@ * ****************************************************************************/ -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - #include "mavlink_log_handler.h" #include "mavlink_main.h" +#include #include -#include -#include - -#define MOUNTPOINT PX4_STORAGEDIR -static const char *kLogRoot = MOUNTPOINT "/log"; -static const char *kLogData = MOUNTPOINT "/logdata.txt"; -static const char *kTmpData = MOUNTPOINT "/$log$.txt"; +static constexpr int MAX_BYTES_BURST = 256 * 1024; +static const char *kLogListFilePath = PX4_STORAGEDIR "/logdata.txt"; +static const char *kLogListFilePathTemp = PX4_STORAGEDIR "/$log$.txt"; +static const char *kLogDir = PX4_STORAGEDIR "/log"; #ifdef __PX4_NUTTX #define PX4LOG_REGULAR_FILE DTYPE_FILE #define PX4LOG_DIRECTORY DTYPE_DIRECTORY +#define PX4_MAX_FILEPATH CONFIG_PATH_MAX #else +#ifndef PATH_MAX +#define PATH_MAX 1024 // maximum on macOS +#endif #define PX4LOG_REGULAR_FILE DT_REG #define PX4LOG_DIRECTORY DT_DIR +#define PX4_MAX_FILEPATH PATH_MAX #endif -//#define MAVLINK_LOG_HANDLER_VERBOSE - -#ifdef MAVLINK_LOG_HANDLER_VERBOSE -#define PX4LOG_WARN(fmt, ...) warnx(fmt, ##__VA_ARGS__) -#else -#define PX4LOG_WARN(fmt, ...) -#endif +MavlinkLogHandler::MavlinkLogHandler(Mavlink &mavlink) + : _mavlink(mavlink) +{} -//------------------------------------------------------------------- -static bool -stat_file(const char *file, time_t *date = nullptr, uint32_t *size = nullptr) +MavlinkLogHandler::~MavlinkLogHandler() { - struct stat st; - - if (stat(file, &st) == 0) { - if (date) { *date = st.st_mtime; } + perf_free(_create_file_elapsed); + perf_free(_listing_elapsed); - if (size) { *size = st.st_size; } - - return true; + if (_current_entry.fp) { + fclose(_current_entry.fp); } - return false; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); } -//------------------------------------------------------------------- -MavlinkLogHandler::MavlinkLogHandler(Mavlink *mavlink) - : _mavlink(mavlink) +void MavlinkLogHandler::send() { + switch (_state) { + case LogHandlerState::Idle: { + state_idle(); + break; + } -} -MavlinkLogHandler::~MavlinkLogHandler() -{ - _close_and_unlink_files(); + case LogHandlerState::Listing: { + state_listing(); + break; + } + + case LogHandlerState::SendingData: { + state_sending_data(); + break; + } + } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::handle_message(const mavlink_message_t *msg) +void MavlinkLogHandler::handle_message(const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_LOG_REQUEST_LIST: - _log_request_list(msg); + handle_log_request_list(msg); break; case MAVLINK_MSG_ID_LOG_REQUEST_DATA: - _log_request_data(msg); + handle_log_request_data(msg); break; - case MAVLINK_MSG_ID_LOG_ERASE: - _log_request_erase(msg); + case MAVLINK_MSG_ID_LOG_REQUEST_END: + handle_log_request_end(msg); break; - case MAVLINK_MSG_ID_LOG_REQUEST_END: - _log_request_end(msg); + case MAVLINK_MSG_ID_LOG_ERASE: + handle_log_erase(msg); break; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::send() +void MavlinkLogHandler::state_idle() { - //-- An arbitrary count of max bytes in one go (one of the two below but never both) -#define MAX_BYTES_SEND 256 * 1024 - size_t count = 0; - - //-- Log Entries - while (_current_status == LogHandlerState::Listing - && _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_ENTRY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_listing(); - } - - //-- Log Data - while (_current_status == LogHandlerState::SendingData - && _mavlink->get_free_tx_buf() > MAVLINK_MSG_ID_LOG_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES - && count < MAX_BYTES_SEND) { - count += _log_send_data(); + if (_current_entry.fp && _file_send_finished) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + + _current_entry.id = 0xffff; + _current_entry.time_utc = 0; + _current_entry.size_bytes = 0; + _current_entry.filepath[0] = 0; + _current_entry.offset = 0; + + _entry_request.id = 0xffff; + _entry_request.start_offset = 0; + _entry_request.byte_count = 0; } } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_list(const mavlink_message_t *msg) +void MavlinkLogHandler::state_listing() { - mavlink_log_request_list_t request; - mavlink_msg_log_request_list_decode(msg, &request); - - //-- Check for re-requests (data loss) or new request - if (_current_status != LogHandlerState::Inactive) { - //-- Is this a new request? - if (request.start == 0) { - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - } else { - _current_status = LogHandlerState::Idle; + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_ENTRY_LEN; - } + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE) { + return; } - if (_current_status == LogHandlerState::Inactive) { - //-- Prepare new request + DIR *dp = opendir(kLogDir); - _reset_list_helper(); - _init_list_helper(); - _current_status = LogHandlerState::Idle; - } - - if (_log_count) { - //-- Define (and clamp) range - _next_entry = request.start < _log_count ? request.start : - _log_count - 1; - _last_entry = request.end < _log_count ? request.end : - _log_count - 1; + if (!dp) { + PX4_DEBUG("No logs available"); + return; } - PX4LOG_WARN("\nMavlinkLogHandler::_log_request_list: start: %d last: %d count: %d", - _next_entry, - _last_entry, - _log_count); - //-- Enable streaming - _current_status = LogHandlerState::Listing; -} + FILE *fp = fopen(kLogListFilePath, "r"); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_data(const mavlink_message_t *msg) -{ - //-- If we haven't listed, we can't do much - if (_current_status == LogHandlerState::Inactive) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Log request with no list requested."); + if (!fp) { + PX4_DEBUG("Failed to open log list file"); + closedir(dp); return; } - mavlink_log_request_data_t request; - mavlink_msg_log_request_data_decode(msg, &request); + fseek(fp, _list_request.file_index, SEEK_SET); - //-- Does the requested log exist? - if (request.id >= _log_count) { - PX4LOG_WARN("MavlinkLogHandler::_log_request_data Requested log %" PRIu16 " but we only have %u.", request.id, - _log_count); - return; - } + size_t bytes_sent = 0; - //-- If we were sending log entries, stop it - _current_status = LogHandlerState::Idle; + char line[PX4_MAX_FILEPATH]; - if (_current_log_index != request.id) { - //-- Init send log dataset - _current_log_filename[0] = 0; - _current_log_index = request.id; - uint32_t time_utc = 0; + perf_begin(_listing_elapsed); - if (!_get_entry(_current_log_index, _current_log_size, time_utc, - _current_log_filename, sizeof(_current_log_filename))) { - PX4LOG_WARN("LogListHelper::get_entry failed."); - return; + while (fgets(line, sizeof(line), fp)) { + + if (_list_request.current_id < _list_request.first_id) { + _list_request.current_id++; + continue; } - _open_for_transmit(); - } + // We can send! + uint32_t size_bytes = 0; + uint32_t time_utc = 0; + char filepath[PX4_MAX_FILEPATH]; - _current_log_data_offset = request.ofs; + // If parsed lined successfully, send the entry + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &time_utc, &size_bytes, filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; + } - if (_current_log_data_offset >= _current_log_size) { - _current_log_data_remaining = 0; + send_log_entry(time_utc, size_bytes); + bytes_sent += sizeof(mavlink_log_entry_t); + _list_request.current_id++; - } else { - _current_log_data_remaining = _current_log_size - request.ofs; + // Yield if we've exceed mavlink burst or buffer limit + if (_mavlink.get_free_tx_buf() <= MAVLINK_PACKET_SIZE || bytes_sent >= MAX_BYTES_BURST) { + _list_request.file_index = ftell(fp); + fclose(fp); + closedir(dp); + perf_end(_listing_elapsed); + return; + } } - if (_current_log_data_remaining > request.count) { - _current_log_data_remaining = request.count; - } + perf_end(_listing_elapsed); - //-- Enable streaming - _current_status = LogHandlerState::SendingData; -} + fclose(fp); + closedir(dp); -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_erase(const mavlink_message_t * /*msg*/) -{ - /* - mavlink_log_erase_t request; - mavlink_msg_log_erase_decode(msg, &request); - */ - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); - - //-- Delete all logs - _delete_all(kLogRoot); + _list_request.current_id = 0; + _list_request.file_index = 0; + _state = LogHandlerState::Idle; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_log_request_end(const mavlink_message_t * /*msg*/) +void MavlinkLogHandler::state_sending_data() { - PX4LOG_WARN("MavlinkLogHandler::_log_request_end"); + static constexpr uint32_t MAVLINK_PACKET_SIZE = MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_MSG_ID_LOG_DATA_LEN; + size_t bytes_sent = 0; - _current_status = LogHandlerState::Inactive; - _close_and_unlink_files(); -} + while (_mavlink.get_free_tx_buf() > MAVLINK_PACKET_SIZE && bytes_sent < MAX_BYTES_BURST) { -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_listing() -{ - mavlink_log_entry_t response; - uint32_t size, date; - _get_entry(_next_entry, size, date); - response.size = size; - response.time_utc = date; - response.id = _next_entry; - response.num_logs = _log_count; - response.last_log_num = _last_entry; - mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); - - //-- If we're done listing, flag it. - if (_next_entry >= _last_entry) { - _current_status = LogHandlerState::Idle; - - } else { - _next_entry++; - } + // Only seek if we need to + long int offset = _current_entry.offset - ftell(_current_entry.fp); - PX4LOG_WARN("MavlinkLogHandler::_log_send_listing id: %" PRIu16 " count: %" PRIu16 " last: %" PRIu16 " size: %" PRIu32 - " date: %" PRIu32 " status: %" PRIu32, - response.id, - response.num_logs, - response.last_log_num, - response.size, - response.time_utc, - (uint32_t)_current_status); - return sizeof(response); -} + if (offset && fseek(_current_entry.fp, offset, SEEK_CUR)) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + PX4_DEBUG("seek error"); + _state = LogHandlerState::Idle; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_log_send_data() -{ - mavlink_log_data_t response; - memset(&response, 0, sizeof(response)); - uint32_t len = _current_log_data_remaining; + // Prepare mavlink message + mavlink_log_data_t msg; - if (len > sizeof(response.data)) { - len = sizeof(response.data); - } + if (_current_entry.offset >= _current_entry.size_bytes) { + PX4_DEBUG("Entry offset equal to file size"); + _state = LogHandlerState::Idle; + return; + } - size_t read_size = _get_log_data(len, response.data); - response.ofs = _current_log_data_offset; - response.id = _current_log_index; - response.count = read_size; - mavlink_msg_log_data_send_struct(_mavlink->get_channel(), &response); - _current_log_data_offset += read_size; - _current_log_data_remaining -= read_size; + size_t bytes_to_read = _current_entry.size_bytes - _current_entry.offset; - if (read_size < sizeof(response.data) || _current_log_data_remaining == 0) { - _current_status = LogHandlerState::Idle; - } + if (bytes_to_read > sizeof(msg.data)) { + bytes_to_read = sizeof(msg.data); + } - return sizeof(response); -} + msg.count = fread(msg.data, 1, bytes_to_read, _current_entry.fp); + msg.id = _current_entry.id; + msg.ofs = _current_entry.offset; -//------------------------------------------------------------------- -void MavlinkLogHandler::_close_and_unlink_files() -{ - if (_current_log_filep) { - ::fclose(_current_log_filep); - _reset_list_helper(); - } + mavlink_msg_log_data_send_struct(_mavlink.get_channel(), &msg); + + bytes_sent += MAVLINK_PACKET_SIZE; + _current_entry.offset += msg.count; - // Remove log data files (if any) - unlink(kLogData); - unlink(kTmpData); + bool chunk_finished = _current_entry.offset >= (_entry_request.byte_count + _entry_request.start_offset); + _file_send_finished = _current_entry.offset >= _current_entry.size_bytes; + + if (chunk_finished || _file_send_finished) { + _state = LogHandlerState::Idle; + return; + } + } } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_entry(int idx, uint32_t &size, uint32_t &date, char *filename, int filename_len) +void MavlinkLogHandler::handle_log_request_list(const mavlink_message_t *msg) { - //-- Find log file in log list file created during init() - size = 0; - date = 0; - bool result = false; - //-- Open list of log files - FILE *f = ::fopen(kLogData, "r"); - - if (f) { - //--- Find requested entry - char line[160]; - int count = 0; - - while (fgets(line, sizeof(line), f)) { - //-- Found our "index" - if (count++ == idx) { - char file[160]; - - if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &date, &size, file) == 3) { - if (filename && filename_len > 0) { - strncpy(filename, file, filename_len); - filename[filename_len - 1] = 0; // ensure null-termination - } - - result = true; - break; - } - } - } + mavlink_log_request_list_t request; + mavlink_msg_log_request_list_decode(msg, &request); - fclose(f); + if (!create_log_list_file()) { + return; } - return result; + _list_request.first_id = request.start; + _list_request.last_id = request.end == 0xffff ? _num_logs : request.end; + _list_request.current_id = 0; + _list_request.file_index = 0; + _logs_listed = true; + _state = LogHandlerState::Listing; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_open_for_transmit() +void MavlinkLogHandler::handle_log_request_data(const mavlink_message_t *msg) { - if (_current_log_filep) { - ::fclose(_current_log_filep); - _current_log_filep = nullptr; + if (!_logs_listed) { + PX4_DEBUG("Logs not yet listed"); + _state = LogHandlerState::Idle; + return; } - _current_log_filep = ::fopen(_current_log_filename, "rb"); + mavlink_log_request_data_t request; + mavlink_msg_log_request_data_decode(msg, &request); - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::open_for_transmit Could not open %s", _current_log_filename); - return false; + if (request.id >= _num_logs) { + PX4_DEBUG("Requested log %" PRIu16 " but we only have %u", request.id, _num_logs); + _state = LogHandlerState::Idle; + return; } - return true; -} + // Handle switching to new request ID + if (request.id != _current_entry.id) { + // Close the old file + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } -//------------------------------------------------------------------- -size_t -MavlinkLogHandler::_get_log_data(uint8_t len, uint8_t *buffer) -{ - if (!_current_log_filename[0]) { - return 0; - } + LogEntry entry = {}; - if (!_current_log_filep) { - PX4LOG_WARN("MavlinkLogHandler::get_log_data file not open %s", _current_log_filename); - return 0; - } + if (!log_entry_from_id(request.id, &entry)) { + PX4_DEBUG("Log file ID %u does not exist", request.id); + _state = LogHandlerState::Idle; + return; + } - long int offset = _current_log_data_offset - ftell(_current_log_filep); + entry.fp = fopen(entry.filepath, "rb"); + entry.offset = request.ofs; - if (offset && fseek(_current_log_filep, offset, SEEK_CUR)) { - fclose(_current_log_filep); - _current_log_filep = nullptr; - PX4LOG_WARN("MavlinkLogHandler::get_log_data Seek error in %s", _current_log_filename); - return 0; + if (!entry.fp) { + PX4_DEBUG("Failed to open file %s", entry.filepath); + return; + } + + _current_entry = entry; } - size_t result = fread(buffer, 1, len, _current_log_filep); - return result; -} + // Stop if offset request is larger than file + if (request.ofs >= _current_entry.size_bytes) { + PX4_DEBUG("Request offset %" PRIu32 "greater than file size %" PRIu32, request.ofs, + _current_entry.size_bytes); + _state = LogHandlerState::Idle; + return; + } + _entry_request.id = request.id; + _entry_request.start_offset = request.ofs; + _entry_request.byte_count = request.count; + // Set the offset of the current entry to the requested offset + _current_entry.offset = _entry_request.start_offset; + _file_send_finished = false; + _state = LogHandlerState::SendingData; +} -void -MavlinkLogHandler::_reset_list_helper() +void MavlinkLogHandler::handle_log_request_end(const mavlink_message_t *msg) { - _next_entry = 0; - _last_entry = 0; - _log_count = 0; - _current_log_index = UINT16_MAX; - _current_log_size = 0; - _current_log_data_offset = 0; - _current_log_data_remaining = 0; - _current_log_filep = nullptr; + _state = LogHandlerState::Idle; } -void -MavlinkLogHandler::_init_list_helper() +void MavlinkLogHandler::handle_log_erase(const mavlink_message_t *msg) { - /* + if (_current_entry.fp) { + fclose(_current_entry.fp); + _current_entry.fp = nullptr; + } - When this helper is created, it scans the log directory - and collects all log files found into one file for easy, - subsequent access. - */ + _state = LogHandlerState::Idle; + unlink(kLogListFilePath); + unlink(kLogListFilePathTemp); - _current_log_filename[0] = 0; + delete_all_logs(kLogDir); +} - // Remove old log data file (if any) - unlink(kLogData); - // Open log directory - DIR *dp = opendir(kLogRoot); +bool MavlinkLogHandler::create_log_list_file() +{ + perf_begin(_create_file_elapsed); - if (dp == nullptr) { - // No log directory. Nothing to do. - return; + // clean up old file + unlink(kLogListFilePath); + _num_logs = 0; + + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_DEBUG("No logs available"); + return false; } - // Create work file - FILE *f = ::fopen(kTmpData, "w"); + FILE *temp_fp = fopen(kLogListFilePathTemp, "w"); - if (!f) { - PX4LOG_WARN("MavlinkLogHandler::init Error creating %s", kTmpData); + if (!temp_fp) { + PX4_DEBUG("Failed to create temp file"); closedir(dp); - return; + return false; } - // Scan directory and collect log files struct dirent *result = nullptr; - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_DIRECTORY) { - time_t tt = 0; - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", kLogRoot, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + // Iterate over the log/ directory which contains subdirectories formatted: yyyy-mm-dd + while (1) { + result = readdir(dp); - if (path_is_ok) { - if (_get_session_date(log_path, result->d_name, tt)) { - _scan_logs(f, log_path, tt); - } - } + if (!result) { + // Reached end of directory + break; + } + + if (result->d_type != PX4LOG_DIRECTORY) { + // Skip stray files + continue; + } + + // Skip the '.' and '..' entries + if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) { + continue; + } + + // Open up the sub directory + char dirpath[PX4_MAX_FILEPATH]; + int ret = snprintf(dirpath, sizeof(dirpath), "%s/%s", kLogDir, result->d_name); + + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(dirpath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", dirpath); + continue; } + + // Iterate over files inside the subdir and write them to the file + write_entries_to_file(temp_fp, dirpath); } + fclose(temp_fp); closedir(dp); - fclose(f); // Rename temp file to data file - if (rename(kTmpData, kLogData)) { - PX4LOG_WARN("MavlinkLogHandler::init Error renaming %s", kTmpData); - _log_count = 0; + if (rename(kLogListFilePathTemp, kLogListFilePath)) { + PX4_DEBUG("Failed to rename temp file"); + return false; } + + perf_end(_create_file_elapsed); + + return true; } -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_session_date(const char *path, const char *dir, time_t &date) +void MavlinkLogHandler::write_entries_to_file(FILE *fp, const char *dir) { - if (strlen(dir) > 4) { - // Always try to get file time first - if (stat_file(path, &date)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } + DIR *dp = opendir(dir); + struct dirent *result = nullptr; + + while (1) { + result = readdir(dp); + + if (!result) { + // Reached end of directory + break; } - // Convert "sess000" to 00:00 Jan 1 1970 (day per session) - if (strncmp(dir, "sess", 4) == 0) { - unsigned u; + if (result->d_type != PX4LOG_REGULAR_FILE) { + // Skip non files + continue; + } - if (sscanf(&dir[4], "%u", &u) == 1) { - date = u * 60 * 60 * 24; - return true; - } + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); + + if (!path_is_ok) { + PX4_DEBUG("Log subdir path error: %s", filepath); + continue; + } + + struct stat filestat; + + if (stat(filepath, &filestat) != 0) { + PX4_DEBUG("stat() failed: %s", filepath); + continue; } + + // Write to file using format: + // [ time ] [ size_bytes ] [ filepath ] + fprintf(fp, "%u %u %s\n", unsigned(filestat.st_mtime), unsigned(filestat.st_size), filepath); + _num_logs++; } - return false; + closedir(dp); } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_scan_logs(FILE *f, const char *dir, time_t &date) +void MavlinkLogHandler::send_log_entry(uint32_t time_utc, uint32_t size_bytes) { - DIR *dp = opendir(dir); + mavlink_log_entry_t msg; + msg.time_utc = time_utc; + msg.size = size_bytes; + msg.id = _list_request.current_id; + msg.num_logs = _num_logs; + msg.last_log_num = _list_request.last_id; + mavlink_msg_log_entry_send_struct(_mavlink.get_channel(), &msg); +} - if (dp) { - struct dirent *result = nullptr; - - while ((result = readdir(dp))) { - if (result->d_type == PX4LOG_REGULAR_FILE) { - time_t ldate = date; - uint32_t size = 0; - char log_file_path[128]; - int ret = snprintf(log_file_path, sizeof(log_file_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_file_path)); - - if (path_is_ok) { - if (_get_log_time_size(log_file_path, result->d_name, ldate, size)) { - //-- Write result->out to list file - fprintf(f, "%u %u %s\n", (unsigned)ldate, (unsigned)size, log_file_path); - _log_count++; - } - } - } - } +bool MavlinkLogHandler::log_entry_from_id(uint16_t log_id, LogEntry *entry) +{ + DIR *dp = opendir(kLogDir); + + if (!dp) { + PX4_INFO("No logs available"); + return false; + } + + FILE *fp = fopen(kLogListFilePath, "r"); + if (!fp) { + PX4_DEBUG("Failed to open %s", kLogListFilePath); closedir(dp); + return false; } -} -//------------------------------------------------------------------- -bool -MavlinkLogHandler::_get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size) -{ - if (file && file[0]) { - if (strstr(file, ".px4log") || strstr(file, ".ulg")) { - // Always try to get file time first - if (stat_file(path, &date, &size)) { - // Try to prevent taking date if it's around 1970 (use the logic below instead) - if (date > 60 * 60 * 24) { - return true; - } - } + bool found_entry = false; + uint16_t current_id = 0; + char line[PX4_MAX_FILEPATH]; - // Convert "log000" to 00:00 (minute per flight in session) - if (strncmp(file, "log", 3) == 0) { - unsigned u; + while (fgets(line, sizeof(line), fp)) { - if (sscanf(&file[3], "%u", &u) == 1) { - date += (u * 60); + if (current_id != log_id) { + current_id++; + continue; + } - if (stat_file(path, nullptr, &size)) { - return true; - } - } - } + if (sscanf(line, "%" PRIu32 " %" PRIu32 " %s", &(entry->time_utc), &(entry->size_bytes), entry->filepath) != 3) { + PX4_DEBUG("sscanf failed"); + continue; } + + entry->id = log_id; + found_entry = true; + break; } - return false; + fclose(fp); + closedir(dp); + + return found_entry; } -//------------------------------------------------------------------- -void -MavlinkLogHandler::_delete_all(const char *dir) +void MavlinkLogHandler::delete_all_logs(const char *dir) { //-- Open log directory DIR *dp = opendir(dir); @@ -600,27 +540,27 @@ MavlinkLogHandler::_delete_all(const char *dir) } if (result->d_type == PX4LOG_DIRECTORY && result->d_name[0] != '.') { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - _delete_all(log_path); //Recursive call. TODO: consider add protection + delete_all_logs(filepath); - if (rmdir(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error removing %s", log_path); + if (rmdir(filepath)) { + PX4_DEBUG("Error removing %s", filepath); } } } if (result->d_type == PX4LOG_REGULAR_FILE) { - char log_path[128]; - int ret = snprintf(log_path, sizeof(log_path), "%s/%s", dir, result->d_name); - bool path_is_ok = (ret > 0) && (ret < (int)sizeof(log_path)); + char filepath[PX4_MAX_FILEPATH]; + int ret = snprintf(filepath, sizeof(filepath), "%s/%s", dir, result->d_name); + bool path_is_ok = (ret > 0) && (ret < (int)sizeof(filepath)); if (path_is_ok) { - if (unlink(log_path)) { - PX4LOG_WARN("MavlinkLogHandler::delete_all Error deleting %s", log_path); + if (unlink(filepath)) { + PX4_DEBUG("Error unlinking %s", filepath); } } } diff --git a/src/modules/mavlink/mavlink_log_handler.h b/src/modules/mavlink/mavlink_log_handler.h index 8993bf41004f..eb521a61ee4f 100644 --- a/src/modules/mavlink/mavlink_log_handler.h +++ b/src/modules/mavlink/mavlink_log_handler.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014, 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2024 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,76 +33,86 @@ #pragma once -/// @file mavlink_log_handler.h -/// @author px4dev, Gus Grubba - -#include -#include -#include -#include -#include -#include - +#include #include "mavlink_bridge_header.h" class Mavlink; -// MAVLink LOG_* Message Handler class MavlinkLogHandler { public: - MavlinkLogHandler(Mavlink *mavlink); + MavlinkLogHandler(Mavlink &mavlink); ~MavlinkLogHandler(); - // Handle possible LOG message + void send(); void handle_message(const mavlink_message_t *msg); - /** - * Handle sending of messages. Call this regularly at a fixed frequency. - * @param t current time - */ - void send(); +private: + struct LogEntry { + uint16_t id{0xffff}; + uint32_t time_utc{}; + uint32_t size_bytes{}; + FILE *fp{nullptr}; + char filepath[60]; + uint32_t offset{}; + }; - unsigned get_size(); + struct LogEntryRequest { + uint16_t id{0xffff}; + uint32_t start_offset{}; + uint32_t byte_count{}; + }; + + struct LogListRequest { + uint16_t first_id{0}; + uint16_t last_id{0}; + uint16_t current_id{0}; + int file_index{0}; + }; -private: enum class LogHandlerState { - Inactive, //There is no active action of log handler - Idle, //The log handler is not sending list/data, but list has been sent - Listing, //File list is being send - SendingData //File Data is being send + Idle, + Listing, + SendingData }; - void _log_message(const mavlink_message_t *msg); - void _log_request_list(const mavlink_message_t *msg); - void _log_request_data(const mavlink_message_t *msg); - void _log_request_erase(const mavlink_message_t *msg); - void _log_request_end(const mavlink_message_t *msg); - - void _reset_list_helper(); - void _init_list_helper(); - bool _get_session_date(const char *path, const char *dir, time_t &date); - void _scan_logs(FILE *f, const char *dir, time_t &date); - bool _get_log_time_size(const char *path, const char *file, time_t &date, uint32_t &size); - static void _delete_all(const char *dir); - bool _get_entry(int idx, uint32_t &size, uint32_t &date, char *filename = 0, int filename_len = 0); - bool _open_for_transmit(); - size_t _get_log_data(uint8_t len, uint8_t *buffer); - void _close_and_unlink_files(); - - size_t _log_send_listing(); - size_t _log_send_data(); - - LogHandlerState _current_status{LogHandlerState::Inactive}; - Mavlink *_mavlink; - - int _next_entry{0}; - int _last_entry{0}; - int _log_count{0}; - - uint16_t _current_log_index{UINT16_MAX}; - uint32_t _current_log_size{0}; - uint32_t _current_log_data_offset{0}; - uint32_t _current_log_data_remaining{0}; - FILE *_current_log_filep{nullptr}; - char _current_log_filename[128]; //TODO: consider to allocate on runtime + + // mavlink message handlers + void handle_log_request_list(const mavlink_message_t *msg); + void handle_log_request_data(const mavlink_message_t *msg); + void handle_log_request_end(const mavlink_message_t *msg); + void handle_log_erase(const mavlink_message_t *msg); + + // state functions + void state_idle(); + void state_listing(); + void state_sending_data(); + + // Log request list + bool create_log_list_file(); + void write_entries_to_file(FILE *f, const char *dir); + void send_log_entry(uint32_t size, uint32_t time_utc); + + // Log request data + bool log_entry_from_id(uint16_t log_id, LogEntry *entry); + + // Log erase + void delete_all_logs(const char *dir); + + +private: + LogHandlerState _state{LogHandlerState::Idle}; + Mavlink &_mavlink; + + // Log list + LogListRequest _list_request{}; + int _num_logs{0}; + bool _logs_listed{false}; + + // Log data + LogEntry _current_entry{}; + LogEntryRequest _entry_request{}; + bool _file_send_finished{}; + + perf_counter_t _create_file_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": create file")}; + perf_counter_t _listing_elapsed{perf_alloc(PC_ELAPSED, MODULE_NAME": listing")}; }; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2467554e97a5..54e1a8549eeb 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -100,7 +100,7 @@ bool Mavlink::_boot_complete = false; Mavlink::Mavlink() : ModuleParams(nullptr), - _receiver(this) + _receiver(*this) { // initialise parameter cache mavlink_update_parameters(); @@ -438,12 +438,12 @@ Mavlink::serial_instance_exists(const char *device_name, Mavlink *self) } bool -Mavlink::component_was_seen(int system_id, int component_id, Mavlink *self) +Mavlink::component_was_seen(int system_id, int component_id, Mavlink &self) { LockGuard lg{mavlink_module_mutex}; for (Mavlink *inst : mavlink_module_instances) { - if (inst && (inst != self) && (inst->_receiver.component_was_seen(system_id, component_id))) { + if (inst && (inst != &self) && (inst->_receiver.component_was_seen(system_id, component_id))) { return true; } } @@ -1057,10 +1057,9 @@ Mavlink::send_statustext_emergency(const char *string) bool Mavlink::send_autopilot_capabilities() { - uORB::Subscription status_sub{ORB_ID(vehicle_status)}; vehicle_status_s status; - if (status_sub.copy(&status)) { + if (_vehicle_status_sub.copy(&status)) { mavlink_autopilot_version_t msg{}; msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; @@ -1440,7 +1439,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 0.5f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.1f); configure_stream_local("WIND_COV", 0.5f); @@ -1454,6 +1452,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) + configure_stream_local("FUEL_STATUS", 1.0f); +#endif // MAVLINK_MSG_ID_FUEL_STATUS #endif // !CONSTRAINED_FLASH break; @@ -1510,7 +1511,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 10.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1524,6 +1524,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) + configure_stream_local("FUEL_STATUS", 1.0f); +#endif // MAVLINK_MSG_ID_FUEL_STATUS #endif // !CONSTRAINED_FLASH break; @@ -1576,7 +1579,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f); configure_stream_local("SYS_STATUS", 5.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1590,6 +1592,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 2.0f); #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) + configure_stream_local("FUEL_STATUS", 1.0f); +#endif // MAVLINK_MSG_ID_FUEL_STATUS #endif // !CONSTRAINED_FLASH break; @@ -1651,6 +1656,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f); configure_stream_local("GPS_RAW_INT", unlimited_rate); configure_stream_local("GPS_STATUS", 1.0f); + configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 0.5f); + configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f); configure_stream_local("HIGHRES_IMU", 50.0f); configure_stream_local("HOME_POSITION", 0.5f); configure_stream_local("HYGROMETER_SENSOR", 1.0f); @@ -1675,7 +1682,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYS_STATUS", 1.0f); configure_stream_local("SYSTEM_TIME", 1.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 20.0f); configure_stream_local("VIBRATION", 2.5f); configure_stream_local("WIND_COV", 10.0f); @@ -1689,12 +1695,15 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) + configure_stream_local("FUEL_STATUS", 2.0f); +#endif // MAVLINK_MSG_ID_FUEL_STATUS #endif // !CONSTRAINED_FLASH break; case MAVLINK_MODE_IRIDIUM: - configure_stream_local("HIGH_LATENCY2", 0.015f); + configure_stream_local("HIGH_LATENCY2", _high_latency_freq); break; case MAVLINK_MODE_MINIMAL: @@ -1760,7 +1769,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) configure_stream_local("SYSTEM_TIME", 2.0f); configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f); configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f); - configure_stream_local("UTM_GLOBAL_POSITION", 1.0f); configure_stream_local("VFR_HUD", 4.0f); configure_stream_local("VIBRATION", 0.5f); configure_stream_local("WIND_COV", 1.0f); @@ -1773,6 +1781,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream) #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f); #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) + configure_stream_local("FUEL_STATUS", 1.0f); +#endif // MAVLINK_MSG_ID_FUEL_STATUS #endif // !CONSTRAINED_FLASH break; @@ -1849,7 +1860,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:c:F:fswxzZp", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { @@ -1969,10 +1980,6 @@ Mavlink::task_main(int argc, char *argv[]) break; #endif -// case 'e': -// _mavlink_link_termination_allowed = true; -// break; - case 'm': { int mode; @@ -2037,6 +2044,26 @@ Mavlink::task_main(int argc, char *argv[]) break; } + case 'F': { + float freq; + + if (px4_get_parameter_value(myoptarg, freq) != 0) { + PX4_ERR("iridium mode frequency parsing failed"); + err_flag = true; + + } else { + if (freq >= 0.f) { + _high_latency_freq = freq; + + } else { + PX4_ERR("Invalid value for iridium mode frequency."); + err_flag = true; + } + } + + break; + } + case 'f': _forwarding_on = true; break; @@ -2080,11 +2107,6 @@ Mavlink::task_main(int argc, char *argv[]) /* USB has no baudrate, but use a magic number for 'fast' */ _baudrate = 2000000; - - if (_mode == MAVLINK_MODE_COUNT) { - _mode = MAVLINK_MODE_CONFIG; - } - _ftp_on = true; _is_usb_uart = true; @@ -2209,11 +2231,24 @@ Mavlink::task_main(int argc, char *argv[]) /* open the UART device after setting the instance, as it might block */ if (get_protocol() == Protocol::SERIAL) { - _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); - if (_uart_fd < 0) { - PX4_ERR("could not open %s", _device_name); - return PX4_ERROR; + // NOTE: we attempt to open the port multiple times due to sercon returning before + // the port is ready to be opened. This avoids needing to sleep() after sercon_main. + int attempts = 0; + static const int max_attempts = 3; + + while (_uart_fd < 0) { + _uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control); + attempts++; + + if (_uart_fd < 0 && attempts < max_attempts) { + PX4_ERR("could not open %s, retrying", _device_name); + px4_usleep(1_s); + + } else if (_uart_fd < 0) { + PX4_ERR("failed to open %s after %d attempts, exiting!", _device_name, attempts); + return PX4_ERROR; + } } } @@ -2247,6 +2282,9 @@ Mavlink::task_main(int argc, char *argv[]) if (!should_transmit()) { check_requested_subscriptions(); + handleStatus(); + handleCommands(); + handleAndGetCurrentCommandAck(); continue; } @@ -2269,157 +2307,9 @@ Mavlink::task_main(int argc, char *argv[]) configure_sik_radio(); - if (_vehicle_status_sub.updated()) { - vehicle_status_s vehicle_status; - - if (_vehicle_status_sub.copy(&vehicle_status)) { - /* switch HIL mode if required */ - set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); - - if (_mode == MAVLINK_MODE_IRIDIUM) { - - if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost && - !_transmitting_enabled_commanded && _first_heartbeat_sent) { - - _transmitting_enabled = false; - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name); - events::send(events::ID("mavlink_iridium_disable"), events::Log::Info, - "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); - - } else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) { - _transmitting_enabled = true; - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name); - events::send(events::ID("mavlink_iridium_enable"), events::Log::Info, - "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); - } - } - } - } - - - // MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY - if (_mode == MAVLINK_MODE_IRIDIUM) { - int vehicle_command_updates = 0; - - while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { - vehicle_command_updates++; - const unsigned last_generation = _vehicle_command_sub.get_last_generation(); - vehicle_command_s vehicle_cmd; - - if (_vehicle_command_sub.update(&vehicle_cmd)) { - if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); - } - - if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && - _mode == MAVLINK_MODE_IRIDIUM) { - - if (vehicle_cmd.param1 > 0.5f) { - if (!_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, - "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = true; - _transmitting_enabled_commanded = true; - - } else { - if (_transmitting_enabled) { - mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", - _device_name); - events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, - "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); - } - - _transmitting_enabled = false; - _transmitting_enabled_commanded = false; - } - - // send positive command ack - vehicle_command_ack_s command_ack{}; - command_ack.command = vehicle_cmd.command; - command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - command_ack.from_external = !vehicle_cmd.from_external; - command_ack.target_system = vehicle_cmd.source_system; - command_ack.target_component = vehicle_cmd.source_component; - command_ack.timestamp = vehicle_cmd.timestamp; - _vehicle_command_ack_pub.publish(command_ack); - } - } - } - } - - /* send command ACK */ - bool cmd_logging_start_acknowledgement = false; - bool cmd_logging_stop_acknowledgement = false; - - if (_vehicle_command_ack_sub.updated()) { - static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) { - vehicle_command_ack_s command_ack; - const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation(); - - if (_vehicle_command_ack_sub.update(&command_ack)) { - if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) { - PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation, - _vehicle_command_ack_sub.get_last_generation()); - } - - const bool is_target_known = _receiver.component_was_seen(command_ack.target_system, command_ack.target_component); - - if (!command_ack.from_external - && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START - && is_target_known - && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) { - - mavlink_command_ack_t msg{}; - msg.result = command_ack.result; - msg.command = command_ack.command; - msg.progress = command_ack.result_param1; - msg.result_param2 = command_ack.result_param2; - msg.target_system = command_ack.target_system; - msg.target_component = command_ack.target_component; - - mavlink_msg_command_ack_send_struct(get_channel(), &msg); - - if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { - cmd_logging_start_acknowledgement = true; - - } else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP - && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { - cmd_logging_stop_acknowledgement = true; - } - } - } - } - } - - // For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands. - // We don't care about acks for these though. - if (_gimbal_v1_command_sub.updated()) { - vehicle_command_s cmd; - _gimbal_v1_command_sub.copy(&cmd); - - // FIXME: filter for target system/component - - mavlink_command_long_t msg{}; - msg.param1 = cmd.param1; - msg.param2 = cmd.param2; - msg.param3 = cmd.param3; - msg.param4 = cmd.param4; - msg.param5 = cmd.param5; - msg.param6 = cmd.param6; - msg.param7 = cmd.param7; - msg.command = cmd.command; - msg.target_system = cmd.target_system; - msg.target_component = cmd.target_component; - msg.confirmation = 0; - - mavlink_msg_command_long_send_struct(get_channel(), &msg); - } + handleStatus(); + handleCommands(); + handleAndGetCurrentCommandAck(); /* check for shell output */ if (_mavlink_shell && _mavlink_shell->available() > 0) { @@ -2458,25 +2348,15 @@ Mavlink::task_main(int argc, char *argv[]) /* check for ulog streaming messages */ if (_mavlink_ulog) { - if (cmd_logging_stop_acknowledgement) { - _mavlink_ulog->stop(); - _mavlink_ulog = nullptr; + const int ret = _mavlink_ulog->handle_update(get_channel()); - } else { - if (cmd_logging_start_acknowledgement) { - _mavlink_ulog->start_ack_received(); + if (ret < 0) { // abort the streaming on error + if (ret != -1) { + PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); } - int ret = _mavlink_ulog->handle_update(get_channel()); - - if (ret < 0) { //abort the streaming on error - if (ret != -1) { - PX4_WARN("mavlink ulog stream update failed, stopping (%i)", ret); - } - - _mavlink_ulog->stop(); - _mavlink_ulog = nullptr; - } + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; } } @@ -2588,6 +2468,177 @@ Mavlink::task_main(int argc, char *argv[]) return OK; } +void Mavlink::handleStatus() +{ + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.copy(&vehicle_status)) { + /* switch HIL mode if required */ + set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); + + if (_mode == MAVLINK_MODE_IRIDIUM) { + + if (_transmitting_enabled && (!vehicle_status.gcs_connection_lost || (vehicle_status.high_latency_data_link_lost && + !_transmitting_enabled_commanded && _first_heartbeat_sent))) { + + _transmitting_enabled = false; + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name); + events::send(events::ID("mavlink_iridium_disable"), events::Log::Info, + "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); + + } else if (!_transmitting_enabled && vehicle_status.gcs_connection_lost + && !vehicle_status.high_latency_data_link_lost) { + _transmitting_enabled = true; + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name); + events::send(events::ID("mavlink_iridium_enable"), events::Log::Info, + "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id); + } + } + } + } +} + +void Mavlink::handleCommands() +{ + if (_mode == MAVLINK_MODE_IRIDIUM) { + int vehicle_command_updates = 0; + + while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) { + vehicle_command_updates++; + const unsigned last_generation = _vehicle_command_sub.get_last_generation(); + vehicle_command_s vehicle_cmd; + + if (_vehicle_command_sub.update(&vehicle_cmd)) { + if (_vehicle_command_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation()); + } + + if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) && + _mode == MAVLINK_MODE_IRIDIUM) { + + if (vehicle_cmd.param1 > 0.5f) { + if (!_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info, + "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); + } + + _transmitting_enabled = true; + _transmitting_enabled_commanded = true; + + } else { + if (_transmitting_enabled) { + mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t", + _device_name); + events::send(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info, + "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id); + } + + _transmitting_enabled = false; + _transmitting_enabled_commanded = false; + } + + // send positive command ack + vehicle_command_ack_s command_ack{}; + command_ack.command = vehicle_cmd.command; + command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + command_ack.from_external = !vehicle_cmd.from_external; + command_ack.target_system = vehicle_cmd.source_system; + command_ack.target_component = vehicle_cmd.source_component; + command_ack.timestamp = vehicle_cmd.timestamp; + _vehicle_command_ack_pub.publish(command_ack); + } + } + } + } + + // For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands. + // We don't care about acks for these though. + if (_gimbal_v1_command_sub.updated()) { + vehicle_command_s cmd; + _gimbal_v1_command_sub.copy(&cmd); + + // FIXME: filter for target system/component + + mavlink_command_long_t msg{}; + msg.param1 = cmd.param1; + msg.param2 = cmd.param2; + msg.param3 = cmd.param3; + msg.param4 = cmd.param4; + msg.param5 = cmd.param5; + msg.param6 = cmd.param6; + msg.param7 = cmd.param7; + msg.command = cmd.command; + msg.target_system = cmd.target_system; + msg.target_component = cmd.target_component; + msg.confirmation = 0; + + mavlink_msg_command_long_send_struct(get_channel(), &msg); + } +} + +void Mavlink::handleAndGetCurrentCommandAck() +{ + if (_vehicle_command_ack_sub.updated()) { + static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + + while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) { + vehicle_command_ack_s command_ack; + const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation(); + + if (_vehicle_command_ack_sub.update(&command_ack)) { + if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) { + PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation, + _vehicle_command_ack_sub.get_last_generation()); + } + + const bool is_target_known = _receiver.component_was_seen(command_ack.target_system, command_ack.target_component); + + if (!command_ack.from_external + && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START + && is_target_known + && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) { + + mavlink_command_ack_t msg{}; + msg.result = command_ack.result; + msg.command = command_ack.command; + msg.progress = command_ack.result_param1; + msg.result_param2 = command_ack.result_param2; + msg.target_system = command_ack.target_system; + msg.target_component = command_ack.target_component; + + // Handle logging acks before sending out the mavlink message to prevent a race condition + if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) { + if (_mavlink_ulog) { + _mavlink_ulog->start_ack_received(); + } + + } else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP + && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) { + if (_mavlink_ulog) { + _mavlink_ulog->stop(); + _mavlink_ulog = nullptr; + } + } + + if (_mode == MAVLINK_MODE_IRIDIUM) { + if (command_ack.from_external) { + // for MAVLINK_MODE_IRIDIUM send only if external + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + } + + } else { + mavlink_msg_command_ack_send_struct(get_channel(), &msg); + } + + } + } + } + } +} + void Mavlink::check_requested_subscriptions() { if (_subscribe_to_stream != nullptr) { @@ -2888,6 +2939,11 @@ Mavlink::display_status() _ftp_on ? "YES" : "NO", _transmitting_enabled ? "YES" : "NO"); printf("\tmode: %s\n", mavlink_mode_str(_mode)); + + if (_mode == MAVLINK_MODE_IRIDIUM) { + printf("\t iridium tx freq: %.3f\n", (double)(_high_latency_freq)); + } + printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off"); printf("\tMAVLink version: %" PRId32 "\n", _protocol_version); @@ -3268,6 +3324,7 @@ Start mavlink on UDP port 14556 and enable the HIGHRES_IMU message with 50Hz: #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_FLOAT('F', 0.015, 0.0, 50.0, "Sets the transmission frequency for iridium mode", true); 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_main.h b/src/modules/mavlink/mavlink_main.h index 1ba71c6a2ba1..d7a7307b3ebf 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -172,7 +172,7 @@ class Mavlink final : public ModuleParams static bool serial_instance_exists(const char *device_name, Mavlink *self); - static bool component_was_seen(int system_id, int component_id, Mavlink *self = nullptr); + static bool component_was_seen(int system_id, int component_id, Mavlink &self); static void forward_message(const mavlink_message_t *msg, Mavlink *self); @@ -584,6 +584,7 @@ class Mavlink final : public ModuleParams int _baudrate{57600}; int _datarate{1000}; ///< data rate for normal streams (attitude, position, etc.) float _rate_mult{1.0f}; + float _high_latency_freq{0.015f}; ///< frequency of HIGH_LATENCY2 stream bool _radio_status_available{false}; bool _radio_status_critical{false}; @@ -596,8 +597,6 @@ class Mavlink final : public ModuleParams */ unsigned int _mavlink_param_queue_index{0}; - bool _mavlink_link_termination_allowed{false}; - char *_subscribe_to_stream{nullptr}; float _subscribe_to_stream_rate{0.0f}; ///< rate of stream to subscribe to (0=disable, -1=unlimited, -2=default) bool _udp_initialised{false}; @@ -707,6 +706,12 @@ class Mavlink final : public ModuleParams void check_requested_subscriptions(); + void handleCommands(); + + void handleAndGetCurrentCommandAck(); + + void handleStatus(); + /** * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID * diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d9a9dc213910..638de9cce5f4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -123,6 +123,9 @@ #if defined(MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS) #include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp" #endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS +#if defined(MAVLINK_MSG_ID_FUEL_STATUS) +#include "streams/FUEL_STATUS.hpp" +#endif // MAVLINK_MSG_ID_FUEL_STATUS #ifdef MAVLINK_MSG_ID_AVAILABLE_MODES // Only defined if development.xml is used #include "streams/AVAILABLE_MODES.hpp" @@ -132,6 +135,7 @@ #if !defined(CONSTRAINED_FLASH) # include "streams/ADSB_VEHICLE.hpp" # include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp" +# include "streams/BATTERY_INFO.hpp" # include "streams/DEBUG.hpp" # include "streams/DEBUG_FLOAT_ARRAY.hpp" # include "streams/DEBUG_VECT.hpp" @@ -147,10 +151,8 @@ # include "streams/ODOMETRY.hpp" # include "streams/SCALED_PRESSURE2.hpp" # include "streams/SCALED_PRESSURE3.hpp" -# include "streams/SMART_BATTERY_INFO.hpp" # include "streams/UAVIONIX_ADSB_OUT_CFG.hpp" # include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp" -# include "streams/UTM_GLOBAL_POSITION.hpp" #endif // !CONSTRAINED_FLASH // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align @@ -259,9 +261,9 @@ static const StreamListItem streams_list[] = { create_stream_list_item(), #endif // SYS_STATUS_HPP create_stream_list_item(), -#if defined(SMART_BATTERY_INFO_HPP) - create_stream_list_item(), -#endif // SMART_BATTERY_INFO_HPP +#if defined(BATTERY_INFO_HPP) + create_stream_list_item(), +#endif // BATTERY_INFO_HPP #if defined(HIGHRES_IMU_HPP) create_stream_list_item(), #endif // HIGHRES_IMU_HPP @@ -419,9 +421,6 @@ static const StreamListItem streams_list[] = { #if defined(ADSB_VEHICLE_HPP) create_stream_list_item(), #endif // ADSB_VEHICLE_HPP -#if defined(UTM_GLOBAL_POSITION_HPP) - create_stream_list_item(), -#endif // UTM_GLOBAL_POSITION_HPP #if defined(COLLISION_HPP) create_stream_list_item(), #endif // COLLISION_HPP @@ -473,6 +472,9 @@ static const StreamListItem streams_list[] = { #if defined(FLIGHT_INFORMATION_HPP) create_stream_list_item(), #endif // FLIGHT_INFORMATION_HPP +#if defined(FUEL_STATUS_HPP) + create_stream_list_item(), +#endif // FUEL_STATUS_HPP #if defined(GPS_STATUS_HPP) create_stream_list_item(), #endif // GPS_STATUS_HPP diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 5057c1625dbe..1c5437270a1e 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -72,7 +72,7 @@ constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ (_msg.target_component == MAV_COMP_ID_ALL))) -MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : +MavlinkMissionManager::MavlinkMissionManager(Mavlink &mavlink) : _mavlink(mavlink) { if (!_dataman_init) { @@ -209,7 +209,7 @@ MavlinkMissionManager::update_geofence_count(dm_item_t fence_dataman_id, unsigne } else { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); + _mavlink.send_statustext_critical("Mission storage: Unable to write to microSD\t"); events::send(events::ID("mavlink_mission_storage_write_failure2"), events::Log::Critical, "Mission: Unable to write to storage"); } @@ -245,7 +245,7 @@ MavlinkMissionManager::update_safepoint_count(dm_item_t safepoint_dataman_id, un } else { if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); + _mavlink.send_statustext_critical("Mission storage: Unable to write to microSD\t"); events::send(events::ID("mavlink_mission_storage_write_failure3"), events::Log::Critical, "Mission: Unable to write to storage"); } @@ -270,7 +270,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t wpa.mission_type = _mission_type; wpa.opaque_id = opaque_id; - mavlink_msg_mission_ack_send_struct(_mavlink->get_channel(), &wpa); + mavlink_msg_mission_ack_send_struct(_mavlink.get_channel(), &wpa); PX4_DEBUG("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } @@ -284,7 +284,7 @@ MavlinkMissionManager::send_mission_current(uint16_t seq) wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION]; wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE]; wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY]; - mavlink_msg_mission_current_send_struct(_mavlink->get_channel(), &wpc); + mavlink_msg_mission_current_send_struct(_mavlink.get_channel(), &wpc); PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq); } @@ -303,7 +303,7 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_ wpc.mission_type = mission_type; wpc.opaque_id = opaque_id; - mavlink_msg_mission_count_send_struct(_mavlink->get_channel(), &wpc); + mavlink_msg_mission_count_send_struct(_mavlink.get_channel(), &wpc); PX4_DEBUG("WPM: Send MISSION_COUNT %u to ID %u, mission type=%i", wpc.count, wpc.target_system, mission_type); } @@ -350,7 +350,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t break; default: - _mavlink->send_statustext_critical("Received unknown mission type, abort.\t"); + _mavlink.send_statustext_critical("Received unknown mission type, abort.\t"); events::send(events::ID("mavlink_mission_recv_unknown_mis_type"), events::Log::Error, "Received unknown mission type, abort"); break; @@ -368,7 +368,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_int_send_struct(_mavlink->get_channel(), &wp); + mavlink_msg_mission_item_int_send_struct(_mavlink.get_channel(), &wp); PX4_DEBUG("WPM: Send MISSION_ITEM_INT seq %u to ID %u", wp.seq, wp.target_system); @@ -381,7 +381,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t wp.seq = seq; wp.current = (_current_seq == seq) ? 1 : 0; - mavlink_msg_mission_item_send_struct(_mavlink->get_channel(), &wp); + mavlink_msg_mission_item_send_struct(_mavlink.get_channel(), &wp); PX4_DEBUG("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } @@ -390,7 +390,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - mavlink_log_critical(_mavlink->get_mavlink_log_pub(), + mavlink_log_critical(_mavlink.get_mavlink_log_pub(), "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type); /* EVENT * @description Mission type: {1} @@ -448,7 +448,7 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 wpr.target_component = compid; wpr.seq = seq; wpr.mission_type = _mission_type; - mavlink_msg_mission_request_int_send_struct(_mavlink->get_channel(), &wpr); + mavlink_msg_mission_request_int_send_struct(_mavlink.get_channel(), &wpr); PX4_DEBUG("WPM: Send MISSION_REQUEST_INT seq %u to ID %u", wpr.seq, wpr.target_system); @@ -460,13 +460,13 @@ MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint1 wpr.seq = seq; wpr.mission_type = _mission_type; - mavlink_msg_mission_request_send_struct(_mavlink->get_channel(), &wpr); + mavlink_msg_mission_request_send_struct(_mavlink.get_channel(), &wpr); PX4_DEBUG("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } } else { - _mavlink->send_statustext_critical("ERROR: Waypoint index exceeds list capacity\t"); + _mavlink.send_statustext_critical("ERROR: Waypoint index exceeds list capacity\t"); events::send(events::ID("mavlink_mission_wp_index_exceeds_list"), events::Log::Error, "Waypoint index eceeds list capacity (maximum: {1})", current_max_item_count()); @@ -481,7 +481,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_mission_item_reached_send_struct(_mavlink->get_channel(), &wp_reached); + mavlink_msg_mission_item_reached_send_struct(_mavlink.get_channel(), &wp_reached); PX4_DEBUG("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } @@ -490,7 +490,7 @@ void MavlinkMissionManager::send() { // do not send anything over high latency communication - if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { + if (_mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { return; } @@ -509,7 +509,7 @@ MavlinkMissionManager::send() send_mission_current(_current_seq); } else { - _mavlink->send_statustext_critical("ERROR: wp index out of bounds\t"); + _mavlink.send_statustext_critical("ERROR: wp index out of bounds\t"); events::send(events::ID("mavlink_mission_wp_index_out_of_bounds"), events::Log::Error, "Waypoint index out of bounds (current {1} \\>= total {2})", mission_result.seq_current, mission_result.seq_total); } @@ -562,7 +562,7 @@ MavlinkMissionManager::send() } else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0) && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) { - _mavlink->send_statustext_critical("Operation timeout\t"); + _mavlink.send_statustext_critical("Operation timeout\t"); events::send(events::ID("mavlink_mission_op_timeout"), events::Log::Error, "Operation timeout, aborting transfer"); @@ -674,7 +674,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch\t"); + _mavlink.send_statustext_critical("REJ. WP CMD: partner id mismatch\t"); events::send(events::ID("mavlink_mission_partner_id_mismatch"), events::Log::Error, "Rejecting waypoint command, component or system ID mismatch"); @@ -699,7 +699,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); - _mavlink->send_statustext_critical("WPM: WP CURR CMD: Not in list\t"); + _mavlink.send_statustext_critical("WPM: WP CURR CMD: Not in list\t"); events::send(events::ID("mavlink_mission_seq_out_of_bounds"), events::Log::Error, "New mission waypoint sequence out of bounds"); } @@ -707,7 +707,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_SET_CURRENT ERROR: busy"); - _mavlink->send_statustext_critical("WPM: IGN WP CURR CMD: Busy\t"); + _mavlink.send_statustext_critical("WPM: IGN WP CURR CMD: Busy\t"); events::send(events::ID("mavlink_mission_state_busy"), events::Log::Error, "Mission manager currently busy, ignoring new waypoint index"); } @@ -768,7 +768,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_REQUEST_LIST ERROR: busy"); - _mavlink->send_statustext_info("Mission download request ignored, already active\t"); + _mavlink.send_statustext_info("Mission download request ignored, already active\t"); events::send(events::ID("mavlink_mission_req_ignored"), events::Log::Warning, "Mission download request ignored, already active"); } @@ -843,7 +843,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) switch_to_idle_state(); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); + _mavlink.send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); events::send(events::ID("mavlink_mission_wp_unexpected"), events::Log::Error, "Unexpected waypoint index, aborting transfer"); return; @@ -860,7 +860,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) switch_to_idle_state(); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); - _mavlink->send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); + _mavlink.send_statustext_critical("WPM: REJ. CMD: Req. WP was unexpected\t"); events::send(events::ID("mavlink_mission_wp_unexpected2"), events::Log::Error, "Unexpected waypoint index, aborting mission transfer"); } @@ -869,18 +869,18 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg) PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: no transfer"); // Silently ignore this as some OSDs have buggy mission protocol implementations - //_mavlink->send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer"); + //_mavlink.send_statustext_critical("IGN MISSION_ITEM_REQUEST(_INT): No active transfer"); } else { PX4_DEBUG("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: busy (state %d).", _state); - _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy\t"); + _mavlink.send_statustext_critical("WPM: REJ. CMD: Busy\t"); events::send(events::ID("mavlink_mission_mis_req_ignored_busy"), events::Log::Error, "Ignoring mission request, currently busy"); } } else { - _mavlink->send_statustext_critical("WPM: REJ. CMD: partner id mismatch\t"); + _mavlink.send_statustext_critical("WPM: REJ. CMD: partner id mismatch\t"); events::send(events::ID("mavlink_mission_partner_id_mismatch2"), events::Log::Error, "Rejecting mission request command, component or system ID mismatch"); @@ -1000,7 +1000,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); - _mavlink->send_statustext_critical("WPM: REJ. CMD: Busy\t"); + _mavlink.send_statustext_critical("WPM: REJ. CMD: Busy\t"); events::send(events::ID("mavlink_mission_getlist_busy"), events::Log::Error, "Mission upload busy, already receiving waypoint"); @@ -1011,7 +1011,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_COUNT ERROR: busy, state %i", _state); - _mavlink->send_statustext_critical("WPM: IGN MISSION_COUNT: Busy\t"); + _mavlink.send_statustext_critical("WPM: IGN MISSION_COUNT: Busy\t"); events::send(events::ID("mavlink_mission_ignore_mis_count"), events::Log::Error, "Mission upload busy, ignoring MISSION_COUNT"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1088,7 +1088,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_ITEM ERROR: no transfer"); - _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer\t"); + _mavlink.send_statustext_critical("IGN MISSION_ITEM: No transfer\t"); events::send(events::ID("mavlink_mission_no_transfer"), events::Log::Error, "Ignoring mission item, no transfer in progress"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1099,7 +1099,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) } else { PX4_DEBUG("WPM: MISSION_ITEM ERROR: busy, state %i", _state); - _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy\t"); + _mavlink.send_statustext_critical("IGN MISSION_ITEM: Busy\t"); events::send(events::ID("mavlink_mission_mis_item_busy"), events::Log::Error, "Ignoring mission item, busy"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); @@ -1113,7 +1113,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) if (ret != PX4_OK) { PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); - _mavlink->send_statustext_critical("IGN MISSION_ITEM: Invalid item\t"); + _mavlink.send_statustext_critical("IGN MISSION_ITEM: Invalid item\t"); events::send(events::ID("mavlink_mission_mis_item_invalid"), events::Log::Error, "Ignoring mission item, invalid item"); @@ -1208,7 +1208,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) break; default: - _mavlink->send_statustext_critical("Received unknown mission type, abort.\t"); + _mavlink.send_statustext_critical("Received unknown mission type, abort.\t"); events::send(events::ID("mavlink_mission_unknown_mis_type"), events::Log::Error, "Received unknown mission type, abort"); break; @@ -1220,7 +1220,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); if (write_failed) { - _mavlink->send_statustext_critical("Unable to write on micro SD\t"); + _mavlink.send_statustext_critical("Unable to write on micro SD\t"); events::send(events::ID("mavlink_mission_storage_failure"), events::Log::Error, "Mission: unable to write to storage"); } @@ -1361,7 +1361,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) } } else { - _mavlink->send_statustext_critical("WPM: IGN CLEAR CMD: Busy\t"); + _mavlink.send_statustext_critical("WPM: IGN CLEAR CMD: Busy\t"); events::send(events::ID("mavlink_mission_ignore_clear"), events::Log::Error, "Ignoring mission clear command, busy"); @@ -1839,7 +1839,7 @@ void MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(struct miss void MavlinkMissionManager::check_active_mission() { // do not send anything over high latency communication - if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { + if (_mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) { return; } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 2251ef9e61da..0b5c6e25d815 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -77,7 +77,7 @@ class Mavlink; class MavlinkMissionManager { public: - explicit MavlinkMissionManager(Mavlink *mavlink); + explicit MavlinkMissionManager(Mavlink &mavlink); ~MavlinkMissionManager() = default; @@ -146,7 +146,7 @@ class MavlinkMissionManager MavlinkRateLimiter _slow_rate_limiter{1000 * 1000}; ///< Rate limit sending of the current WP sequence to 1 Hz - Mavlink *_mavlink; + Mavlink &_mavlink; static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT = 2; ///< Error count limit before stopping to report FS errors diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index c7332bff7bdc..2d8c47da823d 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -46,7 +46,7 @@ #include "mavlink_main.h" #include -MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : +MavlinkParametersManager::MavlinkParametersManager(Mavlink &mavlink) : _mavlink(mavlink) { } @@ -111,7 +111,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) /* Whatever the value is, we're being told to stop sending */ if (strncmp(name, "_HASH_CHECK", sizeof(name)) == 0) { - if (_mavlink->hash_check_enabled()) { + if (_mavlink.hash_check_enabled()) { _send_all_index = -1; } @@ -189,7 +189,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); param_value.param_type = MAV_PARAM_TYPE_UINT32; memcpy(¶m_value.param_value, &hash, sizeof(hash)); - mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value); + mavlink_msg_param_value_send_struct(_mavlink.get_channel(), ¶m_value); } else { /* local name buffer to enforce null-terminated string */ @@ -249,7 +249,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) size_t i = map_rc.parameter_rc_channel_index; if (i >= sizeof(_rc_param_map.param_index) / sizeof(_rc_param_map.param_index[0])) { - mavlink_log_warning(_mavlink->get_mavlink_log_pub(), "parameter_rc_channel_index out of bounds\t"); + mavlink_log_warning(_mavlink.get_mavlink_log_pub(), "parameter_rc_channel_index out of bounds\t"); events::send(events::ID("mavlink_param_rc_chan_out_of_bounds"), events::Log::Warning, "parameter_rc_channel_index out of bounds"); break; @@ -292,10 +292,6 @@ MavlinkParametersManager::send() param_find("BAT_CRIT_THR"); param_find("BAT_EMERGEN_THR"); param_find("BAT_LOW_THR"); - param_find("BAT_N_CELLS"); // deprecated - param_find("BAT_V_CHARGED"); // deprecated - param_find("BAT_V_EMPTY"); // deprecated - param_find("BAT_V_LOAD_DROP"); // deprecated param_find("CAL_ACC0_ID"); param_find("CAL_GYRO0_ID"); param_find("CAL_MAG0_ID"); @@ -314,12 +310,15 @@ MavlinkParametersManager::send() param_find("TRIG_MODE"); param_find("UAVCAN_ENABLE"); + // parameter only used in startup script but should show on ground station + param_find("SYS_PARAM_VER"); + _first_send = true; } int max_num_to_send; - if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) { + if (_mavlink.get_protocol() == Protocol::SERIAL && !_mavlink.is_usb_uart()) { max_num_to_send = 3; } else { @@ -330,7 +329,7 @@ MavlinkParametersManager::send() int i = 0; // Send while burst is not exceeded, we still have buffer space and still something to send - while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + while ((i++ < max_num_to_send) && (_mavlink.get_free_tx_buf() >= get_size()) && !_mavlink.radio_status_critical() && send_params()) {} } @@ -394,7 +393,7 @@ MavlinkParametersManager::send_untransmitted() break; } } - } while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical() + } while ((_mavlink.get_free_tx_buf() >= get_size()) && !_mavlink.radio_status_critical() && (_param_update_index < (int) param_count())); // Flag work as done once all params have been sent @@ -426,7 +425,7 @@ MavlinkParametersManager::send_one() strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); msg.param_type = MAV_PARAM_TYPE_UINT32; memcpy(&msg.param_value, &hash, sizeof(hash)); - mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); + mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &msg); /* after this we should start sending all params */ _send_all_index = 0; @@ -468,7 +467,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id) } /* no free TX buf to send this param */ - if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) { + if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) { return 1; } @@ -535,13 +534,13 @@ MavlinkParametersManager::send_param(param_t param, int component_id) /* default component ID */ if (component_id < 0) { - mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg); + mavlink_msg_param_value_send_struct(_mavlink.get_channel(), &msg); } else { // Re-pack the message with a different component ID mavlink_message_t mavlink_packet; - mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink->get_channel(), &mavlink_packet, &msg); - _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); + mavlink_msg_param_value_encode_chan(mavlink_system.sysid, component_id, _mavlink.get_channel(), &mavlink_packet, &msg); + _mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet); } return 0; @@ -595,9 +594,9 @@ bool MavlinkParametersManager::send_uavcan() // Re-pack the message with the UAVCAN node ID mavlink_message_t mavlink_packet{}; - mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink->get_channel(), &mavlink_packet, + mavlink_msg_param_value_encode_chan(mavlink_system.sysid, value.node_id, _mavlink.get_channel(), &mavlink_packet, &msg); - _mavlink_resend_uart(_mavlink->get_channel(), &mavlink_packet); + _mavlink_resend_uart(_mavlink.get_channel(), &mavlink_packet); return true; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index bf8d48d78506..c1042d0e71b5 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -64,7 +64,7 @@ class Mavlink; class MavlinkParametersManager { public: - explicit MavlinkParametersManager(Mavlink *mavlink); + explicit MavlinkParametersManager(Mavlink &mavlink); ~MavlinkParametersManager() = default; /** @@ -159,7 +159,7 @@ class MavlinkParametersManager hrt_abstime _param_update_time{0}; int _param_update_index{0}; - Mavlink *_mavlink; + Mavlink &_mavlink; bool _first_send{false}; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6030d9b651ba..58d2c044ccd0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,7 +106,7 @@ static constexpr vehicle_odometry_s vehicle_odometry_empty { .quality = 0 }; -MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : +MavlinkReceiver::MavlinkReceiver(Mavlink &parent) : ModuleParams(nullptr), _mavlink(parent), _mavlink_ftp(parent), @@ -224,10 +224,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_adsb_vehicle(msg); break; - case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION: - handle_message_utm_global_position(msg); - break; - case MAVLINK_MSG_ID_COLLISION: handle_message_collision(msg); break; @@ -348,7 +344,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) * Accept HIL GPS messages if use_hil_gps flag is true. * This allows to provide fake gps measurements to the system. */ - if (_mavlink->get_hil_enabled()) { + if (_mavlink.get_hil_enabled()) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_SENSOR: handle_message_hil_sensor(msg); @@ -368,7 +364,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + if (_mavlink.get_hil_enabled() || (_mavlink.get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_GPS: handle_message_hil_gps(msg); @@ -380,9 +376,62 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - /* If we've received a valid message, mark the flag indicating so. - This is used in the '-w' command-line flag. */ - _mavlink->set_has_received_messages(true); + /* handle packet with mission manager */ + _mission_manager.handle_message(msg); + + /* handle packet with parameter component */ + if (_mavlink.boot_complete()) { + // make sure mavlink app has booted before we start processing parameter sync + _parameters_manager.handle_message(msg); + + } else { + if (hrt_elapsed_time(&_mavlink.get_first_start_time()) > 20_s) { + PX4_ERR("system boot did not complete in 20 seconds"); + _mavlink.set_boot_complete(); + } + } + + if (_mavlink.ftp_enabled()) { + /* handle packet with ftp component */ + _mavlink_ftp.handle_message(msg); + } + + /* handle packet with log component */ + _mavlink_log_handler.handle_message(msg); + + /* handle packet with timesync component */ + _mavlink_timesync.handle_message(msg); + + /* handle packet with parent object */ + _mavlink.handle_message(msg); +} + +void MavlinkReceiver::handle_messages_in_gimbal_mode(mavlink_message_t &msg) +{ + switch (msg.msgid) { + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE: + handle_message_gimbal_manager_set_attitude(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL: + handle_message_gimbal_manager_set_manual_control(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION: + handle_message_gimbal_device_information(&msg); + break; + + case MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS: + handle_message_gimbal_device_attitude_status(&msg); + break; + } + + // Message forwarding + _mavlink.handle_message(&msg); } bool @@ -506,8 +555,8 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const if (!target_ok) { // Reject alien commands only if there is no forwarding or we've never seen target component before - if (!_mavlink->get_forwarding_on() - || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { + if (!_mavlink.get_forwarding_on() + || !_mavlink.component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) { acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED); } @@ -547,7 +596,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const vehicle_command.param5, vehicle_command.param6, vehicle_command.param7); } else if (cmd_mavlink.command == MAV_CMD_INJECT_FAILURE) { - if (_mavlink->failure_injection_enabled()) { + if (_mavlink.failure_injection_enabled()) { _cmd_pub.publish(vehicle_command); send_ack = false; @@ -673,19 +722,19 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const // check that we have enough bandwidth available: this is given by the configured logger topics // and rates. The 5000 is somewhat arbitrary, but makes sure that we cannot enable log streaming // on a radio link - if (_mavlink->get_data_rate() < 5000) { + if (_mavlink.get_data_rate() < 5000) { send_ack = true; result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; - _mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming\t"); + _mavlink.send_statustext_critical("Not enough bandwidth to enable log streaming\t"); events::send(events::ID("mavlink_log_not_enough_bw"), events::Log::Error, - "Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink->get_data_rate()); + "Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink.get_data_rate()); } else { // we already instanciate the streaming object, because at this point we know on which // mavlink channel streaming was requested. But in fact it's possible that the logger is // not even running. The main mavlink thread takes care of this by waiting for an ack // from the logger. - _mavlink->try_start_ulog_streaming(msg->sysid, msg->compid); + _mavlink.try_start_ulog_streaming(msg->sysid, msg->compid); } } @@ -705,7 +754,7 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo bool stream_found = false; bool message_sent = false; - for (const auto &stream : _mavlink->get_streams()) { + for (const auto &stream : _mavlink.get_streams()) { if (stream->get_id() == message_id) { stream_found = true; message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); @@ -718,10 +767,10 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo const char *stream_name = get_stream_name(message_id); if (stream_name != nullptr) { - _mavlink->configure_stream_threadsafe(stream_name, 0.0f); + _mavlink.configure_stream_threadsafe(stream_name, 0.0f); // Now we try again to send it. - for (const auto &stream : _mavlink->get_streams()) { + for (const auto &stream : _mavlink.get_streams()) { if (stream->get_id() == message_id) { message_sent = stream->request_message(param2, param3, param4, param5, param6, param7); break; @@ -741,7 +790,7 @@ MavlinkReceiver::handle_message_command_ack(mavlink_message_t *msg) mavlink_command_ack_t ack; mavlink_msg_command_ack_decode(msg, &ack); - MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink->get_channel()); + MavlinkCommandSender::instance().handle_mavlink_command_ack(ack, msg->sysid, msg->compid, _mavlink.get_channel()); vehicle_command_ack_s command_ack{}; @@ -772,7 +821,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.bus = _mavlink.get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_MAVLINK; @@ -818,7 +867,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.bus = _mavlink.get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM; @@ -895,7 +944,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg) device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.bus = _mavlink.get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK; @@ -967,7 +1016,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t mavlink_msg_set_position_target_local_ned_decode(msg, &target_local_ned); /* Only accept messages which are intended for this system */ - if (_mavlink->get_forward_externalsp() && + if (_mavlink.get_forward_externalsp() && (mavlink_system.sysid == target_local_ned.target_system || target_local_ned.target_system == 0) && (mavlink_system.compid == target_local_ned.target_component || target_local_ned.target_component == 0)) { @@ -1087,7 +1136,7 @@ MavlinkReceiver::handle_message_set_position_target_global_int(mavlink_message_t mavlink_msg_set_position_target_global_int_decode(msg, &target_global_int); /* Only accept messages which are intended for this system */ - if (_mavlink->get_forward_externalsp() && + if (_mavlink.get_forward_externalsp() && (mavlink_system.sysid == target_global_int.target_system || target_global_int.target_system == 0) && (mavlink_system.compid == target_global_int.target_component || target_global_int.target_component == 0)) { @@ -1203,13 +1252,13 @@ MavlinkReceiver::handle_message_set_gps_global_origin(mavlink_message_t *msg) mavlink_set_gps_global_origin_t gps_global_origin; mavlink_msg_set_gps_global_origin_decode(msg, &gps_global_origin); - if (gps_global_origin.target_system == _mavlink->get_system_id()) { + if (gps_global_origin.target_system == _mavlink.get_system_id()) { vehicle_command_s vcmd{}; vcmd.param5 = (double)gps_global_origin.latitude * 1.e-7; vcmd.param6 = (double)gps_global_origin.longitude * 1.e-7; vcmd.param7 = (float)gps_global_origin.altitude * 1.e-3f; vcmd.command = vehicle_command_s::VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN; - vcmd.target_system = _mavlink->get_system_id(); + vcmd.target_system = _mavlink.get_system_id(); vcmd.target_component = MAV_COMP_ID_ALL; vcmd.source_system = msg->sysid; vcmd.source_component = msg->compid; @@ -1493,7 +1542,7 @@ void MavlinkReceiver::fill_thrust(float *thrust_body_array, uint8_t vehicle_type { // Fill correct field by checking frametype // TODO: add as needed - switch (_mavlink->get_system_type()) { + switch (_mavlink.get_system_type()) { case MAV_TYPE_GENERIC: break; @@ -1549,7 +1598,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) mavlink_msg_set_attitude_target_decode(msg, &attitude_target); /* Only accept messages which are intended for this system */ - if (_mavlink->get_forward_externalsp() && + if (_mavlink.get_forward_externalsp() && (mavlink_system.sysid == attitude_target.target_system || attitude_target.target_system == 0) && (mavlink_system.compid == attitude_target.target_component || attitude_target.target_component == 0)) { @@ -1577,11 +1626,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) const matrix::Quatf q{attitude_target.q}; q.copyTo(attitude_setpoint.q_d); - matrix::Eulerf euler{q}; - attitude_setpoint.roll_body = euler.phi(); - attitude_setpoint.pitch_body = euler.theta(); - attitude_setpoint.yaw_body = euler.psi(); - // TODO: review use case attitude_setpoint.yaw_sp_move_rate = (type_mask & ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE) ? (float)NAN : attitude_target.body_yaw_rate; @@ -1638,7 +1682,7 @@ void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { /* telemetry status supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ - if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(msg, &rstatus); @@ -1653,7 +1697,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) status.rxerrors = rstatus.rxerrors; status.fix = rstatus.fixed; - _mavlink->update_radio_status(status); + _mavlink.update_radio_status(status); _radio_status_pub.publish(status); } @@ -1670,7 +1714,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) ping.target_system = msg->sysid; ping.target_component = msg->compid; - mavlink_msg_ping_send_struct(_mavlink->get_channel(), &ping); + mavlink_msg_ping_send_struct(_mavlink.get_channel(), &ping); } else if ((ping.target_system == mavlink_system.sysid) && (ping.target_component == @@ -1682,7 +1726,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) float rtt_ms = (now - ping.time_usec) / 1000.0f; // Update ping statistics - struct Mavlink::ping_statistics_s &pstats = _mavlink->get_ping_statistics(); + struct Mavlink::ping_statistics_s &pstats = _mavlink.get_ping_statistics(); pstats.last_ping_time = now; @@ -1707,7 +1751,7 @@ MavlinkReceiver::handle_message_ping(mavlink_message_t *msg) pstats.min_rtt = pstats.min_rtt > 0.0f ? fminf(rtt_ms, pstats.min_rtt) : rtt_ms; /* Ping status is supported only on first ORB_MULTI_MAX_INSTANCES mavlink channels */ - if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { ping_s uorb_ping_msg{}; @@ -1749,9 +1793,7 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg) } battery_status.voltage_v = voltage_sum; - battery_status.voltage_filtered_v = voltage_sum; battery_status.current_a = (float)(battery_mavlink.current_battery) / 100.0f; - battery_status.current_filtered_a = battery_status.current_a; battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f; battery_status.discharged_mah = (float)battery_mavlink.current_consumed; battery_status.cell_count = cell_count; @@ -1793,18 +1835,18 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg) return; } - MavlinkShell *shell = _mavlink->get_shell(); + MavlinkShell *shell = _mavlink.get_shell(); if (shell) { // we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message - if (serial_control_mavlink.count > 0) { + if (serial_control_mavlink.count > 0 && serial_control_mavlink.count <= sizeof(serial_control_mavlink.data)) { shell->setTargetID(msg->sysid, msg->compid); shell->write(serial_control_mavlink.data, serial_control_mavlink.count); } // if no response requested, assume the shell is no longer used if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) { - _mavlink->close_shell(); + _mavlink.close_shell(); } } } @@ -1815,7 +1857,7 @@ MavlinkReceiver::handle_message_logging_ack(mavlink_message_t *msg) mavlink_logging_ack_t logging_ack; mavlink_msg_logging_ack_decode(msg, &logging_ack); - MavlinkULog *ulog_streaming = _mavlink->get_ulog_streaming(); + MavlinkULog *ulog_streaming = _mavlink.get_ulog_streaming(); if (ulog_streaming) { ulog_streaming->handle_ack(logging_ack); @@ -1992,7 +2034,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) mavlink_msg_rc_channels_override_decode(msg, &man); // Check target - if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { + if (man.target_system != 0 && man.target_system != _mavlink.get_system_id()) { return; } @@ -2079,7 +2121,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) mavlink_msg_manual_control_decode(msg, &mavlink_manual_control); // Check target - if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink->get_system_id()) { + if (mavlink_manual_control.target != 0 && mavlink_manual_control.target != _mavlink.get_system_id()) { return; } @@ -2089,7 +2131,22 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) // For backwards compatibility at the moment interpret throttle in range [0,1000] manual_control_setpoint.throttle = ((mavlink_manual_control.z / 1000.f) * 2.f) - 1.f; manual_control_setpoint.yaw = mavlink_manual_control.r / 1000.f; - manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink->get_instance_id(); + // Pass along the button states + manual_control_setpoint.buttons = mavlink_manual_control.buttons; + + if (mavlink_manual_control.enabled_extensions & (1u << 2)) { manual_control_setpoint.aux1 = mavlink_manual_control.aux1 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 3)) { manual_control_setpoint.aux2 = mavlink_manual_control.aux2 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 4)) { manual_control_setpoint.aux3 = mavlink_manual_control.aux3 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 5)) { manual_control_setpoint.aux4 = mavlink_manual_control.aux4 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 6)) { manual_control_setpoint.aux5 = mavlink_manual_control.aux5 / 1000.0f; } + + if (mavlink_manual_control.enabled_extensions & (1u << 7)) { manual_control_setpoint.aux6 = mavlink_manual_control.aux6 / 1000.0f; } + + manual_control_setpoint.data_source = manual_control_setpoint_s::SOURCE_MAVLINK_0 + _mavlink.get_instance_id(); manual_control_setpoint.timestamp = manual_control_setpoint.timestamp_sample = hrt_absolute_time(); manual_control_setpoint.valid = true; _manual_control_input_pub.publish(manual_control_setpoint); @@ -2099,7 +2156,7 @@ void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ - if (_mavlink->get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { + if (_mavlink.get_channel() < (mavlink_channel_t)ORB_MULTI_MAX_INSTANCES) { const hrt_abstime now = hrt_absolute_time(); @@ -2143,13 +2200,13 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) case MAV_TYPE_PARACHUTE: _heartbeat_type_parachute = now; - _mavlink->telemetry_status().parachute_system_healthy = + _mavlink.telemetry_status().parachute_system_healthy = (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); break; case MAV_TYPE_ODID: _heartbeat_type_open_drone_id = now; - _mavlink->telemetry_status().open_drone_id_system_healthy = + _mavlink.telemetry_status().open_drone_id_system_healthy = (hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE); break; @@ -2174,7 +2231,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) case MAV_COMP_ID_OBSTACLE_AVOIDANCE: _heartbeat_component_obstacle_avoidance = now; - _mavlink->telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); + _mavlink.telemetry_status().avoidance_system_healthy = (hb.system_status == MAV_STATE_ACTIVE); break; case MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY: @@ -2211,7 +2268,7 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) } if (data_rate > 0) { - _mavlink->set_data_rate(data_rate); + _mavlink.set_data_rate(data_rate); } // configure_stream wants a rate (msgs/second), so convert here. @@ -2233,7 +2290,7 @@ MavlinkReceiver::set_message_interval(int msgId, float interval, int data_rate) const char *stream_name = get_stream_name(msgId); if (stream_name != nullptr) { - _mavlink->configure_stream_threadsafe(stream_name, rate); + _mavlink.configure_stream_threadsafe(stream_name, rate); found_id = true; } } @@ -2246,7 +2303,7 @@ MavlinkReceiver::get_message_interval(int msgId) { unsigned interval = 0; - for (const auto &stream : _mavlink->get_streams()) { + for (const auto &stream : _mavlink.get_streams()) { if (stream->get_id() == msgId) { interval = stream->get_interval(); break; @@ -2254,7 +2311,7 @@ MavlinkReceiver::get_message_interval(int msgId) } // send back this value... - mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval); + mavlink_msg_message_interval_send(_mavlink.get_channel(), msgId, interval); } void @@ -2350,7 +2407,6 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_battery_status.timestamp = timestamp; hil_battery_status.voltage_v = 16.0f; - hil_battery_status.voltage_filtered_v = 16.0f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; hil_battery_status.connected = true; @@ -2371,7 +2427,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) device::Device::DeviceId device_id; device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_MAVLINK; - device_id.devid_s.bus = _mavlink->get_instance_id(); + device_id.devid_s.bus = _mavlink.get_instance_id(); device_id.devid_s.address = msg->sysid; device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; @@ -2538,91 +2594,6 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg) _transponder_report_pub.publish(t); } -void -MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg) -{ - mavlink_utm_global_position_t utm_pos; - mavlink_msg_utm_global_position_decode(msg, &utm_pos); - - bool is_self_published = false; - - -#ifndef BOARD_HAS_NO_UUID - px4_guid_t px4_guid; - board_get_px4_guid(px4_guid); - is_self_published = sizeof(px4_guid) == sizeof(utm_pos.uas_id) - && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) == 0; -#else - - is_self_published = msg->sysid == _mavlink->get_system_id(); -#endif /* BOARD_HAS_NO_UUID */ - - - //Ignore selfpublished UTM messages - if (is_self_published) { - return; - } - - // Convert cm/s to m/s - float vx = utm_pos.vx / 100.0f; - float vy = utm_pos.vy / 100.0f; - float vz = utm_pos.vz / 100.0f; - - transponder_report_s t{}; - t.timestamp = hrt_absolute_time(); - mav_array_memcpy(t.uas_id, utm_pos.uas_id, PX4_GUID_BYTE_LENGTH); - t.icao_address = msg->sysid; - t.lat = utm_pos.lat * 1e-7; - t.lon = utm_pos.lon * 1e-7; - t.altitude = utm_pos.alt / 1000.0f; - t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC; - // UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s. - t.heading = atan2f(vy, vx); - t.hor_velocity = sqrtf(vy * vy + vx * vx); - t.ver_velocity = -vz; - // TODO: Callsign - // For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null - // terminator could cause problems. - memset(&t.callsign[0], 0, sizeof(t.callsign)); - t.emitter_type = ADSB_EMITTER_TYPE_UAV; // TODO: Is this correct?x2? - - // The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of - // an 8-bit int, or if this is the first communication. - // Here, I assume that if this is the first communication, tslc = 0. - // If tslc > 255, then tslc = 255. - unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000; - - if (_last_utm_global_pos_com == 0) { - time_passed = 0; - - } else if (time_passed > UINT8_MAX) { - time_passed = UINT8_MAX; - } - - t.tslc = (uint8_t) time_passed; - - t.flags = 0; - - if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) { - t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; - } - - if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) { - t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; - } - - if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) { - t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; - t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; - } - - // Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not - // provide these. - _transponder_report_pub.publish(t); - - _last_utm_global_pos_com = t.timestamp; -} - void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg) { @@ -2789,7 +2760,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) { 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(); @@ -2970,7 +2940,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) } if ((t >= _last_heartbeat_check + (TIMEOUT / 2)) || force) { - telemetry_status_s &tstatus = _mavlink->telemetry_status(); + telemetry_status_s &tstatus = _mavlink.telemetry_status(); tstatus.heartbeat_type_antenna_tracker = (t <= TIMEOUT + _heartbeat_type_antenna_tracker); tstatus.heartbeat_type_gcs = (t <= TIMEOUT + _heartbeat_type_gcs); @@ -2990,14 +2960,14 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_component_udp_bridge = (t <= TIMEOUT + _heartbeat_component_udp_bridge); tstatus.heartbeat_component_uart_bridge = (t <= TIMEOUT + _heartbeat_component_uart_bridge); - _mavlink->telemetry_status_updated(); + _mavlink.telemetry_status_updated(); _last_heartbeat_check = t; } } void MavlinkReceiver::handle_message_request_event(mavlink_message_t *msg) { - _mavlink->get_events_protocol().handle_request_event(*msg); + _mavlink.get_events_protocol().handle_request_event(*msg); } void @@ -3120,7 +3090,7 @@ MavlinkReceiver::run() /* set thread name */ { char thread_name[17]; - snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink->get_instance_id()); + snprintf(thread_name, sizeof(thread_name), "mavlink_rcv_if%d", _mavlink.get_instance_id()); px4_prctl(PR_SET_NAME, thread_name, px4_getpid()); } @@ -3141,8 +3111,8 @@ MavlinkReceiver::run() struct pollfd fds[1] = {}; - if (_mavlink->get_protocol() == Protocol::SERIAL) { - fds[0].fd = _mavlink->get_uart_fd(); + if (_mavlink.get_protocol() == Protocol::SERIAL) { + fds[0].fd = _mavlink.get_uart_fd(); fds[0].events = POLLIN; } @@ -3150,8 +3120,8 @@ MavlinkReceiver::run() struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); - if (_mavlink->get_protocol() == Protocol::UDP) { - fds[0].fd = _mavlink->get_socket_fd(); + if (_mavlink.get_protocol() == Protocol::UDP) { + fds[0].fd = _mavlink.get_socket_fd(); fds[0].events = POLLIN; } @@ -3160,7 +3130,7 @@ MavlinkReceiver::run() ssize_t nread = 0; hrt_abstime last_send_update = 0; - while (!_mavlink->should_exit()) { + while (!_mavlink.should_exit()) { // check for parameter updates if (_parameter_update_sub.updated()) { @@ -3175,7 +3145,7 @@ MavlinkReceiver::run() int ret = poll(&fds[0], 1, timeout); if (ret > 0) { - if (_mavlink->get_protocol() == Protocol::SERIAL) { + if (_mavlink.get_protocol() == Protocol::SERIAL) { /* non-blocking read. read may return negative values */ nread = ::read(fds[0].fd, buf, sizeof(buf)); @@ -3186,21 +3156,21 @@ MavlinkReceiver::run() #if defined(MAVLINK_UDP) - else if (_mavlink->get_protocol() == Protocol::UDP) { + else if (_mavlink.get_protocol() == Protocol::UDP) { if (fds[0].revents & POLLIN) { - nread = recvfrom(_mavlink->get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); + nread = recvfrom(_mavlink.get_socket_fd(), buf, sizeof(buf), 0, (struct sockaddr *)&srcaddr, &addrlen); } - struct sockaddr_in &srcaddr_last = _mavlink->get_client_source_address(); + struct sockaddr_in &srcaddr_last = _mavlink.get_client_source_address(); int localhost = (127 << 24) + 1; - if (!_mavlink->get_client_source_initialized()) { + if (!_mavlink.get_client_source_initialized()) { // set the address either if localhost or if 3 seconds have passed // this ensures that a GCS running on localhost can get a hold of // the system within the first N seconds - hrt_abstime stime = _mavlink->get_start_time(); + hrt_abstime stime = _mavlink.get_start_time(); if ((stime != 0 && (hrt_elapsed_time(&stime) > 3_s)) || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) { @@ -3208,7 +3178,7 @@ MavlinkReceiver::run() srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last.sin_port = srcaddr.sin_port; - _mavlink->set_client_source_initialized(); + _mavlink.set_client_source_initialized(); PX4_INFO("partner IP: %s", inet_ntoa(srcaddr.sin_addr)); } @@ -3216,51 +3186,30 @@ MavlinkReceiver::run() } // only start accepting messages on UDP once we're sure who we talk to - if (_mavlink->get_protocol() != Protocol::UDP || _mavlink->get_client_source_initialized()) { + if (_mavlink.get_protocol() != Protocol::UDP || _mavlink.get_client_source_initialized()) { #endif // MAVLINK_UDP /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { - if (mavlink_parse_char(_mavlink->get_channel(), buf[i], &msg, &_status)) { + if (mavlink_parse_char(_mavlink.get_channel(), buf[i], &msg, &_status)) { /* check if we received version 2 and request a switch. */ - if (!(_mavlink->get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) { + if (!(_mavlink.get_status()->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)) { /* this will only switch to proto version 2 if allowed in settings */ - _mavlink->set_proto_version(2); + _mavlink.set_proto_version(2); } - /* handle generic messages and commands */ - handle_message(&msg); - - /* handle packet with mission manager */ - _mission_manager.handle_message(&msg); + switch (_mavlink.get_mode()) { + case Mavlink::MAVLINK_MODE::MAVLINK_MODE_GIMBAL: + handle_messages_in_gimbal_mode(msg); + break; - /* handle packet with parameter component */ - if (_mavlink->boot_complete()) { - // make sure mavlink app has booted before we start processing parameter sync - _parameters_manager.handle_message(&msg); - - } else { - if (hrt_elapsed_time(&_mavlink->get_first_start_time()) > 20_s) { - PX4_ERR("system boot did not complete in 20 seconds"); - _mavlink->set_boot_complete(); - } + default: + handle_message(&msg); + break; } - if (_mavlink->ftp_enabled()) { - /* handle packet with ftp component */ - _mavlink_ftp.handle_message(&msg); - } - - /* handle packet with log component */ - _mavlink_log_handler.handle_message(&msg); - - /* handle packet with timesync component */ - _mavlink_timesync.handle_message(&msg); - - /* handle packet with parent object */ - _mavlink->handle_message(&msg); - + _mavlink.set_has_received_messages(true); // Received first message, unlock wait to transmit '-w' command-line flag update_rx_stats(msg); if (_message_statistics_enabled) { @@ -3271,9 +3220,9 @@ MavlinkReceiver::run() /* count received bytes (nread will be -1 on read error) */ if (nread > 0) { - _mavlink->count_rxbytes(nread); + _mavlink.count_rxbytes(nread); - telemetry_status_s &tstatus = _mavlink->telemetry_status(); + telemetry_status_s &tstatus = _mavlink.telemetry_status(); tstatus.rx_message_count = _total_received_counter; tstatus.rx_message_lost_count = _total_lost_counter; tstatus.rx_message_lost_rate = static_cast(_total_lost_counter) / static_cast(_total_received_counter); @@ -3311,11 +3260,11 @@ MavlinkReceiver::run() _mission_manager.check_active_mission(); _mission_manager.send(); - if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { + if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { _parameters_manager.send(); } - if (_mavlink->ftp_enabled()) { + if (_mavlink.ftp_enabled()) { _mavlink_ftp.send(); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index e5575b7efb5f..b95bdca59a72 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -126,7 +126,7 @@ class Mavlink; class MavlinkReceiver : public ModuleParams { public: - MavlinkReceiver(Mavlink *parent); + MavlinkReceiver(Mavlink &parent); ~MavlinkReceiver() override; void start(); @@ -155,6 +155,7 @@ class MavlinkReceiver : public ModuleParams float param4 = 0.0f, float param5 = 0.0f, float param6 = 0.0f, float param7 = 0.0f); void handle_message(mavlink_message_t *msg); + void handle_messages_in_gimbal_mode(mavlink_message_t &msg); void handle_message_adsb_vehicle(mavlink_message_t *msg); void handle_message_att_pos_mocap(mavlink_message_t *msg); @@ -246,7 +247,7 @@ class MavlinkReceiver : public ModuleParams */ void updateParams() override; - Mavlink *_mavlink; + Mavlink &_mavlink; MavlinkFTP _mavlink_ftp; MavlinkLogHandler _mavlink_log_handler; diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 574a5164ad16..2accd21c5411 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -43,7 +43,7 @@ #include "../mavlink_ftp.h" #ifdef __PX4_NUTTX -#define PX4_MAVLINK_TEST_DATA_DIR "/fs/microsd/ftp_unit_test_data" +#define PX4_MAVLINK_TEST_DATA_DIR CONFIG_BOARD_ROOT_PATH "/ftp_unit_test_data" #else #define PX4_MAVLINK_TEST_DATA_DIR "ftp_unit_test_data" #endif @@ -77,7 +77,7 @@ MavlinkFtpTest::MavlinkFtpTest() : void MavlinkFtpTest::_init() { _expected_seq_number = 0; - _ftp_server = new MavlinkFTP(nullptr); + _ftp_server = new MavlinkFTP(_mavlink); _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message_handler_generic, this); _create_test_files(); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index b32ebd628422..46da67c5f1f9 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -37,12 +37,9 @@ #pragma once #include -#ifndef MAVLINK_FTP_UNIT_TEST #include "../mavlink_bridge_header.h" -#else -#include -#endif #include "../mavlink_ftp.h" +#include "../mavlink_main.h" class MavlinkFtpTest : public UnitTest { @@ -130,6 +127,7 @@ class MavlinkFtpTest : public UnitTest bool _receive_message_handler_burst(const mavlink_file_transfer_protocol_t *ftp_req, BurstInfo *burst_info); MavlinkFTP *_ftp_server; + Mavlink _mavlink; uint16_t _expected_seq_number; mavlink_file_transfer_protocol_t _reply_msg; diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp index 7bbe8f8dfacd..410dd84512a6 100644 --- a/src/modules/mavlink/mavlink_timesync.cpp +++ b/src/modules/mavlink/mavlink_timesync.cpp @@ -43,7 +43,7 @@ #include -MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) : +MavlinkTimesync::MavlinkTimesync(Mavlink &mavlink) : _mavlink(mavlink) { } @@ -66,7 +66,7 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg) rsync.tc1 = now * 1000ULL; rsync.ts1 = tsync.ts1; - mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync); + mavlink_msg_timesync_send_struct(_mavlink.get_channel(), &rsync); return; diff --git a/src/modules/mavlink/mavlink_timesync.h b/src/modules/mavlink/mavlink_timesync.h index a3e8295ae09e..0cb32a229cc3 100644 --- a/src/modules/mavlink/mavlink_timesync.h +++ b/src/modules/mavlink/mavlink_timesync.h @@ -49,7 +49,7 @@ class Mavlink; class MavlinkTimesync { public: - explicit MavlinkTimesync(Mavlink *mavlink); + explicit MavlinkTimesync(Mavlink &mavlink); ~MavlinkTimesync() = default; void handle_message(const mavlink_message_t *msg); @@ -61,6 +61,6 @@ class MavlinkTimesync uint64_t sync_stamp(uint64_t usec) { return _timesync.sync_stamp(usec); } private: - Mavlink *const _mavlink; + Mavlink &_mavlink; Timesync _timesync{}; }; diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index dfa9a2da7ff3..6715b8b62ad7 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -32,6 +32,10 @@ serial_config: then set MAV_ARGS "${MAV_ARGS} -z" fi + if param compare MAV_${i}_MODE 6 + then + set MAV_ARGS "${MAV_ARGS} -F p:MAV_${i}_HL_FREQ" + fi mavlink start ${MAV_ARGS} -x port_config_param: name: MAV_${i}_CONFIG @@ -178,3 +182,21 @@ parameters: num_instances: *max_num_config_instances default: [2, 2, 2] reboot_required: true + + MAV_${i}_HL_FREQ: + description: + short: Configures the frequency of HIGH_LATENCY2 stream for instance ${i} + long: | + Positive real value that configures the transmission frequency of the + HIGH_LATENCY2 stream for instance ${i}, configured in iridium mode. + This parameter has no effect if the instance mode is different from iridium. + + type: float + decimal: 3 + increment: 0.001 + unit: Hz + min: 0.0 + max: 50.0 + num_instances: *max_num_config_instances + default: [0.015, 0.015, 0.015] + reboot_required: true diff --git a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/modules/mavlink/streams/BATTERY_INFO.hpp similarity index 67% rename from src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp rename to src/modules/mavlink/streams/BATTERY_INFO.hpp index 3476db3a02b2..fd72acce45ed 100644 --- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp +++ b/src/modules/mavlink/streams/BATTERY_INFO.hpp @@ -31,30 +31,30 @@ * ****************************************************************************/ -#ifndef SMART_BATTERY_INFO_HPP -#define SMART_BATTERY_INFO_HPP +#ifndef BATTERY_INFO_HPP +#define BATTERY_INFO_HPP #include -class MavlinkStreamSmartBatteryInfo : public MavlinkStream +class MavlinkStreamBatteryInfo : public MavlinkStream { public: - static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamSmartBatteryInfo(mavlink); } + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamBatteryInfo(mavlink); } - static constexpr const char *get_name_static() { return "SMART_BATTERY_INFO"; } - static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_SMART_BATTERY_INFO; } + static constexpr const char *get_name_static() { return "BATTERY_INFO"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_BATTERY_INFO; } const char *get_name() const override { return get_name_static(); } uint16_t get_id() override { return get_id_static(); } unsigned get_size() override { - static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + static constexpr unsigned size_per_battery = MAVLINK_MSG_ID_BATTERY_INFO_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; return size_per_battery * _battery_status_subs.advertised_count(); } private: - explicit MavlinkStreamSmartBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} + explicit MavlinkStreamBatteryInfo(Mavlink *mavlink) : MavlinkStream(mavlink) {} uORB::SubscriptionMultiArray _battery_status_subs{ORB_ID::battery_status}; @@ -67,36 +67,50 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream if (battery_sub.update(&battery_status)) { if (battery_status.serial_number == 0) { - // This is not smart battery + // Required to emit continue; } - mavlink_smart_battery_info_t msg{}; + mavlink_battery_info_t msg{}; msg.id = battery_status.id - 1; - msg.capacity_full_specification = battery_status.capacity; - msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f); + msg.design_capacity = (float)(battery_status.capacity * 1000); + msg.full_charge_capacity = (float)(battery_status.state_of_health * battery_status.capacity * 1000.f) / 100.f; msg.cycle_count = battery_status.cycle_count; if (battery_status.manufacture_date) { uint16_t day = battery_status.manufacture_date % 32; uint16_t month = (battery_status.manufacture_date >> 5) % 16; - uint16_t year = (80 + (battery_status.manufacture_date >> 9)) % 100; + uint16_t year = (80 + (battery_status.manufacture_date >> 9)); + uint16_t year2dig = year % 100; + //Formatted as 'ddmmyyyy' (maxed 9 chars) + snprintf(msg.manufacture_date, sizeof(msg.manufacture_date), "%d%d%d", day, month, year); //Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars) - snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year, battery_status.serial_number); + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year2dig, + battery_status.serial_number); } else { + snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number); } - //msg.device_name = ?? - msg.weight = -1; - msg.discharge_minimum_voltage = -1; - msg.charging_minimum_voltage = -1; - msg.resting_minimum_voltage = -1; - - mavlink_msg_smart_battery_info_send_struct(_mavlink->get_channel(), &msg); + // Not supported by PX4 (not in battery_status uorb topic) + /* + msg.name = 0; // char[50] + msg.weight = 0; + msg.discharge_minimum_voltage = 0; + msg.charging_minimum_voltage = 0; + msg.resting_minimum_voltage = 0; + msg.charging_maximum_voltage = 0; + msg.charging_maximum_current = 0; + msg.discharge_maximum_current = 0; + msg.discharge_maximum_burst_current = 0; + msg.cells_in_series = 0; + msg.nominal_voltage = 0; + */ + + mavlink_msg_battery_info_send_struct(_mavlink->get_channel(), &msg); updated = true; } } @@ -105,4 +119,4 @@ class MavlinkStreamSmartBatteryInfo : public MavlinkStream } }; -#endif // SMART_BATTERY_INFO_HPP +#endif // BATTERY_INFO_HPP diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 73068f0de360..633807a86e62 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -74,7 +74,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream bat_msg.type = MAV_BATTERY_TYPE_LIPO; bat_msg.current_consumed = (battery_status.connected) ? battery_status.discharged_mah : -1; bat_msg.energy_consumed = -1; - bat_msg.current_battery = (battery_status.connected) ? battery_status.current_filtered_a * 100 : -1; + bat_msg.current_battery = (battery_status.connected) ? battery_status.current_a * 100 : -1; bat_msg.battery_remaining = (battery_status.connected) ? roundf(battery_status.remaining * 100.f) : -1; // MAVLink extension: 0 is unsupported, in uORB it's NAN bat_msg.time_remaining = (battery_status.connected && (PX4_ISFINITE(battery_status.time_remaining_s))) ? @@ -114,20 +114,7 @@ class MavlinkStreamBatteryStatus : public MavlinkStream break; } - switch (battery_status.mode) { - case (battery_status_s::BATTERY_MODE_AUTO_DISCHARGING): - bat_msg.mode = MAV_BATTERY_MODE_AUTO_DISCHARGING; - break; - - case (battery_status_s::BATTERY_MODE_HOT_SWAP): - bat_msg.mode = MAV_BATTERY_MODE_HOT_SWAP; - break; - - default: - bat_msg.mode = MAV_BATTERY_MODE_UNKNOWN; - break; - } - + bat_msg.mode = MAV_BATTERY_MODE_UNKNOWN; bat_msg.fault_bitmask = battery_status.faults; // check if temperature valid @@ -161,10 +148,10 @@ class MavlinkStreamBatteryStatus : public MavlinkStream // If it doesn't fit, we have to split it into UINT16-1 chunks and the remaining // voltage for the subsequent field. // This won't work for voltages of more than 655 volts. - const int num_fields_required = static_cast(battery_status.voltage_filtered_v * 1000.f) / (UINT16_MAX - 1) + 1; + const int num_fields_required = static_cast(battery_status.voltage_v * 1000.f) / (UINT16_MAX - 1) + 1; if (num_fields_required <= mavlink_cell_slots) { - float remaining_voltage = battery_status.voltage_filtered_v * 1000.f; + float remaining_voltage = battery_status.voltage_v * 1000.f; for (int i = 0; i < num_fields_required - 1; ++i) { bat_msg.voltages[i] = UINT16_MAX - 1; diff --git a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp index 3764940c312e..0a27a90eb093 100644 --- a/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp +++ b/src/modules/mavlink/streams/ESTIMATOR_STATUS.hpp @@ -80,7 +80,7 @@ class MavlinkStreamEstimatorStatus : public MavlinkStream est_msg.vel_ratio = est.vel_test_ratio; est_msg.pos_horiz_ratio = est.pos_test_ratio; est_msg.pos_vert_ratio = est.hgt_test_ratio; - est_msg.mag_ratio = est.mag_test_ratio; + est_msg.mag_ratio = est.hdg_test_ratio; est_msg.hagl_ratio = est.hagl_test_ratio; est_msg.tas_ratio = est.tas_test_ratio; est_msg.pos_horiz_accuracy = est.pos_horiz_accuracy; diff --git a/src/modules/mavlink/streams/FUEL_STATUS.hpp b/src/modules/mavlink/streams/FUEL_STATUS.hpp new file mode 100644 index 000000000000..350f9c9fc7dc --- /dev/null +++ b/src/modules/mavlink/streams/FUEL_STATUS.hpp @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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. + * + ****************************************************************************/ + +#ifndef FUEL_STATUS_HPP +#define FUEL_STATUS_HPP + +#include + +class MavlinkStreamFuelStatus : public MavlinkStream +{ +public: + static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamFuelStatus(mavlink); } + + static constexpr const char *get_name_static() { return "FUEL_STATUS"; } + static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FUEL_STATUS; } + + const char *get_name() const override { return MavlinkStreamFuelStatus::get_name_static(); } + uint16_t get_id() override { return get_id_static(); } + + unsigned get_size() override + { + return _fuel_tank_status_sub.advertised() ? MAVLINK_MSG_ID_FUEL_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + } + +private: + explicit MavlinkStreamFuelStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {} + + uORB::Subscription _fuel_tank_status_sub{ORB_ID(fuel_tank_status)}; + + bool send() override + { + fuel_tank_status_s fuel_status; + + if (_fuel_tank_status_sub.update(&fuel_status)) { + mavlink_fuel_status_t msg{}; + + msg.id = fuel_status.fuel_tank_id; + msg.maximum_fuel = fuel_status.maximum_fuel_capacity; + msg.consumed_fuel = fuel_status.consumed_fuel; + msg.remaining_fuel = fuel_status.remaining_fuel; + msg.percent_remaining = fuel_status.percent_remaining; + msg.flow_rate = fuel_status.fuel_consumption_rate; + msg.temperature = fuel_status.temperature; + msg.fuel_type = fuel_status.fuel_type; + + mavlink_msg_fuel_status_send_struct(_mavlink->get_channel(), &msg); + + return true; + } + + return false; + } + +}; + +#endif // FUEL_STATUS_HPP diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index c349340ac3a5..6aca9f7d9837 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -125,18 +126,21 @@ class MavlinkStreamHighLatency2 : public MavlinkStream updated |= _temperature.valid(); updated |= _throttle.valid(); updated |= _windspeed.valid(); - updated |= write_airspeed(&msg); - updated |= write_attitude_sp(&msg); - updated |= write_battery_status(&msg); - updated |= write_estimator_status(&msg); - updated |= write_fw_ctrl_status(&msg); - updated |= write_geofence_result(&msg); - updated |= write_global_position(&msg); - updated |= write_mission_result(&msg); - updated |= write_tecs_status(&msg); - updated |= write_vehicle_status(&msg); + updated |= write_attitude_setpoint_if_updated(&msg); + updated |= write_estimator_status_if_updated(&msg); + updated |= write_fw_ctrl_status_if_updated(&msg); + updated |= write_geofence_result_if_updated(&msg); + updated |= write_global_position_if_updated(&msg); + updated |= write_heading_if_updated(&msg); + updated |= write_mission_result_if_updated(&msg); updated |= write_failsafe_flags(&msg); - updated |= write_wind(&msg); + + // these topics are already updated in update_data() and thus we just copy them here + write_airspeed(&msg); + write_battery_status(&msg); + write_tecs_status(&msg); + write_vehicle_status(&msg); + write_wind(&msg); if (updated) { msg.timestamp = t / 1000; @@ -244,7 +248,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { airspeed_s airspeed; - if (_airspeed_sub.update(&airspeed)) { + if (_airspeed_sub.copy(&airspeed)) { if (airspeed.confidence < 0.95f) { // the same threshold as for the commander msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE; } @@ -255,12 +259,14 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return false; } - bool write_attitude_sp(mavlink_high_latency2_t *msg) + bool write_attitude_setpoint_if_updated(mavlink_high_latency2_t *msg) { vehicle_attitude_setpoint_s attitude_sp; if (_attitude_sp_sub.update(&attitude_sp)) { - msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(attitude_sp.yaw_body)) * 0.5f); + + msg->target_heading = static_cast(math::degrees(matrix::wrap_2pi(matrix::Eulerf(matrix::Quatf( + attitude_sp.q_d)).psi())) * 0.5f); return true; } @@ -274,7 +280,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) { battery_status_s battery; - if (_batteries[i].subscription.update(&battery)) { + if (_batteries[i].subscription.copy(&battery)) { updated = true; _batteries[i].connected = battery.connected; @@ -287,11 +293,12 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return updated; } - bool write_estimator_status(mavlink_high_latency2_t *msg) + bool write_estimator_status_if_updated(mavlink_high_latency2_t *msg) { // use primary estimator_status - if (_estimator_selector_status_sub.updated()) { - estimator_selector_status_s estimator_selector_status; + estimator_selector_status_s estimator_selector_status; + + if (_estimator_selector_status_sub.update(&estimator_selector_status)) { if (_estimator_selector_status_sub.copy(&estimator_selector_status)) { if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) { @@ -304,8 +311,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream if (_estimator_status_sub.update(&estimator_status)) { if (estimator_status.gps_check_fail_flags > 0 || - estimator_status.filter_fault_flags > 0 || - estimator_status.innovation_check_flags > 0) { + estimator_status.filter_fault_flags > 0) { msg->failure_flags |= HL_FAILURE_FLAG_ESTIMATOR; } @@ -320,7 +326,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return false; } - bool write_fw_ctrl_status(mavlink_high_latency2_t *msg) + bool write_fw_ctrl_status_if_updated(mavlink_high_latency2_t *msg) { position_controller_status_s pos_ctrl_status; @@ -334,7 +340,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return false; } - bool write_geofence_result(mavlink_high_latency2_t *msg) + bool write_geofence_result_if_updated(mavlink_high_latency2_t *msg) { geofence_result_s geofence; @@ -350,12 +356,12 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return false; } - bool write_global_position(mavlink_high_latency2_t *msg) + bool write_global_position_if_updated(mavlink_high_latency2_t *msg) { vehicle_global_position_s global_pos; vehicle_local_position_s local_pos; - if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) { + if (_global_pos_sub.update(&global_pos) && _local_pos_sub.copy(&local_pos)) { msg->latitude = global_pos.lat * 1e7; msg->longitude = global_pos.lon * 1e7; @@ -370,7 +376,20 @@ class MavlinkStreamHighLatency2 : public MavlinkStream msg->altitude = altitude; - msg->heading = static_cast(math::degrees(matrix::wrap_2pi(local_pos.heading)) * 0.5f); + return true; + } + + return false; + } + + bool write_heading_if_updated(mavlink_high_latency2_t *msg) + { + vehicle_attitude_s attitude; + + if (_attitude_sub.update(&attitude)) { + + const matrix::Eulerf euler = matrix::Quatf(attitude.q); + msg->heading = static_cast(math::degrees(matrix::wrap_2pi(euler.psi())) * 0.5f); return true; } @@ -378,7 +397,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream return false; } - bool write_mission_result(mavlink_high_latency2_t *msg) + bool write_mission_result_if_updated(mavlink_high_latency2_t *msg) { mission_result_s mission_result; @@ -394,7 +413,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { tecs_status_s tecs_status; - if (_tecs_status_sub.update(&tecs_status)) { + if (_tecs_status_sub.copy(&tecs_status)) { int16_t target_altitude; convert_limit_safe(tecs_status.altitude_sp, target_altitude); msg->target_altitude = target_altitude; @@ -409,7 +428,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { vehicle_status_s status; - if (_status_sub.update(&status)) { + if (_status_sub.copy(&status)) { health_report_s health_report; if (_health_report_sub.copy(&health_report)) { @@ -476,7 +495,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream { wind_s wind; - if (_wind_sub.update(&wind)) { + if (_wind_sub.copy(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, wind.windspeed_north))) * 0.5f); return true; @@ -625,6 +644,7 @@ class MavlinkStreamHighLatency2 : public MavlinkStream uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0}; uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1}; uORB::Subscription _airspeed_sub{ORB_ID(airspeed)}; + uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)}; diff --git a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp index 8be68fdc3fad..3feabf124d68 100644 --- a/src/modules/mavlink/streams/MANUAL_CONTROL.hpp +++ b/src/modules/mavlink/streams/MANUAL_CONTROL.hpp @@ -68,10 +68,10 @@ class MavlinkStreamManualControl : public MavlinkStream mavlink_manual_control_t msg{}; msg.target = mavlink_system.sysid; - msg.x = manual_control_setpoint.pitch * 1000; - msg.y = manual_control_setpoint.roll * 1000; - msg.z = manual_control_setpoint.throttle * 1000; - msg.r = manual_control_setpoint.yaw * 1000; + msg.x = manual_control_setpoint.pitch * 1000.f; + msg.y = manual_control_setpoint.roll * 1000.f; + msg.z = manual_control_setpoint.throttle * 1000.f; + msg.r = manual_control_setpoint.yaw * 1000.f; manual_control_switches_s manual_control_switches{}; @@ -84,6 +84,36 @@ class MavlinkStreamManualControl : public MavlinkStream msg.buttons |= (manual_control_switches.kill_switch << (shift * 6)); } + if (PX4_ISFINITE(manual_control_setpoint.aux1)) { + msg.enabled_extensions |= (1u << 2); + msg.aux1 = manual_control_setpoint.aux1 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux2)) { + msg.enabled_extensions |= (1u << 3); + msg.aux2 = manual_control_setpoint.aux2 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux3)) { + msg.enabled_extensions |= (1u << 4); + msg.aux3 = manual_control_setpoint.aux3 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux4)) { + msg.enabled_extensions |= (1u << 5); + msg.aux4 = manual_control_setpoint.aux4 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux5)) { + msg.enabled_extensions |= (1u << 6); + msg.aux5 = manual_control_setpoint.aux5 * 1000.f; + } + + if (PX4_ISFINITE(manual_control_setpoint.aux6)) { + msg.enabled_extensions |= (1u << 7); + msg.aux6 = manual_control_setpoint.aux6 * 1000.f; + } + mavlink_msg_manual_control_send_struct(_mavlink->get_channel(), &msg); return true; diff --git a/src/modules/mavlink/streams/SYS_STATUS.hpp b/src/modules/mavlink/streams/SYS_STATUS.hpp index 6859ea7f4be1..c875eaceaae9 100644 --- a/src/modules/mavlink/streams/SYS_STATUS.hpp +++ b/src/modules/mavlink/streams/SYS_STATUS.hpp @@ -150,7 +150,12 @@ class MavlinkStreamSysStatus : public MavlinkStream fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_3D_MAG, health_component_t::magnetometer, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_GPS, health_component_t::gps, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_RC_RECEIVER, health_component_t::remote_control, msg); - fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::local_position_estimate, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, health_component_t::attitude_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, health_component_t::local_position_estimate, + msg); + fillOutComponent(health_report, MAV_SYS_STATUS_AHRS, health_component_t::attitude_estimate, msg); + fillOutComponent(health_report, MAV_SYS_STATUS_LOGGING, health_component_t::logging, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, health_component_t::absolute_pressure, msg); fillOutComponent(health_report, MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, health_component_t::differential_pressure, msg); @@ -165,8 +170,8 @@ class MavlinkStreamSysStatus : public MavlinkStream const battery_status_s &lowest_battery = battery_status[lowest_battery_index]; if (lowest_battery.connected) { - msg.voltage_battery = lowest_battery.voltage_filtered_v * 1000.0f; - msg.current_battery = lowest_battery.current_filtered_a * 100.0f; + msg.voltage_battery = lowest_battery.voltage_v * 1000.0f; + msg.current_battery = lowest_battery.current_a * 100.0f; msg.battery_remaining = ceilf(lowest_battery.remaining * 100.0f); } else { diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index 31ca3eafa019..db1dcd3c1576 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -164,4 +164,3 @@ class MulticopterAttitudeControl : public ModuleBase (ParamFloat) _param_com_spoolup_time ) }; - diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 39cb033e74dc..f6cc5db43d88 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -182,12 +182,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, q_sp.copyTo(attitude_setpoint.q_d); - // Transform to euler angles for logging only - const Eulerf euler_sp(q_sp); - attitude_setpoint.roll_body = euler_sp(0); - attitude_setpoint.pitch_body = euler_sp(1); - attitude_setpoint.yaw_body = euler_sp(2); - attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); attitude_setpoint.timestamp = hrt_absolute_time(); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index ca8360c80452..5f8e5f680b5e 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -42,14 +42,11 @@ using namespace matrix; MulticopterPositionControl::MulticopterPositionControl(bool vtol) : - SuperBlock(nullptr, "MPC"), ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)), - _vel_x_deriv(this, "VELD"), - _vel_y_deriv(this, "VELD"), - _vel_z_deriv(this, "VELD") + _vehicle_attitude_setpoint_pub(vtol ? ORB_ID(mc_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)) { + _sample_interval_s.update(0.01f); // 100 Hz default parameters_update(true); _tilt_limit_slew_rate.setSlewRate(.2f); _takeoff_status_pub.advertise(); @@ -83,7 +80,42 @@ void MulticopterPositionControl::parameters_update(bool force) // update parameters from storage ModuleParams::updateParams(); - SuperBlock::updateParams(); + + float sample_freq_hz = 1.f / _sample_interval_s.mean(); + + // velocity notch filter + if ((_param_mpc_vel_nf_frq.get() > 0.f) && (_param_mpc_vel_nf_bw.get() > 0.f)) { + _vel_xy_notch_filter.setParameters(sample_freq_hz, _param_mpc_vel_nf_frq.get(), _param_mpc_vel_nf_bw.get()); + _vel_z_notch_filter.setParameters(sample_freq_hz, _param_mpc_vel_nf_frq.get(), _param_mpc_vel_nf_bw.get()); + + } else { + _vel_xy_notch_filter.disable(); + _vel_z_notch_filter.disable(); + } + + // velocity xy/z low pass filter + if (_param_mpc_vel_lp.get() > 0.f) { + _vel_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); + _vel_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_vel_lp.get()); + + } else { + // disable filtering + _vel_xy_lp_filter.setAlpha(1.f); + _vel_z_lp_filter.setAlpha(1.f); + } + + // velocity derivative xy/z low pass filter + if (_param_mpc_veld_lp.get() > 0.f) { + _vel_deriv_xy_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); + _vel_deriv_z_lp_filter.setCutoffFreq(sample_freq_hz, _param_mpc_veld_lp.get()); + + } else { + // disable filtering + _vel_deriv_xy_lp_filter.setAlpha(1.f); + _vel_deriv_z_lp_filter.setAlpha(1.f); + } + + int num_changed = 0; @@ -167,6 +199,7 @@ void MulticopterPositionControl::parameters_update(bool force) Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()), Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get())); _control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get()); + _control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get()); _goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get()); _goto_control.setParamMpcAccDownMax(_param_mpc_acc_down_max.get()); _goto_control.setParamMpcAccUpMax(_param_mpc_acc_up_max.get()); @@ -275,7 +308,7 @@ void MulticopterPositionControl::parameters_update(bool force) } PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehicle_local_position_s - &vehicle_local_position) + &vehicle_local_position, const float dt_s) { PositionControlStates states; @@ -299,29 +332,42 @@ PositionControlStates MulticopterPositionControl::set_vehicle_states(const vehic const Vector2f velocity_xy(vehicle_local_position.vx, vehicle_local_position.vy); if (vehicle_local_position.v_xy_valid && velocity_xy.isAllFinite()) { - states.velocity.xy() = velocity_xy; - states.acceleration(0) = _vel_x_deriv.update(velocity_xy(0)); - states.acceleration(1) = _vel_y_deriv.update(velocity_xy(1)); + const Vector2f vel_xy_prev = _vel_xy_lp_filter.getState(); + + // vel xy notch filter, then low pass filter + states.velocity.xy() = _vel_xy_lp_filter.update(_vel_xy_notch_filter.apply(velocity_xy)); + + // vel xy derivative low pass filter + states.acceleration.xy() = _vel_deriv_xy_lp_filter.update((_vel_xy_lp_filter.getState() - vel_xy_prev) / dt_s); } else { states.velocity(0) = states.velocity(1) = NAN; states.acceleration(0) = states.acceleration(1) = NAN; - // reset derivatives to prevent acceleration spikes when regaining velocity - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); + // reset filters to prevent acceleration spikes when regaining velocity + _vel_xy_lp_filter.reset({}); + _vel_xy_notch_filter.reset(); + _vel_deriv_xy_lp_filter.reset({}); } if (PX4_ISFINITE(vehicle_local_position.vz) && vehicle_local_position.v_z_valid) { - states.velocity(2) = vehicle_local_position.vz; - states.acceleration(2) = _vel_z_deriv.update(states.velocity(2)); + + const float vel_z_prev = _vel_z_lp_filter.getState(); + + // vel z notch filter, then low pass filter + states.velocity(2) = _vel_z_lp_filter.update(_vel_z_notch_filter.apply(vehicle_local_position.vz)); + + // vel z derivative low pass filter + states.acceleration(2) = _vel_deriv_z_lp_filter.update((_vel_z_lp_filter.getState() - vel_z_prev) / dt_s); } else { states.velocity(2) = NAN; states.acceleration(2) = NAN; - // reset derivative to prevent acceleration spikes when regaining velocity - _vel_z_deriv.reset(); + // reset filters to prevent acceleration spikes when regaining velocity + _vel_z_lp_filter.reset({}); + _vel_z_notch_filter.reset(); + _vel_deriv_z_lp_filter.reset({}); } states.yaw = vehicle_local_position.heading; @@ -350,8 +396,7 @@ void MulticopterPositionControl::Run() math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); _time_stamp_last_loop = vehicle_local_position.timestamp_sample; - // set _dt in controllib Block for BlockDerivative - setDt(dt); + _sample_interval_s.update(dt); if (_vehicle_control_mode_sub.updated()) { const bool previous_position_control_enabled = _vehicle_control_mode.flag_multicopter_position_control_enabled; @@ -379,7 +424,7 @@ void MulticopterPositionControl::Run() } } - PositionControlStates states{set_vehicle_states(vehicle_local_position)}; + PositionControlStates states{set_vehicle_states(vehicle_local_position, dt)}; // if a goto setpoint available this publishes a trajectory setpoint to go there if (_goto_control.checkForSetpoint(vehicle_local_position.timestamp_sample, @@ -637,12 +682,13 @@ void MulticopterPositionControl::adjustSetpointForEKFResets(const vehicle_local_ } if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) { - _vel_x_deriv.reset(); - _vel_y_deriv.reset(); + _vel_xy_lp_filter.reset(_vel_xy_lp_filter.getState() + Vector2f(vehicle_local_position.delta_vxy)); + _vel_xy_notch_filter.reset(); } if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) { - _vel_z_deriv.reset(); + _vel_z_lp_filter.reset(_vel_z_lp_filter.getState() + vehicle_local_position.delta_vz); + _vel_z_notch_filter.reset(); } // save latest reset counters diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp index b1f88fde57d3..f7cd9bbbe09a 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp @@ -42,7 +42,9 @@ #include "GotoControl/GotoControl.hpp" #include -#include +#include +#include +#include #include #include #include @@ -68,8 +70,8 @@ using namespace time_literals; -class MulticopterPositionControl : public ModuleBase, public control::SuperBlock, - public ModuleParams, public px4::ScheduledWorkItem +class MulticopterPositionControl : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem { public: MulticopterPositionControl(bool vtol = false); @@ -146,6 +148,12 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_tiltmax_air, (ParamFloat) _param_mpc_thr_hover, (ParamBool) _param_mpc_use_hte, + (ParamBool) _param_mpc_acc_decouple, + + (ParamFloat) _param_mpc_vel_lp, + (ParamFloat) _param_mpc_vel_nf_frq, + (ParamFloat) _param_mpc_vel_nf_bw, + (ParamFloat) _param_mpc_veld_lp, // Takeoff / Land (ParamFloat) _param_com_spoolup_time, /**< time to let motors spool up after arming */ @@ -184,9 +192,16 @@ class MulticopterPositionControl : public ModuleBase (ParamFloat) _param_mpc_yawrauto_acc ); - control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */ - control::BlockDerivative _vel_y_deriv; /**< velocity derivative in y */ - control::BlockDerivative _vel_z_deriv; /**< velocity derivative in z */ + math::WelfordMean _sample_interval_s{}; + + AlphaFilter _vel_xy_lp_filter{}; + AlphaFilter _vel_z_lp_filter{}; + + math::NotchFilter _vel_xy_notch_filter{}; + math::NotchFilter _vel_z_notch_filter{}; + + AlphaFilter _vel_deriv_xy_lp_filter{}; + AlphaFilter _vel_deriv_z_lp_filter{}; GotoControl _goto_control; ///< class for handling smooth goto position setpoints PositionControl _control; ///< class for core PID position control @@ -223,7 +238,7 @@ class MulticopterPositionControl : public ModuleBase /** * Check for validity of positon/velocity states. */ - PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos); + PositionControlStates set_vehicle_states(const vehicle_local_position_s &local_pos, const float dt_s); /** * Generate setpoint to bridge no executable setpoint being available. diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index b4eba96115cd..3d1b1268c2cc 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -111,12 +111,6 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // copy quaternion setpoint to attitude setpoint topic const Quatf q_sp{R_sp}; q_sp.copyTo(att_sp.q_d); - - // calculate euler angles, for logging only, must not be used for control - const Eulerf euler{R_sp}; - att_sp.roll_body = euler.phi(); - att_sp.pitch_body = euler.theta(); - att_sp.yaw_body = euler.psi(); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp index e9b6fba12fb4..ebb23d7ed14e 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp @@ -108,18 +108,20 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) float yaw = 0.f; vehicle_attitude_setpoint_s att{}; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, 0.f); + EXPECT_FLOAT_EQ(att.q_d[0], 1.f); + EXPECT_FLOAT_EQ(att.q_d[1], 0.f); + EXPECT_FLOAT_EQ(att.q_d[2], 0.f); + EXPECT_FLOAT_EQ(att.q_d[3], 0.f); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but with 90 yaw * reason: only yaw changed */ yaw = M_PI_2_F; thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, 0.f); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); /* expected: same as before but roll 180 @@ -127,9 +129,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping) * order is: 1. roll, 2. pitch, 3. yaw */ thr = Vector3f(0.f, 0.f, 1.f); thrustToAttitude(thr, yaw, att); - EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F); - EXPECT_FLOAT_EQ(att.pitch_body, 0.f); - EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F); + Eulerf euler_att2(Quatf(att.q_d)); + EXPECT_FLOAT_EQ(std::abs(euler_att2.phi()), std::abs(M_PI_F)); + EXPECT_FLOAT_EQ(euler_att2.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att2.psi(), M_PI_2_F); EXPECT_FLOAT_EQ(att.thrust_body[2], -1.f); } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 955f3f4b44a7..72ae5a4a3173 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -185,15 +185,15 @@ void PositionControl::_velocityControl(const float dt) // Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output // see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990 const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust); - const float arw_gain = 2.f / _gain_vel_p(0); // The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently). // The ARW loop needs to run if the signal is saturated only. - const Vector2f acc_sp_xy = _acc_sp.xy(); - const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared()) - ? acc_sp_xy_produced - : acc_sp_xy; - vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy); + if (_acc_sp.xy().norm_squared() > acc_sp_xy_produced.norm_squared()) { + const float arw_gain = 2.f / _gain_vel_p(0); + const Vector2f acc_sp_xy = _acc_sp.xy(); + + vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_sp_xy_produced); + } // Make sure integral doesn't get NAN ControlMath::setZeroIfNanVector3f(vel_error); @@ -204,13 +204,20 @@ void PositionControl::_velocityControl(const float dt) void PositionControl::_accelerationControl() { // Assume standard acceleration due to gravity in vertical direction for attitude generation - Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized(); + float z_specific_force = -CONSTANTS_ONE_G; + + if (!_decouple_horizontal_and_vertical_acceleration) { + // Include vertical acceleration setpoint for better horizontal acceleration tracking + z_specific_force += _acc_sp(2); + } + + Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized(); ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt); - // Scale thrust assuming hover thrust produces standard gravity - float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; + // Convert to thrust assuming hover thrust produces standard gravity + const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust; // Project thrust to planned body attitude - collective_thrust /= (Vector3f(0, 0, 1).dot(body_z)); - collective_thrust = math::min(collective_thrust, -_lim_thr_min); + const float cos_ned_body = (Vector3f(0, 0, 1).dot(body_z)); + const float collective_thrust = math::min(thrust_ned_z / cos_ned_body, -_lim_thr_min); _thr_sp = body_z * collective_thrust; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp index 4eb909e1fa3e..85af876cd136 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp @@ -163,6 +163,11 @@ class PositionControl */ void resetIntegral() { _vel_int.setZero(); } + /** + * If set, the tilt setpoint is computed by assuming no vertical acceleration + */ + void decoupleHorizontalAndVecticalAcceleration(bool val) { _decouple_horizontal_and_vertical_acceleration = val; } + /** * Get the controllers output local position setpoint * These setpoints are the ones which were executed on including PID output and feed-forward. @@ -211,6 +216,7 @@ class PositionControl float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation + bool _decouple_horizontal_and_vertical_acceleration{true}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint // States matrix::Vector3f _pos; /**< current position */ diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp index 0e5a60619a44..9c1e72b1a5c8 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp @@ -56,9 +56,10 @@ TEST(PositionControlTest, EmptySetpoint) vehicle_attitude_setpoint_s attitude{}; position_control.getAttitudeSetpoint(attitude); - EXPECT_FLOAT_EQ(attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(attitude.pitch_body, 0.f); - EXPECT_FLOAT_EQ(attitude.yaw_body, 0.f); + Eulerf euler_att(Quatf(attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); + EXPECT_FLOAT_EQ(euler_att.psi(), 0.f); EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f); EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f)); EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f)); @@ -184,7 +185,8 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit) EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -MAXIMUM_THRUST); // Then the horizontal margin results in a tilt with the ratio of: horizontal margin / maximum thrust - EXPECT_FLOAT_EQ(_attitude.roll_body, asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); // TODO: add this line back once attitude setpoint generation strategy does not align body yaw with heading all the time anymore // EXPECT_FLOAT_EQ(_attitude.pitch_body, -asin((HORIZONTAL_THRUST_MARGIN / sqrt(2.f)) / MAXIMUM_THRUST)); } @@ -198,9 +200,10 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit) EXPECT_FLOAT_EQ(thrust.length(), 0.1f); EXPECT_FLOAT_EQ(_attitude.thrust_body[2], -0.1f); + Eulerf euler_att(Quatf(_attitude.q_d)); - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, -1.f); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), -1.f); } TEST_F(PositionControlBasicTest, FailsafeInput) @@ -377,6 +380,7 @@ TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint) EXPECT_TRUE(runController()); // THEN: the integral did not wind up and produce unexpected deviation - EXPECT_FLOAT_EQ(_attitude.roll_body, 0.f); - EXPECT_FLOAT_EQ(_attitude.pitch_body, 0.f); + Eulerf euler_att(Quatf(_attitude.q_d)); + EXPECT_FLOAT_EQ(euler_att.phi(), 0.f); + EXPECT_FLOAT_EQ(euler_att.theta(), 0.f); } diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c index 3f654fade71a..9174b1ef0d83 100644 --- a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -95,7 +95,7 @@ PARAM_DEFINE_INT32(MPC_ALT_MODE, 2); /** * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) * - * Only used with MPC_POS_MODE 0 or MPC_ALT_MODE 2 + * Only used with MPC_POS_MODE Direct velocity or MPC_ALT_MODE 2 * * @unit m/s * @min 0 diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c index 1bfc336fce48..e2e453d975ed 100644 --- a/src/modules/mc_pos_control/multicopter_autonomous_params.c +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -76,7 +76,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); /** * Acceleration for autonomous and for manual modes * - * When piloting manually, this parameter is only used in MPC_POS_MODE 4. + * When piloting manually, this parameter is only used in MPC_POS_MODE Acceleration based. * * @unit m/s^2 * @min 2 @@ -172,6 +172,7 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f); * @value 2 away from home * @value 3 along trajectory * @value 4 towards waypoint (yaw first) + * @value 5 yaw fixed * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); diff --git a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c index a60732cfa8c2..8307b82d7259 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_limits_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -138,3 +138,15 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); * @group Multicopter Position Control */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); + +/** + * Acceleration to tilt coupling + * + * Set to decouple tilt from vertical acceleration. + * This provides smoother flight but slightly worse tracking in position and auto modes. + * Unset if accurate position tracking during dynamic maneuvers is more important than a smooth flight. + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ACC_DECOUPLE, 1); diff --git a/src/modules/mc_pos_control/multicopter_position_control_params.c b/src/modules/mc_pos_control/multicopter_position_control_params.c index a8c00fb16738..94435b1b3352 100644 --- a/src/modules/mc_pos_control/multicopter_position_control_params.c +++ b/src/modules/mc_pos_control/multicopter_position_control_params.c @@ -75,11 +75,56 @@ PARAM_DEFINE_INT32(MPC_USE_HTE, 1); PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); /** - * Numerical velocity derivative low pass cutoff frequency + * Velocity low pass cutoff frequency + * + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_LP, 0.0f); + +/** + * Velocity notch filter frequency + * + * The center frequency for the 2nd order notch filter on the velocity. + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_NF_FRQ, 0.0f); + +/** + * Velocity notch filter bandwidth + * + * A value of 0 disables the filter. + * + * @unit Hz + * @min 0 + * @max 50 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_NF_BW, 5.0f); + +/** + * Velocity derivative low pass cutoff frequency + * + * A value of 0 disables the filter. * * @unit Hz * @min 0 - * @max 10 + * @max 50 * @decimal 1 * @increment 0.5 * @group Multicopter Position Control diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c index 56adad1144bf..a65ef4f5713d 100644 --- a/src/modules/mc_pos_control/multicopter_position_mode_params.c +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -35,12 +35,15 @@ * Position/Altitude mode variant * * The supported sub-modes are: - * 0 Sticks directly map to velocity setpoints without smoothing. + * - "Direct velocity": + * Sticks directly map to velocity setpoints without smoothing. * Also applies to vertical direction and Altitude mode. * Useful for velocity control tuning. - * 3 Sticks map to velocity but with maximum acceleration and jerk limits based on + * - "Smoothed velocity": + * Sticks map to velocity but with maximum acceleration and jerk limits based on * jerk optimized trajectory generator (different algorithm than 1). - * 4 Sticks map to acceleration and there's a virtual brake drag + * - "Acceleration based": + * Sticks map to acceleration and there's a virtual brake drag * * @value 0 Direct velocity * @value 3 Smoothed velocity @@ -122,7 +125,7 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); * * Setting this to the maximum value essentially disables the limit. * - * Only used with smooth MPC_POS_MODE 3 and 4. + * Only used with smooth MPC_POS_MODE Smoothed velocity and Acceleration based. * * @unit m/s^3 * @min 0.5 diff --git a/src/modules/muorb/apps/CMakeLists.txt b/src/modules/muorb/apps/CMakeLists.txt index 5a608bec5301..532ada62413b 100644 --- a/src/modules/muorb/apps/CMakeLists.txt +++ b/src/modules/muorb/apps/CMakeLists.txt @@ -39,7 +39,7 @@ px4_add_module( INCLUDES ../test ../aggregator - libfc-sensor-api/inc + ${PX4_BOARD_DIR}/libfc-sensor-api/inc SRCS uORBAppsProtobufChannel.cpp muorb_main.cpp diff --git a/src/modules/muorb/apps/Kconfig b/src/modules/muorb/apps/Kconfig index 21969d591d13..f0afac8b6204 100644 --- a/src/modules/muorb/apps/Kconfig +++ b/src/modules/muorb/apps/Kconfig @@ -4,3 +4,11 @@ menuconfig MODULES_MUORB_APPS depends on PLATFORM_POSIX ---help--- Enable support for muorb apps + + + config MUORB_APPS_SYNC_TIMESTAMP + bool "Sync timestamp with external processor" + depends on MODULES_MUORB_APPS + default y + help + causes HRT timestamp to use an externally calculated offset for synchronization diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index b27ef969aaeb..c2b3a8119807 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -49,7 +49,6 @@ void FeasibilityChecker::reset() _mission_validity_failed = false; _takeoff_failed = false; _land_pattern_validity_failed = false; - _distance_first_waypoint_failed = false; _distance_between_waypoints_failed = false; _fixed_wing_land_approach_failed = false; _takeoff_land_available_failed = false; @@ -118,12 +117,6 @@ void FeasibilityChecker::updateData() _is_landed = land_detected.landed; } - if (_vehicle_global_position_sub.updated()) { - vehicle_global_position_s vehicle_global_position = {}; - _vehicle_global_position_sub.copy(&vehicle_global_position); - _current_position_lat_lon = matrix::Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); - } - if (_rtl_status_sub.updated()) { rtl_status_s rtl_status = {}; _rtl_status_sub.copy(&rtl_status); @@ -199,8 +192,8 @@ void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int _distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item); } - if (!_distance_first_waypoint_failed) { - _distance_first_waypoint_failed = !checkHorizontalDistanceToFirstWaypoint(mission_item); + if (!_first_waypoint_found) { + checkHorizontalDistanceToFirstWaypoint(mission_item); } if (!_takeoff_failed) { @@ -587,7 +580,10 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() break; case 5: - if (!_is_landed && !_has_vtol_approach) { + if (_is_landed) { + result = hasMissionBothOrNeitherTakeoffAndLanding(); + + } else if (!_has_vtol_approach) { result = _landing_valid; if (!result) { @@ -595,9 +591,6 @@ bool FeasibilityChecker::checkTakeoffLandAvailable() events::send(events::ID("feasibility_mis_in_air_landing_req"), {events::Log::Error, events::LogInternal::Info}, "Mission rejected: Landing waypoint/pattern required"); } - - } else { - result = hasMissionBothOrNeitherTakeoffAndLanding(); } break; @@ -631,31 +624,32 @@ bool FeasibilityChecker::hasMissionBothOrNeitherTakeoffAndLanding() bool FeasibilityChecker::checkHorizontalDistanceToFirstWaypoint(mission_item_s &mission_item) { if (_param_mis_dist_1wp > FLT_EPSILON && - (_current_position_lat_lon.isAllFinite()) && !_first_waypoint_found && + (_home_lat_lon.isAllFinite()) && MissionBlock::item_contains_position(mission_item)) { _first_waypoint_found = true; - float dist_to_1wp_from_current_pos = 1e6f; - - if (_current_position_lat_lon.isAllFinite()) { - dist_to_1wp_from_current_pos = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _current_position_lat_lon(0), _current_position_lat_lon(1)); - } + const float dist_to_1wp_from_home_pos = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, + _home_lat_lon(0), _home_lat_lon(1)); - if (dist_to_1wp_from_current_pos < _param_mis_dist_1wp) { + if (dist_to_1wp_from_home_pos < _param_mis_dist_1wp) { return true; } else { /* item is too far from current position */ mavlink_log_critical(_mavlink_log_pub, - "First waypoint too far away: %dm, %d max\t", - (int)dist_to_1wp_from_current_pos, (int)_param_mis_dist_1wp); - events::send(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info}, - "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp_from_current_pos, - (uint32_t)_param_mis_dist_1wp); + "First waypoint far away from home: %dm. Correct mission loaded?\t", + (int)dist_to_1wp_from_home_pos); + /* EVENT + * @description + * + * This check can be configured via MIS_DIST_1WP parameter. + * + */ + events::send(events::ID("navigator_mis_first_wp_far"), {events::Log::Warning, events::LogInternal::Info}, + "First waypoint far away from Home: {1m} Correct mission loaded?", (uint32_t)dist_to_1wp_from_home_pos); return false; } diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp index fd18cbcfb68e..d0905d363545 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp @@ -78,7 +78,6 @@ class FeasibilityChecker : public ModuleParams bool someCheckFailed() { return _takeoff_failed || - _distance_first_waypoint_failed || _distance_between_waypoints_failed || _land_pattern_validity_failed || _fixed_wing_land_approach_failed || @@ -97,7 +96,6 @@ class FeasibilityChecker : public ModuleParams uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)}; - uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; uORB::Subscription _rtl_status_sub{ORB_ID(rtl_status)}; // parameters @@ -110,14 +108,12 @@ class FeasibilityChecker : public ModuleParams float _home_alt_msl{NAN}; bool _has_vtol_approach{false}; matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); - matrix::Vector2d _current_position_lat_lon = matrix::Vector2d((double)NAN, (double)NAN); VehicleType _vehicle_type{VehicleType::RotaryWing}; // internal flags to keep track of which checks failed bool _mission_validity_failed{false}; bool _takeoff_failed{false}; bool _land_pattern_validity_failed{false}; - bool _distance_first_waypoint_failed{false}; bool _distance_between_waypoints_failed{false}; bool _fixed_wing_land_approach_failed{false}; bool _takeoff_land_available_failed{false}; diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp index 0de54f33766c..43662441488a 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp @@ -170,9 +170,9 @@ TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint) mission_item.lat = lat_new; mission_item.lon = lon_new; - // THEN: fail + // THEN: pass checker.processNextItem(mission_item, 0, 1); - ASSERT_EQ(checker.someCheckFailed(), true); + ASSERT_EQ(checker.someCheckFailed(), false); // BUT WHEN: valid current position fist WP 499m away from current checker.reset(); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 55917331a614..00a1e3f94011 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -104,7 +104,7 @@ void Geofence::run() _initiate_fence_updated = false; _dataman_state = DatamanState::Read; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_LOADING; @@ -159,7 +159,7 @@ void Geofence::run() _dataman_state = DatamanState::UpdateRequestWait; _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -179,7 +179,7 @@ void Geofence::run() _updateFence(); _fence_updated = true; - geofence_status_s status; + geofence_status_s status{}; status.timestamp = hrt_absolute_time(); status.geofence_id = _opaque_id; status.status = geofence_status_s::GF_STATUS_READY; @@ -406,17 +406,25 @@ bool Geofence::checkPointAgainstPolygonCircle(const PolygonInfo &polygon, double { bool checksPass = true; - if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + switch (polygon.fence_type) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: checksPass &= insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: checksPass &= !insideCircle(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: checksPass &= insidePolygon(polygon, lat, lon, altitude); + break; - } else if (polygon.fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: checksPass &= !insidePolygon(polygon, lat, lon, altitude); + break; + + default: // unknown fence type + break; } return checksPass; @@ -452,12 +460,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, break; } - if (temp_vertex_i.frame != NAV_FRAME_GLOBAL && temp_vertex_i.frame != NAV_FRAME_GLOBAL_INT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && temp_vertex_i.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (temp_vertex_i.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame); - break; + return c; + } if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && @@ -482,12 +496,18 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, return false; } - if (circle_point.frame != NAV_FRAME_GLOBAL && circle_point.frame != NAV_FRAME_GLOBAL_INT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT - && circle_point.frame != NAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { + switch (circle_point.frame) { + case NAV_FRAME_GLOBAL: + case NAV_FRAME_GLOBAL_INT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT: + case NAV_FRAME_GLOBAL_RELATIVE_ALT_INT: + break; + + default: // TODO: handle different frames PX4_ERR("Frame type %i not supported", (int)circle_point.frame); return false; + } if (!_projection_reference.isInitialized()) { @@ -648,7 +668,7 @@ Geofence::loadFromFile(const char *filename) } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); - events::send(events::ID("navigator_geofence_import_failed"), events::Log::Error, "Geofence: import error"); + events::send(events::ID("navigator_geofence_import_failed"), events::Log::Critical, "Geofence: import error"); } updateFence(); @@ -675,20 +695,26 @@ void Geofence::printStatus() for (int i = 0; i < _num_polygons; ++i) { total_num_vertices += _polygons[i].vertex_count; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION) { + switch (_polygons[i].fence_type) { + case NAV_CMD_FENCE_POLYGON_VERTEX_INCLUSION: ++num_inclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION) { + case NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION: ++num_exclusion_polygons; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_INCLUSION) { + case NAV_CMD_FENCE_CIRCLE_INCLUSION: ++num_inclusion_circles; - } + break; - if (_polygons[i].fence_type == NAV_CMD_FENCE_CIRCLE_EXCLUSION) { + case NAV_CMD_FENCE_CIRCLE_EXCLUSION: ++num_exclusion_circles; + break; + + default: // unknown fence type + break; + } } diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index b6a951d1b766..8277378a6670 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -42,7 +42,7 @@ #include "navigator.h" Land::Land(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) { } @@ -57,8 +57,13 @@ Land::on_activation() /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous.valid = false; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _navigator->calculate_breaking_stop(_mission_item.lat, _mission_item.lon); + } + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->previous.valid = false; pos_sp_triplet->next.valid = false; _navigator->set_position_setpoint_triplet_updated(); @@ -93,7 +98,7 @@ Land::on_active() if (_navigator->get_land_detected()->landed) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND); + _navigator->mode_completed(getNavigatorStateId()); set_idle_item(&_mission_item); struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 15408baaff05..db61030528c1 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -43,7 +43,7 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER), ModuleParams(navigator) { } diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 06b4151bdb9b..ea7dae675217 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -65,7 +65,7 @@ using namespace time_literals; static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; Mission::Mission(Navigator *navigator) : - MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { } @@ -216,6 +216,23 @@ void Mission::setActiveMissionItems() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; + /* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/ + if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + !_land_detected_sub.get().landed) { + if (setNextMissionItem()) { + if (!loadCurrentMissionItem()) { + setEndOfMissionItems(); + return; + } + + } else { + setEndOfMissionItems(); + return; + } + } + if (item_contains_position(_mission_item)) { handleTakeoff(new_work_item_type, next_mission_items, num_found_items); diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp index 4a376995a098..8963a0685d08 100644 --- a/src/modules/navigator/mission_base.cpp +++ b/src/modules/navigator/mission_base.cpp @@ -46,8 +46,8 @@ #include "mission_feasibility_checker.h" #include "navigator.h" -MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : - MissionBlock(navigator), +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id) : + MissionBlock(navigator, navigator_state_id), ModuleParams(navigator), _dataman_cache_size_signed(dataman_cache_size_signed) { @@ -235,6 +235,7 @@ MissionBase::on_activation() checkClimbRequired(_mission.current_seq); set_mission_items(); + _mission_activation_index = _mission.current_seq; _inactivation_index = -1; // reset // reset cruise speed @@ -253,6 +254,7 @@ MissionBase::on_active() updateMavlinkMission(); updateDatamanCache(); + updateMissionAltAfterHomeChanged(); /* Check the mission */ if (!_mission_checked && canRunMissionFeasibility()) { @@ -292,17 +294,27 @@ MissionBase::on_active() _align_heading_necessary = false; } - // replay gimbal and camera commands immediately after resuming mission - if (haveCachedGimbalOrCameraItems()) { - replayCachedGimbalCameraItems(); + // Replay camera mode commands immediately upon mission resume + if (haveCachedCameraModeItems()) { + replayCachedCameraModeItems(); } - // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set - if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { - replayCachedTriggerItems(); - } - replayCachedSpeedChangeItems(); + // Replay cached mission commands once the last mission waypoint is re-reached after the mission interruption. + // Each replay function also clears the cached items afterwards + if (_mission.current_seq > _mission_activation_index) { + // replay gimbal commands + if (haveCachedGimbalItems()) { + replayCachedGimbalItems(); + } + + // replay trigger commands + if (cameraWasTriggering()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + } /* lets check if we reached the current mission item */ if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { @@ -351,11 +363,13 @@ MissionBase::on_active() } else if (_navigator->get_precland()->is_activated()) { _navigator->get_precland()->on_inactivation(); } + + updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } void MissionBase::update_mission() { - if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_navigator->get_mission_result()->valid) { + if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !isMissionValid()) { if (_land_detected_sub.get().landed) { /* landed, refusing to take off without a mission */ mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); @@ -440,24 +454,33 @@ MissionBase::advance_mission() void MissionBase::set_mission_items() { - if (_is_current_planned_mission_item_valid) { + bool set_end_of_mission{false}; + + if (_is_current_planned_mission_item_valid && _mission_type == MissionType::MISSION_TYPE_MISSION && isMissionValid()) { /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ - loadCurrentMissionItem(); + if (loadCurrentMissionItem()) { + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; + } - /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { - _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; - } + setActiveMissionItems(); - setActiveMissionItems(); + } else { + set_end_of_mission = true; + } } else { + set_end_of_mission = true; + } + + if (set_end_of_mission) { setEndOfMissionItems(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + _navigator->mode_completed(getNavigatorStateId()); } } -void MissionBase::loadCurrentMissionItem() +bool MissionBase::loadCurrentMissionItem() { const dm_item_t dm_item = static_cast(_mission.mission_dataman_id); bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), @@ -468,6 +491,8 @@ void MissionBase::loadCurrentMissionItem() events::send(events::ID("mission_item_set_failed"), events::Log::Error, "Mission item could not be set"); } + + return success; } void MissionBase::setEndOfMissionItems() @@ -856,7 +881,6 @@ void MissionBase::publish_navigator_mission_item() { navigator_mission_item_s navigator_mission_item{}; - navigator_mission_item.instance_count = _navigator->mission_instance_count(); navigator_mission_item.sequence_current = _mission.current_seq; navigator_mission_item.nav_cmd = _mission_item.nav_cmd; navigator_mission_item.latitude = _mission_item.lat; @@ -1242,7 +1266,7 @@ void MissionBase::cacheItem(const mission_item_s &mission_item) } } -void MissionBase::replayCachedGimbalCameraItems() +void MissionBase::replayCachedGimbalItems() { if (_last_gimbal_configure_item.nav_cmd > 0) { issue_command(_last_gimbal_configure_item); @@ -1253,7 +1277,10 @@ void MissionBase::replayCachedGimbalCameraItems() issue_command(_last_gimbal_control_item); _last_gimbal_control_item = {}; // delete cached item } +} +void MissionBase::replayCachedCameraModeItems() +{ if (_last_camera_mode_item.nav_cmd > 0) { issue_command(_last_camera_mode_item); _last_camera_mode_item = {}; // delete cached item @@ -1284,11 +1311,15 @@ void MissionBase::resetItemCache() _last_camera_trigger_item = {}; } -bool MissionBase::haveCachedGimbalOrCameraItems() +bool MissionBase::haveCachedGimbalItems() { return _last_gimbal_configure_item.nav_cmd > 0 || - _last_gimbal_control_item.nav_cmd > 0 || - _last_camera_mode_item.nav_cmd > 0; + _last_gimbal_control_item.nav_cmd > 0; +} + +bool MissionBase::haveCachedCameraModeItems() +{ + return _last_camera_mode_item.nav_cmd > 0; } bool MissionBase::cameraWasTriggering() @@ -1375,3 +1406,29 @@ bool MissionBase::canRunMissionFeasibility() (_geofence_status_sub.get().geofence_id == _mission.geofence_id) && (_geofence_status_sub.get().status == geofence_status_s::GF_STATUS_READY); } + +void MissionBase::updateMissionAltAfterHomeChanged() +{ + if (_navigator->get_home_position()->update_count > _home_update_counter) { + float new_alt = get_absolute_altitude_for_item(_mission_item); + float altitude_diff = new_alt - _navigator->get_position_setpoint_triplet()->current.alt; + + if (_navigator->get_position_setpoint_triplet()->previous.valid + && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->previous.alt)) { + _navigator->get_position_setpoint_triplet()->previous.alt = _navigator->get_position_setpoint_triplet()->previous.alt + + altitude_diff; + } + + _navigator->get_position_setpoint_triplet()->current.alt = _navigator->get_position_setpoint_triplet()->current.alt + + altitude_diff; + + if (_navigator->get_position_setpoint_triplet()->next.valid + && PX4_ISFINITE(_navigator->get_position_setpoint_triplet()->next.alt)) { + _navigator->get_position_setpoint_triplet()->next.alt = _navigator->get_position_setpoint_triplet()->next.alt + + altitude_diff; + } + + _navigator->set_position_setpoint_triplet_updated(); + _home_update_counter = _navigator->get_home_position()->update_count; + } +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index 7f234bb7d277..f819b5c0e51a 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -65,7 +65,7 @@ class Navigator; class MissionBase : public MissionBlock, public ModuleParams { public: - MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed, uint8_t navigator_state_id); ~MissionBase() override = default; virtual void on_inactive() override; @@ -118,12 +118,12 @@ class MissionBase : public MissionBlock, public ModuleParams void getNextPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, uint8_t max_num_items); /** - * @brief Has Mission a Land Start or Land Item + * @brief Mission has a land start, a land, and is valid * - * @return true If mission has a land start of land item and a land item + * @return true If mission has a land start and a land item and is valid * @return false otherwise */ - bool hasMissionLandStart() const { return _mission.land_start_index >= 0 && _mission.land_index >= 0;}; + bool hasMissionLandStart() const { return _mission.land_start_index >= 0 && _mission.land_index >= 0 && isMissionValid();}; /** * @brief Go to next Mission Item * Go to next non jump mission item @@ -250,8 +250,9 @@ class MissionBase : public MissionBlock, public ModuleParams * @brief Load current mission item * * Load current mission item from dataman cache. + * @return true, if the mission item could be loaded, false otherwise */ - void loadCurrentMissionItem(); + bool loadCurrentMissionItem(); /** * Set the mission result @@ -313,6 +314,13 @@ class MissionBase : public MissionBlock, public ModuleParams */ bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + /** + * @brief Set the Mission Index + * + * @param[in] index Index of the mission item + */ + void setMissionIndex(int32_t index); + bool _is_current_planned_mission_item_valid{false}; /**< Flag indicating if the currently loaded mission item is valid*/ bool _mission_has_been_activated{false}; /**< Flag indicating if the mission has been activated*/ bool _mission_checked{false}; /**< Flag indicating if the mission has been checked by the mission validator*/ @@ -320,6 +328,7 @@ class MissionBase : public MissionBlock, public ModuleParams mission_s _mission; /**< Currently active mission*/ float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ 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. + int _mission_activation_index{-1}; /**< Index of the mission item that will bring the vehicle back to a mission waypoint */ int32_t _load_mission_index{-1}; /**< Mission inted of loaded mission items in dataman cache*/ int32_t _dataman_cache_size_signed; /**< Size of the dataman cache. A negativ value indicates that previous mission items should be loaded, a positiv value the next mission items*/ @@ -390,9 +399,14 @@ class MissionBase : public MissionBlock, public ModuleParams void updateCachedItemsUpToIndex(int end_index); /** - * @brief Replay the cached gimbal and camera mode items + * @brief Replay the cached gimbal items */ - void replayCachedGimbalCameraItems(); + void replayCachedGimbalItems(); + + /** + * @brief Replay the cached camera mode items + */ + void replayCachedCameraModeItems(); /** * @brief Replay the cached trigger items @@ -407,25 +421,25 @@ class MissionBase : public MissionBlock, public ModuleParams void replayCachedSpeedChangeItems(); /** - * @brief Check if there are cached gimbal or camera mode items to be replayed + * @brief Check if there are cached gimbal items to be replayed * * @return true if there are cached items */ - bool haveCachedGimbalOrCameraItems(); + bool haveCachedGimbalItems(); /** - * @brief Check if the camera was triggering + * @brief Check if there are cached camera mode items to be replayed * - * @return true if there was a camera trigger command in the cached items that didn't disable triggering + * @return true if there are cached items */ - bool cameraWasTriggering(); + bool haveCachedCameraModeItems(); /** - * @brief Set the Mission Index + * @brief Check if the camera was triggering * - * @param[in] index Index of the mission item + * @return true if there was a camera trigger command in the cached items that didn't disable triggering */ - void setMissionIndex(int32_t index); + bool cameraWasTriggering(); /** * @brief Parameters update @@ -448,8 +462,16 @@ class MissionBase : public MissionBlock, public ModuleParams */ bool checkMissionDataChanged(mission_s new_mission); + /** + * @brief update current mission altitude after the home position has changed. + */ + + void updateMissionAltAfterHomeChanged(); + bool canRunMissionFeasibility(); + uint32_t _home_update_counter = 0; /**< Variable to store the previous value for home change detection.*/ + 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 {}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 17dd3e620ca2..2659ad95cc57 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -55,8 +55,8 @@ using matrix::wrap_pi; -MissionBlock::MissionBlock(Navigator *navigator) : - NavigatorMode(navigator) +MissionBlock::MissionBlock(Navigator *navigator, uint8_t navigator_state_id) : + NavigatorMode(navigator, navigator_state_id) { } @@ -1011,3 +1011,60 @@ void MissionBlock::startPrecLand(uint16_t land_precision) _navigator->get_precland()->on_activation(); } } + +void MissionBlock::updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item) +{ + // Avoid flying into terrain using the distance sensor. Enable through the parameter NAV_MIN_GND_DIST. + // Only active during commanded descents with vz>0 (to prevent climb-aways), excluding landing and VTOL transitions. + // It changes the altitude setpoint in the triplet to maintain the current altitude and republish the triplet. + // We also change the mission item altitude used for acceptance calculations to prevent getting stuck in a loop. + + // This threshold is needed to prevent the check from re-triggering due to small altitude over-shoots while + // tracking the new altitude setpoint. + static constexpr float kAltitudeDifferenceForDescentCondition = 2.f; + + + if (_navigator->get_nav_min_gnd_dist_param() > FLT_EPSILON && _mission_item.nav_cmd != NAV_CMD_LAND + && _mission_item.nav_cmd != NAV_CMD_VTOL_LAND && _mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION + && _navigator->get_local_position()->dist_bottom_valid + && _navigator->get_local_position()->dist_bottom < _navigator->get_nav_min_gnd_dist_param() + && _navigator->get_local_position()->vz > FLT_EPSILON + && _navigator->get_global_position()->alt - get_absolute_altitude_for_item(mission_item) > + kAltitudeDifferenceForDescentCondition) { + + _navigator->sendWarningDescentStoppedDueToTerrain(); + + struct position_setpoint_s *curr_sp = &_navigator->get_position_setpoint_triplet()->current; + curr_sp->alt = _navigator->get_global_position()->alt; + _navigator->set_position_setpoint_triplet_updated(); + + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + } +} + +void MissionBlock::updateFailsafeChecks() +{ + updateMaxHaglFailsafe(); +} + +void MissionBlock::updateMaxHaglFailsafe() +{ + const float target_alt = _navigator->get_position_setpoint_triplet()->current.alt; + + if (_navigator->get_global_position()->terrain_alt_valid + && ((target_alt - _navigator->get_global_position()->terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the altitude setpoint is above the maximum HAGL (height above ground level) + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Target altitude higher than max HAGL\t"); + events::send(events::ID("navigator_fail_max_hagl"), events::Log::Error, "Target altitude higher than max HAGL"); + + _navigator->trigger_hagl_failsafe(getNavigatorStateId()); + + // While waiting for a failsafe action from commander, keep the curren position + setLoiterItemFromCurrentPosition(&_mission_item); + + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + _navigator->set_position_setpoint_triplet_updated(); + } +} diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index b97731567a29..c19fcfe11877 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -64,7 +64,7 @@ class MissionBlock : public NavigatorMode /** * Constructor */ - MissionBlock(Navigator *navigator); + MissionBlock(Navigator *navigator, uint8_t navigator_state_id); virtual ~MissionBlock() = default; MissionBlock(const MissionBlock &) = delete; @@ -145,6 +145,8 @@ class MissionBlock : public NavigatorMode void set_align_mission_item(struct mission_item_s *const mission_item, const struct mission_item_s *const mission_item_next) const; + void updateFailsafeChecks() override; + protected: /** * @brief heading mode for setting navigation items @@ -215,6 +217,7 @@ class MissionBlock : public NavigatorMode void setLandMissionItem(mission_item_s &item, const PositionYawSetpoint &pos_yaw_sp) const; void startPrecLand(uint16_t land_precision); + void updateAltToAvoidTerrainCollisionAndRepublishTriplet(mission_item_s mission_item); /** * @brief Issue a command for mission items with a nav_cmd that specifies an action @@ -248,4 +251,8 @@ class MissionBlock : public NavigatorMode bool _payload_deploy_ack_successful{false}; // Flag to keep track of whether we received an acknowledgement for a successful payload deployment hrt_abstime _payload_deployed_time{0}; // Last payload deployment start time to handle timeouts float _payload_deploy_timeout_s{0.0f}; // Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received + +private: + void updateMaxHaglFailsafe(); + }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index dc2420d55bf5..8049df1236cf 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -41,7 +41,6 @@ */ #include "mission_feasibility_checker.h" -#include "MissionFeasibility/FeasibilityChecker.hpp" #include "mission_block.h" #include "navigator.h" diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 051f3c816b22..1c637b8a50d9 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -68,7 +68,7 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f); * @value 2 Require a landing * @value 3 Require a takeoff and a landing * @value 4 Require both a takeoff and a landing, or neither - * @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present + * @value 5 Same as previous when landed, in-air require landing only if no valid VTOL approach is present * @group Mission */ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); @@ -87,7 +87,7 @@ PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0); * @increment 100 * @group Mission */ -PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900); +PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 10000); /** * Enable yaw control of the mount. (Only affects multicopters and ROI mission items) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 5efcb45bf054..ec72881fc527 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -69,6 +69,7 @@ #include #include #include +#include #include #include #include @@ -249,8 +250,6 @@ class Navigator : public ModuleBase, public ModuleParams orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - int mission_instance_count() const { return _mission_result.mission_id; } - void set_mission_failure_heading_timeout(); bool get_mission_start_land_available() { return _mission.get_land_start_available(); } @@ -266,9 +265,9 @@ class Navigator : public ModuleBase, public ModuleParams int get_loiter_min_alt() const { return _param_min_ltr_alt.get(); } int get_landing_abort_min_alt() const { return _param_mis_lnd_abrt_alt.get(); } float get_param_mis_takeoff_alt() const { return _param_mis_takeoff_alt.get(); } - int get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); } float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); } float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); } + float get_nav_min_gnd_dist_param() const { return _param_nav_min_gnd_dist.get(); } float get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; } @@ -285,6 +284,12 @@ class Navigator : public ModuleBase, public ModuleParams void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); + void set_failsafe_status(uint8_t nav_state, bool failsafe); + + void sendWarningDescentStoppedDueToTerrain(); + + void trigger_hagl_failsafe(uint8_t nav_state); + private: int _local_pos_sub{-1}; @@ -305,6 +310,7 @@ class Navigator : public ModuleBase, public ModuleParams uORB::Publication _geofence_result_pub{ORB_ID(geofence_result)}; uORB::Publication _mission_result_pub{ORB_ID(mission_result)}; + uORB::Publication _navigator_status_pub{ORB_ID(navigator_status)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; uORB::Publication _vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)}; uORB::Publication _vehicle_cmd_pub{ORB_ID(vehicle_command)}; @@ -324,6 +330,7 @@ class Navigator : public ModuleBase, public ModuleParams // Publications geofence_result_s _geofence_result{}; + navigator_status_s _navigator_status{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ position_setpoint_triplet_s _reposition_triplet{}; /**< triplet for non-mission direct position command */ position_setpoint_triplet_s _takeoff_triplet{}; /**< triplet for non-mission direct takeoff command */ @@ -333,7 +340,10 @@ class Navigator : public ModuleBase, public ModuleParams Geofence _geofence; /**< class that handles the geofence */ GeofenceBreachAvoidance _gf_breach_avoidance; - hrt_abstime _last_geofence_check = 0; + hrt_abstime _last_geofence_check{0}; + + bool _navigator_status_updated{false}; + hrt_abstime _last_navigator_status_publication{0}; hrt_abstime _wait_for_vehicle_status_timestamp{0}; /**< If non-zero, wait for vehicle_status update before processing next cmd */ @@ -386,6 +396,8 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_mission_result(); + void publish_navigator_status(); + void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result); bool geofence_allows_position(const vehicle_global_position_s &pos); @@ -403,10 +415,11 @@ class Navigator : public ModuleBase, public ModuleParams (ParamFloat) _param_nav_traff_a_ver, /**< avoidance Distance Vertical*/ (ParamInt) _param_nav_traff_collision_time, (ParamFloat) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/ + (ParamFloat) + _param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/ // non-navigator parameters: Mission (MIS_*) (ParamFloat) _param_mis_takeoff_alt, - (ParamInt) _para_mis_takeoff_land_req, (ParamFloat) _param_mis_yaw_tmt, (ParamFloat) _param_mis_yaw_err, (ParamFloat) _param_mis_payload_delivery_timeout, diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8e2f24e1574c..15db1abdc44d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -41,6 +41,7 @@ * @author Julian Oes * @author Anton Babushkin * @author Thomas Gubler + * and many more... */ #include "navigator.h" @@ -897,6 +898,8 @@ void Navigator::run() publish_mission_result(); } + publish_navigator_status(); + _geofence.run(); perf_end(_loop_perf); @@ -1069,7 +1072,7 @@ int Navigator::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_NAVIGATION, - PX4_STACK_ADJUSTED(2160), + PX4_STACK_ADJUSTED(2200), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -1273,6 +1276,9 @@ void Navigator::check_traffic() } } } + + _adsb_conflict.remove_expired_conflicts(); + } bool Navigator::abort_landing() @@ -1352,6 +1358,40 @@ void Navigator::set_mission_failure_heading_timeout() } } +void Navigator::trigger_hagl_failsafe(const uint8_t nav_state) +{ + if ((_navigator_status.failure != navigator_status_s::FAILURE_HAGL) || _navigator_status.nav_state != nav_state) { + _navigator_status.failure = navigator_status_s::FAILURE_HAGL; + _navigator_status.nav_state = nav_state; + + _navigator_status_updated = true; + } +} + +void Navigator::publish_navigator_status() +{ + uint8_t current_nav_state = _vstatus.nav_state; + + if (_navigation_mode != nullptr) { + current_nav_state = _navigation_mode->getNavigatorStateId(); + } + + if (_navigator_status.nav_state != current_nav_state) { + _navigator_status.nav_state = current_nav_state; + _navigator_status.failure = navigator_status_s::FAILURE_NONE; + _navigator_status_updated = true; + } + + if (_navigator_status_updated + || (hrt_elapsed_time(&_last_navigator_status_publication) > 500_ms)) { + _navigator_status.timestamp = hrt_absolute_time(); + _navigator_status_pub.publish(_navigator_status); + + _navigator_status_updated = false; + _last_navigator_status_publication = hrt_absolute_time(); + } +} + void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) { vcmd->timestamp = hrt_absolute_time(); @@ -1519,6 +1559,13 @@ void Navigator::set_gimbal_neutral() publish_vehicle_cmd(&vcmd); } +void Navigator::sendWarningDescentStoppedDueToTerrain() +{ + mavlink_log_critical(&_mavlink_log_pub, "Terrain collision risk, descent is stopped\t"); + events::send(events::ID("navigator_terrain_collision_risk"), events::Log::Critical, + "Terrain collision risk, descent is stopped"); +} + int Navigator::print_usage(const char *reason) { if (reason) { @@ -1544,7 +1591,7 @@ controller. PRINT_MODULE_USAGE_NAME("navigator", "controller"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt"); - PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 4 fake transponder_report_s uORB messages"); + PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 24 fake transponder_report_s uORB messages"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index b0ced9006c56..1c0882923402 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -42,8 +42,9 @@ #include "navigator_mode.h" #include "navigator.h" -NavigatorMode::NavigatorMode(Navigator *navigator) : - _navigator(navigator) +NavigatorMode::NavigatorMode(Navigator *navigator, uint8_t navigator_state_id) : + _navigator(navigator), + _navigator_state_id(navigator_state_id) { /* set initial mission items */ on_inactivation(); @@ -60,6 +61,7 @@ NavigatorMode::run(bool active) } else { /* periodic updates when active */ on_active(); + updateFailsafeChecks(); } } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 60abdcd165e0..9164a92b92fc 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -41,12 +41,14 @@ #pragma once +#include + class Navigator; class NavigatorMode { public: - NavigatorMode(Navigator *navigator); + NavigatorMode(Navigator *navigator, uint8_t navigator_state_id); virtual ~NavigatorMode() = default; NavigatorMode(const NavigatorMode &) = delete; NavigatorMode &operator=(const NavigatorMode &) = delete; @@ -56,6 +58,8 @@ class NavigatorMode bool isActive() {return _active;}; + uint8_t getNavigatorStateId() const { return _navigator_state_id; } + /** * This function is called while the mode is inactive */ @@ -76,9 +80,12 @@ class NavigatorMode */ virtual void on_active(); + virtual void updateFailsafeChecks() {}; + protected: Navigator *_navigator{nullptr}; private: bool _active{false}; + uint8_t _navigator_state_id{0}; }; diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index c300c8e3c848..325b19abf605 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -187,3 +187,21 @@ PARAM_DEFINE_INT32(NAV_FORCE_VT, 1); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_MIN_LTR_ALT, -1.f); + +/** + * Minimum height above ground during Mission and RTL + * + * Minimum height above ground the vehicle is allowed to descend to during Mission and RTL, + * excluding landing commands. + * Requires a distance sensor to be set up. + * Note: only prevents the vehicle from descending further, but does not force it to climb. + * + * Set to a negative value to disable. + * + * @unit m + * @min -1 + * @decimal 1 + * @increment 1 + * @group Mission + */ +PARAM_DEFINE_FLOAT(NAV_MIN_GND_DIST, -1.f); diff --git a/src/modules/navigator/precland.cpp b/src/modules/navigator/precland.cpp index 080491d37ca7..74c81569cf3d 100644 --- a/src/modules/navigator/precland.cpp +++ b/src/modules/navigator/precland.cpp @@ -62,7 +62,7 @@ static constexpr const char *LOST_TARGET_ERROR_MESSAGE = "Lost landing target while landing"; PrecLand::PrecLand(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND), ModuleParams(navigator) { _handle_param_acceleration_hor = param_find("MPC_ACC_HOR"); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index bfa5d486be45..63e2ab185ff1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -55,7 +55,7 @@ static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES{10.0f}; // [m] We static constexpr float MIN_DIST_THRESHOLD = 2.f; RTL::RTL(Navigator *navigator) : - NavigatorMode(navigator), + NavigatorMode(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator), _rtl_direct(navigator) { @@ -283,10 +283,12 @@ void RTL::on_active() case RtlType::RTL_MISSION_FAST_REVERSE: case RtlType::RTL_DIRECT_MISSION_LAND: _rtl_mission_type_handle->on_active(); + _rtl_mission_type_handle->updateFailsafeChecks(); break; case RtlType::RTL_DIRECT: _rtl_direct.on_active(); + _rtl_direct.updateFailsafeChecks(); break; default: @@ -528,13 +530,14 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & // avoid the vehicle touching the ground while still moving horizontally. const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); + const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); + + float return_altitude_amsl = max_return_altitude; if (destination_dist <= _param_nav_acc_rad.get()) { return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { - if (destination_dist <= _param_rtl_min_dist.get()) { // constrain cone half angle to meaningful values. All other cases are already handled above. @@ -549,7 +552,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, _global_pos_sub.get().alt); + return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); } void RTL::init_rtl_mission_type() @@ -617,7 +620,8 @@ void RTL::parameters_update() bool RTL::hasMissionLandStart() const { - return _mission_sub.get().land_start_index >= 0 && _mission_sub.get().land_index >= 0; + return _mission_sub.get().land_start_index >= 0 && _mission_sub.get().land_index >= 0 + && _navigator->get_mission_result()->valid; } bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index ac6a5a75d774..e7b720bd6152 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -97,6 +97,11 @@ class RTL : public NavigatorMode, public ModuleParams }; private: + + /** + * @brief Check mission landing validity + * @return true if mission has a land start, a land and is valid + */ bool hasMissionLandStart() const; /** diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h index dc698687fce6..a4c88e06b026 100644 --- a/src/modules/navigator/rtl_base.h +++ b/src/modules/navigator/rtl_base.h @@ -46,7 +46,7 @@ class RtlBase : public MissionBase { public: RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): - MissionBase(navigator, dataman_cache_size_signed) {}; + MissionBase(navigator, dataman_cache_size_signed, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {}; virtual ~RtlBase() = default; virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index d2936986db9a..145b99d0d1db 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -51,7 +51,7 @@ using namespace math; RtlDirect::RtlDirect(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL), ModuleParams(navigator) { _destination.lat = static_cast(NAN); @@ -104,6 +104,12 @@ void RtlDirect::on_active() set_rtl_item(); } + if (_rtl_state != RTLState::IDLE) { //TODO: rename _rtl_state to _rtl_state_next (when in IDLE we're actually in LAND) + //check for terrain collision and update altitude if needed + // note: it may trigger multiple times during a RTL, as every time the altitude set is reset + updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); + } + if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active(); @@ -312,7 +318,7 @@ void RtlDirect::set_rtl_item() case RTLState::IDLE: { set_idle_item(&_mission_item); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + _navigator->mode_completed(getNavigatorStateId()); break; } @@ -332,6 +338,8 @@ void RtlDirect::set_rtl_item() _navigator->set_position_setpoint_triplet_updated(); } } + + publish_rtl_direct_navigator_mission_item(); // for logging } RtlDirect::RTLState RtlDirect::getActivationLandState() @@ -510,3 +518,32 @@ loiter_point_s RtlDirect::sanitizeLandApproach(loiter_point_s land_approach) con return sanitized_land_approach; } + +void RtlDirect::publish_rtl_direct_navigator_mission_item() +{ + navigator_mission_item_s navigator_mission_item{}; + + navigator_mission_item.sequence_current = static_cast(_rtl_state); + navigator_mission_item.nav_cmd = _mission_item.nav_cmd; + navigator_mission_item.latitude = _mission_item.lat; + navigator_mission_item.longitude = _mission_item.lon; + navigator_mission_item.altitude = _mission_item.altitude; + + navigator_mission_item.time_inside = get_time_inside(_mission_item); + navigator_mission_item.acceptance_radius = _mission_item.acceptance_radius; + navigator_mission_item.loiter_radius = _mission_item.loiter_radius; + navigator_mission_item.yaw = _mission_item.yaw; + + navigator_mission_item.frame = _mission_item.frame; + navigator_mission_item.frame = _mission_item.origin; + + navigator_mission_item.loiter_exit_xtrack = _mission_item.loiter_exit_xtrack; + navigator_mission_item.force_heading = _mission_item.force_heading; + navigator_mission_item.altitude_is_relative = _mission_item.altitude_is_relative; + navigator_mission_item.autocontinue = _mission_item.autocontinue; + navigator_mission_item.vtol_back_transition = _mission_item.vtol_back_transition; + + navigator_mission_item.timestamp = hrt_absolute_time(); + + _navigator_mission_item_pub.publish(navigator_mission_item); +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 211b2779eae0..b1f6a75b81ae 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -137,6 +138,12 @@ class RtlDirect : public MissionBlock, public ModuleParams */ void parameters_update(); + /** + * @brief Publish navigator mission item + * + */ + void publish_rtl_direct_navigator_mission_item(); + RTLState getActivationLandState(); void setLoiterPosition(); @@ -167,4 +174,5 @@ class RtlDirect : public MissionBlock, public ModuleParams uORB::SubscriptionData _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ }; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp index df6674e34ad8..5f55b92affd8 100644 --- a/src/modules/navigator/rtl_direct_mission_land.cpp +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -125,8 +125,7 @@ void RtlDirectMissionLand::setActiveMissionItems() // Climb to altitude if (_needs_climbing && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - // do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT, - // even if current climb altitude is below (e.g. RTL immediately after take off) + // TODO: check if we also should use NAV_CMD_LOITER_TO_ALT for rotary wing if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _mission_item.nav_cmd = NAV_CMD_WAYPOINT; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index 70eb4819ffb0..0bcafd94e8fb 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -52,12 +52,27 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) : } +void RtlMissionFast::on_inactive() +{ + MissionBase::on_inactive(); + _vehicle_status_sub.update(); + _mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ? + _mission.current_seq : -1; +} + void RtlMissionFast::on_activation() { _home_pos_sub.update(); - _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + // set mission item to closest item if not already in mission + if (_mission_index_prior_rtl < 0) { + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + } else { + setMissionIndex(_mission_index_prior_rtl); + _is_current_planned_mission_item_valid = isMissionValid(); + } if (_land_detected_sub.get().landed) { // already landed, no need to do anything, invalidad the position mission item. @@ -77,6 +92,23 @@ void RtlMissionFast::setActiveMissionItems() WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/ + if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + !_land_detected_sub.get().landed) { + if (setNextMissionItem()) { + if (!loadCurrentMissionItem()) { + setEndOfMissionItems(); + return; + } + + } else { + setEndOfMissionItems(); + return; + } + } + // Transition to fixed wing if necessary. if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol && diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h index bb3db38c6465..c782a471bd7a 100644 --- a/src/modules/navigator/rtl_mission_fast.h +++ b/src/modules/navigator/rtl_mission_fast.h @@ -56,6 +56,7 @@ class RtlMissionFast : public RtlBase ~RtlMissionFast() = default; void on_activation() override; + void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -63,5 +64,7 @@ class RtlMissionFast : public RtlBase bool setNextMissionItem() override; void setActiveMissionItems() override; + int _mission_index_prior_rtl{-1}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ }; diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 967e280bfd69..d7c42516e41d 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -52,12 +52,27 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : } +void RtlMissionFastReverse::on_inactive() +{ + MissionBase::on_inactive(); + _vehicle_status_sub.update(); + _mission_index_prior_rtl = _vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ? + _mission.current_seq : -1; +} + void RtlMissionFastReverse::on_activation() { _home_pos_sub.update(); - _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, - _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + // set mission item to closest item if not already in mission. If we are in mission, set to the previous item. + if (_mission_index_prior_rtl < 0) { + _is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK; + + } else { + setMissionIndex(math::max(_mission_index_prior_rtl - 1, 0)); + _is_current_planned_mission_item_valid = isMissionValid(); + } if (_land_detected_sub.get().landed) { // already landed, no need to do anything, invalidate the position mission item. diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 889dd8716b5e..f1301a5ccafd 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -57,6 +57,7 @@ class RtlMissionFastReverse : public RtlBase void on_activation() override; void on_active() override; + void on_inactive() override; rtl_time_estimate_s calc_rtl_time_estimate() override; @@ -65,5 +66,7 @@ class RtlMissionFastReverse : public RtlBase void setActiveMissionItems() override; void handleLanding(WorkItemType &new_work_item_type); + int _mission_index_prior_rtl{-1}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ }; diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c9e59fec1e20..d9b22720f34a 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -43,7 +43,7 @@ #include Takeoff::Takeoff(Navigator *navigator) : - MissionBlock(navigator) + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) { } @@ -68,7 +68,7 @@ Takeoff::on_active() } else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) { _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 7b36201856d3..f9a552ef58fc 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -43,7 +43,7 @@ using matrix::wrap_pi; VtolTakeoff::VtolTakeoff(Navigator *navigator) : - MissionBlock(navigator), + MissionBlock(navigator, vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF), ModuleParams(navigator) { } @@ -151,7 +151,7 @@ VtolTakeoff::on_active() // the VTOL takeoff is done _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF); + _navigator->mode_completed(getNavigatorStateId()); break; } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 9fc40dcbde5c..936c1265055e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -176,19 +176,31 @@ mixer_tick() atomic_modify_clear(&r_status_flags, (PX4IO_P_STATUS_FLAGS_FAILSAFE)); } + const bool armed_output = should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE); + const bool disarmed_output = (!armed_output && should_always_enable_pwm) + || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN); + + if (disarmed_output) { + source = MIX_DISARMED; + } + /* * Run the mixers. */ if (source == MIX_FAILSAFE) { - /* copy failsafe values to the servo outputs */ + // Set failsafe value if the PWM output isn't disabled for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = r_page_servo_failsafe[i]; + if (r_page_servos[i] != 0) { + r_page_servos[i] = r_page_servo_failsafe[i]; + } } } else if (source == MIX_DISARMED) { - /* copy disarmed values to the servo outputs */ + // Set disarmed value if the PWM output isn't disabled for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - r_page_servos[i] = r_page_servo_disarmed[i]; + if (r_page_servos[i] != 0) { + r_page_servos[i] = r_page_servo_disarmed[i]; + } } } @@ -215,9 +227,7 @@ mixer_tick() isr_debug(5, "> PWM disabled"); } - if (mixer_servos_armed - && (should_arm || should_arm_nothrottle || (source == MIX_FAILSAFE)) - && !(r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)) { + if (mixer_servos_armed && (armed_output || disarmed_output)) { /* update the servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { up_pwm_servo_set(i, r_page_servos[i]); @@ -230,22 +240,5 @@ mixer_tick() } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { sbus1_output(_sbus_fd, r_page_servos, PX4IO_SERVO_COUNT); } - - } else if (mixer_servos_armed && (should_always_enable_pwm || (r_setup_arming & PX4IO_P_SETUP_ARMING_LOCKDOWN))) { - /* set the disarmed servo outputs. */ - for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) { - up_pwm_servo_set(i, r_page_servo_disarmed[i]); - /* copy values into reporting register */ - r_page_servos[i] = r_page_servo_disarmed[i]; - } - - /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { - sbus1_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } - - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { - sbus2_output(_sbus_fd, r_page_servo_disarmed, PX4IO_SERVO_COUNT); - } } } diff --git a/src/modules/px4iofirmware/serial.cpp b/src/modules/px4iofirmware/serial.cpp index ef4a18cba7e3..f945f05d4750 100644 --- a/src/modules/px4iofirmware/serial.cpp +++ b/src/modules/px4iofirmware/serial.cpp @@ -379,4 +379,3 @@ dma_reset(void) stm32_dmastart(rx_dma, rx_dma_callback, NULL, false); rCR3 |= USART_CR3_DMAR; } - diff --git a/src/modules/rc_update/params_deprecated.c b/src/modules/rc_update/params_deprecated.c index 93c06dd19c0b..550f2169ee12 100644 --- a/src/modules/rc_update/params_deprecated.c +++ b/src/modules/rc_update/params_deprecated.c @@ -63,143 +63,3 @@ * @value 18 Channel 18 */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); - -/** - * Rattitude switch channel (deprecated) - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); - -/** - * Position Control switch channel (deprecated) - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); - -/** - * Acro switch channel (deprecated) - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); - -/** - * Stabilize switch channel mapping (deprecated) - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_STAB_SW, 0); - -/** - * Manual switch channel mapping (deprecated) - * - * @min 0 - * @max 18 - * @group Radio Switches - * @value 0 Unassigned - * @value 1 Channel 1 - * @value 2 Channel 2 - * @value 3 Channel 3 - * @value 4 Channel 4 - * @value 5 Channel 5 - * @value 6 Channel 6 - * @value 7 Channel 7 - * @value 8 Channel 8 - * @value 9 Channel 9 - * @value 10 Channel 10 - * @value 11 Channel 11 - * @value 12 Channel 12 - * @value 13 Channel 13 - * @value 14 Channel 14 - * @value 15 Channel 15 - * @value 16 Channel 16 - * @value 17 Channel 17 - * @value 18 Channel 18 - */ -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 6aa4ffd20b52..fb5491d7653c 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -162,8 +162,8 @@ void RCUpdate::updateParams() || _param_rc_map_pitch.get() > 0 || _param_rc_map_yaw.get() > 0); - // deprecated parameters, will be removed post v1.12 once QGC is updated { + // deprecated parameter, needs to be fully removed from QGC int32_t rc_map_value = 0; if (param_get(param_find("RC_MAP_MODE_SW"), &rc_map_value) == PX4_OK) { @@ -172,41 +172,6 @@ void RCUpdate::updateParams() param_reset(param_find("RC_MAP_MODE_SW")); } } - - if (param_get(param_find("RC_MAP_RATT_SW"), &rc_map_value) == PX4_OK) { - if (rc_map_value != 0) { - PX4_WARN("RC_MAP_RATT_SW deprecated"); - param_reset(param_find("RC_MAP_RATT_SW")); - } - } - - if (param_get(param_find("RC_MAP_POSCTL_SW"), &rc_map_value) == PX4_OK) { - if (rc_map_value != 0) { - PX4_WARN("RC_MAP_POSCTL_SW deprecated"); - param_reset(param_find("RC_MAP_POSCTL_SW")); - } - } - - if (param_get(param_find("RC_MAP_ACRO_SW"), &rc_map_value) == PX4_OK) { - if (rc_map_value != 0) { - PX4_WARN("RC_MAP_ACRO_SW deprecated"); - param_reset(param_find("RC_MAP_ACRO_SW")); - } - } - - if (param_get(param_find("RC_MAP_STAB_SW"), &rc_map_value) == PX4_OK) { - if (rc_map_value != 0) { - PX4_WARN("RC_MAP_STAB_SW deprecated"); - param_reset(param_find("RC_MAP_STAB_SW")); - } - } - - if (param_get(param_find("RC_MAP_MAN_SW"), &rc_map_value) == PX4_OK) { - if (rc_map_value != 0) { - PX4_WARN("RC_MAP_MAN_SW deprecated"); - param_reset(param_find("RC_MAP_MAN_SW")); - } - } } // Center throttle trim when it's set to the minimum to correct for hardcoded QGC RC calibration diff --git a/src/modules/rover_ackermann/CMakeLists.txt b/src/modules/rover_ackermann/CMakeLists.txt new file mode 100644 index 000000000000..f02a91c0b6dc --- /dev/null +++ b/src/modules/rover_ackermann/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2024 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. +# +############################################################################ + +add_subdirectory(RoverAckermannGuidance) + +px4_add_module( + MODULE modules__rover_ackermann + MAIN rover_ackermann + SRCS + RoverAckermann.cpp + RoverAckermann.hpp + DEPENDS + RoverAckermannGuidance + px4_work_queue + SlewRate + pure_pursuit + MODULE_CONFIG + module.yaml +) diff --git a/src/modules/rover_ackermann/Kconfig b/src/modules/rover_ackermann/Kconfig new file mode 100644 index 000000000000..3ba13506598a --- /dev/null +++ b/src/modules/rover_ackermann/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_ACKERMANN + bool "rover_ackermann" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of ackermann drive rovers diff --git a/src/modules/rover_ackermann/RoverAckermann.cpp b/src/modules/rover_ackermann/RoverAckermann.cpp new file mode 100644 index 000000000000..5409e40d841a --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermann.cpp @@ -0,0 +1,246 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "RoverAckermann.hpp" + +using namespace time_literals; +using namespace matrix; + +RoverAckermann::RoverAckermann() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + _rover_ackermann_status_pub.advertise(); + updateParams(); +} + +bool RoverAckermann::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverAckermann::updateParams() +{ + ModuleParams::updateParams(); + + // Update slew rates + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON) { + _throttle_with_accel_limit.setSlewRate(_param_ra_max_accel.get() / _param_ra_max_speed.get()); + } + + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.setSlewRate((M_DEG_TO_RAD_F * _param_ra_max_steering_rate.get()) / + _param_ra_max_steer_angle.get()); + } +} + +void RoverAckermann::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + updateSubscriptions(); + + // Timestamps + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // Generate motor setpoints + if (_armed) { + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _motor_setpoint.steering = manual_control_setpoint.roll; + _motor_setpoint.throttle = manual_control_setpoint.throttle; + } + + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _motor_setpoint.steering = 0.f; + _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); + break; + } + + } else { // Reset on disarm + _motor_setpoint.steering = 0.f; + _motor_setpoint.throttle = 0.f; + _throttle_with_accel_limit.setForcedValue(0.f); + _steering_with_rate_limit.setForcedValue(0.f); + } + + publishMotorSetpoints(applySlewRates(_motor_setpoint, dt)); +} + +void RoverAckermann::updateSubscriptions() +{ + if (_parameter_update_sub.updated()) { + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + _armed = vehicle_status.arming_state == 2; + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + _actual_speed = rover_velocity.norm(); + } +} +motor_setpoint_struct RoverAckermann::applySlewRates(motor_setpoint_struct motor_setpoint, const float dt) +{ + // Sanitize actuator commands + if (!PX4_ISFINITE(motor_setpoint.steering)) { + motor_setpoint.steering = 0.f; + } + + if (!PX4_ISFINITE(motor_setpoint.throttle)) { + motor_setpoint.throttle = 0.f; + } + + // Acceleration slew rate + if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_speed.get() > FLT_EPSILON + && fabsf(motor_setpoint.throttle) > fabsf(_throttle_with_accel_limit.getState())) { + _throttle_with_accel_limit.update(motor_setpoint.throttle, dt); + + } else { + _throttle_with_accel_limit.setForcedValue(motor_setpoint.throttle); + } + + // Steering slew rate + if (_param_ra_max_steering_rate.get() > FLT_EPSILON && _param_ra_max_steer_angle.get() > FLT_EPSILON) { + _steering_with_rate_limit.update(motor_setpoint.steering, dt); + + } else { + _steering_with_rate_limit.setForcedValue(motor_setpoint.steering); + } + + motor_setpoint_struct motor_setpoint_temp{}; + motor_setpoint_temp.steering = math::constrain(_steering_with_rate_limit.getState(), -1.f, 1.f); + motor_setpoint_temp.throttle = math::constrain(_throttle_with_accel_limit.getState(), -1.f, 1.f); + return motor_setpoint_temp; +} + +void RoverAckermann::publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates) +{ + // Publish rover Ackermann status (logging) + rover_ackermann_status_s rover_ackermann_status{}; + rover_ackermann_status.timestamp = _timestamp; + rover_ackermann_status.throttle_setpoint = _motor_setpoint.throttle; + rover_ackermann_status.steering_setpoint = _motor_setpoint.steering; + rover_ackermann_status.actual_speed = _actual_speed; + _rover_ackermann_status_pub.publish(rover_ackermann_status); + + // Publish to motor + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + actuator_motors.control[0] = motor_setpoint_with_slew_rates.throttle; + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + + // Publish to servo + actuator_servos_s actuator_servos{}; + actuator_servos.control[0] = motor_setpoint_with_slew_rates.steering; + actuator_servos.timestamp = _timestamp; + _actuator_servos_pub.publish(actuator_servos); +} + +int RoverAckermann::task_spawn(int argc, char *argv[]) +{ + RoverAckermann *instance = new RoverAckermann(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RoverAckermann::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int RoverAckermann::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover ackermann module. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_ackermann", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_ackermann_main(int argc, char *argv[]) +{ + return RoverAckermann::main(argc, argv); +} diff --git a/src/modules/rover_ackermann/RoverAckermann.hpp b/src/modules/rover_ackermann/RoverAckermann.hpp new file mode 100644 index 000000000000..3617b5b80ba6 --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermann.hpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// PX4 includes +#include +#include +#include +#include +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Standard library includes +#include + +// Local includes +#include "RoverAckermannGuidance/RoverAckermannGuidance.hpp" +using motor_setpoint_struct = RoverAckermannGuidance::motor_setpoint; + +using namespace time_literals; + +class RoverAckermann : public ModuleBase, public ModuleParams, + public px4::ScheduledWorkItem +{ +public: + /** + * @brief Constructor for RoverAckermann + */ + RoverAckermann(); + ~RoverAckermann() override = default; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + + +protected: + void updateParams() override; + +private: + void Run() override; + + /** + * @brief Update uORB subscriptions. + */ + void updateSubscriptions(); + + /** + * @brief Apply slew rates to motor setpoints. + * @param motor_setpoint Normalized steering and throttle setpoints. + * @param dt Time since last update [s]. + * @return Motor setpoint with applied slew rates. + */ + motor_setpoint_struct applySlewRates(motor_setpoint_struct motor_setpoint, float dt); + + /** + * @brief Publish motor setpoints to ActuatorMotors/ActuatorServos and logging values to RoverAckermannStatus. + * @param motor_setpoint_with_slew_rate Normalized motor_setpoint with applied slew rates. + */ + void publishMotorSetpoints(motor_setpoint_struct motor_setpoint_with_slew_rates); + + // uORB subscriptions + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + + // uORB publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; + uORB::Publication _rover_ackermann_status_pub{ORB_ID(rover_ackermann_status)}; + + // Class instances + RoverAckermannGuidance _ackermann_guidance{this}; + + // Variables + int _nav_state{0}; + motor_setpoint_struct _motor_setpoint; + hrt_abstime _timestamp{0}; + float _actual_speed{0.f}; + SlewRate _steering_with_rate_limit{0.f}; + SlewRate _throttle_with_accel_limit{0.f}; + bool _armed{false}; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_r_rev, + (ParamFloat) _param_ra_max_steer_angle, + (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_ra_max_accel, + (ParamFloat) _param_ra_max_steering_rate + ) +}; diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt b/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt new file mode 100644 index 000000000000..72928c7e25be --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2024 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(RoverAckermannGuidance + RoverAckermannGuidance.cpp +) + +target_link_libraries(RoverAckermannGuidance PUBLIC pid) +target_include_directories(RoverAckermannGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp new file mode 100644 index 000000000000..1acaebb6386f --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp @@ -0,0 +1,322 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 "RoverAckermannGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverAckermannGuidance::RoverAckermannGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + _rover_ackermann_guidance_status_pub.advertise(); + updateParams(); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +void RoverAckermannGuidance::updateParams() +{ + ModuleParams::updateParams(); + pid_set_parameters(&_pid_throttle, + _param_ra_p_speed.get(), // Proportional gain + _param_ra_i_speed.get(), // Integral gain + 0, // Derivative gain + 1, // Integral limit + 1); // Output limit +} + +RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state) +{ + updateSubscriptions(); + + // Catch return to launch + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + _mission_finished = _distance_to_next_wp < _acceptance_radius; + } + + // Guidance logic + if (_mission_finished) { // Mission is finished + _desired_steering = 0.f; + _desired_speed = 0.f; + + } else if (_distance_to_curr_wp < _acceptance_radius) { // Catch delay command + _desired_speed = 0.f; + + } else { // Regular guidance algorithm + _desired_speed = calcDesiredSpeed(_param_ra_miss_vel_def.get(), _param_ra_miss_vel_min.get(), + _param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius, + _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state); + + _desired_steering = calcDesiredSteering(_pure_pursuit, _curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + _param_ra_wheel_base.get(), _desired_speed, _vehicle_yaw, _param_ra_max_steer_angle.get()); + + } + + // Calculate throttle setpoint + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + const float throttle = calcThrottleSetpoint(_pid_throttle, _desired_speed, _actual_speed, _param_ra_max_speed.get(), + dt); + + // Publish ackermann controller status (logging) + _rover_ackermann_guidance_status.timestamp = _timestamp; + _rover_ackermann_guidance_status.desired_speed = _desired_speed; + _rover_ackermann_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_ackermann_guidance_status_pub.publish(_rover_ackermann_guidance_status); + + // Return motor setpoints + motor_setpoint motor_setpoint_temp; + motor_setpoint_temp.steering = math::interpolate(_desired_steering, -_param_ra_max_steer_angle.get(), + _param_ra_max_steer_angle.get(), -1.f, 1.f); // Normalize steering setpoint + motor_setpoint_temp.throttle = throttle; + return motor_setpoint_temp; +} + +void RoverAckermannGuidance::updateSubscriptions() +{ + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); + const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; + _actual_speed = rover_velocity.norm(); + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypointsAndAcceptanceRadius(); + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + matrix::Quatf vehicle_attitude_quaternion = Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + _mission_finished = mission_result.finished; + } +} + +void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid + Vector2d curr_wp(0, 0); + Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL + + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } + + // Distances to waypoints + _distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + prev_wp(0), prev_wp(1)); + _distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + curr_wp(0), curr_wp(1)); + _distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + next_wp(0), next_wp(1)); + + // NED waypoint coordinates + _curr_wp_ned = _global_ned_proj_ref.project(curr_wp(0), curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(prev_wp(0), prev_wp(1)); + _next_wp_ned = _global_ned_proj_ref.project(next_wp(0), next_wp(1)); + + // Update acceptance radius + _prev_acceptance_radius = _acceptance_radius; + + if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { + _acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _param_nav_acc_rad.get(), + _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get()); + + } else { + _acceptance_radius = _param_nav_acc_rad.get(); + } +} + +float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &next_wp_ned, const float default_acceptance_radius, const float acceptance_radius_gain, + const float acceptance_radius_max, const float wheel_base, const float max_steer_angle) +{ + // Setup variables + const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned; + const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned; + float acceptance_radius = default_acceptance_radius; + + // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment + if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) { + float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero(); + cosin = math::constrain(cosin, -1.f, 1.f); // Protect against float precision problem + const float theta = acosf(cosin) / 2.f; + const float min_turning_radius = wheel_base / sinf(max_steer_angle); + const float acceptance_radius_temp = min_turning_radius / tanf(theta); + const float acceptance_radius_temp_scaled = acceptance_radius_gain * + acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects + acceptance_radius = math::constrain(acceptance_radius_temp_scaled, default_acceptance_radius, + acceptance_radius_max); + } + + // Publish updated acceptance radius + position_controller_status_s pos_ctrl_status{}; + pos_ctrl_status.acceptance_radius = acceptance_radius; + pos_ctrl_status.timestamp = hrt_absolute_time(); + _position_controller_status_pub.publish(pos_ctrl_status); + return acceptance_radius; +} + +float RoverAckermannGuidance::calcDesiredSpeed(const float miss_vel_def, const float miss_vel_min, + const float miss_vel_gain, const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad, + const float prev_acc_rad, const float max_accel, const float max_jerk, const int nav_state) +{ + // Catch improper values + if (miss_vel_min < 0.f || miss_vel_min > miss_vel_def || miss_vel_gain < FLT_EPSILON) { + return miss_vel_def; + } + + // Cornering slow down effect + if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / prev_acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) { + const float cornering_speed = miss_vel_gain / acc_rad; + return math::constrain(cornering_speed, miss_vel_min, miss_vel_def); + + } + + // Straight line speed + if (max_accel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) { + float max_velocity{0.f}; + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp, 0.f); + + } else { + const float cornering_speed = miss_vel_gain / acc_rad; + max_velocity = math::trajectory::computeMaxSpeedFromDistance(max_jerk, + max_accel, distance_to_curr_wp - acc_rad, cornering_speed); + } + + return math::constrain(max_velocity, miss_vel_min, miss_vel_def); + + } else { + return miss_vel_def; + } + +} + +float RoverAckermannGuidance::calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, + const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned, const float wheel_base, const float desired_speed, + const float vehicle_yaw, const float max_steering) +{ + const float desired_heading = pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned, + desired_speed); + const float lookahead_distance = pure_pursuit.getLookaheadDistance(); + const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); + // For logging + _rover_ackermann_guidance_status.lookahead_distance = lookahead_distance; + _rover_ackermann_guidance_status.heading_error = (heading_error * 180.f) / (M_PI_F); + + float desired_steering{0.f}; + + if (math::abs_t(heading_error) <= M_PI_2_F) { + desired_steering = atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); + + + } else { + desired_steering = atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - + sign(heading_error) * M_PI_2_F)) / lookahead_distance); + } + + return math::constrain(desired_steering, -max_steering, max_steering); + +} + +float RoverAckermannGuidance::calcThrottleSetpoint(PID_t &pid_throttle, const float desired_speed, + const float actual_speed, const float max_speed, const float dt) +{ + float throttle = 0.f; + + if (desired_speed < FLT_EPSILON) { + pid_reset_integral(&pid_throttle); + + } else { + throttle = pid_calculate(&pid_throttle, desired_speed, actual_speed, 0, dt); + } + + if (_param_ra_max_speed.get() > 0.f) { // Feed-forward term + throttle += math::interpolate(desired_speed, 0.f, max_speed, 0.f, 1.f); + } + + return math::constrain(throttle, 0.f, 1.f); +} diff --git a/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp new file mode 100644 index 000000000000..23f63ed9280e --- /dev/null +++ b/src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp @@ -0,0 +1,231 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + +// PX4 includes +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard library includes +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Class for ackermann drive guidance. + */ +class RoverAckermannGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverAckermannGuidance. + * @param parent The parent ModuleParams object. + */ + RoverAckermannGuidance(ModuleParams *parent); + ~RoverAckermannGuidance() = default; + + /** + * @brief Struct for steering and throttle setpoints. + * @param steering Steering setpoint. + * @param throttle Throttle setpoint. + */ + struct motor_setpoint { + float steering{0.f}; + float throttle{0.f}; + }; + + /** + * @brief Calculate motor setpoints based on the mission plan. + * @param nav_state Vehicle navigation state. + * @return Motor setpoints for throttle and steering. + */ + motor_setpoint computeGuidance(int nav_state); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + /** + * @brief Update uORB subscriptions + */ + void updateSubscriptions(); + + /** + * @brief Update global/NED waypoint coordinates and acceptance radius. + */ + void updateWaypointsAndAcceptanceRadius(); + + /** + * @brief Publish the acceptance radius for current waypoint based on the angle between a line segment + * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of the vehicle. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param next_wp_ned Next waypoint in NED frame [m]. + * @param default_acceptance_radius Default acceptance radius for waypoints [m]. + * @param acceptance_radius_gain Tuning parameter that scales the geometric optimal acceptance radius for the corner cutting [-]. + * @param acceptance_radius_max Maximum value for the acceptance radius [m]. + * @param wheel_base Rover wheelbase [m]. + * @param max_steer_angle Rover maximum steer angle [rad]. + * @return Updated acceptance radius [m]. + */ + float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &next_wp_ned, float default_acceptance_radius, float acceptance_radius_gain, + float acceptance_radius_max, float wheel_base, float max_steer_angle); + + + /** + * @brief Calculate the desired speed setpoint. During cornering the speed is calculated as the inverse + * of the acceptance radius multiplied with a tuning factor. On straight lines it is based on a velocity trajectory + * such that the rover will arrive at the next corner with the desired cornering speed under consideration of the + * maximum acceleration and jerk. + * @param miss_vel_def Default desired velocity for the rover during mission [m/s]. + * @param miss_vel_min Minimum desired velocity for the rover during mission [m/s]. + * @param miss_vel_gain Tuning parameter for the slow down effect during cornering [-]. + * @param distance_to_prev_wp Distance to the previous waypoint [m]. + * @param distance_to_curr_wp Distance to the current waypoint [m]. + * @param acc_rad Acceptance radius of the current waypoint [m]. + * @param prev_acc_rad Acceptance radius of the previous waypoint [m]. + * @param max_accel Maximum allowed acceleration for the rover [m/s^2]. + * @param max_jerk Maximum allowed jerk for the rover [m/s^3]. + * @param nav_state Current nav_state of the rover. + * @return Speed setpoint for the rover [m/s]. + */ + float calcDesiredSpeed(float miss_vel_def, float miss_vel_min, float miss_vel_gain, float distance_to_prev_wp, + float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_accel, float max_jerk, int nav_state); + + /** + * @brief Calculate desired steering angle. The desired steering is calulated as the steering that is required to + * reach the point calculated using the pure pursuit algorithm (see PurePursuit.hpp). + * @param pure_pursuit Pure pursuit class instance. + * @param curr_wp_ned Current waypoint in NED frame [m]. + * @param prev_wp_ned Previous waypoint in NED frame [m]. + * @param curr_pos_ned Current position of the vehicle in NED frame [m]. + * @param wheel_base Rover wheelbase [m]. + * @param desired_speed Desired speed for the rover [m/s]. + * @param vehicle_yaw Current yaw of the rover [rad]. + * @param max_steering Maximum steering angle of the rover [rad]. + * @return Steering setpoint for the rover [rad]. + */ + float calcDesiredSteering(PurePursuit &pure_pursuit, const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, + const Vector2f &curr_pos_ned, float wheel_base, float desired_speed, float vehicle_yaw, float max_steering); + + /** + * @brief Calculate the throttle setpoint. Calculated with a PID controller using the difference between + * the desired/actual speed and a feedforward term based on the full throttle speed. + * @param pid_throttle Reference to PID instance. + * @param desired_speed Reference speed for the rover [m/s]. + * @param actual_speed Actual speed of the rover [m/s]. + * @param max_speed Rover speed at full throttle [m/s]. + * @param dt Time interval since last update [s]. + * @return Normalized throttle setpoint [0, 1]. + */ + float calcThrottleSetpoint(PID_t &pid_throttle, float desired_speed, float actual_speed, float max_speed, float dt); + + // uORB subscriptions + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + + // uORB publications + uORB::Publication _rover_ackermann_guidance_status_pub{ORB_ID(rover_ackermann_guidance_status)}; + uORB::Publication _position_controller_status_pub{ORB_ID(position_controller_status)}; + rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; + + // Class instances + MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates + PurePursuit _pure_pursuit{this}; // Pure pursuit library + + // Rover variables + float _desired_steering{0.f}; + float _vehicle_yaw{0.f}; + float _desired_speed{0.f}; + float _actual_speed{0.f}; + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + PID_t _pid_throttle; + hrt_abstime _timestamp{0}; + + // Waypoint variables + Vector2d _home_position{}; + Vector2f _curr_wp_ned{}; + Vector2f _prev_wp_ned{}; + Vector2f _next_wp_ned{}; + float _distance_to_prev_wp{0.f}; + float _distance_to_curr_wp{0.f}; + float _distance_to_next_wp{0.f}; + float _acceptance_radius{0.5f}; + float _prev_acceptance_radius{0.5f}; + bool _mission_finished{false}; + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_ra_wheel_base, + (ParamFloat) _param_ra_max_steer_angle, + (ParamFloat) _param_ra_acc_rad_max, + (ParamFloat) _param_ra_acc_rad_gain, + (ParamFloat) _param_ra_miss_vel_def, + (ParamFloat) _param_ra_miss_vel_min, + (ParamFloat) _param_ra_miss_vel_gain, + (ParamFloat) _param_ra_p_speed, + (ParamFloat) _param_ra_i_speed, + (ParamFloat) _param_ra_max_speed, + (ParamFloat) _param_ra_max_jerk, + (ParamFloat) _param_ra_max_accel, + (ParamFloat) _param_nav_acc_rad + ) +}; diff --git a/src/modules/rover_ackermann/module.yaml b/src/modules/rover_ackermann/module.yaml new file mode 100644 index 000000000000..6571d0eeebeb --- /dev/null +++ b/src/modules/rover_ackermann/module.yaml @@ -0,0 +1,175 @@ +module_name: Rover Ackermann + +parameters: + - group: Rover Ackermann + definitions: + RA_WHEEL_BASE: + description: + short: Wheel base + long: Distance from the front to the rear axle + type: float + unit: m + min: 0.001 + max: 100 + increment: 0.001 + decimal: 3 + default: 0.5 + + RA_MAX_STR_ANG: + description: + short: Maximum steering angle + long: The maximum angle that the rover can steer + type: float + unit: rad + min: 0.1 + max: 1.5708 + increment: 0.01 + decimal: 2 + default: 0.5236 + + RA_ACC_RAD_MAX: + description: + short: Maximum acceptance radius for the waypoints + long: | + The controller scales the acceptance radius based on the angle between + the previous, current and next waypoint. + Higher value -> smoother trajectory at the cost of how close the rover gets + to the waypoint (Set to -1 to disable corner cutting). + type: float + unit: m + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: 3 + + RA_ACC_RAD_GAIN: + description: + short: Tuning parameter for corner cutting + long: | + The geometric ideal acceptance radius is multiplied by this factor + to account for kinematic and dynamic effects. + Higher value -> The rover starts to cut the corner earlier. + type: float + min: 1 + max: 100 + increment: 0.01 + decimal: 2 + default: 2 + + RA_MISS_VEL_DEF: + description: + short: Default rover velocity during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 2 + + RA_MISS_VEL_MIN: + description: + short: Minimum rover velocity during a mission + long: | + The velocity off the rover is reduced based on the corner it has to take + to smooth the trajectory (Set to -1 to disable) + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_MISS_VEL_GAIN: + description: + short: Tuning parameter for the velocity reduction during cornering + long: | + The cornering speed is equal to the inverse of the acceptance radius + of the WP multiplied with this factor. + Lower value -> More velocity reduction during cornering. + type: float + min: 0.05 + max: 100 + increment: 0.01 + decimal: 2 + default: 5 + + RA_SPEED_P: + description: + short: Proportional gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_SPEED_I: + description: + short: Integral gain for ground speed controller + type: float + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RA_MAX_SPEED: + description: + short: Speed the rover drives at maximum throttle + long: | + This is used for the feed-forward term of the speed controller. + A value of -1 disables the feed-forward term in which case the + Integrator (RA_SPEED_I) becomes necessary to track speed setpoints. + type: float + unit: m/s + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RA_MAX_ACCEL: + description: + short: Maximum acceleration for the rover + long: | + This is used for the acceleration slew rate, the feed-forward term + for the speed controller during missions and the corner slow down effect. + Note: For the corner slow down effect RA_MAX_JERK, RA_MISS_VEL_GAIN and + RA_MISS_VEL_MIN also have to be set. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RA_MAX_JERK: + description: + short: Maximum jerk + long: | + Limit for forwards acc/deceleration change. + This is used for the corner slow down effect. + Note: RA_MAX_ACCEL, RA_MISS_VEL_GAIN and RA_MISS_VEL_MIN also have to be set + for this to be enabled. + type: float + unit: m/s^3 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + + RA_MAX_STR_RATE: + description: + short: Maximum steering rate for the rover + type: float + unit: deg/s + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 diff --git a/src/modules/differential_drive/CMakeLists.txt b/src/modules/rover_differential/CMakeLists.txt similarity index 84% rename from src/modules/differential_drive/CMakeLists.txt rename to src/modules/rover_differential/CMakeLists.txt index 2e0e3583b9af..beaec32a8776 100644 --- a/src/modules/differential_drive/CMakeLists.txt +++ b/src/modules/rover_differential/CMakeLists.txt @@ -31,22 +31,19 @@ # ############################################################################ -add_subdirectory(DifferentialDriveControl) -add_subdirectory(DifferentialDriveGuidance) -add_subdirectory(DifferentialDriveKinematics) +add_subdirectory(RoverDifferentialGuidance) px4_add_module( - MODULE modules__differential_drive - MAIN differential_drive + MODULE modules__rover_differential + MAIN rover_differential SRCS - DifferentialDrive.cpp - DifferentialDrive.hpp + RoverDifferential.cpp + RoverDifferential.hpp DEPENDS - DifferentialDriveControl - DifferentialDriveGuidance - DifferentialDriveKinematics + RoverDifferentialGuidance px4_work_queue modules__control_allocator # for parameter CA_R_REV + pure_pursuit MODULE_CONFIG module.yaml ) diff --git a/src/modules/rover_differential/Kconfig b/src/modules/rover_differential/Kconfig new file mode 100644 index 000000000000..840e2cdbf98f --- /dev/null +++ b/src/modules/rover_differential/Kconfig @@ -0,0 +1,6 @@ +menuconfig MODULES_ROVER_DIFFERENTIAL + bool "rover_differential" + default n + depends on MODULES_CONTROL_ALLOCATOR + ---help--- + Enable support for control of differential rovers diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp new file mode 100644 index 000000000000..ab89392a0ec8 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -0,0 +1,256 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "RoverDifferential.hpp" +using namespace matrix; +using namespace time_literals; + +RoverDifferential::RoverDifferential() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) +{ + updateParams(); + _rover_differential_status_pub.advertise(); + pid_init(&_pid_yaw_rate, PID_MODE_DERIVATIV_NONE, 0.001f); +} + +bool RoverDifferential::init() +{ + ScheduleOnInterval(10_ms); // 100 Hz + return true; +} + +void RoverDifferential::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + + pid_set_parameters(&_pid_yaw_rate, + _param_rd_p_gain_yaw_rate.get(), // Proportional gain + _param_rd_i_gain_yaw_rate.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit + +} + +void RoverDifferential::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_parameter_update_sub.updated()) { + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); + updateParams(); + } + + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status{}; + _vehicle_status_sub.copy(&vehicle_status); + _nav_state = vehicle_status.nav_state; + } + + if (_vehicle_angular_velocity_sub.updated()) { + vehicle_angular_velocity_s vehicle_angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity); + _vehicle_body_yaw_rate = vehicle_angular_velocity.xyz[2]; + } + + if (_vehicle_attitude_sub.updated()) { + vehicle_attitude_s vehicle_attitude{}; + _vehicle_attitude_sub.copy(&vehicle_attitude); + _vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q); + _vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi(); + } + + if (_vehicle_local_position_sub.updated()) { + vehicle_local_position_s vehicle_local_position{}; + _vehicle_local_position_sub.copy(&vehicle_local_position); + Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz); + Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame); + _vehicle_forward_speed = velocity_in_body_frame(0); + } + + // Navigation modes + switch (_nav_state) { + case vehicle_status_s::NAVIGATION_STATE_MANUAL: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + + } + + _differential_setpoint.closed_loop_yaw_rate = false; + } break; + + case vehicle_status_s::NAVIGATION_STATE_ACRO: { + manual_control_setpoint_s manual_control_setpoint{}; + + if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { + _differential_setpoint.throttle = manual_control_setpoint.throttle; + _differential_setpoint.yaw_rate = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, + -_max_yaw_rate, _max_yaw_rate); + } + + _differential_setpoint.closed_loop_yaw_rate = true; + } break; + + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + _differential_setpoint = _rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, + _nav_state); + break; + + default: // Unimplemented nav states will stop the rover + _differential_setpoint.throttle = 0.f; + _differential_setpoint.yaw_rate = 0.f; + _differential_setpoint.closed_loop_yaw_rate = false; + break; + } + + float speed_diff_normalized = _differential_setpoint.yaw_rate; + + // Closed loop yaw rate control + if (_differential_setpoint.closed_loop_yaw_rate) { + if (fabsf(_differential_setpoint.yaw_rate - _vehicle_body_yaw_rate) < YAW_RATE_ERROR_THRESHOLD) { + speed_diff_normalized = 0.f; + pid_reset_integral(&_pid_yaw_rate); + + } else { + const float speed_diff = _differential_setpoint.yaw_rate * _param_rd_wheel_track.get(); // Feedforward + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), + _param_rd_max_speed.get(), -1.f, 1.f); + speed_diff_normalized = math::constrain(speed_diff_normalized + + pid_calculate(&_pid_yaw_rate, _differential_setpoint.yaw_rate, _vehicle_body_yaw_rate, 0, dt), + -1.f, 1.f); // Feedback + } + + } else { + pid_reset_integral(&_pid_yaw_rate); + } + + // Publish rover differential status (logging) + rover_differential_status_s rover_differential_status{}; + rover_differential_status.timestamp = _timestamp; + rover_differential_status.actual_speed = _vehicle_forward_speed; + rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _differential_setpoint.yaw_rate; + rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * _vehicle_body_yaw_rate; + rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; + _rover_differential_status_pub.publish(rover_differential_status); + + // Publish to motors + actuator_motors_s actuator_motors{}; + actuator_motors.reversible_flags = _param_r_rev.get(); + computeMotorCommands(_differential_setpoint.throttle, speed_diff_normalized).copyTo(actuator_motors.control); + actuator_motors.timestamp = _timestamp; + _actuator_motors_pub.publish(actuator_motors); + +} + +matrix::Vector2f RoverDifferential::computeMotorCommands(float forward_speed, const float speed_diff) +{ + float combined_velocity = fabsf(forward_speed) + fabsf(speed_diff); + + if (combined_velocity > 1.0f) { // Prioritize yaw rate + float excess_velocity = fabsf(combined_velocity - 1.0f); + forward_speed -= sign(forward_speed) * excess_velocity; + } + + // Calculate the left and right wheel speeds + return Vector2f(forward_speed - speed_diff, + forward_speed + speed_diff); +} + +int RoverDifferential::task_spawn(int argc, char *argv[]) +{ + RoverDifferential *instance = new RoverDifferential(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int RoverDifferential::custom_command(int argc, char *argv[]) +{ + return print_usage("unk_timestampn command"); +} + +int RoverDifferential::print_usage(const char *reason) +{ + if (reason) { + PX4_ERR("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Rover Differential controller. +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("rover_differential", "controller"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + return 0; +} + +extern "C" __EXPORT int rover_differential_main(int argc, char *argv[]) +{ + return RoverDifferential::main(argc, argv); +} diff --git a/src/modules/differential_drive/DifferentialDrive.hpp b/src/modules/rover_differential/RoverDifferential.hpp similarity index 53% rename from src/modules/differential_drive/DifferentialDrive.hpp rename to src/modules/rover_differential/RoverDifferential.hpp index 255bb0c52607..3dafe99b0566 100644 --- a/src/modules/differential_drive/DifferentialDrive.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -33,31 +33,41 @@ #pragma once +// PX4 includes #include #include #include #include #include + +// uORB includes #include +#include #include -#include #include #include -#include #include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include -#include "DifferentialDriveControl/DifferentialDriveControl.hpp" -#include "DifferentialDriveGuidance/DifferentialDriveGuidance.hpp" -#include "DifferentialDriveKinematics/DifferentialDriveKinematics.hpp" +// Local includes +#include "RoverDifferentialGuidance/RoverDifferentialGuidance.hpp" using namespace time_literals; -class DifferentialDrive : public ModuleBase, public ModuleParams, +class RoverDifferential : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - DifferentialDrive(); - ~DifferentialDrive() override = default; + RoverDifferential(); + ~RoverDifferential() override = default; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -70,36 +80,57 @@ class DifferentialDrive : public ModuleBase, public ModulePar bool init(); + /** + * @brief Computes motor commands for differential drive. + * + * @param forward_speed Linear velocity along the x-axis. + * @param speed_diff Speed difference between left and right wheels. + * @return matrix::Vector2f Motor velocities for the right and left motors. + */ + matrix::Vector2f computeMotorCommands(float forward_speed, const float speed_diff); + protected: void updateParams() override; private: void Run() override; + + // uORB Subscriptions uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - uORB::Publication _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)}; - bool _manual_driving = false; - bool _mission_driving = false; - bool _acro_driving = false; - hrt_abstime _time_stamp_last{0}; /**< time stamp when task was last updated */ + // uORB Publications + uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; + uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + + // Instances + RoverDifferentialGuidance _rover_differential_guidance{this}; - DifferentialDriveGuidance _differential_drive_guidance{this}; - DifferentialDriveControl _differential_drive_control{this}; - DifferentialDriveKinematics _differential_drive_kinematics{this}; + // Variables + float _vehicle_body_yaw_rate{0.f}; + float _vehicle_forward_speed{0.f}; + float _vehicle_yaw{0.f}; + float _max_yaw_rate{0.f}; + int _nav_state{0}; + matrix::Quatf _vehicle_attitude_quaternion{}; + hrt_abstime _timestamp{0}; + PID_t _pid_yaw_rate; // The PID controller for yaw rate + RoverDifferentialGuidance::differential_setpoint _differential_setpoint; - float _max_speed{0.f}; - float _max_angular_velocity{0.f}; + // Constants + static constexpr float YAW_RATE_ERROR_THRESHOLD = 0.1f; // [rad/s] Error threshold for the closed loop yaw rate control DEFINE_PARAMETERS( - (ParamFloat) _param_rdd_ang_velocity_scale, - (ParamFloat) _param_rdd_speed_scale, - (ParamFloat) _param_rdd_wheel_base, - (ParamFloat) _param_rdd_wheel_speed, - (ParamFloat) _param_rdd_wheel_radius, - (ParamFloat) _param_com_spoolup_time + (ParamFloat) _param_rd_man_yaw_scale, + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_p_gain_yaw_rate, + (ParamFloat) _param_rd_i_gain_yaw_rate, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt similarity index 89% rename from src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt rename to src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt index fa833c50ae42..0fd7b68c394e 100644 --- a/src/modules/differential_drive/DifferentialDriveGuidance/CMakeLists.txt +++ b/src/modules/rover_differential/RoverDifferentialGuidance/CMakeLists.txt @@ -31,8 +31,9 @@ # ############################################################################ -px4_add_library(DifferentialDriveGuidance - DifferentialDriveGuidance.cpp +px4_add_library(RoverDifferentialGuidance + RoverDifferentialGuidance.cpp ) -target_include_directories(DifferentialDriveGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(RoverDifferentialGuidance PUBLIC pid) +target_include_directories(RoverDifferentialGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp new file mode 100644 index 000000000000..efad52e7b874 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -0,0 +1,247 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 "RoverDifferentialGuidance.hpp" + +#include + +using namespace matrix; +using namespace time_literals; + +RoverDifferentialGuidance::RoverDifferentialGuidance(ModuleParams *parent) : ModuleParams(parent) +{ + updateParams(); + _rover_differential_guidance_status_pub.advertise(); + pid_init(&_pid_heading, PID_MODE_DERIVATIV_NONE, 0.001f); + pid_init(&_pid_throttle, PID_MODE_DERIVATIV_NONE, 0.001f); + +} + +void RoverDifferentialGuidance::updateParams() +{ + ModuleParams::updateParams(); + + _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + pid_set_parameters(&_pid_heading, + _param_rd_p_gain_heading.get(), // Proportional gain + _param_rd_i_gain_heading.get(), // Integral gain + 0.f, // Derivative gain + _max_yaw_rate, // Integral limit + _max_yaw_rate); // Output limit + pid_set_parameters(&_pid_throttle, + _param_rd_p_gain_speed.get(), // Proportional gain + _param_rd_i_gain_speed.get(), // Integral gain + 0.f, // Derivative gain + 1.f, // Integral limit + 1.f); // Output limit +} + +RoverDifferentialGuidance::differential_setpoint RoverDifferentialGuidance::computeGuidance(const float yaw, + const float actual_speed, const int nav_state) +{ + // Initializations + bool mission_finished{false}; + float desired_speed{0.f}; + float desired_yaw_rate{0.f}; + hrt_abstime timestamp_prev = _timestamp; + _timestamp = hrt_absolute_time(); + const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f; + + // uORB subscriber updates + if (_vehicle_global_position_sub.updated()) { + vehicle_global_position_s vehicle_global_position{}; + _vehicle_global_position_sub.copy(&vehicle_global_position); + _curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon); + } + + if (_local_position_sub.updated()) { + vehicle_local_position_s local_position{}; + _local_position_sub.copy(&local_position); + + if (!_global_ned_proj_ref.isInitialized() + || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { + _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); + } + + _curr_pos_ned = Vector2f(local_position.x, local_position.y); + } + + if (_position_setpoint_triplet_sub.updated()) { + updateWaypoints(); + } + + if (_mission_result_sub.updated()) { + mission_result_s mission_result{}; + _mission_result_sub.copy(&mission_result); + mission_finished = mission_result.finished; + } + + if (_home_position_sub.updated()) { + home_position_s home_position{}; + _home_position_sub.copy(&home_position); + _home_position = Vector2d(home_position.lat, home_position.lon); + } + + const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, + math::max(actual_speed, 0.f)); + + const float heading_error = matrix::wrap_pi(desired_heading - yaw); + + const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1), + _curr_wp(0), + _curr_wp(1)); + + if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL + && distance_to_next_wp < _param_nav_acc_rad.get()) { // Return to launch + mission_finished = true; + } + + // State machine + if (!mission_finished && distance_to_next_wp > _param_nav_acc_rad.get()) { + if (_currentState == GuidanceState::STOPPED) { + _currentState = GuidanceState::DRIVING; + } + + if (_currentState == GuidanceState::DRIVING && fabsf(heading_error) > _param_rd_trans_drv_trn.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::SPOT_TURNING; + + } else if (_currentState == GuidanceState::SPOT_TURNING && fabsf(heading_error) < _param_rd_trans_trn_drv.get()) { + pid_reset_integral(&_pid_heading); + _currentState = GuidanceState::DRIVING; + } + + } else { // Mission finished or delay command + _currentState = GuidanceState::STOPPED; + } + + // Guidance logic + switch (_currentState) { + case GuidanceState::DRIVING: { + desired_speed = _param_rd_miss_spd_def.get(); + + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + desired_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), + _param_rd_max_accel.get(), distance_to_next_wp, 0.0f); + desired_speed = math::constrain(desired_speed, -_param_rd_max_speed.get(), _param_rd_max_speed.get()); + } + + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); + } break; + + case GuidanceState::SPOT_TURNING: + if (actual_speed < TURN_MAX_VELOCITY) { // Wait for the rover to stop + desired_yaw_rate = pid_calculate(&_pid_heading, heading_error, 0.f, 0.f, dt); // Turn on the spot + } + + break; + + case GuidanceState::STOPPED: + default: + desired_speed = 0.f; + desired_yaw_rate = 0.f; + break; + + } + + // Closed loop speed control + float throttle{0.f}; + + if (fabsf(desired_speed) < FLT_EPSILON) { + pid_reset_integral(&_pid_throttle); + + } else { + throttle = pid_calculate(&_pid_throttle, desired_speed, actual_speed, 0, + dt); + + if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term + throttle += math::interpolate(desired_speed, + 0.f, _param_rd_max_speed.get(), + 0.f, 1.f); + } + } + + // Publish differential controller status (logging) + _rover_differential_guidance_status.timestamp = _timestamp; + _rover_differential_guidance_status.desired_speed = desired_speed; + _rover_differential_guidance_status.pid_throttle_integral = _pid_throttle.integral; + _rover_differential_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance(); + _rover_differential_guidance_status.pid_heading_integral = _pid_heading.integral; + _rover_differential_guidance_status.heading_error_deg = M_RAD_TO_DEG_F * heading_error; + _rover_differential_guidance_status.state_machine = (uint8_t) _currentState; + _rover_differential_guidance_status_pub.publish(_rover_differential_guidance_status); + + // Return setpoints + differential_setpoint differential_setpoint_temp; + differential_setpoint_temp.throttle = math::constrain(throttle, 0.f, 1.f); + differential_setpoint_temp.yaw_rate = math::constrain(desired_yaw_rate, -_max_yaw_rate, + _max_yaw_rate); + differential_setpoint_temp.closed_loop_yaw_rate = true; + return differential_setpoint_temp; +} + +void RoverDifferentialGuidance::updateWaypoints() +{ + position_setpoint_triplet_s position_setpoint_triplet{}; + _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); + + // Global waypoint coordinates + if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat) + && PX4_ISFINITE(position_setpoint_triplet.current.lon)) { + _curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon); + + } else { + _curr_wp = Vector2d(0, 0); + } + + if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat) + && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { + _prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon); + + } else { + _prev_wp = _curr_pos; + } + + if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat) + && PX4_ISFINITE(position_setpoint_triplet.next.lon)) { + _next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon); + + } else { + _next_wp = _home_position; + } + + // NED waypoint coordinates + _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1)); + _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1)); + +} diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp new file mode 100644 index 000000000000..29a131f8d8e2 --- /dev/null +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2023-2024 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 + +// PX4 includes +#include +#include + +// uORB includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Standard libraries +#include +#include +#include +#include +#include +#include + +using namespace matrix; + +/** + * @brief Enum class for the different states of guidance. + */ +enum class GuidanceState { + SPOT_TURNING, // The vehicle is currently turning on the spot. + DRIVING, // The vehicle is currently driving. + STOPPED // The vehicle is stopped. +}; + +/** + * @brief Class for differential rover guidance. + */ +class RoverDifferentialGuidance : public ModuleParams +{ +public: + /** + * @brief Constructor for RoverDifferentialGuidance. + * @param parent The parent ModuleParams object. + */ + RoverDifferentialGuidance(ModuleParams *parent); + ~RoverDifferentialGuidance() = default; + + struct differential_setpoint { + float throttle{0.f}; + float yaw_rate{0.f}; + bool closed_loop_yaw_rate{false}; + }; + + /** + * @brief Compute guidance for the vehicle. + * @param yaw The yaw orientation of the vehicle in radians. + * @param actual_speed The velocity of the vehicle in m/s. + * @param dt The time step in seconds. + * @param nav_state Navigation state of the rover. + */ + RoverDifferentialGuidance::differential_setpoint computeGuidance(float yaw, float actual_speed, + int nav_state); + + /** + * @brief Update global/ned waypoint coordinates + */ + void updateWaypoints(); + +protected: + /** + * @brief Update the parameters of the module. + */ + void updateParams() override; + +private: + // uORB subscriptions + uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; + uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)}; + uORB::Subscription _home_position_sub{ORB_ID(home_position)}; + + // uORB publications + uORB::Publication _rover_differential_guidance_status_pub{ORB_ID(rover_differential_guidance_status)}; + rover_differential_guidance_status_s _rover_differential_guidance_status{}; + + // Variables + MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates. + GuidanceState _currentState{GuidanceState::DRIVING}; // The current state of guidance. + PurePursuit _pure_pursuit{this}; // Pure pursuit library + hrt_abstime _timestamp{0}; + float _max_yaw_rate{0.f}; + + + // Waypoints + Vector2d _curr_pos{}; + Vector2f _curr_pos_ned{}; + Vector2d _prev_wp{}; + Vector2f _prev_wp_ned{}; + Vector2d _curr_wp{}; + Vector2f _curr_wp_ned{}; + Vector2d _next_wp{}; + Vector2d _home_position{}; + + // Controllers + PID_t _pid_heading; // The PID controller for the heading + PID_t _pid_throttle; // The PID controller for velocity + + // Constants + static constexpr float TURN_MAX_VELOCITY = 0.2f; // Velocity threshhold for starting the spot turn [m/s] + + // Parameters + DEFINE_PARAMETERS( + (ParamFloat) _param_rd_p_gain_heading, + (ParamFloat) _param_rd_i_gain_heading, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rd_max_jerk, + (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_miss_spd_def, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_trans_trn_drv, + (ParamFloat) _param_rd_trans_drv_trn + + ) +}; diff --git a/src/modules/differential_drive/module.yaml b/src/modules/rover_differential/module.yaml similarity index 55% rename from src/modules/differential_drive/module.yaml rename to src/modules/rover_differential/module.yaml index 50ab9bb837ca..be0a76c60e9b 100644 --- a/src/modules/differential_drive/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -1,11 +1,12 @@ -module_name: Differential Drive +module_name: Rover Differential parameters: - - group: Rover Differential Drive + - group: Rover Differential definitions: - RDD_WHEEL_BASE: + + RD_WHEEL_TRACK: description: - short: Wheel base + short: Wheel track long: Distance from the center of the right wheel to the center of the left wheel type: float unit: m @@ -14,55 +15,41 @@ parameters: increment: 0.001 decimal: 3 default: 0.5 - RDD_WHEEL_RADIUS: - description: - short: Wheel radius - long: Size of the wheel, half the diameter of the wheel - type: float - unit: m - min: 0.001 - max: 100 - increment: 0.001 - decimal: 3 - default: 0.1 - RDD_SPEED_SCALE: - description: - short: Manual speed scale - type: float - min: 0 - max: 1 - increment: 0.01 - decimal: 2 - default: 1 - RDD_ANG_SCALE: + + RD_MAN_YAW_SCALE: description: - short: Manual angular velocity scale + short: Manual yaw rate scale + long: | + In manual mode the setpoint for the yaw rate received from the rc remote + is scaled by this value. type: float - min: 0 + min: 0.01 max: 1 increment: 0.01 decimal: 2 default: 1 - RDD_WHEEL_SPEED: + + RD_HEADING_P: description: - short: Maximum wheel speed + short: Proportional gain for heading controller type: float - unit: rad/s min: 0 max: 100 increment: 0.01 decimal: 2 - default: 0.3 - RDD_P_HEADING: + default: 1 + + RD_HEADING_I: description: - short: Proportional gain for heading controller + short: Integral gain for heading controller type: float min: 0 max: 100 increment: 0.01 decimal: 2 - default: 1 - RDD_P_SPEED: + default: 0.1 + + RD_SPEED_P: description: short: Proportional gain for speed controller type: float @@ -71,7 +58,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_SPEED: + + RD_SPEED_I: description: short: Integral gain for ground speed controller type: float @@ -80,7 +68,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_P_ANG_VEL: + + RD_YAW_RATE_P: description: short: Proportional gain for angular velocity controller type: float @@ -89,7 +78,8 @@ parameters: increment: 0.01 decimal: 2 default: 1 - RDD_I_ANG_VEL: + + RD_YAW_RATE_I: description: short: Integral gain for angular velocity controller type: float @@ -98,7 +88,8 @@ parameters: increment: 0.01 decimal: 2 default: 0 - RDD_MAX_JERK: + + RD_MAX_JERK: description: short: Maximum jerk long: Limit for forwards acc/deceleration change. @@ -109,7 +100,8 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 - RDD_MAX_ACCEL: + + RD_MAX_ACCEL: description: short: Maximum acceleration long: Maximum acceleration is used to limit the acceleration of the rover @@ -120,3 +112,61 @@ parameters: increment: 0.01 decimal: 2 default: 0.5 + + RD_MAX_SPEED: + description: + short: Maximum speed the rover can drive + long: This parameter is used to map desired speeds to normalized motor commands. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 7 + + RD_MAX_YAW_RATE: + description: + short: Maximum allowed yaw rate for the rover. + long: | + This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode. + type: float + unit: deg/s + min: 0.01 + max: 1000 + increment: 0.01 + decimal: 2 + default: 90 + + RD_MISS_SPD_DEF: + description: + short: Default rover speed during a mission + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 1 + + RD_TRANS_TRN_DRV: + description: + short: Heading error threshhold to switch from spot turning to driving + type: float + unit: rad + min: 0.001 + max: 3.14159 + increment: 0.01 + decimal: 3 + default: 0.0872665 + + RD_TRANS_DRV_TRN: + description: + short: Heading error threshhold to switch from driving to spot turning + type: float + unit: rad + min: 0.001 + max: 3.14159 + increment: 0.01 + decimal: 3 + default: 0.174533 diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..928ce13c6edb 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -122,8 +122,8 @@ RoverPositionControl::manual_control_setpoint_poll() if (_control_mode.flag_control_attitude_enabled) { // STABILIZED mode generate the attitude setpoint from manual user inputs - _att_sp.roll_body = 0.0; - _att_sp.pitch_body = 0.0; + float roll_body = 0.0; + float pitch_body = 0.0; /* reset yaw setpoint to current position if needed */ if (_reset_yaw_sp) { @@ -137,10 +137,10 @@ RoverPositionControl::manual_control_setpoint_poll() _manual_yaw_sp = wrap_pi(_manual_yaw_sp + _att_sp.yaw_sp_move_rate * dt); } - _att_sp.yaw_body = _manual_yaw_sp; + float yaw_body = _manual_yaw_sp; _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; - Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); + const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); q.copyTo(_att_sp.q_d); _att_sp.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/data_validator/DataValidatorGroup.cpp index a7ded78d26cb..eabf61788ca4 100644 --- a/src/modules/sensors/data_validator/DataValidatorGroup.cpp +++ b/src/modules/sensors/data_validator/DataValidatorGroup.cpp @@ -230,6 +230,10 @@ float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) if (_first_failover_time == 0) { _first_failover_time = timestamp; } + + if (max_confidence < FLT_EPSILON) { + max_index = -1; + } } } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index b1eac451529a..01750b42598b 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -277,7 +277,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u // if no gyro was selected use the first valid sensor_gyro_fifo if (!device_id_valid) { device_id = sensor_gyro_fifo_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id); + PX4_DEBUG("no gyro selected, using sensor_gyro_fifo:%" PRIu8 " %" PRIu32, i, sensor_gyro_fifo_sub.get().device_id); } if (sensor_gyro_fifo_sub.get().device_id == device_id) { @@ -319,7 +319,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(const hrt_abstime &time_now_u // if no gyro was selected use the first valid sensor_gyro if (!device_id_valid) { device_id = sensor_gyro_sub.get().device_id; - PX4_WARN("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); + PX4_DEBUG("no gyro selected, using sensor_gyro:%" PRIu8 " %" PRIu32, i, sensor_gyro_sub.get().device_id); } if (sensor_gyro_sub.get().device_id == device_id) { diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index 569ab38c6ade..a417092ed94b 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -2,6 +2,7 @@ menu "Simulation" menuconfig COMMON_SIMULATION bool "Common simulation modules" default n + depends on PLATFORM_POSIX select MODULES_SIMULATION_BATTERY_SIMULATOR select MODULES_SIMULATION_PWM_OUT_SIM select MODULES_SIMULATION_SENSOR_AIRSPEED_SIM diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index ba36f448d35f..c7d36e36a5ad 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -92,6 +92,8 @@ if(gz-transport_FOUND) default windy baylands + lawn + rover ) # find corresponding airframes diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 2821213272c2..3464c628694c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -196,6 +196,13 @@ int GZBridge::init() return PX4_ERROR; } + // Laser Scan: optional + std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan"; + + if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) { + PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str()); + } + #if 0 // Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name + @@ -719,8 +726,6 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec()); } - _timestamp_prev = time_us; - // initialize gps position if (!_pos_ref.isInitialized()) { _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); @@ -743,6 +748,87 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) pthread_mutex_unlock(&_node_mutex); } +void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan) +{ + static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each + + double angle_min_deg = scan.angle_min() * 180 / M_PI; + double angle_step_deg = scan.angle_step() * 180 / M_PI; + + int samples_per_sector = std::round(SECTOR_SIZE_DEG / angle_step_deg); + int number_of_sectors = scan.ranges_size() / samples_per_sector; + + std::vector ds_array(number_of_sectors, UINT16_MAX); + + // Downsample -- take average of samples per sector + for (int i = 0; i < number_of_sectors; i++) { + + double sum = 0; + + int samples_used_in_sector = 0; + + for (int j = 0; j < samples_per_sector; j++) { + + double distance = scan.ranges()[i * samples_per_sector + j]; + + // inf values mean no object + if (isinf(distance)) { + continue; + } + + sum += distance; + samples_used_in_sector++; + } + + // If all samples in a sector are inf then it means the sector is clear + if (samples_used_in_sector == 0) { + ds_array[i] = scan.range_max(); + + } else { + ds_array[i] = sum / samples_used_in_sector; + } + } + + // Publish to uORB + obstacle_distance_s obs {}; + + // Initialize unknown + for (auto &i : obs.distances) { + i = UINT16_MAX; + } + + obs.timestamp = hrt_absolute_time(); + obs.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD; + obs.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER; + obs.min_distance = static_cast(scan.range_min() * 100.); + obs.max_distance = static_cast(scan.range_max() * 100.); + obs.angle_offset = static_cast(angle_min_deg); + obs.increment = static_cast(SECTOR_SIZE_DEG); + + // Map samples in FOV into sectors in ObstacleDistance + int index = 0; + + // Iterate in reverse because array is FLU and we need FRD + for (std::vector::reverse_iterator i = ds_array.rbegin(); i != ds_array.rend(); ++i) { + + uint16_t distance_cm = (*i) * 100.; + + if (distance_cm >= obs.max_distance) { + obs.distances[index] = obs.max_distance + 1; + + } else if (distance_cm < obs.min_distance) { + obs.distances[index] = 0; + + } else { + obs.distances[index] = distance_cm; + } + + index++; + } + + _obstacle_distance_pub.publish(obs); +} + 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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6d2e2670a15e..8a832f7961b2 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,7 @@ #include #include #include +#include using namespace time_literals; @@ -106,6 +108,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void poseInfoCallback(const gz::msgs::Pose_V &pose); void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry); void navSatCallback(const gz::msgs::NavSat &nav_sat); + void laserScanCallback(const gz::msgs::LaserScan &scan); /** * @@ -121,6 +124,7 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp index 2fed14dfe716..2198bacbaaf3 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp @@ -78,8 +78,8 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA for (unsigned i = 0; i < active_output_count; i++) { // Offsetting the output allows for negative values despite unsigned integer to reverse the wheels - static constexpr float output_offset = 100.0f; - float scaled_output = (float)outputs[i] - output_offset; + static constexpr double output_offset = 100.0; + double scaled_output = (double)outputs[i] - output_offset; wheel_velocity_message.set_velocity(i, scaled_output); } diff --git a/src/modules/simulation/gz_bridge/parameters.c b/src/modules/simulation/gz_bridge/parameters.c index 8dd6022ff6a0..26f874e8933e 100644 --- a/src/modules/simulation/gz_bridge/parameters.c +++ b/src/modules/simulation/gz_bridge/parameters.c @@ -39,4 +39,3 @@ * @group UAVCAN */ PARAM_DEFINE_INT32(SIM_GZ_EN, 0); - diff --git a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp index ff5e5e31aae2..97c2626f75b4 100644 --- a/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp +++ b/src/modules/simulation/sensor_mag_sim/SensorMagSim.cpp @@ -113,11 +113,11 @@ void SensorMagSim::Run() if (gpos.eph < 1000) { // magnetic field data returned by the geo library using the current GPS position - const float mag_declination_gps = get_mag_declination_radians(gpos.lat, gpos.lon); - const float mag_inclination_gps = get_mag_inclination_radians(gpos.lat, gpos.lon); - const float mag_strength_gps = get_mag_strength_gauss(gpos.lat, gpos.lon); + const float declination_rad = math::radians(get_mag_declination_degrees(gpos.lat, gpos.lon)); + const float inclination_rad = math::radians(get_mag_inclination_degrees(gpos.lat, gpos.lon)); + const float field_strength_gauss = get_mag_strength_gauss(gpos.lat, gpos.lon); - _mag_earth_pred = Dcmf(Eulerf(0, -mag_inclination_gps, mag_declination_gps)) * Vector3f(mag_strength_gps, 0, 0); + _mag_earth_pred = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(field_strength_gauss, 0, 0); _mag_earth_available = true; } diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 1fa076568be5..399a4ecc938d 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -243,8 +243,8 @@ void Sih::parameters_updated() _KDW = _sih_kdw.get(); _H0 = _sih_h0.get(); - _LAT0 = (double)_sih_lat0.get() * 1.0e-7; - _LON0 = (double)_sih_lon0.get() * 1.0e-7; + _LAT0 = (double)_sih_lat0.get(); + _LON0 = (double)_sih_lon0.get(); _COS_LAT0 = cosl((long double)radians(_LAT0)); _MASS = _sih_mass.get(); diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index a4227978be6b..8988474e01b1 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -273,8 +273,8 @@ class Sih : public ModuleBase, public ModuleParams (ParamFloat) _sih_l_pitch, (ParamFloat) _sih_kdv, (ParamFloat) _sih_kdw, - (ParamInt) _sih_lat0, - (ParamInt) _sih_lon0, + (ParamFloat) _sih_lat0, + (ParamFloat) _sih_lon0, (ParamFloat) _sih_h0, (ParamFloat) _sih_distance_snsr_min, (ParamFloat) _sih_distance_snsr_max, diff --git a/src/modules/simulation/simulator_sih/sih_params.c b/src/modules/simulation/simulator_sih/sih_params.c index 60dd336a9bde..16c58430f63e 100644 --- a/src/modules/simulation/simulator_sih/sih_params.c +++ b/src/modules/simulation/simulator_sih/sih_params.c @@ -235,33 +235,31 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f); * Initial geodetic latitude * * This value represents the North-South location on Earth where the simulation begins. - * A value of 45 deg should be written 450000000. * * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others * to represent a physical ground location on Earth. * - * @unit deg*1e7 - * @min -850000000 - * @max 850000000 + * @unit deg + * @min -90 + * @max 90 * @group Simulation In Hardware */ -PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160); +PARAM_DEFINE_FLOAT(SIH_LOC_LAT0, 47.397742f); /** * Initial geodetic longitude * * This value represents the East-West location on Earth where the simulation begins. - * A value of 45 deg should be written 450000000. * * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others * to represent a physical ground location on Earth. * - * @unit deg*1e7 - * @min -1800000000 - * @max 1800000000 + * @unit deg + * @min -180 + * @max 180 * @group Simulation In Hardware */ -PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370); +PARAM_DEFINE_FLOAT(SIH_LOC_LON0, 8.545594f); /** * Initial AMSL ground altitude @@ -282,7 +280,7 @@ PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370); * @increment 0.01 * @group Simulation In Hardware */ -PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f); +PARAM_DEFINE_FLOAT(SIH_LOC_H0, 489.4f); /** * distance sensor minimum range diff --git a/src/modules/uuv_att_control/uuv_att_control.cpp b/src/modules/uuv_att_control/uuv_att_control.cpp index 58992ccb7803..52baccb8dde6 100644 --- a/src/modules/uuv_att_control/uuv_att_control.cpp +++ b/src/modules/uuv_att_control/uuv_att_control.cpp @@ -138,9 +138,10 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude */ Eulerf euler_angles(matrix::Quatf(attitude.q)); - float roll_body = attitude_setpoint.roll_body; - float pitch_body = attitude_setpoint.pitch_body; - float yaw_body = attitude_setpoint.yaw_body; + const Eulerf setpoint_euler_angles(matrix::Quatf(attitude_setpoint.q_d)); + const float roll_body = setpoint_euler_angles(0); + const float pitch_body = setpoint_euler_angles(1); + const float yaw_body = setpoint_euler_angles(2); float roll_rate_desired = rates_setpoint.roll; float pitch_rate_desired = rates_setpoint.pitch; @@ -227,9 +228,8 @@ void UUVAttitudeControl::Run() _vehicle_rates_setpoint_sub.update(&_rates_setpoint); if (input_mode == 1) { // process manual data - _attitude_setpoint.roll_body = _param_direct_roll.get(); - _attitude_setpoint.pitch_body = _param_direct_pitch.get(); - _attitude_setpoint.yaw_body = _param_direct_yaw.get(); + Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get())); + attitude_setpoint.copyTo(_attitude_setpoint.q_d); _attitude_setpoint.thrust_body[0] = _param_direct_thrust.get(); _attitude_setpoint.thrust_body[1] = 0.f; _attitude_setpoint.thrust_body[2] = 0.f; diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index d402941b4a16..272ea56def8d 100644 --- a/src/modules/uuv_pos_control/uuv_pos_control.cpp +++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp @@ -97,9 +97,8 @@ void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {}; vehicle_attitude_setpoint.timestamp = hrt_absolute_time(); - vehicle_attitude_setpoint.roll_body = roll_des; - vehicle_attitude_setpoint.pitch_body = pitch_des; - vehicle_attitude_setpoint.yaw_body = yaw_des; + const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des)); + attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d); vehicle_attitude_setpoint.thrust_body[0] = thrust_x; vehicle_attitude_setpoint.thrust_body[1] = thrust_y; diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index cade47b5fdce..3c4311e6c333 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -43,6 +43,7 @@ struct SendSubscription { const struct orb_metadata *orb_meta; uxrObjectId data_writer; const char* dds_type_name; + const char* topic; uint32_t topic_size; UcdrSerializeMethod ucdr_serialize_method; }; @@ -54,6 +55,7 @@ struct SendTopicsSubs { { ORB_ID(@(pub['topic_simple'])), uxr_object_id(0, UXR_INVALID_ID), "@(pub['dds_type'])", + "@(pub['topic'])", ucdr_topic_size_@(pub['simple_base_type'])(), &ucdr_serialize_@(pub['simple_base_type']), }, @@ -96,7 +98,7 @@ void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream orb_copy(send_subscriptions[idx].orb_meta, fds[idx].fd, &topic_data); if (send_subscriptions[idx].data_writer.id == UXR_INVALID_ID) { // data writer not created yet - create_data_writer(session, reliable_out_stream_id, participant_id, static_cast(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].orb_meta->o_name, + create_data_writer(session, reliable_out_stream_id, participant_id, static_cast(send_subscriptions[idx].orb_meta->o_id), client_namespace, send_subscriptions[idx].topic, send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } @@ -169,7 +171,7 @@ bool RcvTopicsPubs::init(uxrSession *session, uxrStreamId reliable_out_stream_id @[ for idx, sub in enumerate(subscriptions + subscriptions_multi)]@ { uint16_t queue_depth = orb_get_queue_size(ORB_ID(@(sub['simple_base_type']))) * 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'])", "@(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 255c8e0b5417..e9e67cff25ed 100644 --- a/src/modules/uxrce_dds_client/dds_topics.yaml +++ b/src/modules/uxrce_dds_client/dds_topics.yaml @@ -44,6 +44,9 @@ publications: # - topic: /fmu/out/vehicle_angular_velocity # type: px4_msgs::msg::VehicleAngularVelocity + - topic: /fmu/out/vehicle_land_detected + type: px4_msgs::msg::VehicleLandDetected + - topic: /fmu/out/vehicle_attitude type: px4_msgs::msg::VehicleAttitude @@ -127,9 +130,6 @@ subscriptions: - topic: /fmu/in/vehicle_rates_setpoint type: px4_msgs::msg::VehicleRatesSetpoint - - topic: /fmu/in/differential_drive_setpoint - type: px4_msgs::msg::DifferentialDriveSetpoint - - topic: /fmu/in/vehicle_visual_odometry type: px4_msgs::msg::VehicleOdometry diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 5c7fb2e9dc2d..70886ec795ff 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -23,25 +23,29 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) return uxrObjectId{}; } -static bool generate_topic_name(char *topic, const char *client_namespace, const char *direction, const char *name) +static bool generate_topic_name(char *topic_name, const char *client_namespace, const char *topic) { + if (topic[0] == '/') { + topic++; + } + if (client_namespace != nullptr) { - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/%s/fmu/%s/%s", client_namespace, direction, name); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s/%s", client_namespace, topic); return (ret > 0 && ret < TOPIC_NAME_SIZE); } - int ret = snprintf(topic, TOPIC_NAME_SIZE, "rt/fmu/%s/%s", direction, name); + int ret = snprintf(topic_name, TOPIC_NAME_SIZE, "rt/%s", topic); return (ret > 0 && ret < TOPIC_NAME_SIZE); } static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrObjectId participant_id, - ORB_ID orb_id, const char *client_namespace, const char *topic_name_simple, const char *type_name, + ORB_ID orb_id, const char *client_namespace, const char *topic, const char *type_name, uxrObjectId &datawriter_id) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, "out", topic_name_simple)) { + if (!generate_topic_name(topic_name, client_namespace, topic)) { PX4_ERR("topic path too long"); return false; } @@ -87,13 +91,13 @@ static bool create_data_writer(uxrSession *session, uxrStreamId reliable_out_str } static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId input_stream_id, - uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic_name_simple, + uxrObjectId participant_id, uint16_t index, const char *client_namespace, const char *topic, const char *type_name, uint16_t queue_depth) { // topic char topic_name[TOPIC_NAME_SIZE]; - if (!generate_topic_name(topic_name, client_namespace, "in", topic_name_simple)) { + if (!generate_topic_name(topic_name, client_namespace, topic)) { PX4_ERR("topic path too long"); 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 1412414b6300..20428c691ebb 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -831,7 +831,7 @@ int UxrceddsClient::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("uxrce_dds_client", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - PX4_STACK_ADJUSTED(10000), + PX4_STACK_ADJUSTED(12000), (px4_main_t)&run_trampoline, (char *const *)argv); diff --git a/src/modules/vtol_att_control/CMakeLists.txt b/src/modules/vtol_att_control/CMakeLists.txt index e6316b14593f..780217ccc6dc 100644 --- a/src/modules/vtol_att_control/CMakeLists.txt +++ b/src/modules/vtol_att_control/CMakeLists.txt @@ -40,4 +40,3 @@ px4_add_module( tailsitter.cpp standard.cpp ) - diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d74ab45f4225..105ee1ee6efa 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -172,6 +172,11 @@ void Standard::update_transition_state() VtolType::update_transition_state(); + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + // we get attitude setpoint from a multirotor flighttask if climbrate is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -181,7 +186,7 @@ void Standard::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); } else { // we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active) @@ -200,8 +205,11 @@ void Standard::update_transition_state() } else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) { // ramp up throttle to the target throttle value + const float dt = math::min((now - _last_time_pusher_transition_update) / 1e6f, 0.05f); _pusher_throttle = math::min(_pusher_throttle + - _param_vt_psher_slew.get() * _dt, _param_vt_f_trans_thr.get()); + _param_vt_psher_slew.get() * dt, _param_vt_f_trans_thr.get()); + + _last_time_pusher_transition_update = now; } _airspeed_trans_blend_margin = getTransitionAirspeed() - getBlendAirspeed(); @@ -224,21 +232,19 @@ void Standard::update_transition_state() } // ramp up FW_PSP_OFF - _v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); - + pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight); _v_att_sp->thrust_body[0] = _pusher_throttle; - - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); } else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) { if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _pusher_throttle = 0.0f; diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index 280f2e5042ee..6136f872075f 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -76,6 +76,7 @@ class Standard : public VtolType float _pusher_throttle{0.0f}; float _airspeed_trans_blend_margin{0.0f}; + hrt_abstime _last_time_pusher_transition_update{0}; void parameters_update() override; diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index b16a344ca6e8..eaacbe68d611 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -67,6 +67,10 @@ void Tailsitter::update_vtol_state() if (_vtol_vehicle_status->fixed_wing_system_failure) { // Failsafe event, switch to MC mode immediately + if (_vtol_mode != vtol_mode::MC_MODE) { + _transition_start_timestamp = hrt_absolute_time(); + } + _vtol_mode = vtol_mode::MC_MODE; } else if (!_attc->is_fixed_wing_requested()) { @@ -121,6 +125,7 @@ void Tailsitter::update_vtol_state() case vtol_mode::TRANSITION_BACK: // failsafe into fixed wing mode _vtol_mode = vtol_mode::FW_MODE; + _trans_finished_ts = hrt_absolute_time(); break; } } @@ -174,7 +179,8 @@ void Tailsitter::update_transition_state() // the yaw setpoint and zero roll since we want wings level transition. // If for some reason the fw attitude setpoint is not recent then don't use it and assume 0 pitch if (_fw_virtual_att_sp->timestamp > (now - 1_s)) { - _q_trans_start = Eulerf(0.f, _fw_virtual_att_sp->pitch_body, yaw_sp); + const float pitch_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).theta(); + _q_trans_start = Eulerf(0.f, pitch_body, yaw_sp); } else { _q_trans_start = Eulerf(0.f, 0.f, yaw_sp); @@ -186,7 +192,8 @@ void Tailsitter::update_transition_state() } else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) { // initial attitude setpoint for the transition should be with wings level - _q_trans_start = Eulerf(0.f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body); + const Eulerf setpoint_euler(Quatf(_mc_virtual_att_sp->q_d)); + _q_trans_start = Eulerf(0.f, setpoint_euler.theta(), setpoint_euler.psi()); Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1.f, 0.f, 0.f); _trans_rot_axis = -x.cross(Vector3f(0.f, 0.f, -1.f)); } @@ -225,20 +232,21 @@ void Tailsitter::update_transition_state() _v_att_sp->thrust_body[2] = _mc_virtual_att_sp->thrust_body[2]; + if (_vtol_mode == vtol_mode::TRANSITION_BACK) { + const float progress = math::constrain(_time_since_trans_start / B_TRANS_THRUST_BLENDING_DURATION, 0.f, 1.f); + blendThrottleBeginningBackTransition(progress); + } + _v_att_sp->timestamp = hrt_absolute_time(); const Eulerf euler_sp(_q_trans_sp); - _v_att_sp->roll_body = euler_sp.phi(); - _v_att_sp->pitch_body = euler_sp.theta(); - _v_att_sp->yaw_body = euler_sp.psi(); - _q_trans_sp.copyTo(_v_att_sp->q_d); } void Tailsitter::waiting_on_tecs() { // copy the last trust value from the front transition - _v_att_sp->thrust_body[0] = _thrust_transition; + _v_att_sp->thrust_body[0] = -_last_thr_in_mc; } void Tailsitter::update_fw_state() @@ -294,12 +302,27 @@ void Tailsitter::fill_actuator_outputs() _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get(); } + // for the short period after switching to FW where there is no thrust published yet from the FW controller, + // keep publishing the last MC thrust to keep the motors running + if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) { + _thrust_setpoint_0->xyz[2] = _last_thr_in_mc; + _torque_setpoint_0->xyz[0] = 0.f; + _torque_setpoint_0->xyz[1] = 0.f; + _torque_setpoint_0->xyz[2] = 0.f; + } + } else { + _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; + + // for the short period after starting the backtransition where there is no thrust published yet from the MC controller, + // keep publishing the last FW thrust to keep the motors running + if (_vtol_mode != vtol_mode::TRANSITION_FRONT_P1 && hrt_elapsed_time(&_transition_start_timestamp) < 50_ms) { + _thrust_setpoint_0->xyz[2] = -_last_thr_in_fw_mode; + } + _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0]; _torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1]; _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2]; - - _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; } // Control surfaces @@ -330,3 +353,14 @@ bool Tailsitter::isFrontTransitionCompletedBase() return transition_to_fw; } + +void Tailsitter::blendThrottleAfterFrontTransition(float scale) +{ + // note: MC throttle is negative (as in negative z), while FW throttle is positive (positive x) + _v_att_sp->thrust_body[0] = scale * _v_att_sp->thrust_body[0] + (1.f - scale) * (-_last_thr_in_mc); +} + +void Tailsitter::blendThrottleBeginningBackTransition(float scale) +{ + _v_att_sp->thrust_body[2] = scale * _v_att_sp->thrust_body[2] + (1.f - scale) * (-_last_thr_in_fw_mode); +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index 93fa0b2491be..135acd6c92aa 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -54,6 +54,9 @@ static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_FW = -1.05f; // -60° // [rad] Pitch threshold required for completing transition to hover in automatic transitions static constexpr float PITCH_THRESHOLD_AUTO_TRANSITION_TO_MC = -0.26f; // -15° +// [s] Thrust blending duration from fixed-wing to back transition throttle +static constexpr float B_TRANS_THRUST_BLENDING_DURATION = 0.5f; + class Tailsitter : public VtolType { @@ -66,6 +69,8 @@ class Tailsitter : public VtolType void update_fw_state() override; void fill_actuator_outputs() override; void waiting_on_tecs() override; + void blendThrottleAfterFrontTransition(float scale) override; + void blendThrottleBeginningBackTransition(float scale); private: enum class vtol_mode { diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 0956e1003698..3a1904257993 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state() const hrt_abstime now = hrt_absolute_time(); + const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d)); + float roll_body = attitude_setpoint_euler.phi(); + float pitch_body = attitude_setpoint_euler.theta(); + float yaw_body = attitude_setpoint_euler.psi(); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. if (_v_control_mode->flag_control_climb_rate_enabled) { @@ -212,7 +217,7 @@ void Tiltrotor::update_transition_state() } memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi(); _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; } else { @@ -290,7 +295,7 @@ void Tiltrotor::update_transition_state() // control backtransition deceleration using pitch. if (_v_control_mode->flag_control_climb_rate_enabled) { - _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); + pitch_body = update_and_get_backtransition_pitch_sp(); } if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { @@ -321,7 +326,7 @@ void Tiltrotor::update_transition_state() _v_att_sp->thrust_body[2] = -_thrust_transition; - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + const Quatf q_sp(Eulerf(roll_body, pitch_body, yaw_body)); q_sp.copyTo(_v_att_sp->q_d); _mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index b464793c99ce..199dcac84c56 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -284,10 +284,10 @@ VtolAttitudeControl::Run() return; } - const hrt_abstime now = hrt_absolute_time(); - #if !defined(ENABLE_LOCKSTEP_SCHEDULER) + const hrt_abstime now = hrt_absolute_time(); + // prevent excessive scheduling (> 500 Hz) if (now - _last_run_timestamp < 2_ms) { return; @@ -295,9 +295,6 @@ VtolAttitudeControl::Run() #endif // !ENABLE_LOCKSTEP_SCHEDULER - const float dt = math::min((now - _last_run_timestamp) / 1e6f, kMaxVTOLAttitudeControlTimeStep); - _last_run_timestamp = now; - if (!_initialized) { if (_vtol_type->init()) { @@ -309,8 +306,6 @@ VtolAttitudeControl::Run() } } - _vtol_type->setDt(dt); - perf_begin(_loop_perf); bool updated_fw_in = _vehicle_torque_setpoint_virtual_fw_sub.update(&_vehicle_torque_setpoint_virtual_fw); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 132448177f33..6ef046f05f8a 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -92,8 +92,6 @@ using namespace time_literals; extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); -static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s] - class VtolAttitudeControl : public ModuleBase, public ModuleParams, public px4::WorkItem { public: @@ -211,7 +209,9 @@ class VtolAttitudeControl : public ModuleBase, public Modul float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos}; // [kg/m^3] - hrt_abstime _last_run_timestamp{0}; +#if !defined(ENABLE_LOCKSTEP_SCHEDULER) + hrt_abstime _last_run_timestamp {0}; +#endif // !ENABLE_LOCKSTEP_SCHEDULER /* For multicopters it is usual to have a non-zero idle speed of the engines * for fixed wings we want to have an idle speed of zero since we do not want diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 808c9c03c2a9..458b3927fb9c 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -164,6 +164,8 @@ void VtolType::update_transition_state() _time_since_trans_start = (float)(t_now - _transition_start_timestamp) * 1e-6f; check_quadchute_condition(); + + _last_thr_in_mc = _vehicle_thrust_setpoint_virtual_mc->xyz[2]; } float VtolType::update_and_get_backtransition_pitch_sp() @@ -324,7 +326,7 @@ void VtolType::handleEkfResets() _altitude_reset_counter = _local_pos->z_reset_counter; if (PX4_ISFINITE(_quadchute_ref_alt)) { - _quadchute_ref_alt += _local_pos->delta_z; + _quadchute_ref_alt -= _local_pos->delta_z; } } @@ -561,10 +563,10 @@ float VtolType::pusher_assist() tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints - _v_att_sp->pitch_body = atan2f(tilt_new(0), tilt_new(2)); - _v_att_sp->roll_body = -asinf(tilt_new(1)); + const float pitch_body = atan2f(tilt_new(0), tilt_new(2)); + const float roll_body = -asinf(tilt_new(1)); - const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, euler_sp(2))); + const Quatf q_sp(Eulerf(roll_body, pitch_body, euler_sp(2))); q_sp.copyTo(_v_att_sp->q_d); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d59887372efb..8f9ae9ef3861 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -251,12 +251,6 @@ class VtolType : public ModuleParams virtual void parameters_update() = 0; - /** - * @brief Set current time delta - * - * @param dt Current time delta [s] - */ - void setDt(float dt) {_dt = dt; } /** * @brief Resets the transition timer states. @@ -305,6 +299,7 @@ class VtolType : public ModuleParams // motors spinning up or cutting too fast when doing transitions. float _thrust_transition = 0.0f; // thrust value applied during a front transition (tailsitter & tiltrotor only) float _last_thr_in_fw_mode = 0.0f; + float _last_thr_in_mc = 0.f; hrt_abstime _trans_finished_ts = 0; hrt_abstime _transition_start_timestamp{0}; @@ -326,8 +321,6 @@ class VtolType : public ModuleParams bool isFrontTransitionCompleted(); virtual bool isFrontTransitionCompletedBase(); - float _dt{0.0025f}; // time step [s] - float _local_position_z_start_of_transition{0.f}; // altitude at start of transition int _altitude_reset_counter{0}; diff --git a/src/modules/zenoh/CMakeLists.txt b/src/modules/zenoh/CMakeLists.txt index 889a1994a427..e3a8166dff73 100644 --- a/src/modules/zenoh/CMakeLists.txt +++ b/src/modules/zenoh/CMakeLists.txt @@ -57,6 +57,7 @@ target_compile_options(zenohpico PUBLIC -Wno-cast-align -DZ_BATCH_SIZE_TX=512 -DZ_FRAG_MAX_SIZE=1024) +target_compile_options(zenohpico PRIVATE -Wno-missing-prototypes) if(CONFIG_PLATFORM_NUTTX) target_compile_options(zenohpico PRIVATE -DUNIX_NO_MULTICAST_IF) endif() @@ -72,7 +73,6 @@ px4_add_module( SRCS zenoh.cpp zenoh_config.cpp - zenoh.h publishers/zenoh_publisher.cpp subscribers/zenoh_subscriber.cpp MODULE_CONFIG @@ -96,6 +96,5 @@ px4_add_module( -Wno-double-promotion -Wno-unused -DZENOH_LINUX - -DZENOH_NO_STDATOMIC -D_Bool=int8_t ) diff --git a/src/modules/zenoh/Kconfig b/src/modules/zenoh/Kconfig index d868dae392a0..c3c042df1317 100644 --- a/src/modules/zenoh/Kconfig +++ b/src/modules/zenoh/Kconfig @@ -6,18 +6,6 @@ menuconfig MODULES_ZENOH Enable support for Zenoh if MODULES_ZENOH - config ZENOH_CONFIG_PATH - string "Config path" - default "/fs/microsd/zenoh" - help - Path to store network, publishers and subscribers configuration - - config ZENOH_SERIAL - bool "Zenoh serial transport" - default n - help - Enables transport over serial (Not yet supported on NuttX/Linux) - config ZENOH_DEBUG int "Zenoh debug level" default 0 diff --git a/src/modules/zenoh/Kconfig.topics b/src/modules/zenoh/Kconfig.topics index 14866f5ebff6..ee9fe7ce7362 100644 --- a/src/modules/zenoh/Kconfig.topics +++ b/src/modules/zenoh/Kconfig.topics @@ -85,6 +85,10 @@ menu "Zenoh publishers/subscribers" bool "camera_trigger" default n + config ZENOH_PUBSUB_CAN_INTERFACE_STATUS + bool "can_interface_status" + default n + config ZENOH_PUBSUB_CELLULAR_STATUS bool "cellular_status" default n @@ -241,6 +245,10 @@ menu "Zenoh publishers/subscribers" bool "follow_target_status" default n + config ZENOH_PUBSUB_FUEL_TANK_STATUS + bool "fuel_tank_status" + default n + config ZENOH_PUBSUB_GENERATOR_STATUS bool "generator_status" default n @@ -249,6 +257,10 @@ menu "Zenoh publishers/subscribers" bool "geofence_result" default n + config ZENOH_PUBSUB_GEOFENCE_STATUS + bool "geofence_status" + default n + config ZENOH_PUBSUB_GIMBAL_CONTROLS bool "gimbal_controls" default n @@ -465,6 +477,22 @@ menu "Zenoh publishers/subscribers" bool "orbit_status" default n + config ZENOH_PUBSUB_PARAMETER_RESET_REQUEST + bool "parameter_reset_request" + default n + + config ZENOH_PUBSUB_PARAMETER_SET_USED_REQUEST + bool "parameter_set_used_request" + default n + + config ZENOH_PUBSUB_PARAMETER_SET_VALUE_REQUEST + bool "parameter_set_value_request" + default n + + config ZENOH_PUBSUB_PARAMETER_SET_VALUE_RESPONSE + bool "parameter_set_value_response" + default n + config ZENOH_PUBSUB_PARAMETER_UPDATE bool "parameter_update" default n @@ -541,10 +569,22 @@ menu "Zenoh publishers/subscribers" bool "register_ext_component_request" default n + config ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + bool "rover_ackermann_guidance_status" + default n + + config ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS + bool "rover_ackermann_status" + default n + config ZENOH_PUBSUB_RPM bool "rpm" default n + config ZENOH_PUBSUB_RTL_STATUS + bool "rtl_status" + default n + config ZENOH_PUBSUB_RTL_TIME_ESTIMATE bool "rtl_time_estimate" default n @@ -851,6 +891,7 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_CAMERA_CAPTURE select ZENOH_PUBSUB_CAMERA_STATUS select ZENOH_PUBSUB_CAMERA_TRIGGER + select ZENOH_PUBSUB_CAN_INTERFACE_STATUS select ZENOH_PUBSUB_CELLULAR_STATUS select ZENOH_PUBSUB_COLLISION_CONSTRAINTS select ZENOH_PUBSUB_COLLISION_REPORT @@ -890,8 +931,10 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_FOLLOW_TARGET select ZENOH_PUBSUB_FOLLOW_TARGET_ESTIMATOR select ZENOH_PUBSUB_FOLLOW_TARGET_STATUS + select ZENOH_PUBSUB_FUEL_TANK_STATUS select ZENOH_PUBSUB_GENERATOR_STATUS select ZENOH_PUBSUB_GEOFENCE_RESULT + select ZENOH_PUBSUB_GEOFENCE_STATUS select ZENOH_PUBSUB_GIMBAL_CONTROLS select ZENOH_PUBSUB_GIMBAL_DEVICE_ATTITUDE_STATUS select ZENOH_PUBSUB_GIMBAL_DEVICE_INFORMATION @@ -946,6 +989,10 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_ORB_TEST_LARGE select ZENOH_PUBSUB_ORB_TEST_MEDIUM select ZENOH_PUBSUB_ORBIT_STATUS + select ZENOH_PUBSUB_PARAMETER_RESET_REQUEST + select ZENOH_PUBSUB_PARAMETER_SET_USED_REQUEST + select ZENOH_PUBSUB_PARAMETER_SET_VALUE_REQUEST + select ZENOH_PUBSUB_PARAMETER_SET_VALUE_RESPONSE select ZENOH_PUBSUB_PARAMETER_UPDATE select ZENOH_PUBSUB_PING select ZENOH_PUBSUB_POSITION_CONTROLLER_LANDING_STATUS @@ -965,7 +1012,10 @@ config ZENOH_PUBSUB_ALL_SELECTION select ZENOH_PUBSUB_RC_PARAMETER_MAP select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REPLY select ZENOH_PUBSUB_REGISTER_EXT_COMPONENT_REQUEST + select ZENOH_PUBSUB_ROVER_ACKERMANN_GUIDANCE_STATUS + select ZENOH_PUBSUB_ROVER_ACKERMANN_STATUS select ZENOH_PUBSUB_RPM + select ZENOH_PUBSUB_RTL_STATUS select ZENOH_PUBSUB_RTL_TIME_ESTIMATE select ZENOH_PUBSUB_SATELLITE_INFO select ZENOH_PUBSUB_SENSOR_ACCEL diff --git a/src/modules/zenoh/publishers/uorb_publisher.hpp b/src/modules/zenoh/publishers/uorb_publisher.hpp index b541bfd1dd7c..fd41e4f2e88c 100644 --- a/src/modules/zenoh/publishers/uorb_publisher.hpp +++ b/src/modules/zenoh/publishers/uorb_publisher.hpp @@ -51,7 +51,7 @@ class uORB_Zenoh_Publisher : public Zenoh_Publisher { public: uORB_Zenoh_Publisher(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Publisher(true), + Zenoh_Publisher(), _uorb_meta{meta}, _cdr_ops(ops) { diff --git a/src/modules/zenoh/publishers/zenoh_publisher.cpp b/src/modules/zenoh/publishers/zenoh_publisher.cpp index 1e312ffea502..8ce688d2b787 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.cpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.cpp @@ -42,9 +42,8 @@ #include "zenoh_publisher.hpp" -Zenoh_Publisher::Zenoh_Publisher(bool rostopic) +Zenoh_Publisher::Zenoh_Publisher() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -59,24 +58,18 @@ int Zenoh_Publisher::undeclare_publisher() return 0; } -int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) +int Zenoh_Publisher::declare_publisher(z_owned_session_t s, const char *keyexpr) { - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, keyexpr, sizeof(this->_topic)); + if (z_declare_publisher(&_pub, z_loan(s), z_loan(ke), NULL) < 0) { + printf("Unable to declare publisher for key expression!\n"); + return -1; } - _pub = z_declare_publisher(s, z_keyexpr(this->_topic), NULL); - if (!z_publisher_check(&_pub)) { printf("Unable to declare publisher for key expression!\n"); return -1; @@ -87,9 +80,13 @@ int Zenoh_Publisher::declare_publisher(z_session_t s, const char *keyexpr) int8_t Zenoh_Publisher::publish(const uint8_t *buf, int size) { - z_publisher_put_options_t options = z_publisher_put_options_default(); - options.encoding = z_encoding(Z_ENCODING_PREFIX_APP_CUSTOM, NULL); - return z_publisher_put(z_publisher_loan(&_pub), buf, size, &options); + z_publisher_put_options_t options; + z_publisher_put_options_default(&options); + options.encoding = NULL; + + z_owned_bytes_t payload; + z_bytes_serialize_from_slice(&payload, buf, size); + return z_publisher_put(z_loan(_pub), z_move(payload), &options); } void Zenoh_Publisher::print() diff --git a/src/modules/zenoh/publishers/zenoh_publisher.hpp b/src/modules/zenoh/publishers/zenoh_publisher.hpp index 1d88a453e2f7..a12849c78682 100644 --- a/src/modules/zenoh/publishers/zenoh_publisher.hpp +++ b/src/modules/zenoh/publishers/zenoh_publisher.hpp @@ -51,10 +51,10 @@ class Zenoh_Publisher : public ListNode { public: - Zenoh_Publisher(bool rostopic = true); + Zenoh_Publisher(); virtual ~Zenoh_Publisher(); - virtual int declare_publisher(z_session_t s, const char *keyexpr); + virtual int declare_publisher(z_owned_session_t s, const char *keyexpr); virtual int undeclare_publisher(); @@ -66,11 +66,5 @@ class Zenoh_Publisher : public ListNode int8_t publish(const uint8_t *, int size); z_owned_publisher_t _pub; - char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/subscribers/uorb_subscriber.hpp b/src/modules/zenoh/subscribers/uorb_subscriber.hpp index 550c1a3f4584..bd59adb84606 100644 --- a/src/modules/zenoh/subscribers/uorb_subscriber.hpp +++ b/src/modules/zenoh/subscribers/uorb_subscriber.hpp @@ -50,7 +50,7 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber { public: uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) : - Zenoh_Subscriber(true), + Zenoh_Subscriber(), _uorb_meta{meta}, _cdr_ops(ops) { @@ -61,11 +61,14 @@ class uORB_Zenoh_Subscriber : public Zenoh_Subscriber ~uORB_Zenoh_Subscriber() override = default; // Update the uORB Subscription and broadcast a Zenoh ROS2 message - void data_handler(const z_sample_t *sample) + void data_handler(const z_loaned_sample_t *sample) { char data[_uorb_meta->o_size]; - dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast(sample->payload.len), + const z_loaned_bytes_t *payload = z_sample_payload(sample); + size_t len = z_bytes_len(payload); + + dds_istream_t is = {.m_buffer = (unsigned char *)(payload), .m_size = static_cast(len), .m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2 }; dds_stream_read(&is, data, &dds_allocator, _cdr_ops); diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp index aab66ee5bdba..79f37d89812a 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.cpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.cpp @@ -41,22 +41,23 @@ #include "zenoh_subscriber.hpp" -static void data_handler_cb(const z_sample_t *sample, void *arg) +static void data_handler_cb(const z_loaned_sample_t *sample, void *arg) { static_cast(arg)->data_handler(sample); } -void Zenoh_Subscriber::data_handler(const z_sample_t *sample) +void Zenoh_Subscriber::data_handler(const z_loaned_sample_t *sample) { - z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr); - printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len); - z_str_drop(z_str_move(&keystr)); + z_view_string_t keystr; + z_keyexpr_as_view_string(z_sample_keyexpr(sample), &keystr); + z_owned_slice_t value; + z_bytes_deserialize_into_slice(z_sample_payload(sample), &value); + printf(">> [Subscriber] Received ('%s' size '%d')\n", z_string_data(z_loan(keystr)), (int)z_slice_len(z_loan(value))); } -Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic) +Zenoh_Subscriber::Zenoh_Subscriber() { - this->_rostopic = rostopic; this->_topic[0] = 0x0; } @@ -71,27 +72,21 @@ int Zenoh_Subscriber::undeclare_subscriber() return 0; } -int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr) +int Zenoh_Subscriber::declare_subscriber(z_owned_session_t s, const char *keyexpr) { - z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this); + z_owned_closure_sample_t callback; + z_closure_sample(&callback, data_handler_cb, NULL, this); - if (_rostopic) { - strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset); + strncpy(this->_topic, keyexpr, sizeof(this->_topic)); - if (keyexpr[0] == '/') { - strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset); + z_view_keyexpr_t ke; + z_view_keyexpr_from_str(&ke, this->_topic); - } else { - strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset); - } - - } else { - strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic)); + if (z_declare_subscriber(&_sub, z_loan(s), z_loan(ke), z_closure_sample_move(&callback), NULL) < 0) { + printf("Unable to declare subscriber.\n"); + exit(-1); } - _sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL); - - if (!z_subscriber_check(&_sub)) { printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr); return -1; diff --git a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp index e3f1200e9245..1b6d10922045 100644 --- a/src/modules/zenoh/subscribers/zenoh_subscriber.hpp +++ b/src/modules/zenoh/subscribers/zenoh_subscriber.hpp @@ -55,14 +55,14 @@ class Zenoh_Subscriber : public ListNode { public: - Zenoh_Subscriber(bool rostopic = true); + Zenoh_Subscriber(); virtual ~Zenoh_Subscriber(); - virtual int declare_subscriber(z_session_t s, const char *keyexpr); + virtual int declare_subscriber(z_owned_session_t s, const char *keyexpr); virtual int undeclare_subscriber(); - virtual void data_handler(const z_sample_t *sample); + virtual void data_handler(const z_loaned_sample_t *sample); virtual void print(); @@ -71,10 +71,4 @@ class Zenoh_Subscriber : public ListNode z_owned_subscriber_t _sub; char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it. - - - // Indicates ROS2 Topic namespace - bool _rostopic; - const char *_rt_prefix = "rt/"; - const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars }; diff --git a/src/modules/zenoh/zenoh-pico b/src/modules/zenoh/zenoh-pico index 22bbfc215092..f093aa7955bb 160000 --- a/src/modules/zenoh/zenoh-pico +++ b/src/modules/zenoh/zenoh-pico @@ -1 +1 @@ -Subproject commit 22bbfc215092a051341c234fdcf68f5baa267dec +Subproject commit f093aa7955bb24dfa3e626de084583711e3bac5c diff --git a/src/modules/zenoh/zenoh.cpp b/src/modules/zenoh/zenoh.cpp index 0caf2c5eaa5f..6411734a2918 100644 --- a/src/modules/zenoh/zenoh.cpp +++ b/src/modules/zenoh/zenoh.cpp @@ -32,6 +32,8 @@ ****************************************************************************/ #include "zenoh.h" +#include "zenoh-pico/api/macros.h" +#include "zenoh-pico/api/primitives.h" #include #include #include @@ -78,27 +80,39 @@ void ZENOH::run() z_config.getNetworkConfig(mode, locator); - z_owned_config_t config = z_config_default(); - zp_config_insert(z_config_loan(&config), Z_CONFIG_MODE_KEY, z_string_make(mode)); + z_owned_config_t config; + z_config_default(&config); + zp_config_insert(z_loan_mut(config), Z_CONFIG_MODE_KEY, mode); if (locator[0] != 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(locator)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, locator); } else if (strcmp(Z_CONFIG_MODE_PEER, mode) == 0) { - zp_config_insert(z_config_loan(&config), Z_CONFIG_PEER_KEY, z_string_make(Z_CONFIG_MULTICAST_LOCATOR_DEFAULT)); + zp_config_insert(z_loan_mut(config), Z_CONFIG_CONNECT_KEY, Z_CONFIG_MULTICAST_LOCATOR_DEFAULT); } PX4_INFO("Opening session..."); - z_owned_session_t s = z_open(z_config_move(&config)); + z_owned_session_t s; + ret = z_open(&s, z_move(config)); + + if (ret < 0) { + PX4_ERR("Unable to open session, ret: %d", ret); + return; + } + + PX4_INFO("Checking session..."); if (!z_session_check(&s)) { - PX4_ERR("Unable to open session!"); + PX4_ERR("Unable to check session!"); return; } + PX4_INFO("Starting reading/writing tasks..."); + // Start read and lease tasks for zenoh-pico - if (zp_start_read_task(z_session_loan(&s), NULL) < 0 || zp_start_lease_task(z_session_loan(&s), NULL) < 0) { + if (zp_start_read_task(z_loan_mut(s), NULL) < 0 || zp_start_lease_task(z_loan_mut(s), NULL) < 0) { PX4_ERR("Unable to start read and lease tasks"); + z_close(z_move(s)); return; } @@ -114,7 +128,7 @@ void ZENOH::run() _zenoh_subscribers[i] = genSubscriber(type); if (_zenoh_subscribers[i] != 0) { - _zenoh_subscribers[i]->declare_subscriber(z_session_loan(&s), topic); + _zenoh_subscribers[i]->declare_subscriber(s, topic); } @@ -141,7 +155,7 @@ void ZENOH::run() _zenoh_publishers[i] = genPublisher(type); if (_zenoh_publishers[i] != 0) { - _zenoh_publishers[i]->declare_publisher(z_session_loan(&s), topic); + _zenoh_publishers[i]->declare_publisher(s, topic); _zenoh_publishers[i]->setPollFD(&pfds[i]); } } @@ -154,7 +168,7 @@ void ZENOH::run() if (_pub_count == 0) { // Nothing to publish but we don't want to stop this thread while (!should_exit()) { - sleep(2); + usleep(1000); } } @@ -194,8 +208,8 @@ void ZENOH::run() free(_zenoh_publishers); // Stop read and lease tasks for zenoh-pico - zp_stop_read_task(z_session_loan(&s)); - zp_stop_lease_task(z_session_loan(&s)); + zp_stop_read_task(z_session_loan_mut(&s)); + zp_stop_lease_task(z_session_loan_mut(&s)); z_close(z_session_move(&s)); exit_and_cleanup(); @@ -234,8 +248,8 @@ Zenoh demo bridge PX4_INFO_RAW(" addsubscriber Publish Zenoh topic to uORB\n"); PX4_INFO_RAW(" net Zenoh network mode\n"); PX4_INFO_RAW(" values: client|peer \n"); - PX4_INFO_RAW(" client: locator address for router\n"); - PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.225:7447#iface=eth0\n"); + PX4_INFO_RAW(" client: locator address e.g. tcp/10.41.10.1:7447#iface=eth0\n"); + PX4_INFO_RAW(" peer: multicast address e.g. udp/224.0.0.224:7446#iface=eth0\n"); return 0; } diff --git a/src/modules/zenoh/zenoh_config.cpp b/src/modules/zenoh/zenoh_config.cpp index 8069956585cb..83f81fb31bbe 100644 --- a/src/modules/zenoh/zenoh_config.cpp +++ b/src/modules/zenoh/zenoh_config.cpp @@ -58,7 +58,7 @@ const char *default_sub_config = ""; //TODO maybe use YAML Zenoh_Config::Zenoh_Config() { bool correct_config = true; - DIR *dir = opendir(ZENOH_SD_ROOT_PATH); + DIR *dir = opendir(ZENOH_ROOT_PATH); fp_mapping = NULL; if (dir) { @@ -342,14 +342,14 @@ void Zenoh_Config::generate_clean_config() printf("Generate clean\n"); FILE *fp; - DIR *dir = opendir(ZENOH_SD_ROOT_PATH); + DIR *dir = opendir(ZENOH_ROOT_PATH); if (dir) { printf("Zenoh directory exists\n"); } else { /* Create zenoh dir. */ - if (mkdir(ZENOH_SD_ROOT_PATH, 0700) < 0) { + if (mkdir(ZENOH_ROOT_PATH, 0700) < 0) { printf("Failed to create Zenoh directory\n"); return; } diff --git a/src/modules/zenoh/zenoh_config.hpp b/src/modules/zenoh/zenoh_config.hpp index 72ae22d391d1..68a1f7121fb4 100644 --- a/src/modules/zenoh/zenoh_config.hpp +++ b/src/modules/zenoh/zenoh_config.hpp @@ -49,10 +49,10 @@ #include #define ZENOH_MAX_PATH_LENGTH (128 + 40) -#define ZENOH_SD_ROOT_PATH CONFIG_ZENOH_CONFIG_PATH -#define ZENOH_PUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/pub.csv" -#define ZENOH_SUB_CONFIG_PATH ZENOH_SD_ROOT_PATH"/sub.csv" -#define ZENOH_NET_CONFIG_PATH ZENOH_SD_ROOT_PATH"/net.txt" +#define ZENOH_ROOT_PATH CONFIG_BOARD_ROOT_PATH"/zenoh" +#define ZENOH_PUB_CONFIG_PATH ZENOH_ROOT_PATH"/pub.csv" +#define ZENOH_SUB_CONFIG_PATH ZENOH_ROOT_PATH"/sub.csv" +#define ZENOH_NET_CONFIG_PATH ZENOH_ROOT_PATH"/net.txt" #define NET_MODE_SIZE sizeof("client") #define NET_LOCATOR_SIZE 64 diff --git a/src/systemcmds/actuator_test/CMakeLists.txt b/src/systemcmds/actuator_test/CMakeLists.txt index a71e080b5bef..a919bf17c785 100644 --- a/src/systemcmds/actuator_test/CMakeLists.txt +++ b/src/systemcmds/actuator_test/CMakeLists.txt @@ -37,4 +37,3 @@ px4_add_module( SRCS actuator_test.cpp ) - diff --git a/src/systemcmds/bl_update/Kconfig b/src/systemcmds/bl_update/Kconfig index f84e2f923efe..59e072f5b53c 100644 --- a/src/systemcmds/bl_update/Kconfig +++ b/src/systemcmds/bl_update/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_BL_UPDATE bool "bl_update" default n + depends on PLATFORM_NUTTX ---help--- Enable support for bl_update diff --git a/src/systemcmds/bsondump/bsondump.cpp b/src/systemcmds/bsondump/bsondump.cpp index 50021eba147f..a7615d0143c2 100644 --- a/src/systemcmds/bsondump/bsondump.cpp +++ b/src/systemcmds/bsondump/bsondump.cpp @@ -36,19 +36,27 @@ #include #include +#include #include -static void print_usage(const char *reason = nullptr) +static void print_usage(const char *reason = nullptr, const char *command = nullptr) { if (reason) { PX4_ERR("%s", reason); } - PRINT_MODULE_DESCRIPTION("read BSON from a file and print in human form"); + PRINT_MODULE_DESCRIPTION("Utility to read BSON from a file and print or output document size."); - PRINT_MODULE_USAGE_NAME_SIMPLE("bsondump", "command"); - PRINT_MODULE_USAGE_ARG("", "File name", false); + if (command == nullptr || strcmp(command, "docsize") == 0) { + PRINT_MODULE_USAGE_NAME_SIMPLE("bsondump docsize", "command"); + PRINT_MODULE_USAGE_ARG("", "The BSON file to decode for document size.", false); + } + + if (command == nullptr) { + PRINT_MODULE_USAGE_NAME_SIMPLE("bsondump", "command"); + PRINT_MODULE_USAGE_ARG("", "The BSON file to decode and print.", false); + } } static int bson_print_callback(bson_decoder_t decoder, bson_node_t node) @@ -82,58 +90,140 @@ static int bson_print_callback(bson_decoder_t decoder, bson_node_t node) return -1; } +static int +bson_docsize_callback(bson_decoder_t decoder, bson_node_t node) +{ + + if (node->type == BSON_EOO) { + PX4_DEBUG("end of parameters"); + return 0; + } + + return 1; +} + extern "C" __EXPORT int bsondump_main(int argc, char *argv[]) { - if (argc != 2) { - print_usage(); + + if (argc < 2) { + print_usage("Invalid number of arguments."); + return -1; + } + + const char *command = argv[1]; + + if (strcmp(command, "docsize") == 0) { + + if (argc != 3) { + print_usage("Usage: bsondump docsize ", "docsize"); + return -1; + } + + const char *file_name = argv[2]; + int source_fd = open(file_name, O_RDONLY); + + if (source_fd < 0) { + PX4_ERR("open '%s' failed (%i)", file_name, errno); + return 1; + } + + bson_decoder_s decoder{}; + int result = -1; + + if (bson_decoder_init_file(&decoder, source_fd, bson_docsize_callback) == 0) { + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + + PX4_INFO("DECODED_SIZE:%" PRId32 " SAVED_SIZE:%" PRId32 "\n", + decoder.total_decoded_size, decoder.total_document_size); + } + + close(source_fd); + + if (decoder.total_decoded_size != decoder.total_document_size && decoder.total_document_size == 0) { + + PX4_WARN("Mismatch in BSON sizes and saved size is zero. Setting document size to decoded size."); + + source_fd = open(file_name, O_RDWR); + + if (source_fd == -1) { + perror("Failed to re-open source file for reading and writing"); + return -1; + } + + // Modify the first 4 bytes with the correct decoded size + uint32_t corrected_size = decoder.total_decoded_size; + + if (lseek(source_fd, 0, SEEK_SET) == (off_t) -1) { + perror("Failed to seek to the beginning of the file"); + close(source_fd); + return -1; + } + + if (write(source_fd, &corrected_size, sizeof(corrected_size)) != sizeof(corrected_size)) { + perror("Failed to write the corrected size to the file"); + close(source_fd); + return -1; + } + + close(source_fd); + + return 1; + } + + return 0; + } else { - char *file_name = argv[1]; - if (file_name) { - int fd = open(file_name, O_RDONLY); + const char *file_name = argv[1]; - if (fd < 0) { - PX4_ERR("open '%s' failed (%i)", file_name, errno); - return 1; + int fd = open(file_name, O_RDONLY); - } else { - PX4_INFO_RAW("[bsondump] reading from %s\n", file_name); + if (fd < 0) { + PX4_ERR("open '%s' failed (%i)", file_name, errno); + return 1; - bson_decoder_s decoder{}; + } else { + PX4_INFO_RAW("[bsondump] reading from %s\n", file_name); - if (bson_decoder_init_file(&decoder, fd, bson_print_callback) == 0) { - PX4_INFO_RAW("BSON document size %" PRId32 "\n", decoder.total_document_size); + bson_decoder_s decoder{}; - int result = -1; + if (bson_decoder_init_file(&decoder, fd, bson_print_callback) == 0) { + PX4_INFO_RAW("BSON document size %" PRId32 "\n", decoder.total_document_size); - do { - result = bson_decoder_next(&decoder); + int result = -1; - } while (result > 0); + do { + result = bson_decoder_next(&decoder); - close(fd); + } while (result > 0); - if (result == 0) { - PX4_INFO_RAW("BSON decoded %" PRId32 " bytes (double:%" PRIu16 ", string:%" PRIu16 ", bin:%" PRIu16 ", bool:%" PRIu16 - ", int32:%" PRIu16 ", int64:%" PRIu16 ")\n", - decoder.total_decoded_size, - decoder.count_node_double, decoder.count_node_string, decoder.count_node_bindata, decoder.count_node_bool, - decoder.count_node_int32, decoder.count_node_int64); + close(fd); - return 0; + if (result == 0) { + PX4_INFO_RAW("BSON decoded %" PRId32 " bytes (double:%" PRIu16 ", string:%" PRIu16 ", bin:%" PRIu16 ", bool:%" PRIu16 + ", int32:%" PRIu16 ", int64:%" PRIu16 ")\n", + decoder.total_decoded_size, + decoder.count_node_double, decoder.count_node_string, decoder.count_node_bindata, decoder.count_node_bool, + decoder.count_node_int32, decoder.count_node_int64); - } else if (result == -ENODATA) { - PX4_WARN("no data"); - return -1; + return 0; - } else { - PX4_ERR("failed (%d)", result); - return -1; - } + } else if (result == -ENODATA) { + PX4_WARN("no data"); + return -1; + + } else { + PX4_ERR("failed (%d)", result); + return -1; } } } + } return -1; diff --git a/src/systemcmds/dumpfile/CMakeLists.txt b/src/systemcmds/dumpfile/CMakeLists.txt index e8a7c1c5ac20..00fce2ce7bf8 100644 --- a/src/systemcmds/dumpfile/CMakeLists.txt +++ b/src/systemcmds/dumpfile/CMakeLists.txt @@ -39,4 +39,3 @@ px4_add_module( dumpfile.cpp DEPENDS ) - diff --git a/src/systemcmds/hardfault_log/CMakeLists.txt b/src/systemcmds/hardfault_log/CMakeLists.txt index fca7fc683929..8cd4ca9fea86 100644 --- a/src/systemcmds/hardfault_log/CMakeLists.txt +++ b/src/systemcmds/hardfault_log/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN hardfault_log COMPILE_FLAGS -Wno-cast-align # TODO: fix and enable + STACK_MAIN 4096 SRCS hardfault_log.c DEPENDS diff --git a/src/systemcmds/hardfault_log/Kconfig b/src/systemcmds/hardfault_log/Kconfig index 50cfca75fca2..2c3908ed0de2 100644 --- a/src/systemcmds/hardfault_log/Kconfig +++ b/src/systemcmds/hardfault_log/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_HARDFAULT_LOG bool "hardfault_log" default n + depends on PLATFORM_NUTTX ---help--- Enable support for hardfault_log diff --git a/src/systemcmds/hardfault_log/hardfault_log.c b/src/systemcmds/hardfault_log/hardfault_log.c index 0978fc653601..f8ff269ec6b8 100644 --- a/src/systemcmds/hardfault_log/hardfault_log.c +++ b/src/systemcmds/hardfault_log/hardfault_log.c @@ -141,6 +141,21 @@ static int genfault(int fault) /* This is not going to happen */ break; + case 2: + asm("BX %0" : : "r"(0x0)); + /* This is not going to happen */ + break; + + case 3: { + char buffer[128] = {0}; + void *dest = (void *)0x0; + + memcpy(dest, &buffer, 128); + /* This is not going to happen */ + } + break; + + default: break; @@ -1270,7 +1285,8 @@ static void print_usage(void) PRINT_MODULE_USAGE_COMMAND_DESCR("rearm", "Drop an uncommitted hardfault"); PRINT_MODULE_USAGE_COMMAND_DESCR("fault", "Generate a hardfault (this command crashes the system :)"); - PRINT_MODULE_USAGE_ARG("0|1", "Hardfault type: 0=divide by 0, 1=Assertion (default=0)", true); + PRINT_MODULE_USAGE_ARG("0|1|2|3", + "Hardfault type: 0=divide by 0, 1=Assertion, 2=jump to 0x0, 3=write to 0x0 (default=0)", true); PRINT_MODULE_USAGE_COMMAND_DESCR("commit", "Write uncommitted hardfault to /fs/microsd/fault_%i.txt (and rearm, but don't reset)"); diff --git a/src/systemcmds/io_bypass_control/Kconfig b/src/systemcmds/io_bypass_control/Kconfig index 4a50afd948ba..16c89bd13021 100644 --- a/src/systemcmds/io_bypass_control/Kconfig +++ b/src/systemcmds/io_bypass_control/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_IO_BYPASS_CONTROL bool "IO Bypass control deamon" default n + depends on PLATFORM_NUTTX ---help--- Simple daemon that listens uORB actuator_outputs to control PWM output Useful for full offboard control using RTPS. diff --git a/src/systemcmds/led_control/CMakeLists.txt b/src/systemcmds/led_control/CMakeLists.txt index a7396160cce1..7b742d0f5297 100644 --- a/src/systemcmds/led_control/CMakeLists.txt +++ b/src/systemcmds/led_control/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( led_control.cpp DEPENDS ) - diff --git a/src/systemcmds/mft/Kconfig b/src/systemcmds/mft/Kconfig index a104b1fd0dd3..554c9199fbfd 100644 --- a/src/systemcmds/mft/Kconfig +++ b/src/systemcmds/mft/Kconfig @@ -1,5 +1,6 @@ menuconfig SYSTEMCMDS_MFT bool "mft" default n + depends on PLATFORM_NUTTX ---help--- Enable support for mft diff --git a/src/systemcmds/microbench/microbench_main.cpp b/src/systemcmds/microbench/microbench_main.cpp index f56dbd6a2c68..56e441cc0936 100644 --- a/src/systemcmds/microbench/microbench_main.cpp +++ b/src/systemcmds/microbench/microbench_main.cpp @@ -69,7 +69,7 @@ const struct { {"microbench_matrix", test_microbench_matrix, 0}, {"microbench_uorb", test_microbench_uorb, 0}, - {nullptr, nullptr, 0} + {"null", nullptr, 0} }; #define NMICROBENCHMARKS (sizeof(microbenchmarks) / sizeof(microbenchmarks[0])) diff --git a/src/systemcmds/mtd/CMakeLists.txt b/src/systemcmds/mtd/CMakeLists.txt index 1f2d23cb47e7..4f068d107316 100644 --- a/src/systemcmds/mtd/CMakeLists.txt +++ b/src/systemcmds/mtd/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( mtd.cpp DEPENDS ) - diff --git a/src/systemcmds/mtd/Kconfig b/src/systemcmds/mtd/Kconfig index 661e8ee7f68e..dd01cfc3d21f 100644 --- a/src/systemcmds/mtd/Kconfig +++ b/src/systemcmds/mtd/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_MTD bool "mtd" default n + depends on PLATFORM_NUTTX ---help--- Enable support for mtd diff --git a/src/systemcmds/netman/Kconfig b/src/systemcmds/netman/Kconfig index d2d122339793..6502969ddf8a 100644 --- a/src/systemcmds/netman/Kconfig +++ b/src/systemcmds/netman/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_NETMAN bool "netman" default n + depends on PLATFORM_NUTTX ---help--- Enable support for netman @@ -13,8 +14,8 @@ menuconfig USER_NETMAN config NETMAN_FALLBACK_IPADDR hex "Fallback IPv4 address" - default 0XC0A80003 + default 0xA290A02 depends on SYSTEMCMDS_NETMAN ---help--- If NETINIT_DHCPC is set, getting an IP from DHCP server is first attempted. - If this fails, this IP address is used as a static fallback. + If this fails, this IP address is used as a static fallback (default=10.41.10.2). diff --git a/src/systemcmds/netman/netman.cpp b/src/systemcmds/netman/netman.cpp index b9e653bdfeb1..a7014c62de28 100644 --- a/src/systemcmds/netman/netman.cpp +++ b/src/systemcmds/netman/netman.cpp @@ -51,7 +51,7 @@ #include #include -constexpr char DEFAULT_NETMAN_CONFIG[] = "/fs/microsd/net.cfg"; +constexpr char DEFAULT_NETMAN_CONFIG[] = CONFIG_BOARD_ROOT_PATH "/net.cfg"; #if defined(CONFIG_NETINIT_DHCPC) # define DEFAULT_PROTO IPv4PROTO_FALLBACK # define DEFAULT_IP CONFIG_NETMAN_FALLBACK_IPADDR diff --git a/src/systemcmds/nshterm/CMakeLists.txt b/src/systemcmds/nshterm/CMakeLists.txt index 14f669c3de44..757ade306303 100644 --- a/src/systemcmds/nshterm/CMakeLists.txt +++ b/src/systemcmds/nshterm/CMakeLists.txt @@ -39,4 +39,3 @@ px4_add_module( nshterm.cpp DEPENDS ) - diff --git a/src/systemcmds/nshterm/Kconfig b/src/systemcmds/nshterm/Kconfig index f9d20b0bfb72..3745b7bf238b 100644 --- a/src/systemcmds/nshterm/Kconfig +++ b/src/systemcmds/nshterm/Kconfig @@ -1,6 +1,7 @@ menuconfig SYSTEMCMDS_NSHTERM bool "nshterm" default n + depends on PLATFORM_NUTTX ---help--- Enable support for nshterm diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt index 0a27cf958949..056401daf75e 100644 --- a/src/systemcmds/param/CMakeLists.txt +++ b/src/systemcmds/param/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_module( MAIN param COMPILE_FLAGS -Wno-array-bounds + STACK_MAIN 4096 SRCS param.cpp DEPENDS diff --git a/src/systemcmds/reboot/CMakeLists.txt b/src/systemcmds/reboot/CMakeLists.txt index 60f4cee88951..5987399037ef 100644 --- a/src/systemcmds/reboot/CMakeLists.txt +++ b/src/systemcmds/reboot/CMakeLists.txt @@ -39,4 +39,3 @@ px4_add_module( DEPENDS px4_platform ) - diff --git a/src/systemcmds/reflect/CMakeLists.txt b/src/systemcmds/reflect/CMakeLists.txt index 19241deaeca5..eb0e4042a72e 100644 --- a/src/systemcmds/reflect/CMakeLists.txt +++ b/src/systemcmds/reflect/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( reflect.c DEPENDS ) - diff --git a/src/systemcmds/reflect/reflect.c b/src/systemcmds/reflect/reflect.c index 4ef393682deb..c74ae06a887e 100644 --- a/src/systemcmds/reflect/reflect.c +++ b/src/systemcmds/reflect/reflect.c @@ -121,7 +121,9 @@ reflect_main(int argc, char *argv[]) } if (n > 0) { - write(1, buf, n); + if (write(1, buf, n) < 0) { + return -1; + } } total += n; diff --git a/src/systemcmds/sd_bench/CMakeLists.txt b/src/systemcmds/sd_bench/CMakeLists.txt index 9ab943a90b58..84e200eb2a28 100644 --- a/src/systemcmds/sd_bench/CMakeLists.txt +++ b/src/systemcmds/sd_bench/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( sd_bench.cpp DEPENDS ) - diff --git a/src/systemcmds/sd_bench/sd_bench.cpp b/src/systemcmds/sd_bench/sd_bench.cpp index 990932432162..6912c5a3368c 100644 --- a/src/systemcmds/sd_bench/sd_bench.cpp +++ b/src/systemcmds/sd_bench/sd_bench.cpp @@ -50,11 +50,13 @@ #include +#define MAX(a,b) ((a) > (b) ? (a) : (b)) + typedef struct sdb_config { int num_runs; ///< number of runs int run_duration; ///< duration of a single run [ms] bool synchronized; ///< call fsync after each block? - bool aligned; + int unaligned; unsigned int total_blocks_written; } sdb_config_t; @@ -83,6 +85,7 @@ static void usage() PRINT_MODULE_USAGE_PARAM_FLAG('k', "Keep the test file", true); PRINT_MODULE_USAGE_PARAM_FLAG('s', "Call fsync after each block (default=at end of each run)", true); PRINT_MODULE_USAGE_PARAM_FLAG('u', "Test performance with unaligned data", true); + PRINT_MODULE_USAGE_PARAM_FLAG('U', "Test performance with forced byte unaligned data", true); PRINT_MODULE_USAGE_PARAM_FLAG('v', "Verify data and block number", true); } @@ -98,10 +101,11 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[]) cfg.synchronized = false; cfg.num_runs = 5; cfg.run_duration = 2000; - cfg.aligned = true; + cfg.unaligned = 0; uint8_t *block = nullptr; + uint8_t *block_alloc = nullptr; - while ((ch = px4_getopt(argc, argv, "b:r:d:ksuv", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:ksuUv", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': block_size = strtol(myoptarg, nullptr, 0); @@ -124,7 +128,11 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[]) break; case 'u': - cfg.aligned = false; + cfg.unaligned = 2; + break; + + case 'U': + cfg.unaligned = 1; break; case 'v': @@ -151,11 +159,24 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[]) } //create some data block - if (cfg.aligned) { - block = (uint8_t *)px4_cache_aligned_alloc(block_size); + if (cfg.unaligned == 0) { + block_alloc = (uint8_t *)px4_cache_aligned_alloc(block_size); + block = block_alloc; } else { - block = (uint8_t *)malloc(block_size); + block_alloc = (uint8_t *)malloc(block_size + 4); + + if (block_alloc) { + // Force odd byte alignment + if (cfg.unaligned == 1 && ((uintptr_t)block_alloc % 0x1) == 0) { + block = block_alloc + 1; + + } else { + block = block_alloc; + } + } + + printf("Block ptr %p\n", block); } if (!block) { @@ -177,7 +198,7 @@ extern "C" __EXPORT int sd_bench_main(int argc, char *argv[]) read_test(bench_fd, &cfg, block, block_size); } - free(block); + free(block_alloc); close(bench_fd); if (!keep) { @@ -202,6 +223,7 @@ void write_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) unsigned int total_blocks = 0; cfg->total_blocks_written = 0; unsigned int *blocknumber = (unsigned int *)(void *)&block[0]; + unsigned int max_max_write_time = 0; for (int run = 0; run < cfg->num_runs; ++run) { hrt_abstime start = hrt_absolute_time(); @@ -245,24 +267,40 @@ void write_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) total_elapsed += elapsed; total_blocks += num_blocks; + max_max_write_time = MAX(max_max_write_time, max_write_time); } cfg->total_blocks_written = total_blocks; PX4_INFO(" Avg : %8.2lf KB/s", (double)block_size * total_blocks / total_elapsed / 1024.); + PX4_INFO(" Overall max write time: %i ms", max_max_write_time); } int read_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) { uint8_t *read_block = nullptr; + uint8_t *block_alloc = nullptr; PX4_INFO(""); PX4_INFO("Testing Sequential Read Speed of %d blocks", cfg->total_blocks_written); - if (cfg->aligned) { - read_block = (uint8_t *)px4_cache_aligned_alloc(block_size); + if (cfg->unaligned == 0) { + block_alloc = (uint8_t *)px4_cache_aligned_alloc(block_size); + read_block = block_alloc; } else { - read_block = (uint8_t *)malloc(block_size); + block_alloc = (uint8_t *)malloc(block_size + 4); + + if (block_alloc) { + // Force odd byte alignment + if (cfg->unaligned == 1 && ((uintptr_t)block_alloc % 0x1) == 0) { + read_block = block_alloc + 1; + + } else { + read_block = block_alloc; + } + } + + printf("Read Block ptr %p\n", read_block); } if (!read_block) { @@ -326,6 +364,6 @@ int read_test(int fd, sdb_config_t *cfg, uint8_t *block, int block_size) PX4_INFO(" Avg : %8.2lf KB/s %d blocks read and verified", (double)block_size * total_blocks / total_elapsed / 1024., total_blocks); - free(read_block); + free(block_alloc); return 0; } diff --git a/src/systemcmds/serial_passthru/Kconfig b/src/systemcmds/serial_passthru/Kconfig index 4c4cb107d8ac..aabd8c59fe27 100644 --- a/src/systemcmds/serial_passthru/Kconfig +++ b/src/systemcmds/serial_passthru/Kconfig @@ -11,7 +11,7 @@ config SERIAL_PASSTHRU_UBLOX bool "Detect and Auto Connect on U-Center messages" default n ---help--- - This option will enable the cdc_acm_check to launch + This option will enable the cdcacm_autostart to launch The passthru driver. # diff --git a/src/systemcmds/serial_test/.gitignore b/src/systemcmds/serial_test/.gitignore index ab9845d6fe4b..7d522907613e 100644 --- a/src/systemcmds/serial_test/.gitignore +++ b/src/systemcmds/serial_test/.gitignore @@ -1,3 +1,2 @@ /serial_test - diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index 6f270de7013e..606f39abf309 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -224,4 +224,3 @@ int test_file2(int argc, char *argv[]) return test_corruption(filename, write_chunk, write_size, flags); } - diff --git a/src/systemcmds/tests/test_mount.cpp b/src/systemcmds/tests/test_mount.cpp index d7675714de8b..2d7bb895a436 100644 --- a/src/systemcmds/tests/test_mount.cpp +++ b/src/systemcmds/tests/test_mount.cpp @@ -62,7 +62,7 @@ int test_mount(int argc, char *argv[]) const unsigned iterations = 2000; const unsigned alignments = 10; - const char *cmd_filename = "/fs/microsd/mount_test_cmds.txt"; + const char *cmd_filename = CONFIG_BOARD_ROOT_PATH "/mount_test_cmds.txt"; /* check if microSD card is mounted */ diff --git a/src/systemcmds/topic_listener/CMakeLists.txt b/src/systemcmds/topic_listener/CMakeLists.txt index e5f127fd1ff9..caf6201e4802 100644 --- a/src/systemcmds/topic_listener/CMakeLists.txt +++ b/src/systemcmds/topic_listener/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( SRCS listener_main.cpp ) - diff --git a/src/systemcmds/usb_connected/CMakeLists.txt b/src/systemcmds/usb_connected/CMakeLists.txt index f06ba157a9bd..0d308672bd23 100644 --- a/src/systemcmds/usb_connected/CMakeLists.txt +++ b/src/systemcmds/usb_connected/CMakeLists.txt @@ -38,4 +38,3 @@ px4_add_module( usb_connected.cpp DEPENDS ) - diff --git a/src/templates/template_module/CMakeLists.txt b/src/templates/template_module/CMakeLists.txt index ddda8ba394ed..2e8e0644c24f 100644 --- a/src/templates/template_module/CMakeLists.txt +++ b/src/templates/template_module/CMakeLists.txt @@ -37,4 +37,3 @@ px4_add_module( SRCS template_module.cpp ) - diff --git a/src/templates/template_module/template_module.h b/src/templates/template_module/template_module.h index 0212fe9a3cd0..83a5db2c2c15 100644 --- a/src/templates/template_module/template_module.h +++ b/src/templates/template_module/template_module.h @@ -87,4 +87,3 @@ class TemplateModule : public ModuleBase, public ModuleParams uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; }; - diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index e1bbe9efde85..f3ba47f87176 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -271,9 +271,16 @@ void AutopilotTester::execute_mission() REQUIRE(poll_condition_with_timeout( [this]() { return _mission->start_mission() == Mission::Result::Success; }, std::chrono::seconds(3))); - // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. + float speed_factor = 1.0f; - wait_for_mission_finished(std::chrono::seconds(90)); + if (_info != nullptr) { + speed_factor = _info->get_speed_factor().second; + } + + const float mission_finish_waiting_time_in_simulation_s = 500.f; + float mission_finish_waiting_time_in_real_s = mission_finish_waiting_time_in_simulation_s / speed_factor; + + wait_for_mission_finished(std::chrono::seconds(static_cast(mission_finish_waiting_time_in_real_s))); } void AutopilotTester::execute_mission_and_lose_gps() @@ -388,9 +395,16 @@ void AutopilotTester::execute_mission_raw() { REQUIRE(_mission->start_mission() == Mission::Result::Success); - // TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc. + float speed_factor = 1.0f; + + if (_info != nullptr) { + speed_factor = _info->get_speed_factor().second; + } + + const float waiting_time_simulation_time_s = 300.f; // currently this is tuned for the VTOL wind test + float waiting_time_absolute_s = waiting_time_simulation_time_s / speed_factor; - wait_for_mission_raw_finished(std::chrono::seconds(120)); + wait_for_mission_raw_finished(std::chrono::seconds(static_cast(waiting_time_absolute_s))); } void AutopilotTester::execute_rtl() diff --git a/test/mavsdk_tests/catch2/catch.hpp b/test/mavsdk_tests/catch2/catch.hpp index db1fed3b9813..71c863abeeb8 100644 --- a/test/mavsdk_tests/catch2/catch.hpp +++ b/test/mavsdk_tests/catch2/catch.hpp @@ -17963,4 +17963,3 @@ using Catch::Detail::Approx; // end catch_reenable_warnings.h // end catch.hpp #endif // TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED - diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index f2bf6e74b102..c2af766f2e95 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -14,10 +14,7 @@ "model": "iris", "vehicle": "iris", "test_filter": "[offboard_attitude]", - "timeout_min": 10, - "env": { - "SYS_MC_EST_GROUP": 3 - } + "timeout_min": 10 }, { "model": "standard_vtol", diff --git a/test/mavsdk_tests/test_multicopter_failure_injection.cpp b/test/mavsdk_tests/test_multicopter_failure_injection.cpp index 75d804813282..d553ed759639 100644 --- a/test/mavsdk_tests/test_multicopter_failure_injection.cpp +++ b/test/mavsdk_tests/test_multicopter_failure_injection.cpp @@ -51,4 +51,3 @@ TEST_CASE("Failure Injection - Reject mid-air when it is disabled", "[multicopte std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(180); tester.wait_until_disarmed(until_disarmed_timeout); } - diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index 7160c0db382c..84d75ad049c8 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -49,7 +49,7 @@ TEST_CASE("Offboard takeoff and land", "[multicopter][offboard]") tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout); tester.offboard_land(); tester.wait_until_disarmed(std::chrono::seconds(120)); - tester.check_home_within(1.0f); + tester.check_home_within(2.0f); } TEST_CASE("Offboard position control", "[multicopter][offboard]") @@ -72,7 +72,7 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]") tester.offboard_goto(takeoff_position, acceptance_radius, goto_timeout); tester.offboard_land(); tester.wait_until_disarmed(std::chrono::seconds(120)); - tester.check_home_within(1.0f); + tester.check_home_within(2.0f); } TEST_CASE("Offboard attitude control", "[multicopter][offboard_attitude]") diff --git a/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp b/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp index 1b8266d72b9a..5dafefe71443 100644 --- a/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp +++ b/test/mavsdk_tests/test_vtol_loiter_airspeed_failure_blockage.cpp @@ -47,7 +47,7 @@ TEST_CASE("Fly VTOL Loiter with airspeed failure", "[vtol_airspeed_fail]") const float takeoff_altitude = 10.f; tester.set_takeoff_altitude(takeoff_altitude); - tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_straight_south.plan"); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan"); tester.arm(); tester.takeoff(); diff --git a/test/mavsdk_tests/vtol_mission_straight_south.plan b/test/mavsdk_tests/vtol_mission_straight_south.plan index 38e4edcc5103..fa6d37472b7a 100644 --- a/test/mavsdk_tests/vtol_mission_straight_south.plan +++ b/test/mavsdk_tests/vtol_mission_straight_south.plan @@ -49,7 +49,7 @@ 0, 0, null, - 47.38497919751799, + 47.392, 8.54578066404548, 30 ], diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 3d16eedf31eb..1e1baa103707 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -142,13 +142,13 @@ parameters: # (Extend this list as needed) type: string allowed: [ - '%', 'Hz', 'mAh', + '%', 'Hz', '1/s', 'mAh', 'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s', 'rad s^2/m','rad s/m', 'bit/s', 'B/s', 'deg', 'deg*1e7', 'deg/s', 'deg/s^2', 'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2', 'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3', - 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad', + 'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad', 'g0', 'Ohm', 'V', 'A', 'us', 'ms', 's', 'S', 'A/%', '(m/s^2)^2', 'm/m', 'tan(rad)^2', '(m/s)^2', 'm/rad', @@ -590,5 +590,3 @@ mixer: minlength: 3 maxlength: 3 type: number - -