diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index f6a6275632..a6628c8b6d 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -9,7 +9,7 @@ pipeline { script { def build_nodes = [:] def docker_images = [ - armhf: "px4io/px4-dev-armhf:2022-08-12", + armhf: "px4io/px4-dev-armhf:2023-06-26", arm64: "px4io/px4-dev-aarch64:2022-08-12", base: "px4io/px4-dev-ros2-foxy:2022-08-12", nuttx: "px4io/px4-dev-nuttx-focal:2022-08-12", @@ -40,6 +40,8 @@ pipeline { "ark_can-flow_default", "ark_can-gps_canbootloader", "ark_can-gps_default", + "ark_cannode_canbootloader", + "ark_cannode_default", "ark_can-rtk-gps_canbootloader", "ark_can-rtk-gps_default", "ark_fmu-v6x_bootloader", @@ -53,8 +55,10 @@ pipeline { "cuav_nora_default", "cuav_x7pro_default", "cubepilot_cubeorange_default", + "cubepilot_cubeorangeplus_default", "cubepilot_cubeyellow_default", "diatone_mamba-f405-mk2_default", + "flywoo_gn-f405_default", "freefly_can-rtk-gps_canbootloader", "freefly_can-rtk-gps_default", "holybro_can-gps-v1_canbootloader", @@ -72,7 +76,7 @@ pipeline { "matek_h743_default", "modalai_fc-v1_default", "modalai_fc-v2_default", - "modalai_voxl2-io_default", + "mro_ctrl-zero-classic_default", "mro_ctrl-zero-f7_default", "mro_ctrl-zero-f7-oem_default", "mro_ctrl-zero-h7-oem_default", @@ -85,6 +89,7 @@ pipeline { "nxp_fmuk66-v3_default", "nxp_fmuk66-v3_socketcan", "nxp_fmurt1062-v1_default", + "nxp_mr-canhubk3_default", "nxp_ucans32k146_canbootloader", "nxp_ucans32k146_default", "omnibus_f4sd_default", @@ -112,7 +117,8 @@ pipeline { "spracing_h7extreme_default", "thepeach_k1_default", "thepeach_r1_default", - "uvify_core_default" + "uvify_core_default", + "siyi_n7_default" ], image: docker_images.nuttx, archive: true @@ -164,7 +170,7 @@ pipeline { } options { buildDiscarder(logRotator(numToKeepStr: '5', artifactDaysToKeepStr: '14')) - timeout(time: 90, unit: 'MINUTES') + timeout(time: 120, unit: 'MINUTES') } } diff --git a/.ci/Jenkinsfile-hardware b/.ci/Jenkinsfile-hardware index bb1b8f8a73..73483107a7 100644 --- a/.ci/Jenkinsfile-hardware +++ b/.ci/Jenkinsfile-hardware @@ -12,7 +12,7 @@ pipeline { stage("build cubepilot_cubeorange_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -91,7 +91,7 @@ pipeline { stage("build cuav_x7pro_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -165,7 +165,7 @@ pipeline { stage("build px4_fmu-v4_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -238,7 +238,7 @@ pipeline { stage("build px4_fmu-v4pro_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -312,7 +312,7 @@ pipeline { stage("build px4_fmu-v5_debug") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -362,7 +362,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sd_stress"' // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests file" || true' sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests"' @@ -403,7 +403,7 @@ pipeline { stage("build px4_fmu-v5_stackcheck") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -449,7 +449,7 @@ pipeline { sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb_tests latency_test" || true' // test dataman - sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman"' + sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "tests dataman" --ignore-stdout-errors' } } stage("status") { @@ -486,7 +486,7 @@ pipeline { stage("build px4_fmu-v5_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } @@ -560,7 +560,7 @@ pipeline { stage("build nxp_fmuk66-v3_test") { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-09-08' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '--cpu-shares 512 -e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index b0914106ed..a9ac652d18 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -2,7 +2,7 @@ // https://github.com/microsoft/vscode-dev-containers/tree/v0.134.0/containers/cpp { "name": "px4-dev-nuttx", - "image": "px4io/px4-dev-nuttx-focal:2021-09-08", + "image": "px4io/px4-dev-nuttx-focal:2022-08-12", "runArgs": [ "--cap-add=SYS_PTRACE", "--security-opt", "seccomp=unconfined" ], @@ -29,7 +29,8 @@ "twxs.cmake", "uavcan.dsdl", "wholroyd.jinja", - "zixuanwang.linkerscript" + "zixuanwang.linkerscript", + "ms-vscode.makefile-tools" ], "containerUser": "user", diff --git a/.github/ISSUE_TEMPLATE/1_Bug_report.md b/.github/ISSUE_TEMPLATE/1_Bug_report.md deleted file mode 100644 index cc0bc6d96f..0000000000 --- a/.github/ISSUE_TEMPLATE/1_Bug_report.md +++ /dev/null @@ -1,34 +0,0 @@ ---- -name: 🐛 Bug report -about: Create a report to help us improve -labels: bug-report - ---- - -## Describe the bug -A clear and concise description of the bug. - -## To Reproduce -Steps to reproduce the behavior: -1. Drone switched on '...' -2. Uploaded mission '....' (attach QGC mission file) -3. Took off '....' -4. See error - -## Expected behavior -A clear and concise description of what you expected to happen. - -## Log Files and Screenshots -*Always* provide a link to the flight log file: -- Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)). -- Upload the log to the [PX4 Flight Review](http://logs.px4.io/) -- Share the link to the log (Copy and paste the URL of the log) - -Add screenshots to help explain your problem. - -## Drone (please complete the following information): -- Describe the type of drone. -- Photo of the IMU / autopilot setup if possible. - -## Additional context -Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/2_Feature_request.md b/.github/ISSUE_TEMPLATE/2_Feature_request.md deleted file mode 100644 index adcdc30fd8..0000000000 --- a/.github/ISSUE_TEMPLATE/2_Feature_request.md +++ /dev/null @@ -1,20 +0,0 @@ ---- -name: 🚀 Feature Request -about: Suggest an idea for this project -labels: feature-request - ---- - -For general questions please use [PX4 Discuss](http://discuss.px4.io/) or Discord (you can find an invite link on this project README). - -## Describe problem solved by the proposed feature -A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ... - -## Describe your preferred solution -A clear and concise description of what you want to happen. - -## Describe possible alternatives -A clear and concise description of alternative solutions or features you've considered. - -## Additional context -Add any other context or screenshots for the feature request here. diff --git a/.github/ISSUE_TEMPLATE/3_Support_question.md b/.github/ISSUE_TEMPLATE/3_Support_question.md deleted file mode 100644 index 7c331f2dc1..0000000000 --- a/.github/ISSUE_TEMPLATE/3_Support_question.md +++ /dev/null @@ -1,13 +0,0 @@ ---- -name: ⛔ Support Question -about: See http://discuss.px4.io/ for questions about using PX4. - ---- - -## Attention! Please read the note below - -We use GitHub issues only to discuss PX4 bugs and new features. - -**For questions about using PX4 or related components, please use [PX4 Discuss](http://discuss.px4.io/).** - -Thanks! diff --git a/.github/ISSUE_TEMPLATE/4_Documentation_issue.md b/.github/ISSUE_TEMPLATE/4_Documentation_issue.md deleted file mode 100644 index 0e14e82cc5..0000000000 --- a/.github/ISSUE_TEMPLATE/4_Documentation_issue.md +++ /dev/null @@ -1,11 +0,0 @@ ---- -name: ⛔ Documentation Issue -about: See https://github.com/PX4/px4_user_guide for documentation issues - ---- - -## Attention! Please read the note below - -**Please submit the documentation issue to the [User Guide](https://github.com/PX4/px4_user_guide) repository.** - -Thanks! diff --git a/.github/ISSUE_TEMPLATE/bug_report.yml b/.github/ISSUE_TEMPLATE/bug_report.yml new file mode 100644 index 0000000000..60d93ea9b8 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.yml @@ -0,0 +1,94 @@ +name: 🐛 Bug report +description: Create a report to help us improve +title: "[Bug] " +labels: ["bug-report"] +body: + - type: textarea + attributes: + label: Describe the bug + description: A clear and concise description of the bug. + validations: + required: true + + - type: textarea + attributes: + label: To Reproduce + description: | + Steps to reproduce the behavior. + 1. Drone switched on '...' + 2. Uploaded mission '....' (attach QGC mission file) + 3. Took off '....' + 4. See error + validations: + required: true + + - type: textarea + attributes: + label: Expected behavior + description: A clear and concise description of what you expected to happen. + validations: + required: true + + - type: textarea + attributes: + label: Screenshot / Media + description: Add screenshot / media if you have them + + - type: textarea + attributes: + label: Flight Log + description: | + *Always* provide a link to the flight log file: + - Download the flight log file from the vehicle ([tutorial](https://docs.px4.io/main/en/getting_started/flight_reporting.html)). + - Upload the log to the [PX4 Flight Review](http://logs.px4.io/) + - Share the link to the log (Copy and paste the URL of the log) + placeholder: | + # PASTE HERE THE LINK TO THE LOG + validations: + required: true + + - type: markdown + attributes: + value: | + ## Setup + + - type: textarea + attributes: + label: Software Version + description: | + Which version of PX4 are you using? + placeholder: | + # If you don't know the version, paste the output of `ver all` in the MAVLink Shell of QGC + validations: + required: true + + - type: input + attributes: + label: Flight controller + description: Specify your flight controller model (what type is it, where was it bought from, ...). + validations: + required: true + + - type: dropdown + attributes: + label: Vehicle type + options: + - Multicopter + - Helicopter + - Fixed Wing + - Hybrid VTOL + - Airship/Balloon + - Rover + - Boat + - Submarine + - Other + + - type: textarea + attributes: + label: How are the different components wired up (including port information) + description: Details about how all is wired. + + - type: textarea + attributes: + label: Additional context + description: Add any other context about the problem here. diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000000..92ab6b5184 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,8 @@ +blank_issues_enabled: false +contact_links: + - name: Support Question + url: https://docs.px4.io/main/en/contribute/support.html#forums-and-chat + about: For questions about using PX4 or related components, please use the discuss forum and discord server + - name: Documentation Issue + url: https://github.com/PX4/PX4-user_guide/issues + about: If you found an issue in documentation, please submit it directly to the docs repository issues diff --git a/.github/ISSUE_TEMPLATE/feature_request.yml b/.github/ISSUE_TEMPLATE/feature_request.yml new file mode 100644 index 0000000000..955ef1f353 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.yml @@ -0,0 +1,35 @@ +name: 🚀 Feature Request +description: Suggest an idea for this project +labels: ["feature-request"] +body: + - type: markdown + attributes: + value: | + ## Please note that feature requests are not 'fire and forget' + It is a lot more likely that the feature you would like to have will be implemented if you keep watching your feature request, and provide more details to developers looking into implementing your feature, and help them with testing. + + - type: textarea + attributes: + label: Describe problem solved by the proposed feature + description: A clear and concise description of the problem, if any, this feature will solve. E.g. I'm always frustrated when ... + validations: + required: true + + - type: textarea + attributes: + label: Describe your preferred solution + description: A clear and concise description of what you want to happen. + validations: + required: true + + - type: textarea + attributes: + label: Describe possible alternatives + description: A clear and concise description of any alternative solutions or features you've considered. + validations: + required: true + + - type: textarea + attributes: + label: Additional context + description: Add any other context or screenshots for the feature request here. diff --git a/.github/slack.svg b/.github/slack.svg deleted file mode 100644 index 2b0fbae4aa..0000000000 --- a/.github/slack.svg +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - - - - - slack - slack - Join us! - Join us! - - - diff --git a/.github/stale.yml b/.github/stale.yml deleted file mode 100644 index f20bbd3977..0000000000 --- a/.github/stale.yml +++ /dev/null @@ -1,15 +0,0 @@ -# Number of days of inactivity before an issue becomes stale -daysUntilStale: 90 -# Number of days of inactivity before a stale issue is closed, or `false` to disable -daysUntilClose: false -# Issues with these labels will never be considered stale -exemptLabels: - - pinned -# Label to use when marking an issue as stale -staleLabel: stale -# Comment to post when marking an issue as stale. Set to `false` to disable -markComment: > - This issue has been automatically marked as stale because it has not had - recent activity. Thank you for your contributions. -# Comment to post when closing a stale issue. Set to `false` to disable -closeComment: false diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml index ef15d32171..f8a893e747 100644 --- a/.github/workflows/checks.yml +++ b/.github/workflows/checks.yml @@ -28,7 +28,7 @@ jobs: "parameters_metadata", ] container: - image: px4io/px4-dev-nuttx-focal:2021-09-08 + image: px4io/px4-dev-nuttx-focal:2022-08-12 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml index d14b533475..23e46deab7 100644 --- a/.github/workflows/compile_linux.yml +++ b/.github/workflows/compile_linux.yml @@ -1,9 +1,12 @@ -name: Linux Targets +name: Compile Linux Targets on: push: branches: - 'main' + - 'stable' + - 'beta' + - 'release/*' pull_request: branches: - '*' @@ -11,7 +14,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-armhf:2021-09-08 + container: px4io/px4-dev-armhf:2023-06-26 strategy: matrix: config: [ @@ -24,7 +27,8 @@ jobs: - 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} diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml index 81ecc12569..5c1c318b41 100644 --- a/.github/workflows/compile_linux_arm64.yml +++ b/.github/workflows/compile_linux_arm64.yml @@ -1,9 +1,12 @@ -name: Linux ARM64 Targets +name: Compile Linux ARM64 Targets on: push: branches: - 'main' + - 'stable' + - 'beta' + - 'release/*' pull_request: branches: - '*' @@ -11,7 +14,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-aarch64:2021-09-08 + container: px4io/px4-dev-aarch64:2022-08-12 strategy: matrix: config: [ diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml index 45f6f5a615..227b6d3c05 100644 --- a/.github/workflows/compile_macos.yml +++ b/.github/workflows/compile_macos.yml @@ -10,13 +10,12 @@ on: jobs: build: - runs-on: macos-10.15 + runs-on: macos-latest strategy: matrix: config: [ px4_fmu-v5_default, px4_sitl - #tests, # includes px4_sitl ] steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml index 67b055fdd9..de5a0b7c72 100644 --- a/.github/workflows/compile_nuttx.yml +++ b/.github/workflows/compile_nuttx.yml @@ -1,9 +1,12 @@ -name: Nuttx Targets +name: Compile Nuttx Targets on: push: branches: - 'main' + - 'stable' + - 'beta' + - 'release/*' pull_request: branches: - '*' @@ -11,7 +14,7 @@ on: jobs: build: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2021-09-08 + container: px4io/px4-dev-nuttx-focal:2022-08-12 strategy: fail-fast: false matrix: @@ -70,7 +73,8 @@ jobs: raspberrypi_pico, sky-drones_smartap-airlink, spracing_h7extreme, - uvify_core + uvify_core, + siyi_n7 ] steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml index 8b0925e688..b4b6ebc814 100644 --- a/.github/workflows/deploy_all.yml +++ b/.github/workflows/deploy_all.yml @@ -24,12 +24,15 @@ jobs: needs: enumerate_targets strategy: matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}} - container: px4io/px4-dev-${{ matrix.container }}:2021-09-08 + container: ${{ matrix.container }} steps: - uses: actions/checkout@v1 with: token: ${{secrets.ACCESS_TOKEN}} + - name: ownership workaround + run: git config --system --add safe.directory '*' + - name: make ${{matrix.target}} run: make ${{matrix.target}} diff --git a/.github/workflows/failsafe_sim.yml b/.github/workflows/failsafe_sim.yml index 6d5e73d5a5..4d2ed21728 100644 --- a/.github/workflows/failsafe_sim.yml +++ b/.github/workflows/failsafe_sim.yml @@ -21,7 +21,7 @@ jobs: "failsafe_web", ] container: - image: px4io/px4-dev-nuttx-focal:2021-09-08 + image: px4io/px4-dev-nuttx-focal:2022-08-12 options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined steps: - uses: actions/checkout@v1 diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml index 10799ba38f..e7289b34f7 100644 --- a/.github/workflows/metadata.yml +++ b/.github/workflows/metadata.yml @@ -103,7 +103,7 @@ jobs: uorb_graph: runs-on: ubuntu-latest - container: px4io/px4-dev-nuttx-focal:2021-09-08 + container: px4io/px4-dev-nuttx-focal:2022-08-12 steps: - uses: actions/checkout@v1 with: diff --git a/.github/workflows/stale.yml b/.github/workflows/stale.yml index 7cbba4b282..9e97122d43 100644 --- a/.github/workflows/stale.yml +++ b/.github/workflows/stale.yml @@ -1,4 +1,4 @@ -name: 'Close stale issues and PRs' +name: 'Handle stale issues and PRs' on: schedule: - cron: '30 1 * * *' @@ -7,14 +7,10 @@ jobs: stale: runs-on: ubuntu-latest steps: - - uses: actions/stale@v4.1.1 + - uses: actions/stale@v8 with: - stale-issue-message: 'This issue is stale because it has been open 30 days with no activity. Remove stale label or comment or this will be closed in 5 days.' - stale-pr-message: 'This PR is stale because it has been open 45 days with no activity. Remove stale label or comment or this will be closed in 10 days.' - close-issue-message: 'This issue was closed because it has been stalled for 5 days with no activity.' - close-pr-message: 'This PR was closed because it has been stalled for 10 days with no activity.' - days-before-issue-stale: 30 - days-before-pr-stale: 45 - days-before-issue-close: 5 - days-before-pr-close: 10 - debug-only: true + days-before-stale: 30 + days-before-close: -1 + stale-issue-label: 'stale' + stale-pr-label: 'stale' + remove-stale-when-updated: true diff --git a/.vscode/extensions.json b/.vscode/extensions.json index b49f659083..ec49a0c75f 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -18,6 +18,7 @@ "twxs.cmake", "uavcan.dsdl", "wholroyd.jinja", - "zixuanwang.linkerscript" + "zixuanwang.linkerscript", + "ms-vscode.makefile-tools" ] } diff --git a/.vscode/settings.json b/.vscode/settings.json index 35c3146dcf..67016f0983 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -127,5 +127,6 @@ "terminal.integrated.scrollback": 15000, "yaml.schemas": { "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml" - } + }, + "ros.distro": "humble" } diff --git a/CMakeLists.txt b/CMakeLists.txt index 741e8e574a..b1587b157a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -414,6 +414,8 @@ endif() # add_library(parameters_interface INTERFACE) add_library(kernel_parameters_interface INTERFACE) +add_library(events_interface INTERFACE) +add_library(kernel_events_interface INTERFACE) include(px4_add_library) add_subdirectory(src/lib EXCLUDE_FROM_ALL) @@ -440,8 +442,11 @@ add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL) if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) target_link_libraries(parameters_interface INTERFACE usr_parameters) target_link_libraries(kernel_parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE usr_events) + target_link_libraries(kernel_events_interface INTERFACE events) else() target_link_libraries(parameters_interface INTERFACE parameters) + target_link_libraries(events_interface INTERFACE events) endif() # firmware added last to generate the builtin for included modules diff --git a/Jenkinsfile b/Jenkinsfile index a983c043c0..30683ffe16 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -94,7 +94,7 @@ pipeline { stage('failsafe docs') { agent { - docker { image 'px4io/px4-dev-nuttx-focal:2021-08-18' } + docker { image 'px4io/px4-dev-nuttx-focal:2022-08-12' } } steps { sh '''#!/bin/bash -l @@ -125,7 +125,7 @@ pipeline { stage('uORB graphs') { agent { docker { - image 'px4io/px4-dev-nuttx-focal:2021-08-18' + image 'px4io/px4-dev-nuttx-focal:2022-08-12' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw' } } 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 e59ce4d24f..28c3e34f71 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 @@ -31,6 +31,7 @@ param set-default PWM_MAIN_FUNC4 104 # EKF2 param set-default EKF2_GPS_CTRL 0 +param set-default EKF2_HGT_REF 0 param set-default EKF2_EVP_NOISE 0.05 param set-default EKF2_EVA_NOISE 0.05 param set-default EKF2_OF_CTRL 1 @@ -39,6 +40,9 @@ param set-default EKF2_OF_CTRL 1 param set-default LPE_FUSION 242 param set-default LPE_FAKE_ORIGIN 1 +# Commander +# param set-default COM_HOME_EN 0 # Disable setting of home position + param set-default MPC_ALT_MODE 2 param set-default SENS_FLOW_ROT 6 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid similarity index 100% rename from ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid rename to ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post similarity index 100% rename from ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_obs_avoid.post rename to ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid.post diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera new file mode 100644 index 0000000000..3dc7b0fd75 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_gazebo-classic_iris_depth_camera @@ -0,0 +1,8 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor with a depth camera (forward-facing) +# +# @type Quadrotor Wide +# + +. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera new file mode 100644 index 0000000000..b7e7bf51b9 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1016_gazebo-classic_iris_downward_depth_camera @@ -0,0 +1,8 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor with a depth camera (downward-facing) +# +# @type Quadrotor Wide +# + +. ${R}etc/init.d-posix/airframes/10015_gazebo-classic_iris 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 d730e4a55f..9ce6cb61dd 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 @@ -7,6 +7,7 @@ param set-default FW_LAUN_DETCN_ON 1 param set-default FW_THR_IDLE 0.1 # needs to be running before throw as that's how gazebo detects arming +param set-default FW_LAUN_AC_THLD 10 param set-default FW_LND_ANG 8 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane index 768a8fbf84..a998b87831 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1039_gazebo-classic_advanced_plane @@ -8,7 +8,6 @@ param set-default FW_LND_ANG 8 -param set-default FW_THR_LND_MAX 0 param set-default NPFG_PERIOD 12 @@ -36,7 +35,6 @@ param set-default NAV_DLL_ACT 2 param set-default RWTO_TKOFF 1 -#param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 1 param set-default CA_ROTOR_COUNT 1 @@ -61,7 +59,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 - - -set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix -set MIXER custom diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter new file mode 100644 index 0000000000..707df1bba1 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -0,0 +1,122 @@ +#!/bin/sh +# +# @name 6DoF Omnicopter SITL +# +# @type Quadrotor Wide +# +# @maintainer Jaeyoung Lim +# + +. ${R}etc/init.d/rc.mc_defaults + +PX4_SIMULATOR=${PX4_SIMULATOR:=gz} +PX4_GZ_WORLD=${PX4_GZ_WORLD:=default} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=omnicopter} + +param set-default CA_AIRFRAME 0 + +param set-default CA_ROTOR_COUNT 8 +param set-default CA_R_REV 255 + +param set-default CA_ROTOR0_PX 0.14435 +param set-default CA_ROTOR0_PY -0.14435 +param set-default CA_ROTOR0_PZ -0.14435 +param set-default CA_ROTOR0_KM 0.05 # CCW +param set-default CA_ROTOR0_AX -0.788675 +param set-default CA_ROTOR0_AY -0.211325 +param set-default CA_ROTOR0_AZ -0.57735 + +param set-default CA_ROTOR1_PX -0.14435 +param set-default CA_ROTOR1_PY -0.14435 +param set-default CA_ROTOR1_PZ -0.14435 +param set-default CA_ROTOR1_KM 0.05 +param set-default CA_ROTOR1_AX 0.211325 +param set-default CA_ROTOR1_AY -0.788675 +param set-default CA_ROTOR1_AZ 0.57735 + +param set-default CA_ROTOR2_PX 0.14435 +param set-default CA_ROTOR2_PY 0.14435 +param set-default CA_ROTOR2_PZ -0.14435 +param set-default CA_ROTOR2_KM 0.05 +param set-default CA_ROTOR2_AX -0.211325 +param set-default CA_ROTOR2_AY 0.788675 +param set-default CA_ROTOR2_AZ 0.57735 + +param set-default CA_ROTOR3_PX -0.14435 +param set-default CA_ROTOR3_PY 0.14435 +param set-default CA_ROTOR3_PZ -0.14435 +param set-default CA_ROTOR3_KM 0.05 +param set-default CA_ROTOR3_AX 0.788675 +param set-default CA_ROTOR3_AY 0.211325 +param set-default CA_ROTOR3_AZ -0.57735 + +param set-default CA_ROTOR4_PX 0.14435 +param set-default CA_ROTOR4_PY -0.14435 +param set-default CA_ROTOR4_PZ 0.14435 +param set-default CA_ROTOR4_KM 0.05 +param set-default CA_ROTOR4_AX 0.788675 +param set-default CA_ROTOR4_AY 0.211325 +param set-default CA_ROTOR4_AZ -0.57735 + +param set-default CA_ROTOR5_PX -0.14435 +param set-default CA_ROTOR5_PY -0.14435 +param set-default CA_ROTOR5_PZ 0.14435 +param set-default CA_ROTOR5_KM 0.05 +param set-default CA_ROTOR5_AX -0.211325 +param set-default CA_ROTOR5_AY 0.788675 +param set-default CA_ROTOR5_AZ 0.57735 + +param set-default CA_ROTOR6_PX 0.14435 +param set-default CA_ROTOR6_PY 0.14435 +param set-default CA_ROTOR6_PZ 0.14435 +param set-default CA_ROTOR6_KM 0.05 +param set-default CA_ROTOR6_AX 0.211325 +param set-default CA_ROTOR6_AY -0.788675 +param set-default CA_ROTOR6_AZ 0.57735 + +param set-default CA_ROTOR7_PX -0.14435 +param set-default CA_ROTOR7_PY 0.14435 +param set-default CA_ROTOR7_PZ 0.14435 +param set-default CA_ROTOR7_KM 0.05 +param set-default CA_ROTOR7_AX -0.788675 +param set-default CA_ROTOR7_AY -0.211325 +param set-default CA_ROTOR7_AZ -0.57735 + +param set-default SIM_GZ_EN 1 +param set-default SENS_EN_GPSSIM 1 +param set-default SENS_EN_BAROSIM 0 +param set-default SENS_EN_MAGSIM 1 + +param set-default SIM_GZ_EC_FUNC1 101 +param set-default SIM_GZ_EC_FUNC2 102 +param set-default SIM_GZ_EC_FUNC3 103 +param set-default SIM_GZ_EC_FUNC4 104 +param set-default SIM_GZ_EC_FUNC5 105 +param set-default SIM_GZ_EC_FUNC6 106 +param set-default SIM_GZ_EC_FUNC7 107 +param set-default SIM_GZ_EC_FUNC8 108 + +param set-default SIM_GZ_EC_MIN1 0 +param set-default SIM_GZ_EC_MIN2 0 +param set-default SIM_GZ_EC_MIN3 0 +param set-default SIM_GZ_EC_MIN4 0 +param set-default SIM_GZ_EC_MIN5 0 +param set-default SIM_GZ_EC_MIN6 0 +param set-default SIM_GZ_EC_MIN7 0 +param set-default SIM_GZ_EC_MIN8 0 + +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 SIM_GZ_EC_MAX5 1100 +param set-default SIM_GZ_EC_MAX6 1100 +param set-default SIM_GZ_EC_MAX7 1100 +param set-default SIM_GZ_EC_MAX8 1100 +# disable MC desaturation which improves attitude tracking +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 d54134e437..ded0ace48b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -39,8 +39,10 @@ px4_add_romfs_files( 1012_gazebo-classic_iris_rplidar 1013_gazebo-classic_iris_vision 1013_gazebo-classic_iris_vision.post - 1015_gazebo-classic_iris_obs_avoid - 1015_gazebo-classic_iris_obs_avoid.post + 1014_gazebo-classic_iris_obs_avoid + 1014_gazebo-classic_iris_obs_avoid.post + 1015_gazebo-classic_iris_depth_camera + 1016_gazebo-classic_iris_downward_depth_camera 1017_gazebo-classic_iris_opt_flow_mockup 1019_gazebo-classic_iris_dual_gps 1021_gazebo-classic_uuv_hippocampus @@ -81,6 +83,8 @@ px4_add_romfs_files( 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post + 8011_gz_omnicopter + 10015_gazebo-classic_iris 10016_none_iris 10017_jmavsim_iris @@ -94,4 +98,6 @@ px4_add_romfs_files( 17001_flightgear_tf-g1 17002_flightgear_tf-g2 + + # [22000, 22999] Reserve for custom models ) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 1f4693d4c1..b0025563df 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -38,6 +38,19 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then + # set local coordinate frame reference + if [ -n "${PX4_HOME_LAT}" ]; then + param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT} + fi + + if [ -n "${PX4_HOME_LON}" ]; then + param set SIM_GZ_HOME_LON ${PX4_HOME_LON} + fi + + if [ -n "${PX4_HOME_ALT}" ]; then + param set SIM_GZ_HOME_ALT ${PX4_HOME_LON} + fi + # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH if [ -f ./gz_env.sh ]; then . ./gz_env.sh @@ -59,7 +72,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th fi # look for running ${gz_command} gazebo world - gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) + gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' ) # shellcheck disable=SC2153 if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 7ecbfc3dcb..4421c3c389 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -187,6 +187,11 @@ 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 COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc) diff --git a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross index e918ea0f3c..3b51c32f61 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/2106_albatross +++ b/ROMFS/px4fmu_common/init.d/airframes/2106_albatross @@ -17,6 +17,7 @@ # @maintainer Andreas Antener # # @board bitcraze_crazyflie exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.fw_defaults diff --git a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 index 1fb639236e..e9127020c3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 +++ b/ROMFS/px4fmu_common/init.d/airframes/4019_x500_v2 @@ -34,9 +34,7 @@ param set-default CA_ROTOR3_PX -0.25 param set-default CA_ROTOR3_PY 0.25 param set-default CA_ROTOR3_KM -0.05 -param set-default PWM_AUX_FUNC1 101 -param set-default PWM_AUX_FUNC2 102 -param set-default PWM_AUX_FUNC3 103 -param set-default PWM_AUX_FUNC4 104 -param set-default PWM_AUX_TIM0 -4 - +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/airframes/4900_crazyflie b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie index 211feb4924..7424b9fdb7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie +++ b/ROMFS/px4fmu_common/init.d/airframes/4900_crazyflie @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults @@ -42,7 +43,6 @@ param set-default MC_ROLLRATE_P 0.07 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 -param set-default MPC_THR_MAX 1 param set-default MPC_Z_P 1.5 param set-default MPC_Z_VEL_P_ACC 8 param set-default MPC_Z_VEL_I_ACC 6 diff --git a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 index 3462b7575f..f9bf887f1e 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 +++ b/ROMFS/px4fmu_common/init.d/airframes/4901_crazyflie21 @@ -13,6 +13,7 @@ # @board px4_fmu-v4pro exclude # @board px4_fmu-v5 exclude # @board px4_fmu-v5x exclude +# @board diatone_mamba-f405-mk2 exclude # . ${R}etc/init.d/rc.mc_defaults @@ -41,7 +42,6 @@ param set-default MC_ROLLRATE_P 0.07 param set-default MC_YAW_P 3 param set-default MPC_THR_HOVER 0.7 -param set-default MPC_THR_MAX 1 param set-default MPC_Z_P 1.5 param set-default MPC_Z_VEL_P_ACC 8 param set-default MPC_Z_VEL_I_ACC 6 diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults index 77b10d8cfc..06c6bf717a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults @@ -34,6 +34,7 @@ param set-default EKF2_REQ_EPV 10 param set-default EKF2_REQ_HDRIFT 0.5 param set-default EKF2_REQ_SACC 1 param set-default EKF2_REQ_VDRIFT 1.0 +param set-default EKF2_RNG_QLTY_T 3.0 param set-default RTL_TYPE 1 param set-default RTL_RETURN_ALT 100 diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index dd7a1d6bf2..490038c608 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -172,6 +172,12 @@ then ms5525dso start -X fi +# TE ASP5033 differential pressure sensor external I2C +if param compare -s SENS_EN_ASP5033 1 +then + asp5033 start -X +fi + # SHT3x temperature and hygrometer sensor, external I2C if param compare -s SENS_EN_SHT3X 1 then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8b0dbae4b3..7d53e5b71d 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -93,6 +93,14 @@ then fi fi + # Check for an update of the ext_autostart folder, and replace the old one with it + if [ -e /fs/microsd/ext_autostart_new ] + then + echo "Updating external autostart files" + rm -r $SDCARD_EXT_PATH + mv /fs/microsd/ext_autostart_new $SDCARD_EXT_PATH + fi + set PARAM_FILE /fs/microsd/params set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson" fi @@ -255,6 +263,7 @@ else rgbled start -X -q rgbled_ncp5623c start -X -q rgbled_lp5562 start -X -q + rgbled_is31fl3195 start -X -q # # Override parameters from user configuration file. diff --git a/Tools/HIL/run_nsh_cmd.py b/Tools/HIL/run_nsh_cmd.py index dcdd9ec1e7..7c37679adf 100755 --- a/Tools/HIL/run_nsh_cmd.py +++ b/Tools/HIL/run_nsh_cmd.py @@ -39,7 +39,7 @@ def print_line(line): print('{0}'.format(line), end='') -def do_nsh_cmd(port_url, baudrate, cmd): +def do_nsh_cmd(port_url, baudrate, cmd, ignore_stdout_errors=False): ser = serial.serial_for_url(url=port_url, baudrate=baudrate, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE, stopbits=serial.STOPBITS_ONE, timeout=1, xonxoff=False, rtscts=False, dsrdtr=False, inter_byte_timeout=1) timeout_start = time.monotonic() @@ -106,7 +106,7 @@ def do_nsh_cmd(port_url, baudrate, cmd): if success_cmd in serial_line: sys.exit(return_code) else: - if "ERROR " in serial_line: + if "ERROR " in serial_line and not ignore_stdout_errors: return_code = -1 print_line(serial_line) @@ -148,6 +148,8 @@ def main(): parser.add_argument('--device', "-d", nargs='?', default=default_device, help='', required=device_required) parser.add_argument("--baudrate", "-b", dest="baudrate", type=int, help="serial port baud rate (default=57600)", default=57600) parser.add_argument("--cmd", "-c", dest="cmd", help="Command to run") + parser.add_argument('--ignore-stdout-errors', action='store_true', + help='Ignore errors printed to stdout') args = parser.parse_args() tmp_file = "{0}/pyserial_spy_file.txt".format(tempfile.gettempdir()) @@ -155,7 +157,7 @@ def main(): print("pyserial url: {0}".format(port_url)) - do_nsh_cmd(port_url, args.baudrate, args.cmd) + do_nsh_cmd(port_url, args.baudrate, args.cmd, args.ignore_stdout_errors) if __name__ == "__main__": main() diff --git a/Tools/auterion/remote_update_fmu.sh b/Tools/auterion/remote_update_fmu.sh new file mode 100755 index 0000000000..2eeb66b999 --- /dev/null +++ b/Tools/auterion/remote_update_fmu.sh @@ -0,0 +1,115 @@ +#!/bin/bash +# Flash PX4 to a device running AuterionOS in the local network +if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then + echo "Usage: $0 -f [-c ] -d [-u ] [-p ] [--revert]" + exit 1 +fi + +ssh_port=22 +ssh_user=root + +while getopts ":f:c:d:p:u:r" opt; do + case ${opt} in + f ) + if [ -n "$OPTARG" ]; then + firmware_file="$OPTARG" + else + echo "ERROR: -f requires a non-empty option argument." + exit 1 + fi + ;; + c ) + if [ -f "$OPTARG/rc.autostart" ]; then + config_dir="$OPTARG" + else + echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart" + exit 1 + fi + ;; + d ) + if [ "$OPTARG" ]; then + device="$OPTARG" + else + echo "ERROR: -d requires a non-empty option argument." + exit 1 + fi + ;; + p ) + if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then + ssh_port="$OPTARG" + else + echo "ERROR: -p ssh_port must be a number." + exit 1 + fi + ;; + u ) + if [ "$OPTARG" ]; then + ssh_user="$OPTARG" + else + echo "ERROR: -u requires a non-empty option argument." + exit 1 + fi + ;; + r ) + revert=true + ;; + esac +done + +if [ -z "$device" ]; then + echo "Error: missing device" + exit 1 +fi + +target_dir=/shared_container_dir/fmu +target_file_name="update-dev.tar" + +if [ "$revert" == true ]; then + # revert to the release version which was originally deployed + cmd="cp $target_dir/update.tar $target_dir/$target_file_name" + ssh -t -p $ssh_port $ssh_user@$device "$cmd" +else + # create custom update-dev.tar + tmp_dir="$(mktemp -d)" + config_path="" + firmware_path="" + + if [ -d "$config_dir" ]; then + cp -r "$config_dir" "$tmp_dir/config" + config_path=config + fi + + if [ -f "$firmware_file" ]; then + extension="${firmware_file##*.}" + cp "$firmware_file" "$tmp_dir/firmware.$extension" + if [ "$extension" == "elf" ]; then + # ensure the file is stripped to reduce file size + arm-none-eabi-strip "$tmp_dir/firmware.$extension" + fi + firmware_path="firmware.$extension" + fi + + pushd "$tmp_dir" &>/dev/null + + if [ -z $firmware_path ] && [ -z $config_path ]; then + exit 1 + fi + + tar_name="tar" + + if [ -x "$(command -v gtar)" ]; then + # check if gnu-tar is installed on macOS and use that instead + tar_name="gtar" + fi + + $tar_name -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path + + # send it to the target to start flashing + scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir + popd &>/dev/null + rm -rf "$tmp_dir" +fi + +# grab status output for flashing progress +cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true" +ssh -t -p $ssh_port $ssh_user@$device "$cmd" diff --git a/Tools/auterion/upload_skynode.sh b/Tools/auterion/upload_skynode.sh new file mode 100755 index 0000000000..10d8740b42 --- /dev/null +++ b/Tools/auterion/upload_skynode.sh @@ -0,0 +1,51 @@ +#!/usr/bin/env bash + +DIR="$(dirname $(readlink -f $0))" +DEFAULT_AUTOPILOT_HOST=10.41.1.1 +DEFAULT_AUTOPILOT_PORT=33333 +DEFAULT_AUTOPILOT_USER=auterion + +for i in "$@" +do + case $i in + --file=*) + PX4_BINARY_FILE="${i#*=}" + ;; + --default-ip=*) + DEFAULT_AUTOPILOT_HOST="${i#*=}" + ;; + --default-port=*) + DEFAULT_AUTOPILOT_PORT="${i#*=}" + ;; + --default-user=*) + DEFAULT_AUTOPILOT_USER="${i#*=}" + ;; + --revert) + REVERT_AUTOPILOT_ARGUMENT=-r + ;; + --wifi) + DEFAULT_AUTOPILOT_HOST=10.41.0.1 + ;; + *) + # unknown option + ;; + esac +done + +# allow these to be overridden +[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST +[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT +[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER + +ARGUMENTS=() +ARGUMENTS+=(-d "$AUTOPILOT_HOST") +ARGUMENTS+=(-p "$AUTOPILOT_PORT") +ARGUMENTS+=(-u "$AUTOPILOT_USER") +ARGUMENTS+=(${PX4_BINARY_FILE:+-f "$PX4_BINARY_FILE"}) +ARGUMENTS+=($REVERT_AUTOPILOT_ARGUMENT) + +echo "Flashing $AUTOPILOT_HOST ..." + +"$DIR"/remote_update_fmu.sh "${ARGUMENTS[@]}" + +exit 0 diff --git a/Tools/docker_run.sh b/Tools/docker_run.sh index 3ac640428e..c78b9cc7e1 100755 --- a/Tools/docker_run.sh +++ b/Tools/docker_run.sh @@ -4,16 +4,16 @@ if [ -z ${PX4_DOCKER_REPO+x} ]; then echo "guessing PX4_DOCKER_REPO based on input"; if [[ $@ =~ .*px4_fmu.* ]]; then # nuttx-px4fmu-v{1,2,3,4,5} - PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]]; then # beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26" elif [[ $@ =~ .*pilotpi.arm64 ]]; then # scumaker_pilotpi_arm64 - PX4_DOCKER_REPO="px4io/px4-dev-aarch64:latest" + PX4_DOCKER_REPO="px4io/px4-dev-aarch64:2022-08-12" elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then # posix_rpi_cross, posix_bebop_default - PX4_DOCKER_REPO="px4io/px4-dev-armhf:2021-08-18" + PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26" elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then # clang tools PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04" @@ -27,7 +27,7 @@ fi # otherwise default to nuttx if [ -z ${PX4_DOCKER_REPO+x} ]; then - PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2021-09-08" + PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12" fi # docker hygiene diff --git a/Tools/ecl_ekf/analysis/checks.py b/Tools/ecl_ekf/analysis/checks.py index ac0248a4c1..36f806ead9 100644 --- a/Tools/ecl_ekf/analysis/checks.py +++ b/Tools/ecl_ekf/analysis/checks.py @@ -56,11 +56,14 @@ def perform_imu_checks( # perform the vibration check imu_status['imu_vibration_check'] = 'Pass' for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']: - mean_metric = '{:s}_mean'.format(imu_vibr_metric) - peak_metric = '{:s}_peak'.format(imu_vibr_metric) - if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \ - or imu_metrics[peak_metric] > check_levels['{:s}_warn'.format(peak_metric)]: - imu_status['imu_vibration_check'] = 'Warning' + mean_metric = '{:s}_mean_warn'.format(imu_vibr_metric) + peak_metric = '{:s}_peak_warn'.format(imu_vibr_metric) + mean_key = '{:s}_mean'.format(imu_vibr_metric) + peak_key = '{:s}_peak'.format(imu_vibr_metric) + if mean_key in imu_metrics and peak_key in imu_metrics: + if imu_metrics[mean_key] > check_levels[mean_metric] \ + or imu_metrics[peak_key] > check_levels[peak_metric]: + imu_status['imu_vibration_check'] = 'Warning' if imu_status['imu_vibration_check'] == 'Warning': print('IMU vibration check warning.') diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 6c07c12caa..d435f88af9 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -104,9 +104,9 @@ def calculate_innov_fail_metrics( # calculate innovation check fail metrics for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'), - ('magx', 'reject_mag_x', 'magx_fail_percentage'), - ('magy', 'reject_mag_y', 'magy_fail_percentage'), - ('magz', 'reject_mag_z', 'magz_fail_percentage'), + ('magx', 'fs_bad_mag_x', 'magx_fail_percentage'), + ('magy', 'fs_bad_mag_y', 'magy_fail_percentage'), + ('magz', 'fs_bad_mag_z', 'magz_fail_percentage'), ('yaw', 'reject_yaw', 'yaw_fail_percentage'), ('velh', 'reject_hor_vel', 'vel_fail_percentage'), ('velv', 'reject_ver_vel', 'vel_fail_percentage'), diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index a22e7fec63..785ea4df22 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -222,7 +222,7 @@ def create_pdf_report(ulog: ULog, multi_instance: int, output_plot_filename: str data_plot = CheckFlagsPlot( status_flags_time, estimator_status_flags, [['reject_hor_vel', 'reject_hor_pos'], ['reject_ver_vel', 'reject_ver_pos', 'reject_hagl'], - ['reject_mag_x', 'reject_mag_y', 'reject_mag_z', + ['fs_bad_mag_x', 'fs_bad_mag_y', 'fs_bad_mag_z', 'reject_yaw'], ['reject_airspeed'], ['reject_sideslip'], ['reject_optflow_x', 'reject_optflow_y']], x_label='time (sec)', diff --git a/Tools/generate_board_targets_json.py b/Tools/generate_board_targets_json.py index 83b21e9a74..931d5c1477 100755 --- a/Tools/generate_board_targets_json.py +++ b/Tools/generate_board_targets_json.py @@ -61,18 +61,19 @@ def process_target(px4board_file, target_name): if platform not in excluded_platforms: # get the container based on the platform and toolchain - container = platform if platform == 'posix': - container = 'base-focal' + container = 'px4io/px4-dev-base-focal:2021-09-08' if toolchain: if toolchain.startswith('aarch64'): - container = 'aarch64' + container = 'px4io/px4-dev-aarch64:2022-08-12' elif toolchain == 'arm-linux-gnueabihf': - container = 'armhf' + container = 'px4io/px4-dev-armhf:2023-06-26' else: - if verbose: print(f'possibly unmatched toolchain: {toolchain}') + if verbose: print(f'unmatched toolchain: {toolchain}') elif platform == 'nuttx': - container = 'nuttx-focal' + container = 'px4io/px4-dev-nuttx-focal:2022-08-12' + else: + if verbose: print(f'unmatched platform: {platform}') ret = {'target': target_name, 'container': container} @@ -113,4 +114,3 @@ extra_args = {} if args.pretty: extra_args['indent'] = 2 print(json.dumps(github_action_config, **extra_args)) - diff --git a/Tools/msg/templates/ucdr/msg.h.em b/Tools/msg/templates/ucdr/msg.h.em index 5f68b9256e..d9cbc839f4 100644 --- a/Tools/msg/templates/ucdr/msg.h.em +++ b/Tools/msg/templates/ucdr/msg.h.em @@ -124,8 +124,9 @@ static inline constexpr int ucdr_topic_size_@(topic)() return @(struct_size); } -bool ucdr_serialize_@(topic)(const @(uorb_struct)& topic, ucdrBuffer& buf, int64_t time_offset = 0) +bool ucdr_serialize_@(topic)(const void* data, ucdrBuffer& buf, int64_t time_offset = 0) { + const @(uorb_struct)& topic = *static_cast(data); @{ for field_type, field_name, field_size, padding in fields: if padding > 0: diff --git a/Tools/msg/templates/uorb/msg.cpp.em b/Tools/msg/templates/uorb/msg.cpp.em index e613d9511f..c3a82cc014 100644 --- a/Tools/msg/templates/uorb/msg.cpp.em +++ b/Tools/msg/templates/uorb/msg.cpp.em @@ -77,7 +77,7 @@ topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field constexpr char __orb_@(name_snake_case)_fields[] = "@( ";".join(topic_fields) );"; @[for topic in topics]@ -ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast(ORB_ID::@topic)); +ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(name_snake_case)_fields, static_cast(ORB_ID::@topic)); @[end for] void print_message(const orb_metadata *meta, const @uorb_struct& message) diff --git a/Tools/msg/templates/uorb/uORBTopics.cpp.em b/Tools/msg/templates/uorb/uORBTopics.cpp.em index c8b1c88397..9a087416e4 100644 --- a/Tools/msg/templates/uorb/uORBTopics.cpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.cpp.em @@ -76,5 +76,5 @@ const struct orb_metadata *get_orb_meta(ORB_ID id) return nullptr; } - return uorb_topics_list[static_cast(id)]; + return uorb_topics_list[static_cast(id)]; } diff --git a/Tools/msg/templates/uorb/uORBTopics.hpp.em b/Tools/msg/templates/uorb/uORBTopics.hpp.em index aea7015cc7..425661b2fb 100644 --- a/Tools/msg/templates/uorb/uORBTopics.hpp.em +++ b/Tools/msg/templates/uorb/uORBTopics.hpp.em @@ -62,7 +62,7 @@ static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; } */ extern const struct orb_metadata *const *orb_get_topics() __EXPORT; -enum class ORB_ID : uint8_t { +enum class ORB_ID : orb_id_size_t { @[for idx, topic_name in enumerate(topic_names_all)]@ @(topic_name) = @(idx), @[end for] diff --git a/Tools/setup/optional-requirements.txt b/Tools/setup/optional-requirements.txt index e13696aa4e..844ad80bb5 100644 --- a/Tools/setup/optional-requirements.txt +++ b/Tools/setup/optional-requirements.txt @@ -1 +1 @@ -symforce>=0.5.0 +symforce>=0.9.0 diff --git a/Tools/simulation/gazebo-classic/sitl_gazebo-classic b/Tools/simulation/gazebo-classic/sitl_gazebo-classic index 2e3ed9bfb0..20ded0757b 160000 --- a/Tools/simulation/gazebo-classic/sitl_gazebo-classic +++ b/Tools/simulation/gazebo-classic/sitl_gazebo-classic @@ -1 +1 @@ -Subproject commit 2e3ed9bfb04d8865e59380afa575dd37b0d6c8e0 +Subproject commit 20ded0757b4f2cb362833538716caf1e938b162a diff --git a/Tools/simulation/gz/models/omnicopter/model.config b/Tools/simulation/gz/models/omnicopter/model.config new file mode 100644 index 0000000000..a090295062 --- /dev/null +++ b/Tools/simulation/gz/models/omnicopter/model.config @@ -0,0 +1,15 @@ + + + Omnicopter + 1.0 + model.sdf + + + Jaeyoung Lim + jalim@ethz.ch + + + + Omnicopter model for over actuated system + + diff --git a/Tools/simulation/gz/models/omnicopter/model.sdf b/Tools/simulation/gz/models/omnicopter/model.sdf new file mode 100644 index 0000000000..ad2434fb67 --- /dev/null +++ b/Tools/simulation/gz/models/omnicopter/model.sdf @@ -0,0 +1,8 @@ + + + + omnicopter + 0 0 0 0 0 0 + https://fuel.gazebosim.org/1.0/PX4/models/Omnicopter + + diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index 4b6f384002..6ba98ab149 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -11,6 +11,7 @@ + ogre2 diff --git a/_last_entry b/_last_entry new file mode 100644 index 0000000000..e69de29bb2 diff --git a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig index 9a19661dd1..5b1c5bea7f 100644 --- a/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig +++ b/boards/airmind/mindpx-v2/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -140,6 +140,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-flow/nuttx-config/nsh/defconfig b/boards/ark/can-flow/nuttx-config/nsh/defconfig index 15d145881e..eb4cb5e929 100644 --- a/boards/ark/can-flow/nuttx-config/nsh/defconfig +++ b/boards/ark/can-flow/nuttx-config/nsh/defconfig @@ -115,6 +115,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-flow/src/boot_config.h b/boards/ark/can-flow/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-flow/src/boot_config.h +++ b/boards/ark/can-flow/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-flow/uavcan_board_identity b/boards/ark/can-flow/uavcan_board_identity index 8f389d0657..e49fbcac8a 100644 --- a/boards/ark/can-flow/uavcan_board_identity +++ b/boards/ark/can-flow/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} @@ -14,4 +14,4 @@ add_definitions( -DHW_UAVCAN_NAME=${uavcanblid_name} -DHW_VERSION_MAJOR=${uavcanblid_hw_version_major} -DHW_VERSION_MINOR=${uavcanblid_hw_version_minor} -) \ No newline at end of file +) diff --git a/boards/ark/can-gps/nuttx-config/nsh/defconfig b/boards/ark/can-gps/nuttx-config/nsh/defconfig index f734bc8168..8addad7c7d 100644 --- a/boards/ark/can-gps/nuttx-config/nsh/defconfig +++ b/boards/ark/can-gps/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-gps/src/boot_config.h b/boards/ark/can-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-gps/src/boot_config.h +++ b/boards/ark/can-gps/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-gps/uavcan_board_identity b/boards/ark/can-gps/uavcan_board_identity index ca6d098e59..00b23ea028 100644 --- a/boards/ark/can-gps/uavcan_board_identity +++ b/boards/ark/can-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/can-rtk-gps/init/rc.board_sensors b/boards/ark/can-rtk-gps/init/rc.board_sensors index d74d5f74b2..545850dc8b 100644 --- a/boards/ark/can-rtk-gps/init/rc.board_sensors +++ b/boards/ark/can-rtk-gps/init/rc.board_sensors @@ -6,6 +6,9 @@ gps start -d /dev/ttyS0 -p ubx icm42688p -R 0 -s start -bmp388 -I -b 2 start +if ! bmp388 -I -b 2 start +then + bmp388 -I -b 1 start +fi bmm150 -I -b 1 start diff --git a/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig index ba2d6f2143..34d42179c7 100644 --- a/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig +++ b/boards/ark/can-rtk-gps/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/can-rtk-gps/src/boot_config.h b/boards/ark/can-rtk-gps/src/boot_config.h index eab3c76c6c..76782f9a93 100644 --- a/boards/ark/can-rtk-gps/src/boot_config.h +++ b/boards/ark/can-rtk-gps/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -93,7 +93,7 @@ */ #define OPT_WAIT_FOR_GETNODEINFO 0 #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/can-rtk-gps/uavcan_board_identity b/boards/ark/can-rtk-gps/uavcan_board_identity index 5db21a4ffb..655384bc07 100644 --- a/boards/ark/can-rtk-gps/uavcan_board_identity +++ b/boards/ark/can-rtk-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/cannode/nuttx-config/nsh/defconfig b/boards/ark/cannode/nuttx-config/nsh/defconfig index a0882b71be..8ec02cc3dd 100644 --- a/boards/ark/cannode/nuttx-config/nsh/defconfig +++ b/boards/ark/cannode/nuttx-config/nsh/defconfig @@ -117,6 +117,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/ark/cannode/src/boot_config.h b/boards/ark/cannode/src/boot_config.h index eab3c76c6c..b43795ac4c 100644 --- a/boards/ark/cannode/src/boot_config.h +++ b/boards/ark/cannode/src/boot_config.h @@ -65,7 +65,7 @@ #define OPT_PREFERRED_NODE_ID ANY_NODE_ID //todo:wrap OPT_x in in ifdefs for command line definitions -#define OPT_TBOOT_MS 5000 +#define OPT_TBOOT_MS 3000 #define OPT_NODE_STATUS_RATE_MS 800 #define OPT_NODE_INFO_RATE_MS 50 #define OPT_BL_NUMBER_TIMERS 7 @@ -92,8 +92,12 @@ * */ #define OPT_WAIT_FOR_GETNODEINFO 0 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 1 -#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 0 +/* The ARK CANnode uses PH1 for GPIO_BOOT_CONFIG but it is not + * compatible with px4_arch_gpioread as Port H = 7 which is greater + * than STM32_NPORTS + * #define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO 0 + */ +#define OPT_WAIT_FOR_GETNODEINFO_JUMPER_GPIO_INVERT 1 #define OPT_ENABLE_WD 1 diff --git a/boards/ark/cannode/uavcan_board_identity b/boards/ark/cannode/uavcan_board_identity index 44ea18cc7d..490b7678bd 100644 --- a/boards/ark/cannode/uavcan_board_identity +++ b/boards/ark/cannode/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/ark/fmu-v6x/default.px4board b/boards/ark/fmu-v6x/default.px4board index 4a6af4f6d8..4018e493a2 100644 --- a/boards/ark/fmu-v6x/default.px4board +++ b/boards/ark/fmu-v6x/default.px4board @@ -20,6 +20,7 @@ CONFIG_DRIVERS_HEATER=y 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_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=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 bbe99d9659..f4ec9666cc 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/init/rc.board_defaults b/boards/ark/fmu-v6x/init/rc.board_defaults index 2f2c8cfeae..7a26ee01c8 100644 --- a/boards/ark/fmu-v6x/init/rc.board_defaults +++ b/boards/ark/fmu-v6x/init/rc.board_defaults @@ -17,12 +17,21 @@ param set-default SENS_EN_INA228 0 param set-default SENS_EN_INA226 1 param set-default SENS_EN_THERMAL 1 -param set-default SENS_TEMP_ID 2818058 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 ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +then + param set-default SENS_TEMP_ID 2818058 +fi + +if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +then + param set-default SENS_TEMP_ID 3014666 +fi + if ver hwtypecmp ARKV6X001000 ARKV6X001001 ARKV6X001002 ARKV6X001003 ARKV6X001004 ARKV6X001005 ARKV6X001006 ARKV6X001007 then param set-default SYS_USE_IO 0 diff --git a/boards/ark/fmu-v6x/init/rc.board_sensors b/boards/ark/fmu-v6x/init/rc.board_sensors index 84f985cecb..0e04ed41cb 100644 --- a/boards/ark/fmu-v6x/init/rc.board_sensors +++ b/boards/ark/fmu-v6x/init/rc.board_sensors @@ -3,10 +3,12 @@ # ARK FMUARKV6X specific board sensors init #------------------------------------------------------------------------------ set HAVE_PM2 yes +set HAVE_PM3 yes if ver hwtypecmp ARKV6X005000 ARKV6X005001 ARKV6X005002 ARKV6X005003 ARKV6X005004 then set HAVE_PM2 no + set HAVE_PM3 no fi if param compare -s ADC_ADS1115_EN 1 @@ -25,36 +27,68 @@ then then ina226 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina226 -X -b 3 -t 2 -k start + fi fi if param compare SENS_EN_INA228 1 then # Start Digital power monitors ina228 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] then ina228 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina228 -X -b 3 -t 2 -k start + fi fi if param compare SENS_EN_INA238 1 then # Start Digital power monitors ina238 -X -b 1 -t 1 -k start + if [ $HAVE_PM2 = yes ] then ina238 -X -b 2 -t 2 -k start fi + + if [ $HAVE_PM3 = yes ] + then + ina238 -X -b 3 -t 2 -k start + fi fi -# Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz -iim42652 -R 3 -s -b 1 -C 32051 start +if ver hwtypecmp ARKV6X000000 ARKV6X001000 ARKV6X002000 ARKV6X003000 ARKV6X004000 ARKV6X005000 ARKV6X006000 ARKV6X007000 +then + # Internal SPI bus IIM42652 with SPIX measured frequency of 32.051kHz + iim42652 -R 3 -s -b 1 -C 32051 start -# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz -icm42688p -R 9 -s -b 2 -C 32051 start + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 9 -s -b 2 -C 32051 start -# Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz -icm42688p -R 6 -s -b 3 -C 32051 start + # Internal SPI bus ICM42688p with SPIX measured frequency of 32.051kHz + icm42688p -R 6 -s -b 3 -C 32051 start +fi + +if ver hwtypecmp ARKV6X000001 ARKV6X001001 ARKV6X002001 ARKV6X003001 ARKV6X004001 ARKV6X005001 ARKV6X006001 ARKV6X007001 +then + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 3 -s -b 1 -C 32051 start + + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 9 -s -b 2 -C 32051 start + + # Internal SPI bus IIM42653 with SPIX measured frequency of 32.051kHz + iim42653 -R 6 -s -b 3 -C 32051 start +fi # Internal magnetometer on I2C bmm150 -I start @@ -63,3 +97,4 @@ bmm150 -I start bmp388 -I start unset HAVE_PM2 +unset HAVE_PM3 diff --git a/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig b/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig index 97fe8b7937..c830932873 100644 --- a/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/bootloader/defconfig @@ -7,6 +7,7 @@ # # 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" @@ -48,13 +49,14 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +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 diff --git a/boards/ark/fmu-v6x/nuttx-config/include/board.h b/boards/ark/fmu-v6x/nuttx-config/include/board.h index b29b8fc686..333c20905d 100644 --- a/boards/ark/fmu-v6x/nuttx-config/include/board.h +++ b/boards/ark/fmu-v6x/nuttx-config/include/board.h @@ -387,7 +387,7 @@ #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 | GPIO_PULLDOWN) /* PF8 */ +#define GPIO_UART7_RTS GPIO_UART7_RTS_2 /* PF8 */ #define GPIO_UART7_CTS (GPIO_UART7_CTS_1 | GPIO_PULLDOWN) /* PE10 */ #define GPIO_UART8_RX GPIO_UART8_RX_1 /* PE0 */ diff --git a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig index 212213f994..17da75424e 100644 --- a/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/ark/fmu-v6x/nuttx-config/nsh/defconfig @@ -118,7 +118,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -189,6 +189,8 @@ 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 @@ -292,7 +294,7 @@ CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART5_IFLOWCONTROL=y CONFIG_UART5_OFLOWCONTROL=y CONFIG_UART5_RXDMA=y -CONFIG_UART5_TXBUFSIZE=3000 +CONFIG_UART5_TXBUFSIZE=10000 CONFIG_UART5_TXDMA=y CONFIG_UART7_BAUD=57600 CONFIG_UART7_IFLOWCONTROL=y diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index 1317d42668..001a31ae73 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -211,23 +211,18 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "ARKV6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 8 // Rev 0 and Rev 1 // Base/FMUM -#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Rev 0 -#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, BMI388 I2C2 Rev 1 -#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 -#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 -#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 -#define ARKV6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 -#define ARKV6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 -//#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, HB CM4 base Rev 0 // never shipped -//#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, BMI388 I2C2 HB CM4 base Rev 1 // never shipped -#define ARKV6X43 HW_VER_REV(0x4,0x3) // ARKV6X, Sensor Set HB CM4 base Rev 3 -#define ARKV6X44 HW_VER_REV(0x4,0x4) // ARKV6X, Sensor Set HB CM4 base Rev 4 -#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, ARKV6X Rev 0 with HB Mini Rev 5 -//#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, BMI388 I2C2 HB Mini Rev 1 // never shipped -#define ARKV6X53 HW_VER_REV(0x5,0x3) // ARKV6X, Sensor Set HB Mini Rev 3 -#define ARKV6X54 HW_VER_REV(0x5,0x4) // ARKV6X, Sensor Set HB Mini Rev 4 +#define ARKV6X00 HW_VER_REV(0x0,0x0) // ARKV6X, Sensor Set Rev 0 +#define ARKV6X01 HW_VER_REV(0x0,0x1) // ARKV6X, Sensor Set Rev 1 +//#define ARKV6X03 HW_VER_REV(0x0,0x3) // ARKV6X, Sensor Set Rev 3 +//#define ARKV6X04 HW_VER_REV(0x0,0x4) // ARKV6X, Sensor Set Rev 4 +#define ARKV6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Sensor Set Rev 0 +#define ARKV6X11 HW_VER_REV(0x1,0x1) // NO PX4IO, Sensor Set Rev 1 +#define ARKV6X40 HW_VER_REV(0x4,0x0) // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 +#define ARKV6X41 HW_VER_REV(0x4,0x1) // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 +#define ARKV6X50 HW_VER_REV(0x5,0x0) // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 +#define ARKV6X51 HW_VER_REV(0x5,0x1) // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped #define UAVCAN_NUM_IFACES_RUNTIME 1 diff --git a/boards/ark/fmu-v6x/src/manifest.c b/boards/ark/fmu-v6x/src/manifest.c index 6b8405bcf5..f4e012bfb0 100644 --- a/boards/ark/fmu-v6x/src/manifest.c +++ b/boards/ark/fmu-v6x/src/manifest.c @@ -160,21 +160,14 @@ static const px4_hw_mft_item_t hw_mft_list_v0650[] = { static px4_hw_mft_list_entry_t mft_lists[] = { // ver_rev - {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, - {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 - {ARKV6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 - //{ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // HB CM4 base // never shipped - //{ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2 HB CM4 base // never shipped - {ARKV6X43, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 - {ARKV6X44, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 - {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X Rev 0 with HB Mini Rev 5 - //{ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini // never shipped - {ARKV6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 - {ARKV6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 - {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO - {ARKV6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {ARKV6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 - {ARKV6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 + {ARKV6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 0 + {ARKV6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // ARKV6X, Sensor Set Rev 1 + {ARKV6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 0 + {ARKV6X11, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // NO PX4IO, Sensor Set Rev 1 + {ARKV6X40, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 0 HB CM4 base Rev 3 + {ARKV6X41, hw_mft_list_v0640, arraySize(hw_mft_list_v0640)}, // ARKV6X, Sensor Set Rev 1 HB CM4 base Rev 4 + {ARKV6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 0 HB Mini Rev 5 + {ARKV6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // ARKV6X, Sensor Set Rev 1 HB Mini Rev 1 // never shipped }; /************************************************************************************ diff --git a/boards/ark/fmu-v6x/src/mtd.cpp b/boards/ark/fmu-v6x/src/mtd.cpp index 3ece10aeca..80d41e0b78 100644 --- a/boards/ark/fmu-v6x/src/mtd.cpp +++ b/boards/ark/fmu-v6x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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) }; @@ -45,18 +45,12 @@ static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/ark/fmu-v6x/src/spi.cpp b/boards/ark/fmu-v6x/src/spi.cpp index 2d047cff1d..f83a5a91cd 100644 --- a/boards/ark/fmu-v6x/src/spi.cpp +++ b/boards/ark/fmu-v6x/src/spi.cpp @@ -59,7 +59,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(ARKV6X01, { // Placeholder + initSPIHWVersion(ARKV6X01, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X10, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), }, {GPIO::PortI, GPIO::Pin11}), @@ -81,6 +104,121 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(ARKV6X11, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X40, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X41, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X50, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, 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}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(ARKV6X51, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw); diff --git a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig index 3368ce48e1..b6e976d427 100644 --- a/boards/atl/mantis-edu/nuttx-config/nsh/defconfig +++ b/boards/atl/mantis-edu/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/av/x-v1/nuttx-config/nsh/defconfig b/boards/av/x-v1/nuttx-config/nsh/defconfig index 73efed90a1..d5e4039047 100644 --- a/boards/av/x-v1/nuttx-config/nsh/defconfig +++ b/boards/av/x-v1/nuttx-config/nsh/defconfig @@ -120,7 +120,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_LIBC_LONG_LONG=y diff --git a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig index 85951b83dd..15c99f925e 100644 --- a/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie/nuttx-config/nsh/defconfig @@ -105,7 +105,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/bitcraze/crazyflie/src/board_config.h b/boards/bitcraze/crazyflie/src/board_config.h index 5b9529a682..b69a021a15 100644 --- a/boards/bitcraze/crazyflie/src/board_config.h +++ b/boards/bitcraze/crazyflie/src/board_config.h @@ -140,15 +140,7 @@ #define PX4_PWM_ALTERNATE_RANGES #define PWM_LOWEST_MIN 0 -#define PWM_MOTOR_OFF 0 -#define PWM_SERVO_STOP 0 -#define PWM_DEFAULT_MIN 20 -#define PWM_HIGHEST_MIN 0 #define PWM_HIGHEST_MAX 255 -#define PWM_DEFAULT_MAX 255 -#define PWM_LOWEST_MAX 255 -#define PWM_DEFAULT_TRIM 1500 - /* High-resolution timer */ #define HRT_TIMER 8 /* use timer8 for the HRT */ diff --git a/boards/bitcraze/crazyflie/src/mtd.cpp b/boards/bitcraze/crazyflie/src/mtd.cpp index 2b00e231b2..d81e3e4596 100644 --- a/boards/bitcraze/crazyflie/src/mtd.cpp +++ b/boards/bitcraze/crazyflie/src/mtd.cpp @@ -42,18 +42,12 @@ static const px4_mft_device_t i2c1 = { // 24AA64FT on Base 8K 32 X static const px4_mtd_entry_t fmu_eeprom = { .device = &i2c1, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", .nblocks = 128 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 128 - } }, }; diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index a5e5244cc8..bd99fc6964 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -15,6 +15,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y # CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_MAGNETOMETER is not set # CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y @@ -33,6 +34,7 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_MTD=y diff --git a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig index 4840814672..a8e182bc1d 100644 --- a/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig +++ b/boards/bitcraze/crazyflie21/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/bitcraze/crazyflie21/src/board_config.h b/boards/bitcraze/crazyflie21/src/board_config.h index a8ee6029c2..b636bfd08c 100644 --- a/boards/bitcraze/crazyflie21/src/board_config.h +++ b/boards/bitcraze/crazyflie21/src/board_config.h @@ -141,15 +141,7 @@ #define PX4_PWM_ALTERNATE_RANGES #define PWM_LOWEST_MIN 0 -#define PWM_MOTOR_OFF 0 -#define PWM_SERVO_STOP 0 -#define PWM_DEFAULT_MIN 20 -#define PWM_HIGHEST_MIN 0 #define PWM_HIGHEST_MAX 255 -#define PWM_DEFAULT_MAX 255 -#define PWM_LOWEST_MAX 255 -#define PWM_DEFAULT_TRIM 1500 - /* High-resolution timer */ #define HRT_TIMER 8 /* use timer8 for the HRT */ diff --git a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig index 64f884c2db..aa13dbdb1e 100644 --- a/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/cuav/can-gps-v1/nuttx-config/nsh/defconfig @@ -119,6 +119,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/cuav/can-gps-v1/uavcan_board_identity b/boards/cuav/can-gps-v1/uavcan_board_identity index a547589079..0cf273dde7 100644 --- a/boards/cuav/can-gps-v1/uavcan_board_identity +++ b/boards/cuav/can-gps-v1/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/cuav/nora/nuttx-config/bootloader/defconfig b/boards/cuav/nora/nuttx-config/bootloader/defconfig index a05449844b..cc5ca67d2d 100644 --- a/boards/cuav/nora/nuttx-config/bootloader/defconfig +++ b/boards/cuav/nora/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cuav/nora/nuttx-config/nsh/defconfig b/boards/cuav/nora/nuttx-config/nsh/defconfig index fb84cac440..b84688f9b1 100644 --- a/boards/cuav/nora/nuttx-config/nsh/defconfig +++ b/boards/cuav/nora/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig index b3a4aa12cd..d0e6e6194a 100644 --- a/boards/cuav/x7pro/nuttx-config/bootloader/defconfig +++ b/boards/cuav/x7pro/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cuav/x7pro/nuttx-config/nsh/defconfig b/boards/cuav/x7pro/nuttx-config/nsh/defconfig index 1088a24c02..039d5ee9b9 100644 --- a/boards/cuav/x7pro/nuttx-config/nsh/defconfig +++ b/boards/cuav/x7pro/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -147,6 +147,8 @@ 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 diff --git a/boards/cuav/x7pro/nuttx-config/test/defconfig b/boards/cuav/x7pro/nuttx-config/test/defconfig index 7506c06387..392e47c9b4 100644 --- a/boards/cuav/x7pro/nuttx-config/test/defconfig +++ b/boards/cuav/x7pro/nuttx-config/test/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -147,6 +147,8 @@ 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 diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index 521dd3bde8..91ddcda596 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -19,6 +19,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y +CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=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 diff --git a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig index 2203424943..59041c61c4 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig index fe6a962c27..192e7c713a 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig index 54613e1e61..9bb0b35420 100644 --- a/boards/cubepilot/cubeorange/nuttx-config/test/defconfig +++ b/boards/cubepilot/cubeorange/nuttx-config/test/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -148,6 +148,8 @@ 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 diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index e9f6c2d005..837876ee7d 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -20,6 +20,7 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors index 61a3fbf138..d4a1052f92 100644 --- a/boards/cubepilot/cubeorangeplus/init/rc.board_sensors +++ b/boards/cubepilot/cubeorangeplus/init/rc.board_sensors @@ -8,16 +8,21 @@ board_adc start # 1. Isolated {ICM42688p, ICM20948(with mag)}, body-fixed {ICM20649} # 2. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM20649, ICM45686, AK09918} # 3. Isolated {ICM42688p, ICM42688p}, body-fixed {ICM45686, AK09918} +# 4. Isolated {ICM45686, ICM45686}, body-fixed {ICM45686, AK09918} # SPI4 is isolated, SPI1 is body-fixed # SPI4, isolated ms5611 -s -b 4 start -icm42688p -s -b 4 -R 10 start -c 15 -if ! icm20948 -s -b 4 -R 10 -M -q start -then - icm42688p -s -b 4 -R 6 start -c 13 +if icm42688p -s -b 4 -R 10 -q start -c 15 + if ! icm20948 -s -b 4 -R 10 -M -q start + then + icm42688p -s -b 4 -R 6 start -c 13 + fi +else + icm45686 -s -b 4 -R 10 start -c 15 + icm45686 -s -b 4 -R 6 start -c 13 fi # SPI1, body-fixed diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig index 15ca3bfe79..3861e2c8dc 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/bootloader/defconfig @@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig index 7bab0a36af..3a3d954783 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig b/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig index ba34d8a40f..9b0814ae08 100644 --- a/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig +++ b/boards/cubepilot/cubeorangeplus/nuttx-config/test/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -148,6 +148,8 @@ 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 diff --git a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig index f00b3e93a3..c696b87e08 100644 --- a/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig +++ b/boards/cubepilot/cubeyellow/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index 6655e8e388..32ec4fd47b 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -23,6 +23,8 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOAD_MON=y @@ -36,7 +38,6 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y -CONFIG_SYSTEMCMDS_MFT=y CONFIG_SYSTEMCMDS_MTD=y CONFIG_SYSTEMCMDS_NSHTERM=y CONFIG_SYSTEMCMDS_PARAM=y diff --git a/boards/diatone/mamba-f405-mk2/init/rc.board_defaults b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults index 97bbcd6ee6..61c5953c41 100644 --- a/boards/diatone/mamba-f405-mk2/init/rc.board_defaults +++ b/boards/diatone/mamba-f405-mk2/init/rc.board_defaults @@ -11,8 +11,7 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 +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/nuttx-config/nsh/defconfig b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig index 8fed52b508..07573c0f9e 100644 --- a/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig +++ b/boards/diatone/mamba-f405-mk2/nuttx-config/nsh/defconfig @@ -62,7 +62,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y diff --git a/boards/flywoo/gn-f405/init/rc.board_defaults b/boards/flywoo/gn-f405/init/rc.board_defaults index d5ae67dc8d..c1fc72c526 100644 --- a/boards/flywoo/gn-f405/init/rc.board_defaults +++ b/boards/flywoo/gn-f405/init/rc.board_defaults @@ -17,9 +17,8 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 +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/flywoo/gn-f405/nuttx-config/nsh/defconfig b/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig index 52ed2f86da..ea831c0b91 100644 --- a/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig +++ b/boards/flywoo/gn-f405/nuttx-config/nsh/defconfig @@ -109,7 +109,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y diff --git a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig index 3ea9c492e6..d295254d69 100644 --- a/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig +++ b/boards/freefly/can-rtk-gps/nuttx-config/nsh/defconfig @@ -132,6 +132,8 @@ 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 diff --git a/boards/freefly/can-rtk-gps/uavcan_board_identity b/boards/freefly/can-rtk-gps/uavcan_board_identity index 997b26e2ae..2531725501 100644 --- a/boards/freefly/can-rtk-gps/uavcan_board_identity +++ b/boards/freefly/can-rtk-gps/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig index 4dcee619f1..f95e8f5c0c 100644 --- a/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/can-gps-v1/nuttx-config/nsh/defconfig @@ -120,6 +120,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/holybro/can-gps-v1/uavcan_board_identity b/boards/holybro/can-gps-v1/uavcan_board_identity index c34b51cd77..bb7a514fd6 100644 --- a/boards/holybro/can-gps-v1/uavcan_board_identity +++ b/boards/holybro/can-gps-v1/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig index 14f38276ef..be49e9a560 100644 --- a/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig index 85363a602e..19647f3b1f 100644 --- a/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig +++ b/boards/holybro/durandal-v1/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ 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 diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 6919df33f2..bcaedd9cf2 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -10,16 +10,26 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS4" CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP280=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y +CONFIG_DRIVERS_OSD_ATXXXX=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y -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_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_MAGNETOMETER is not set +# CONFIG_EKF2_RANGE_FINDER is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y CONFIG_MODULES_LOGGER=y @@ -31,5 +41,9 @@ 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_SENSORS_VEHICLE_AIRSPEED is not set +# CONFIG_SENSORS_VEHICLE_MAGNETOMETER is not set +# CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW is not set +CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_TOP=y diff --git a/boards/holybro/kakutef7/init/rc.board_defaults b/boards/holybro/kakutef7/init/rc.board_defaults index 2968cb67ca..7a3bc92e84 100644 --- a/boards/holybro/kakutef7/init/rc.board_defaults +++ b/boards/holybro/kakutef7/init/rc.board_defaults @@ -12,14 +12,11 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 - param set-default SYS_HAS_MAG 0 +# enable gravity fusion +param set-default EKF2_IMU_CONTROL 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/nuttx-config/nsh/defconfig b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig index faf8655412..88d57ea784 100644 --- a/boards/holybro/kakutef7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakutef7/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -150,6 +150,8 @@ 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 diff --git a/boards/holybro/kakutef7/src/board_config.h b/boards/holybro/kakutef7/src/board_config.h index 9254c57c52..3164a97836 100644 --- a/boards/holybro/kakutef7/src/board_config.h +++ b/boards/holybro/kakutef7/src/board_config.h @@ -113,6 +113,7 @@ /* RC Serial port */ #define RC_SERIAL_PORT "/dev/ttyS4" +#define RC_SERIAL_SINGLEWIRE #define GPIO_RSSI_IN /* PC5 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTC|GPIO_PIN5) @@ -129,6 +130,8 @@ GPIO_RSSI_IN, \ } +#define BOARD_ENABLE_CONSOLE_BUFFER + #define BOARD_NUM_IO_TIMERS 4 __BEGIN_DECLS diff --git a/boards/holybro/kakutef7/src/init.c b/boards/holybro/kakutef7/src/init.c index da2a545c2a..9b8a0140f4 100644 --- a/boards/holybro/kakutef7/src/init.c +++ b/boards/holybro/kakutef7/src/init.c @@ -252,9 +252,5 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif - /* Configure the HW based on the manifest */ - - px4_platform_configure(); - return OK; } diff --git a/boards/holybro/kakuteh7/default.px4board b/boards/holybro/kakuteh7/default.px4board index 713c736d50..a9f38ba78c 100644 --- a/boards/holybro/kakuteh7/default.px4board +++ b/boards/holybro/kakuteh7/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7/init/rc.board_defaults b/boards/holybro/kakuteh7/init/rc.board_defaults index fa014c6b48..47dea71be6 100644 --- a/boards/holybro/kakuteh7/init/rc.board_defaults +++ b/boards/holybro/kakuteh7/init/rc.board_defaults @@ -22,13 +22,10 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 - +# use EKF2 without mag param set-default SYS_HAS_MAG 0 +# and enable gravity fusion +param set-default EKF2_IMU_CONTROL 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/kakuteh7/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig index 9e7ac9fa4a..4557ffca77 100644 --- a/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig index c7d29ddb92..4dc58311a1 100644 --- a/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ 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 diff --git a/boards/holybro/kakuteh7mini/default.px4board b/boards/holybro/kakuteh7mini/default.px4board index b019b753f9..3310e79fbb 100644 --- a/boards/holybro/kakuteh7mini/default.px4board +++ b/boards/holybro/kakuteh7mini/default.px4board @@ -32,6 +32,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig index cefb4421b7..bc47e6b75b 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7mini/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig index f40b0ccbfd..685ea6c247 100644 --- a/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7mini/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/holybro/kakuteh7v2/default.px4board b/boards/holybro/kakuteh7v2/default.px4board index d326a78797..9c060d8442 100644 --- a/boards/holybro/kakuteh7v2/default.px4board +++ b/boards/holybro/kakuteh7v2/default.px4board @@ -33,6 +33,12 @@ CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_EKF2=y +# CONFIG_EKF2_AUXVEL is not set +# CONFIG_EKF2_BARO_COMPENSATION is not set +# CONFIG_EKF2_DRAG_FUSION is not set +# CONFIG_EKF2_EXTERNAL_VISION is not set +# CONFIG_EKF2_GNSS_YAW is not set +# CONFIG_EKF2_SIDESLIP is not set CONFIG_MODULES_ESC_BATTERY=y CONFIG_MODULES_EVENTS=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y diff --git a/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig b/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig index 0f46c66f71..10011bc456 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig +++ b/boards/holybro/kakuteh7v2/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig index 8ac32ef349..1b85f5b0fd 100644 --- a/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig +++ b/boards/holybro/kakuteh7v2/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig index 76efc7a11e..370b24dfe6 100644 --- a/boards/holybro/pix32v5/nuttx-config/nsh/defconfig +++ b/boards/holybro/pix32v5/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig index 8c973887bf..0e5f3cf684 100644 --- a/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig +++ b/boards/matek/gnss-m9n-f4/nuttx-config/canbootloader/defconfig @@ -30,7 +30,7 @@ CONFIG_FDCLONE_STDIO=y CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=4096 -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig index fb6ecf3ba4..f74d825a37 100644 --- a/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig +++ b/boards/matek/gnss-m9n-f4/nuttx-config/nsh/defconfig @@ -104,7 +104,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -138,6 +138,8 @@ 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=131072 CONFIG_RAM_START=0x20000000 diff --git a/boards/matek/gnss-m9n-f4/uavcan_board_identity b/boards/matek/gnss-m9n-f4/uavcan_board_identity index 70123f7d91..fb24ed9f11 100755 --- a/boards/matek/gnss-m9n-f4/uavcan_board_identity +++ b/boards/matek/gnss-m9n-f4/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/matek/h743-mini/nuttx-config/bootloader/defconfig b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig index 94223808e7..a5e157f541 100644 --- a/boards/matek/h743-mini/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743-mini/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743-mini/nuttx-config/nsh/defconfig b/boards/matek/h743-mini/nuttx-config/nsh/defconfig index a4c347fe03..600180fa6d 100644 --- a/boards/matek/h743-mini/nuttx-config/nsh/defconfig +++ b/boards/matek/h743-mini/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/matek/h743-slim/nuttx-config/bootloader/defconfig b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig index 7130c17c78..636630ffd7 100644 --- a/boards/matek/h743-slim/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743-slim/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743-slim/nuttx-config/nsh/defconfig b/boards/matek/h743-slim/nuttx-config/nsh/defconfig index 94267a1a2b..cc3124fca0 100644 --- a/boards/matek/h743-slim/nuttx-config/nsh/defconfig +++ b/boards/matek/h743-slim/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/matek/h743/nuttx-config/bootloader/defconfig b/boards/matek/h743/nuttx-config/bootloader/defconfig index 2bdea5edd9..a39e59702e 100644 --- a/boards/matek/h743/nuttx-config/bootloader/defconfig +++ b/boards/matek/h743/nuttx-config/bootloader/defconfig @@ -47,7 +47,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/matek/h743/nuttx-config/nsh/defconfig b/boards/matek/h743/nuttx-config/nsh/defconfig index 39e976d11c..eba77a1a90 100644 --- a/boards/matek/h743/nuttx-config/nsh/defconfig +++ b/boards/matek/h743/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 diff --git a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig index b2d2e7eb77..d1f84d56ee 100644 --- a/boards/modalai/fc-v1/nuttx-config/nsh/defconfig +++ b/boards/modalai/fc-v1/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ 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 diff --git a/boards/modalai/fc-v2/default.px4board b/boards/modalai/fc-v2/default.px4board index becef2d482..a34d1ad7a4 100644 --- a/boards/modalai/fc-v2/default.px4board +++ b/boards/modalai/fc-v2/default.px4board @@ -14,10 +14,6 @@ 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_ICM20649=y -# CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y @@ -29,13 +25,11 @@ CONFIG_DRIVERS_POWER_MONITOR_INA226=y CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y -CONFIG_DRIVERS_RPM=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y -# CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y @@ -54,7 +48,6 @@ 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 diff --git a/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig index 58ac8586ae..1a138a659b 100644 --- a/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig +++ b/boards/modalai/fc-v2/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/modalai/fc-v2/nuttx-config/nsh/defconfig b/boards/modalai/fc-v2/nuttx-config/nsh/defconfig index 11bb3c3da9..7fb9744c18 100644 --- a/boards/modalai/fc-v2/nuttx-config/nsh/defconfig +++ b/boards/modalai/fc-v2/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -152,6 +152,8 @@ 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 diff --git a/boards/modalai/fc-v2/src/mtd.cpp b/boards/modalai/fc-v2/src/mtd.cpp index 644b96581d..48a5bd334c 100644 --- a/boards/modalai/fc-v2/src/mtd.cpp +++ b/boards/modalai/fc-v2/src/mtd.cpp @@ -34,25 +34,19 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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 = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig index c8a5905f54..1167e3eefa 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig index e5169e78ed..0811c67f0a 100644 --- a/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-classic/nuttx-config/nsh/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -149,6 +149,8 @@ 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 diff --git a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig index da43bb2b3f..93af46bab6 100644 --- a/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7-oem/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ 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 diff --git a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig index 8ec771c53c..64878f212a 100644 --- a/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-f7/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ 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 diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig index 6d945355d7..8a44a577f1 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig index a79a19a5e0..cbfdca2b5c 100644 --- a/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7-oem/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig index 8ce4dddb0b..5a696370f9 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig index a0f041492e..c5f40e5b55 100644 --- a/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig +++ b/boards/mro/ctrl-zero-h7/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig index d34cea0b6a..584011e926 100644 --- a/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig index 10446138d7..e8005ce5bb 100644 --- a/boards/mro/pixracerpro/nuttx-config/nsh/defconfig +++ b/boards/mro/pixracerpro/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/mro/x21-777/nuttx-config/nsh/defconfig b/boards/mro/x21-777/nuttx-config/nsh/defconfig index 6d15b6c475..5a768e24ed 100644 --- a/boards/mro/x21-777/nuttx-config/nsh/defconfig +++ b/boards/mro/x21-777/nuttx-config/nsh/defconfig @@ -111,7 +111,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -146,6 +146,8 @@ 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 diff --git a/boards/mro/x21/nuttx-config/nsh/defconfig b/boards/mro/x21/nuttx-config/nsh/defconfig index 9d891cd2f7..681db5c36b 100644 --- a/boards/mro/x21/nuttx-config/nsh/defconfig +++ b/boards/mro/x21/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -141,6 +141,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/nxp/fmuk66-e/default.px4board b/boards/nxp/fmuk66-e/default.px4board index f53c438f88..b8bc5cc7f4 100644 --- a/boards/nxp/fmuk66-e/default.px4board +++ b/boards/nxp/fmuk66-e/default.px4board @@ -32,6 +32,7 @@ CONFIG_DRIVERS_SMART_BATTERY_BATMON=y CONFIG_COMMON_TELEMETRY=y CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_MODULES_AIRSPEED_SELECTOR=y CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y CONFIG_MODULES_BATTERY_STATUS=y diff --git a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig index 6b9fa1a438..57d0c64bb6 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/nsh/defconfig @@ -61,7 +61,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y @@ -168,6 +168,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig index 2d05953375..377f2cd48e 100644 --- a/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-e/nuttx-config/socketcan/defconfig @@ -62,7 +62,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=48 CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig index 23a497e7b0..9ef98c951f 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/nsh/defconfig @@ -65,7 +65,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_KINETIS_ADC0=y CONFIG_KINETIS_ADC1=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig index 4638f1ee76..e99d7c6e90 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/socketcan/defconfig @@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=48 CONFIG_IOB_NCHAINS=18 CONFIG_KINETIS_ADC0=y @@ -170,6 +170,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig index c428852e9b..9818be1f54 100644 --- a/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig +++ b/boards/nxp/fmuk66-v3/nuttx-config/test/defconfig @@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_KINETIS_ADC0=y @@ -171,6 +171,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=262144 CONFIG_RAM_START=0x1fff0000 diff --git a/boards/nxp/fmurt1062-v1/default.px4board b/boards/nxp/fmurt1062-v1/default.px4board index 2a7791db8f..331b6d36bb 100644 --- a/boards/nxp/fmurt1062-v1/default.px4board +++ b/boards/nxp/fmurt1062-v1/default.px4board @@ -22,7 +22,7 @@ CONFIG_DRIVERS_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_TONE_ALARM=y -CONFIG_COMMON_UWB=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y diff --git a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig index 106dc07237..31175960f9 100644 --- a/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig +++ b/boards/nxp/fmurt1062-v1/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_IMXRT_USDHC1=y CONFIG_IMXRT_USDHC1_INVERT_CD=y CONFIG_IMXRT_USDHC1_WIDTH_D1_D4=y CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_LPUART2_BAUD=57600 @@ -184,6 +184,8 @@ CONFIG_PIPES=y CONFIG_PREALLOC_TIMERS=50 CONFIG_PRIORITY_INHERITANCE=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=1048576 CONFIG_RAM_START=0x20200000 diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index c85a77330d..9505bb74a8 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/boards/nxp/mr-canhubk3/fmu.px4board @@ -1,20 +1,25 @@ # CONFIG_BOARD_ROMFSROOT is not set CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS4" CONFIG_BOARD_SERIAL_RC="/dev/ttyS5" CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3" CONFIG_BOARD_UAVCAN_INTERFACES=1 CONFIG_COMMON_LIGHT=y CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_CYPHAL=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y CONFIG_DRIVERS_RC_INPUT=y +CONFIG_DRIVERS_SAFETY_BUTTON=y CONFIG_DRIVERS_UAVCAN=y CONFIG_EXAMPLES_FAKE_GPS=y 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 CONFIG_MODULES_ESC_BATTERY=y diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults index 64dedb9fe8..7ef22a054c 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_defaults +++ b/boards/nxp/mr-canhubk3/init/rc.board_defaults @@ -17,6 +17,17 @@ param set-default MAV_1_UDP_PRT 14550 param set-default SENS_EXT_I2C_PRB 0 param set-default CYPHAL_ENABLE 0 +if ver hwtypecmp MR-CANHUBK3-ADAP +then + param set-default GPS_1_CONFIG 202 + param set-default RC_PORT_CONFIG 104 + param set-default SENS_INT_BARO_EN 0 + param set-default SYS_HAS_BARO 0 + # MR-CANHUBK3-ADAP voltage divider + param set-default BAT1_V_DIV 13.158 + safety_button start +fi + if param greater -s UAVCAN_ENABLE 0 then ifup can0 diff --git a/boards/nxp/mr-canhubk3/init/rc.board_sensors b/boards/nxp/mr-canhubk3/init/rc.board_sensors index fd2dc79a17..642a254efc 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_sensors +++ b/boards/nxp/mr-canhubk3/init/rc.board_sensors @@ -2,22 +2,31 @@ # # NXP MR-CANHUBK3 specific board sensors init #------------------------------------------------------------------------------ +if ver hwtypecmp MR-CANHUBK3-ADAP +then + icm42688p -c 2 -b 3 -R 0 -S -f 15000 start + # Internal magnetometer on I2c on ADAP + bmm150 -X -a 18 start + ist8310 -X -b 1 -R 10 start + # ADC for voltage input sensing + board_adc start -#board_adc start FIXME no ADC drivers + # External SPI bus ICM20649 + icm20649 -b 4 -S -R 10 start -#FMUv5Xbase board orientation + # External SPI bus ICM42688p + icm42688p -c 1 -b 3 -R 10 -S -f 15000 start +else + bmm150 -X start + # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) + ist8310 -X -b 2 -R 10 start -# Internal SPI bus ICM20649 -icm20649 -s -R 6 start + # External SPI bus ICM20649 + icm20649 -b 4 -S -R 6 start -# Internal SPI bus ICM42688p -icm42688p -R 6 -s start - -# Internal magnetometer on I2c -bmm150 -I start - -# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) -ist8310 -X -b 2 -R 10 start + # External SPI bus ICM42688p + icm42688p -c 1 -b 3 -R 6 -S -f 15000 start +fi # External compass on GPS1/I2C1 (the 3rd external bus): Drotek RTK GPS with LIS3MDL Compass lis3mdl -X -b 2 -R 2 start @@ -25,5 +34,5 @@ lis3mdl -X -b 2 -R 2 start # Disable startup of internal baros if param is set to false if param compare SENS_INT_BARO_EN 1 then - bmp388 -I -a 0x77 start + bmp388 -X -a 0x77 start fi diff --git a/boards/nxp/mr-canhubk3/nuttx-config/include/board.h b/boards/nxp/mr-canhubk3/nuttx-config/include/board.h index 04d0478528..457f63c5cf 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/include/board.h +++ b/boards/nxp/mr-canhubk3/nuttx-config/include/board.h @@ -152,6 +152,18 @@ #define PIN_LPUART1_RX (PIN_LPUART1_RX_3 | PIN_INPUT_PULLUP) /* PTC6 */ #define PIN_LPUART1_TX PIN_LPUART1_TX_3 /* PTC7 */ + +/* LPUART4 /dev/ttyS3 P8B 3X7 Pin 3 Single wire RC UART */ + +#define PIN_LPUART4_RX PIN_LPUART4_TX_3 /* Dummy since it's Single Wire TX-only */ +#define PIN_LPUART4_TX PIN_LPUART4_TX_3 /* PTE11 */ + + +/* LPUART7 /dev/ttyS4 P8B 3X7 Pin 3 and Pin 8 */ + +#define PIN_LPUART7_RX (PIN_LPUART7_RX_3 | PIN_INPUT_PULLUP) /* PTE0 */ +#define PIN_LPUART7_TX PIN_LPUART7_TX_3 /* PTE1 */ + /* LPUART9 P24 UART connector */ #define PIN_LPUART9_RX (PIN_LPUART9_RX_1 | PIN_INPUT_PULLUP) /* PTB2 */ @@ -209,7 +221,8 @@ #define PIN_LPSPI4_PCS0 PIN_LPSPI4_PCS0_1 /* PTB8 */ #define PIN_LPSPI4_PCS3 PIN_LPSPI4_PCS3_1 /* PTA16 */ -#define PIN_LPSPI4_PCS (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */ +#define PIN_LPSPI4_CS_P26 (PIN_PTA16 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTA16 */ +#define PIN_LPSPI4_CS_P8B (PIN_PTB8 | GPIO_LOWDRIVE | GPIO_OUTPUT_ONE) /* PTB8 */ /* LPSPI5 P26 external IMU connector */ diff --git a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig index f5facd0fdd..f306f5eb9d 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/nsh/defconfig @@ -109,7 +109,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -245,6 +245,8 @@ CONFIG_S32K3XX_LPUART13=y CONFIG_S32K3XX_LPUART14=y CONFIG_S32K3XX_LPUART1=y CONFIG_S32K3XX_LPUART2=y +CONFIG_S32K3XX_LPUART4=y +CONFIG_S32K3XX_LPUART7=y CONFIG_S32K3XX_LPUART9=y CONFIG_S32K3XX_LPUART_INVERT=y CONFIG_S32K3XX_LPUART_SINGLEWIRE=y diff --git a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig index 32169a8ae5..e9d287c2cd 100644 --- a/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig +++ b/boards/nxp/mr-canhubk3/nuttx-config/sysview/defconfig @@ -110,7 +110,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=900 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y diff --git a/boards/nxp/mr-canhubk3/src/CMakeLists.txt b/boards/nxp/mr-canhubk3/src/CMakeLists.txt index 3a5ac9c0e6..26a13272e2 100644 --- a/boards/nxp/mr-canhubk3/src/CMakeLists.txt +++ b/boards/nxp/mr-canhubk3/src/CMakeLists.txt @@ -38,6 +38,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL "canbootloader") clockconfig.c periphclocks.c timer_config.cpp + hw_rev_ver_canhubk3.c ) target_link_libraries(drivers_board @@ -65,6 +66,8 @@ else() spi.cpp timer_config.cpp s32k3xx_userleds.c + hw_rev_ver_canhubk3.c + manifest.c ) target_link_libraries(drivers_board diff --git a/boards/nxp/mr-canhubk3/src/board_config.h b/boards/nxp/mr-canhubk3/src/board_config.h index 49a3162af6..7ea2a91e8b 100644 --- a/boards/nxp/mr-canhubk3/src/board_config.h +++ b/boards/nxp/mr-canhubk3/src/board_config.h @@ -88,7 +88,7 @@ __BEGIN_DECLS #define DIRECT_PWM_OUTPUT_CHANNELS 8 #define RC_SERIAL_PORT "/dev/ttyS5" -#define RC_SERIAL_SINGLEWIRE +#define RC_SERIAL_SINGLEWIRE_FORCE #define RC_SERIAL_INVERT_RX_ONLY #define BOARD_ENABLE_CONSOLE_BUFFER @@ -110,6 +110,40 @@ __BEGIN_DECLS /* Reboot and ulog we store on a wear-level filesystem */ #define HARDFAULT_REBOOT_PATH "/mnt/progmem/reboot" +/* To detect MR-CANHUBK3-ADAP board */ +#define BOARD_HAS_HW_VERSIONING 1 +#define CANHUBK3_ADAP_DETECT (PIN_PTA12 | GPIO_INPUT | GPIO_PULLUP) + + +/* + * 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. ADC1 has 32 channels, with some a/b selection overlap + * in the AD4-AD7 range on the same ADC. + * + * Only ADC1 is used + * Bits 31:0 are ADC1 channels 31:0 + */ + +#define ADC1_CH(c) (((c) & 0x1f)) /* Define ADC number Channel number */ + +/* ADC defines to be used in sensors.cpp to read from a particular channel */ + +#define ADC_BATTERY_VOLTAGE_CHANNEL ADC1_CH(0) /* BAT_VSENS 85 PTB4 ADC1_SE10 */ +#define ADC_BATTERY_CURRENT_CHANNEL ADC1_CH(1) /* Non-existant but needed for compilation */ + + +/* Mask use to initialize the ADC driver */ + +#define ADC_CHANNELS ((1 << ADC_BATTERY_VOLTAGE_CHANNEL)) + +/* Safety Switch + * TBD + */ +#define GPIO_LED_SAFETY (PIN_PTE26 | GPIO_OUTPUT | GPIO_OUTPUT_ZERO) +#define GPIO_BTN_SAFETY (PIN_PTA11 | GPIO_INPUT | GPIO_PULLDOWN) + /**************************************************************************** * Public Data ****************************************************************************/ diff --git a/boards/nxp/mr-canhubk3/src/hw_rev_ver_canhubk3.c b/boards/nxp/mr-canhubk3/src/hw_rev_ver_canhubk3.c new file mode 100644 index 0000000000..990c17492a --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/hw_rev_ver_canhubk3.c @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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 hw_rev_ver_canhubk3.c + * CANHUBK3 Hardware Revision and Version ID API + */ +#include +#include +#include +#include +#include +#include +#include + +#include + +#if defined(BOARD_HAS_HW_VERSIONING) + + +#define HW_INFO_SIZE HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS + + +/**************************************************************************** + * Private Data + ****************************************************************************/ +static int is_adap_connected = 0; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ +/************************************************************************************ + * Name: board_get_hw_type + * + * Description: + * Optional returns a 0 terminated string defining the HW type. + * + * Input Parameters: + * None + * + * Returned Value: + * a 0 terminated string defining the HW type. This my be a 0 length string "" + * + ************************************************************************************/ + +__EXPORT const char *board_get_hw_type_name() +{ + if (is_adap_connected) { + return (const char *)"MR-CANHUBK3-ADAP"; + + } else { + return (const char *)"MR-CANHUBK344"; + } +} + +/************************************************************************************ + * Name: board_get_hw_version + * + * Description: + * Optional returns a integer HW version + * + * Input Parameters: + * None + * + * Returned Value: + * An integer value of this boards hardware version. + * A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API. + * A value of 0 is the default for boards supporting the API but not having version. + * + ************************************************************************************/ + +__EXPORT int board_get_hw_version() +{ + return 0; +} + +/************************************************************************************ + * Name: board_get_hw_revision + * + * Description: + * Optional returns a integer HW revision + * + * Input Parameters: + * None + * + * Returned Value: + * An integer value of this boards hardware revision. + * A value of -1 is the default for boards not supporting the BOARD_HAS_VERSIONING API. + * A value of 0 is the default for boards supporting the API but not having revision. + * + ************************************************************************************/ + +__EXPORT int board_get_hw_revision() +{ + return 0; +} + +/************************************************************************************ + * Name: board_determine_hw_info + * + * Description: + * Uses GPIO to detect MR-CANHUBK3-ADAP + * + ************************************************************************************/ + +int board_determine_hw_info() +{ + s32k3xx_pinconfig(CANHUBK3_ADAP_DETECT); + is_adap_connected = !s32k3xx_gpioread(CANHUBK3_ADAP_DETECT); + return 0; +} +#endif diff --git a/boards/nxp/mr-canhubk3/src/i2c.cpp b/boards/nxp/mr-canhubk3/src/i2c.cpp index f8639282ae..c99547def4 100644 --- a/boards/nxp/mr-canhubk3/src/i2c.cpp +++ b/boards/nxp/mr-canhubk3/src/i2c.cpp @@ -34,6 +34,6 @@ #include constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { - initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(0)), - initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(1)), + initI2CBusExternal(PX4_BUS_NUMBER_TO_PX4(0)), + initI2CBusInternal(PX4_BUS_NUMBER_TO_PX4(1)), }; diff --git a/boards/nxp/mr-canhubk3/src/init.c b/boards/nxp/mr-canhubk3/src/init.c index 1075daece6..97a41f8430 100644 --- a/boards/nxp/mr-canhubk3/src/init.c +++ b/boards/nxp/mr-canhubk3/src/init.c @@ -44,6 +44,7 @@ #include "board_config.h" #include +#include #if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD) #include @@ -96,6 +97,8 @@ __EXPORT int board_app_initialize(uintptr_t arg) int rv; + board_determine_hw_info(); + #if defined(CONFIG_S32K3XX_LPSPI1) && defined(CONFIG_MMCSD) /* LPSPI1 *****************************************************************/ diff --git a/boards/nxp/mr-canhubk3/src/manifest.c b/boards/nxp/mr-canhubk3/src/manifest.c new file mode 100644 index 0000000000..afd3de58e5 --- /dev/null +++ b/boards/nxp/mr-canhubk3/src/manifest.c @@ -0,0 +1,79 @@ +/**************************************************************************** + * + * 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 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 "systemlib/px4_macros.h" +#include "px4_log.h" + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + + +/************************************************************************************ + * 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) +{ + return px4_hw_mft_unsupported; +} diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c b/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c index 529ed7ed9b..53eda463be 100644 --- a/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_bringup.c @@ -107,6 +107,9 @@ int s32k3xx_bringup(void) s32k3xx_spidev_initialize(); #endif + s32k3xx_pinconfig(GPIO_LED_SAFETY); + s32k3xx_pinconfig(GPIO_BTN_SAFETY); + #ifdef CONFIG_INPUT_BUTTONS /* Register the BUTTON driver */ diff --git a/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c b/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c index 0314fce44f..42cf924059 100644 --- a/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c +++ b/boards/nxp/mr-canhubk3/src/s32k3xx_periphclocks.c @@ -168,6 +168,22 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { .clkgate = true, #else .clkgate = false, +#endif + }, + { + .clkname = LPUART4_CLK, +#ifdef CONFIG_S32K3XX_LPUART4 + .clkgate = true, +#else + .clkgate = false, +#endif + }, + { + .clkname = LPUART7_CLK, +#ifdef CONFIG_S32K3XX_LPUART7 + .clkgate = true, +#else + .clkgate = false, #endif }, { @@ -258,6 +274,10 @@ const struct peripheral_clock_config_s g_peripheral_clockconfig0[] = { .clkname = EMIOS0_CLK, .clkgate = true, }, + { + .clkname = ADC2_CLK, + .clkgate = true, + } }; unsigned int const num_of_peripheral_clocks_0 = diff --git a/boards/nxp/mr-canhubk3/src/spi.cpp b/boards/nxp/mr-canhubk3/src/spi.cpp index 325ad7b7d7..40d9e9a7c1 100644 --- a/boards/nxp/mr-canhubk3/src/spi.cpp +++ b/boards/nxp/mr-canhubk3/src/spi.cpp @@ -70,11 +70,12 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI3, { // SPI3 is ignored only used for FS26 by a NuttX driver initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin17}) }), - initSPIBus(SPI::Bus::SPI4, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20}) + initSPIBusExternal(SPI::Bus::SPI4, { + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin16}, SPI::DRDY{PIN_WKPU20}), + initSPIConfigExternal(SPI::CS{GPIO::PortB, GPIO::Pin8}, SPI::DRDY{PIN_WKPU56}) }), - initSPIBus(SPI::Bus::SPI5, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4}) + initSPIBusExternal(SPI::Bus::SPI5, { + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin14}, SPI::DRDY{PIN_WKPU4}) }), }; @@ -337,7 +338,14 @@ void s32k3xx_lpspi4select(FAR struct spi_dev_s *dev, uint32_t devid, spiinfo("devid: %" PRId32 ", CS: %s\n", devid, selected ? "assert" : "de-assert"); - s32k3xx_gpiowrite(PIN_LPSPI4_PCS, !selected); + devid = ((devid) & 0xF); + + if (devid == 0) { + s32k3xx_gpiowrite(PIN_LPSPI4_CS_P26, !selected); + + } else if (devid == 1) { + s32k3xx_gpiowrite(PIN_LPSPI4_CS_P8B, !selected); + } } uint8_t s32k3xx_lpspi4status(FAR struct spi_dev_s *dev, uint32_t devid) diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index eb28094dd9..f953043875 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -28,6 +28,7 @@ CONFIG_UAVCANNODE_RTK_DATA=y CONFIG_UAVCANNODE_SERVO_ARRAY_COMMAND=y CONFIG_UAVCANNODE_STATIC_PRESSURE=y CONFIG_UAVCANNODE_STATIC_TEMPERATURE=y +CONFIG_DRIVERS_UWB_UWB_SR150=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index 2e52bc7b72..0cd4598d2a 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -3,7 +3,7 @@ # board specific defaults #------------------------------------------------------------------------------ -pwm_out mode_pwm1 start +pwm_out start ifup can0 diff --git a/boards/nxp/ucans32k146/uavcan_board_identity b/boards/nxp/ucans32k146/uavcan_board_identity index dd7e3fcb77..7ff76347f8 100644 --- a/boards/nxp/ucans32k146/uavcan_board_identity +++ b/boards/nxp/ucans32k146/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index ceabc36840..287d991659 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -22,6 +22,7 @@ CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_MODULES_DATAMAN=y CONFIG_MODULES_FLIGHT_MODE_MANAGER=y CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_LOAD_MON=y CONFIG_MODULES_LOGGER=y CONFIG_MODULES_MANUAL_CONTROL=y CONFIG_MODULES_MAVLINK=y @@ -32,6 +33,9 @@ CONFIG_MODULES_NAVIGATOR=y CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set -CONFIG_MODULES_UXRCE_DDS_CLIENT=y +CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_TOP=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y diff --git a/boards/omnibus/f4sd/init/rc.board_defaults b/boards/omnibus/f4sd/init/rc.board_defaults index 5b492c4359..9e6affd345 100644 --- a/boards/omnibus/f4sd/init/rc.board_defaults +++ b/boards/omnibus/f4sd/init/rc.board_defaults @@ -14,8 +14,7 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 +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/nuttx-config/nsh/defconfig b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig index 8cdd6c1e6f..b154c61070 100644 --- a/boards/omnibus/f4sd/nuttx-config/nsh/defconfig +++ b/boards/omnibus/f4sd/nuttx-config/nsh/defconfig @@ -62,7 +62,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y CONFIG_MEMSET_64BIT=y diff --git a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig index e03b0218f4..f3d181112b 100644 --- a/boards/px4/fmu-v2/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v2/nuttx-config/nsh/defconfig @@ -103,7 +103,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -136,6 +136,8 @@ CONFIG_PRIORITY_INHERITANCE=y CONFIG_PTHREAD_MUTEX_DEFAULT_PRIO_INHERIT=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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig index c836579fe4..2c9487ddaf 100644 --- a/boards/px4/fmu-v3/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v3/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/default.px4board b/boards/px4/fmu-v4/default.px4board index f63e0618f4..0e1bb8f547 100644 --- a/boards/px4/fmu-v4/default.px4board +++ b/boards/px4/fmu-v4/default.px4board @@ -54,7 +54,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-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index a8ac1b9a34..3566ccc30c 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -140,6 +140,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/nuttx-config/test/defconfig b/boards/px4/fmu-v4/nuttx-config/test/defconfig index aa3d5890a8..3c90516de3 100644 --- a/boards/px4/fmu-v4/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/test/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4/test.px4board b/boards/px4/fmu-v4/test.px4board index 6f18488d5c..c1c005f905 100644 --- a/boards/px4/fmu-v4/test.px4board +++ b/boards/px4/fmu-v4/test.px4board @@ -1,4 +1,6 @@ CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n diff --git a/boards/px4/fmu-v4/uavcan_board_identity b/boards/px4/fmu-v4/uavcan_board_identity index 0905752e49..507d3ec578 100644 --- a/boards/px4/fmu-v4/uavcan_board_identity +++ b/boards/px4/fmu-v4/uavcan_board_identity @@ -1,6 +1,6 @@ # UAVCAN boot loadable Module ID -set(uavcanblid_sw_version_major 0) -set(uavcanblid_sw_version_minor 1) +set(uavcanblid_sw_version_major ${PX4_VERSION_MAJOR}) +set(uavcanblid_sw_version_minor ${PX4_VERSION_MINOR}) add_definitions( -DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major} -DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor} diff --git a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig index e8ecbaee38..e479b13646 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/nsh/defconfig @@ -107,7 +107,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -140,6 +140,8 @@ 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=393216 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4pro/nuttx-config/test/defconfig b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig index 7825cb6c63..a1145fe284 100644 --- a/boards/px4/fmu-v4pro/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v4pro/nuttx-config/test/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ 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=393216 CONFIG_RAM_START=0x20000000 diff --git a/boards/px4/fmu-v4pro/src/mtd.cpp b/boards/px4/fmu-v4pro/src/mtd.cpp index 4bd93619db..f32f1dee1d 100644 --- a/boards/px4/fmu-v4pro/src/mtd.cpp +++ b/boards/px4/fmu-v4pro/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi2 = { // FM25V01A on FMUM 16K +static const px4_mft_device_t spi2 = { // FM25V01A on FMUM native: 16K X 8, emulated as (512 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -46,7 +46,7 @@ static const px4_mtd_entry_t fmum_fram = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 + .nblocks = (16384 / (1 << CONFIG_RAMTRON_EMULATE_PAGE_SHIFT)) }, }, }; diff --git a/boards/px4/fmu-v5/debug.px4board b/boards/px4/fmu-v5/debug.px4board index 7e590cf43a..0b8ab3fc3e 100644 --- a/boards/px4/fmu-v5/debug.px4board +++ b/boards/px4/fmu-v5/debug.px4board @@ -28,7 +28,6 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y @@ -45,3 +44,4 @@ CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y CONFIG_DRIVERS_MAGNETOMETER_RM3100=y CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y CONFIG_DRIVERS_TEST_PPM=y +CONFIG_SYSTEMCMDS_SD_STRESS=y diff --git a/boards/px4/fmu-v5/default.px4board b/boards/px4/fmu-v5/default.px4board index fe8b0759c6..bfb8b6af46 100644 --- a/boards/px4/fmu-v5/default.px4board +++ b/boards/px4/fmu-v5/default.px4board @@ -98,8 +98,6 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig index ff995b6cf5..171589b494 100644 --- a/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cryptotest/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig b/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig index 9e3596c1f0..2c7bafc1e8 100644 --- a/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/cyphal/defconfig @@ -113,7 +113,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -148,6 +148,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/debug/defconfig b/boards/px4/fmu-v5/nuttx-config/debug/defconfig index 74f4ecac3c..08e2bb9e8a 100644 --- a/boards/px4/fmu-v5/nuttx-config/debug/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/debug/defconfig @@ -157,7 +157,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=1064 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -192,6 +192,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index b0453967b2..ae31f2b0af 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -146,6 +146,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/protected/defconfig b/boards/px4/fmu-v5/nuttx-config/protected/defconfig index 1902014e3b..7733599a7f 100644 --- a/boards/px4/fmu-v5/nuttx-config/protected/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/protected/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="px4_entry" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -152,6 +152,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig index c5450ab1f4..7350cb4532 100644 --- a/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/stackcheck/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=878 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -147,6 +147,8 @@ 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 diff --git a/boards/px4/fmu-v5/nuttx-config/test/defconfig b/boards/px4/fmu-v5/nuttx-config/test/defconfig index 357c5b29b6..21babe66f6 100644 --- a/boards/px4/fmu-v5/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/test/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -146,6 +146,8 @@ 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 diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board index de913d6ac8..e5371b2a7d 100644 --- a/boards/px4/fmu-v5/protected.px4board +++ b/boards/px4/fmu-v5/protected.px4board @@ -36,8 +36,6 @@ CONFIG_SYSTEMCMDS_MTD=n CONFIG_SYSTEMCMDS_NSHTERM=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_SYSTEMCMDS_SYSTEM_TIME=n CONFIG_SYSTEMCMDS_USB_CONNECTED=n CONFIG_BOARD_PROTECTED=y diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board index fb10b4c77a..812a0f7295 100644 --- a/boards/px4/fmu-v5/stackcheck.px4board +++ b/boards/px4/fmu-v5/stackcheck.px4board @@ -3,6 +3,8 @@ CONFIG_COMMON_BAROMETERS=n CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_HYGROMETERS=n +CONFIG_COMMON_OSD=n +CONFIG_COMMON_RC=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n CONFIG_DRIVERS_BATT_SMBUS=n @@ -35,7 +37,6 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=n CONFIG_MODULES_UUV_ATT_CONTROL=n CONFIG_MODULES_UUV_POS_CONTROL=n CONFIG_SYSTEMCMDS_REFLECT=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=n CONFIG_BOARD_CONSTRAINED_FLASH=y CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_BAROMETER_MS5611=y diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board index 6dad087c1c..2dcd6925f2 100644 --- a/boards/px4/fmu-v5/test.px4board +++ b/boards/px4/fmu-v5/test.px4board @@ -1,7 +1,14 @@ +# CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set +CONFIG_COMMON_HYGROMETERS=n +CONFIG_COMMON_OSD=n CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_BATT_SMBUS=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_DRIVERS_PCA9685_PWM_OUT=n +CONFIG_DRIVERS_UAVCAN=n CONFIG_EXAMPLES_FAKE_GPS=n CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n @@ -11,3 +18,5 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_TEST_PPM=y CONFIG_SYSTEMCMDS_MICROBENCH=y +CONFIG_SYSTEMCMDS_SD_STRESS=y +CONFIG_SYSTEMCMDS_SERIAL_TEST=y diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board index 70e35abd44..c9e9823c03 100644 --- a/boards/px4/fmu-v5/uavcanv0periph.px4board +++ b/boards/px4/fmu-v5/uavcanv0periph.px4board @@ -32,8 +32,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=n CONFIG_SYSTEMCMDS_LED_CONTROL=n CONFIG_SYSTEMCMDS_REFLECT=n CONFIG_SYSTEMCMDS_SD_BENCH=n -CONFIG_SYSTEMCMDS_SD_STRESS=n -CONFIG_SYSTEMCMDS_SERIAL_TEST=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/cmake/upload.cmake b/boards/px4/fmu-v5x/cmake/upload.cmake new file mode 100644 index 0000000000..e5c23c3e1c --- /dev/null +++ b/boards/px4/fmu-v5x/cmake/upload.cmake @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4) + +add_custom_target(upload_skynode_usb + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) + +add_custom_target(upload_skynode_wifi + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 5c662fbca8..8ba73bcdcc 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -17,6 +17,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=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_ADIS16448=y @@ -36,6 +37,7 @@ CONFIG_COMMON_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_COMMON_RC=y @@ -82,6 +84,7 @@ CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_UXRCE_DDS_CLIENT=y CONFIG_MODULES_VTOL_ATT_CONTROL=y +CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y CONFIG_SYSTEMCMDS_BSONDUMP=y diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults index ba812e4b7b..50402d17b2 100644 --- a/boards/px4/fmu-v5x/init/rc.board_defaults +++ b/boards/px4/fmu-v5x/init/rc.board_defaults @@ -19,3 +19,14 @@ param set-default SENS_EN_INA226 1 param set-default SYS_USE_IO 1 safety_button start + +# GPIO Expander driver on external I2C3 +if ver hwtypecmp V5X009000 V5X009001 +then + # No USB + mcp23009 start -b 3 -X -D 0xf0 -O 0xf0 -P 0x0f -U 10 +fi +if ver hwtypecmp V5X00a000 V5X00a001 V5X008000 V5X008001 +then + mcp23009 start -b 3 -X -D 0xf1 -O 0xf0 -P 0x0f -U 10 +fi diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink index dad84376ce..ae59f79dff 100644 --- a/boards/px4/fmu-v5x/init/rc.board_mavlink +++ b/boards/px4/fmu-v5x/init/rc.board_mavlink @@ -7,4 +7,7 @@ if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X then # Start MAVLink on the UART connected to the mission computer mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z + + # Ensure nothing else starts on TEL2 (ttyS4) + set PRT_TEL2_ 1 fi diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors index 4f7c1d078d..d6b91f4505 100644 --- a/boards/px4/fmu-v5x/init/rc.board_sensors +++ b/boards/px4/fmu-v5x/init/rc.board_sensors @@ -94,6 +94,9 @@ else # Internal magnetometer on I2c bmm150 -I -R 6 start + # Auto start power monitors + pm_selector_auterion start + fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 5de6af261f..8c21923c64 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -185,6 +185,8 @@ 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 diff --git a/boards/px4/fmu-v5x/nuttx-config/test/defconfig b/boards/px4/fmu-v5x/nuttx-config/test/defconfig index 2b8361d327..12333125f9 100644 --- a/boards/px4/fmu-v5x/nuttx-config/test/defconfig +++ b/boards/px4/fmu-v5x/nuttx-config/test/defconfig @@ -116,7 +116,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -183,6 +183,8 @@ 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 diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h index 281c769f26..67cac1acd3 100644 --- a/boards/px4/fmu-v5x/src/board_config.h +++ b/boards/px4/fmu-v5x/src/board_config.h @@ -250,8 +250,8 @@ #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/gpin4" -#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpin5" +#define BOARD_GPIO_VDD_5V_COMP_VALID "/dev/gpio4" +#define BOARD_GPIO_VDD_5V_CAN1_GPS1_VALID "/dev/gpio5" /* Spare GPIO */ diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp index a6606761db..5987f66ffa 100644 --- a/boards/px4/fmu-v5x/src/init.cpp +++ b/boards/px4/fmu-v5x/src/init.cpp @@ -264,8 +264,9 @@ __EXPORT int board_app_initialize(uintptr_t arg) led_on(LED_RED); } + int ret; #ifdef CONFIG_MMCSD - int ret = stm32_sdio_initialize(); + ret = stm32_sdio_initialize(); if (ret != OK) { led_on(LED_RED); @@ -274,30 +275,11 @@ __EXPORT int board_app_initialize(uintptr_t arg) #endif /* CONFIG_MMCSD */ - int hw_version = board_get_hw_version(); + ret = mcp23009_register_gpios(3, 0x25); - if (hw_version == 0x9 || hw_version == 0xa) { - static MCP23009 mcp23009{3, 0x25}; - - // No USB - if (hw_version == 0x9) { - // < P8 - ret = mcp23009.init(0xf0, 0xf0, 0x0f); - // >= P8 - //ret = mcp23009.init(0xf1, 0xf0, 0x0f); - } - - if (hw_version == 0xa) { - // < P6 - //ret = mcp23009.init(0xf0, 0xf0, 0x0f); - // >= P6 - ret = mcp23009.init(0xf1, 0xf0, 0x0f); - } - - if (ret != OK) { - led_on(LED_RED); - return ret; - } + if (ret != OK) { + led_on(LED_RED); + return ret; } return OK; diff --git a/boards/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp index fc1e97d2c8..c0139d1232 100644 --- a/boards/px4/fmu-v5x/src/mtd.cpp +++ b/boards/px4/fmu-v5x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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) }; @@ -50,18 +50,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board index 87fa5f8101..278cc285a1 100644 --- a/boards/px4/fmu-v5x/test.px4board +++ b/boards/px4/fmu-v5x/test.px4board @@ -2,6 +2,8 @@ CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_COMMON_INS=n CONFIG_COMMON_TELEMETRY=n CONFIG_DRIVERS_ADC_ADS1115=n +CONFIG_DRIVERS_CAMERA_CAPTURE=n +CONFIG_DRIVERS_CAMERA_TRIGGER=n CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n CONFIG_DRIVERS_IRLOCK=n CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n diff --git a/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig index d3a1497dd5..a5269376c6 100644 --- a/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6c/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig index 78963cf699..9e051bd1f9 100644 --- a/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6c/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -151,6 +151,8 @@ 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 diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp index b315ff399d..5efe7bac64 100644 --- a/boards/px4/fmu-v6c/src/mtd.cpp +++ b/boards/px4/fmu-v6c/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi2 = { // FM25V02A on FMUM 32K 512 X 64 +static const px4_mft_device_t spi2 = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; @@ -47,18 +47,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi2, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig index 745b587413..0ca6729cfa 100644 --- a/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/bootloader/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig index f716ae6932..40513352ae 100644 --- a/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6u/nuttx-config/nsh/defconfig @@ -112,7 +112,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_NCHAINS=24 CONFIG_LIBC_FLOATINGPOINT=y @@ -151,6 +151,8 @@ 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 diff --git a/boards/px4/fmu-v6u/src/mtd.cpp b/boards/px4/fmu-v6u/src/mtd.cpp index 644b96581d..48a5bd334c 100644 --- a/boards/px4/fmu-v6u/src/mtd.cpp +++ b/boards/px4/fmu-v6u/src/mtd.cpp @@ -34,25 +34,19 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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 = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6x/cmake/upload.cmake b/boards/px4/fmu-v6x/cmake/upload.cmake new file mode 100644 index 0000000000..7319b46df3 --- /dev/null +++ b/boards/px4/fmu-v6x/cmake/upload.cmake @@ -0,0 +1,49 @@ +############################################################################ +# +# 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. +# +############################################################################ + + +set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4) + +add_custom_target(upload_skynode_usb + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) + +add_custom_target(upload_skynode_wifi + COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh --file=${PX4_FW_NAME} --wifi + DEPENDS ${PX4_FW_NAME} + COMMENT "Uploading PX4" + USES_TERMINAL +) diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board index ba22d9296c..9dc9d9a444 100644 --- a/boards/px4/fmu-v6x/default.px4board +++ b/boards/px4/fmu-v6x/default.px4board @@ -7,11 +7,11 @@ CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6" CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4" CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1" CONFIG_BOARD_SERIAL_EXT2="/dev/ttyS3" +CONFIG_DRIVERS_ADC_ADS1115=y CONFIG_DRIVERS_ADC_BOARD_ADC=y CONFIG_DRIVERS_BAROMETER_BMP388=y CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP201XX=y CONFIG_DRIVERS_BAROMETER_MS5611=y -CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y @@ -19,20 +19,23 @@ CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y +CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y -CONFIG_COMMON_OPTICAL_FLOW=y CONFIG_COMMON_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 @@ -41,6 +44,7 @@ CONFIG_DRIVERS_TONE_ALARM=y CONFIG_DRIVERS_UAVCAN=y CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2 CONFIG_MODULES_AIRSPEED_SELECTOR=y +CONFIG_MODULES_BATTERY_STATUS=y CONFIG_MODULES_CAMERA_FEEDBACK=y CONFIG_MODULES_COMMANDER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink new file mode 100644 index 0000000000..6001553a76 --- /dev/null +++ b/boards/px4/fmu-v6x/init/rc.board_mavlink @@ -0,0 +1,13 @@ +#!/bin/sh +# +# PX4 FMUv6X specific board MAVLink startup script. +#------------------------------------------------------------------------------ + +# if skynode base board is detected start Mavlink on Telem2 +if ver hwtypecmp V6X009010 V6X010010 +then + mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z + + # Ensure nothing else starts on TEL2 (ttyS4) + set PRT_TEL2_ 1 +fi diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors index 52aa8f9c78..5155fcf96c 100644 --- a/boards/px4/fmu-v6x/init/rc.board_sensors +++ b/boards/px4/fmu-v6x/init/rc.board_sensors @@ -47,25 +47,38 @@ then fi fi - -if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 +#Start Auterion Power Module selector for Skynode boards +if ver hwtypecmp V6X009010 V6X010010 then - # Internal SPI bus ICM20649 - icm20649 -s -R 6 start + pm_selector_auterion start +fi + +if ver hwtypecmp V6X000006 V6X004006 V6X005006 +then + # Internal SPI bus ICM45686 + icm45686 -s -R 10 start + iim42652 -s -R 6 start + adis16470 -s -R 0 start else - # Internal SPI BMI088 - if ver hwtypecmp V6X009010 V6X010010 + if ver hwtypecmp V6X000004 V6X001004 V6X004004 V6X005004 then - bmi088 -A -R 6 -s start - bmi088 -G -R 6 -s start + # Internal SPI bus ICM20649 + icm20649 -s -R 6 start else - if ver hwtypecmp V6X000010 + # Internal SPI BMI088 + if ver hwtypecmp V6X009010 V6X010010 then - bmi088 -A -R 0 -s start - bmi088 -G -R 0 -s start + bmi088 -A -R 6 -s start + bmi088 -G -R 6 -s start else - bmi088 -A -R 4 -s start - bmi088 -G -R 4 -s start + if ver hwtypecmp V6X000010 + then + bmi088 -A -R 0 -s start + bmi088 -G -R 0 -s start + else + bmi088 -A -R 4 -s start + bmi088 -G -R 4 -s start + fi fi fi fi @@ -102,13 +115,8 @@ if ver hwtypecmp V6X002001 then rm3100 -I -b 4 start else - if ver hwtypecmp V6X009010 V6X010010 - then - # Internal magnetometer on I2C - bmm150 -I -R 0 start - else - bmm150 -I -R 6 start - fi + # Internal magnetometer on I2C + bmm150 -I -R 0 start fi # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) @@ -117,7 +125,7 @@ ist8310 -X -b 1 -R 10 start # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then - if ver hwtypecmp V6X002001 + if ver hwtypecmp V6X002001 V6X000006 V6X004006 V6X005006 then icp201xx -I -a 0x64 start else diff --git a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig index c2069035c5..41dc164e0a 100644 --- a/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/bootloader/defconfig @@ -49,7 +49,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="bootloader_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_STRERROR=y diff --git a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig index a38915d99f..e5f02a46a7 100644 --- a/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v6x/nuttx-config/nsh/defconfig @@ -118,7 +118,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -189,6 +189,8 @@ 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 @@ -291,7 +293,6 @@ CONFIG_UART4_RXBUFSIZE=600 CONFIG_UART4_TXBUFSIZE=1500 CONFIG_UART5_IFLOWCONTROL=y CONFIG_UART5_OFLOWCONTROL=y -CONFIG_UART7_RXBUFSIZE=1500 CONFIG_UART5_RXDMA=y CONFIG_UART5_TXBUFSIZE=10000 CONFIG_UART5_TXDMA=y diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h index fecd7efc9c..ff0676fab7 100644 --- a/boards/px4/fmu-v6x/src/board_config.h +++ b/boards/px4/fmu-v6x/src/board_config.h @@ -215,24 +215,28 @@ #define GPIO_HW_VER_SENSE /* PH3 */ GPIO_ADC3_INP14 #define HW_INFO_INIT_PREFIX "V6X" -#define BOARD_NUM_SPI_CFG_HW_VERSIONS 11 // Rev 0 and Rev 3,4 Sensor sets +#define BOARD_NUM_SPI_CFG_HW_VERSIONS 13 // Rev 0 and Rev 3,4,6 Sensor sets // Base/FMUM #define V6X00 HW_VER_REV(0x0,0x0) // FMUV6X, Rev 0 #define V6X01 HW_VER_REV(0x0,0x1) // FMUV6X, BMI388 I2C2 Rev 1 #define V6X03 HW_VER_REV(0x0,0x3) // FMUV6X, Sensor Set Rev 3 #define V6X04 HW_VER_REV(0x0,0x4) // FMUV6X, Sensor Set Rev 4 +#define V6X06 HW_VER_REV(0x0,0x6) // FMUV6X, Sensor Set Rev 6 #define V6X10 HW_VER_REV(0x1,0x0) // NO PX4IO, Rev 0 #define V6X13 HW_VER_REV(0x1,0x3) // NO PX4IO, Sensor Set Rev 3 #define V6X14 HW_VER_REV(0x1,0x4) // NO PX4IO, Sensor Set Rev 4 +#define V6X16 HW_VER_REV(0x1,0x6) // NO PX4IO, Sensor Set Rev 6 #define V6X21 HW_VER_REV(0x2,0x1) // FMUV6X, CUAV Sensor Set #define V6X40 HW_VER_REV(0x4,0x0) // FMUV6X, HB CM4 base Rev 0 #define V6X41 HW_VER_REV(0x4,0x1) // FMUV6X, BMI388 I2C2 HB CM4 base Rev 1 #define V6X43 HW_VER_REV(0x4,0x3) // FMUV6X, Sensor Set HB CM4 base Rev 3 #define V6X44 HW_VER_REV(0x4,0x4) // FMUV6X, Sensor Set HB CM4 base Rev 4 +#define V6X46 HW_VER_REV(0x4,0x6) // FMUV6X, Sensor Set HB CM4 base Rev 6 #define V6X50 HW_VER_REV(0x5,0x0) // FMUV6X, HB Mini Rev 0 #define V6X51 HW_VER_REV(0x5,0x1) // FMUV6X, BMI388 I2C2 HB Mini Rev 1 #define V6X53 HW_VER_REV(0x5,0x3) // FMUV6X, Sensor Set HB Mini Rev 3 #define V6X54 HW_VER_REV(0x5,0x4) // FMUV6X, Sensor Set HB Mini Rev 4 +#define V6X56 HW_VER_REV(0x5,0x6) // FMUV6X, Sensor Set HB Mini Rev 6 #define V6X90 HW_VER_REV(0x9,0x0) // Rev 0 #define V6X0910 HW_VER_REV(0x9,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver9 #define V6X1010 HW_VER_REV(0x10,0x10) // FMUV6X, rev from EEPROM Auterion Skynode ver10 diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c index 313d49f9bf..bda800b749 100644 --- a/boards/px4/fmu-v6x/src/manifest.c +++ b/boards/px4/fmu-v6x/src/manifest.c @@ -142,17 +142,20 @@ static px4_hw_mft_list_entry_t mft_lists[] = { {V6X00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X01, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 {V6X03, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 3 + {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 + {V6X06, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X40, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // HB CM4 base {V6X41, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2 HB CM4 base {V6X43, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 3 {V6X44, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, HB CM4 base Sensor Set 4 + {V6X46, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 6 {V6X50, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // HB Mini {V6X51, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2 HB Mini {V6X53, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 3 {V6X54, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 4 + {V6X56, hw_mft_list_v0650, arraySize(hw_mft_list_v0650)}, // BMP388 moved to I2C2, HB Mini Sensor Set 6 {V6X10, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO {V6X13, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 3 - {V6X04, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // BMP388 moved to I2C2, Sensor Set 4 {V6X14, hw_mft_list_v0610, arraySize(hw_mft_list_v0610)}, // No PX4IO BMP388 moved to I2C2, Sensor Set 4 {V6X21, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, {V6X0910, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)}, // FMUV6X, rev from EEPROM Auterion Skynode ver9 diff --git a/boards/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp index fc1e97d2c8..c0139d1232 100644 --- a/boards/px4/fmu-v6x/src/mtd.cpp +++ b/boards/px4/fmu-v6x/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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) }; @@ -50,18 +50,12 @@ static const px4_mft_device_t i2c4 = { // 24LC64T on IMU 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/px4/fmu-v6x/src/spi.cpp b/boards/px4/fmu-v6x/src/spi.cpp index d3bbdb9315..8b002e157e 100644 --- a/boards/px4/fmu-v6x/src/spi.cpp +++ b/boards/px4/fmu-v6x/src/spi.cpp @@ -84,6 +84,29 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X06, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X21, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -132,6 +155,52 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), + initSPIHWVersion(V6X44, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, 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}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + + initSPIHWVersion(V6X46, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X50, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -156,75 +225,6 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION }), }), - initSPIHWVersion(V6X44, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, 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}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // 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}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), - - // never shipped - //initSPIHWVersion(V6X50, { - // initSPIBus(SPI::Bus::SPI1, { - // initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, 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}), - // }, {GPIO::PortF, GPIO::Pin4}), - // initSPIBus(SPI::Bus::SPI3, { - // initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - // initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin6}), - // }, {GPIO::PortE, GPIO::Pin7}), - // // 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}) - // }), - // initSPIBusExternal(SPI::Bus::SPI6, { - // initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - // initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - // }), - //}), - initSPIHWVersion(V6X04, { - initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, 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}), - }, {GPIO::PortF, GPIO::Pin4}), - initSPIBus(SPI::Bus::SPI3, { - initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), - }, {GPIO::PortE, GPIO::Pin7}), - // 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}) - }), - initSPIBusExternal(SPI::Bus::SPI6, { - initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), - initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), - }), - }), initSPIHWVersion(V6X53, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -248,6 +248,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X54, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM42670P, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -270,6 +271,30 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + + initSPIHWVersion(V6X56, { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM45686, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), + }, {GPIO::PortI, GPIO::Pin11}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(DRV_IMU_DEVTYPE_IIM42652, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}), + }, {GPIO::PortF, GPIO::Pin4}), + initSPIBus(SPI::Bus::SPI3, { + initSPIDevice(DRV_IMU_DEVTYPE_ADIS16470, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}), + }, {GPIO::PortE, GPIO::Pin7}), + // 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}) + }), + initSPIBusExternal(SPI::Bus::SPI6, { + initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}), + initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), + }), + }), + initSPIHWVersion(V6X0910, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), @@ -293,6 +318,7 @@ constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSION initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}), }), }), + initSPIHWVersion(V6X1010, { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}), diff --git a/boards/px4/raspberrypi/cmake/upload.cmake b/boards/px4/raspberrypi/cmake/upload.cmake index c77e80cd2e..5abefb01e6 100644 --- a/boards/px4/raspberrypi/cmake/upload.cmake +++ b/boards/px4/raspberrypi/cmake/upload.cmake @@ -38,7 +38,7 @@ else() endif() add_custom_target(upload - COMMAND rsync -arh --progress + COMMAND rsync -arh --progress -e "ssh -o StrictHostKeyChecking=no" ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} ${PX4_SOURCE_DIR}/posix-configs/rpi/*.config ${PX4_BINARY_DIR}/etc # source pi@${AUTOPILOT_HOST}:/home/pi/px4 # destination DEPENDS px4 diff --git a/boards/px4/raspberrypi/default.px4board b/boards/px4/raspberrypi/default.px4board index 3e059cffe7..ade7dd4235 100644 --- a/boards/px4/raspberrypi/default.px4board +++ b/boards/px4/raspberrypi/default.px4board @@ -4,15 +4,15 @@ CONFIG_BOARD_TOOLCHAIN="arm-linux-gnueabihf" CONFIG_BOARD_ARCHITECTURE="cortex-a53" CONFIG_BOARD_TESTING=y CONFIG_DRIVERS_ADC_ADS1115=y -CONFIG_DRIVERS_BAROMETER_MS5611=y +CONFIG_COMMON_BAROMETERS=y CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_GPS=y +CONFIG_COMMON_IMU=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y -CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y -CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y +CONFIG_COMMON_MAGNETOMETER=y CONFIG_DRIVERS_PCA9685_PWM_OUT=y CONFIG_DRIVERS_RC_INPUT=y CONFIG_DRIVERS_RPI_RC_IN=y diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board index db358c3e4d..fa7276f76e 100644 --- a/boards/px4/sitl/default.px4board +++ b/boards/px4/sitl/default.px4board @@ -49,6 +49,7 @@ 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_MODE_NAVIGATOR_VTOL_TAKEOFF=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BSONDUMP=y CONFIG_SYSTEMCMDS_DYN=y diff --git a/boards/raspberrypi/pico/init/rc.board_defaults b/boards/raspberrypi/pico/init/rc.board_defaults index ec0c720608..f1a26f4a51 100644 --- a/boards/raspberrypi/pico/init/rc.board_defaults +++ b/boards/raspberrypi/pico/init/rc.board_defaults @@ -12,10 +12,3 @@ 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_ACC_COMP 0 -# param set-default ATT_W_ACC 0.4000 -# param set-default ATT_W_GYRO_BIAS 0.0000 - -# param set-default SYS_HAS_MAG 0 diff --git a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig index 2efb22fc35..ebce067020 100644 --- a/boards/raspberrypi/pico/nuttx-config/nsh/defconfig +++ b/boards/raspberrypi/pico/nuttx-config/nsh/defconfig @@ -48,7 +48,7 @@ CONFIG_HAVE_CXX=y CONFIG_HAVE_CXXINITIALIZE=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y diff --git a/boards/siyi/n7/bootloader.px4board b/boards/siyi/n7/bootloader.px4board new file mode 100644 index 0000000000..19b6e662be --- /dev/null +++ b/boards/siyi/n7/bootloader.px4board @@ -0,0 +1,3 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_ROMFSROOT="" diff --git a/boards/siyi/n7/default.px4board b/boards/siyi/n7/default.px4board new file mode 100644 index 0000000000..cb42f3530c --- /dev/null +++ b/boards/siyi/n7/default.px4board @@ -0,0 +1,86 @@ +CONFIG_BOARD_TOOLCHAIN="arm-none-eabi" +CONFIG_BOARD_ARCHITECTURE="cortex-m7" +CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0" +CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1" +CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS2" +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_COMMON_DIFFERENTIAL_PRESSURE=y +CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_HEATER=y +CONFIG_DRIVERS_IMU_BOSCH_BMI088=y +CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=y +CONFIG_COMMON_LIGHT=y +CONFIG_COMMON_MAGNETOMETER=y +CONFIG_COMMON_OPTICAL_FLOW=y +CONFIG_DRIVERS_POWER_MONITOR_INA226=y +CONFIG_DRIVERS_PWM_OUT=y +CONFIG_DRIVERS_PX4IO=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_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_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_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 diff --git a/boards/siyi/n7/extras/px4_io-v2_default.bin b/boards/siyi/n7/extras/px4_io-v2_default.bin new file mode 100755 index 0000000000..957f0f13a1 Binary files /dev/null and b/boards/siyi/n7/extras/px4_io-v2_default.bin differ diff --git a/boards/siyi/n7/extras/siyi_n7_bootloader.bin b/boards/siyi/n7/extras/siyi_n7_bootloader.bin new file mode 100755 index 0000000000..6f0ff949f5 Binary files /dev/null and b/boards/siyi/n7/extras/siyi_n7_bootloader.bin differ diff --git a/boards/siyi/n7/firmware.prototype b/boards/siyi/n7/firmware.prototype new file mode 100644 index 0000000000..b4c1d9362a --- /dev/null +++ b/boards/siyi/n7/firmware.prototype @@ -0,0 +1,13 @@ +{ + "board_id": 1123, + "magic": "PX4FWv1", + "description": "Firmware for the N7 board", + "image": "", + "build_time": 0, + "summary": "N7", + "version": "0.1", + "image_size": 0, + "image_maxsize": 1966080, + "git_identity": "", + "board_revision": 0 +} diff --git a/boards/siyi/n7/init/rc.board_defaults b/boards/siyi/n7/init/rc.board_defaults new file mode 100644 index 0000000000..a7d3d20bf9 --- /dev/null +++ b/boards/siyi/n7/init/rc.board_defaults @@ -0,0 +1,14 @@ +#!/bin/sh +# +# board specific defaults +#------------------------------------------------------------------------------ +param set-default MAV_0_RATE 100000 + +param set-default BAT1_V_DIV 18.1 + +param set-default BAT1_A_PER_V 36.367515152 + +# Enable IMU thermal control +param set-default SENS_EN_THERMAL 1 + +param set-default SYS_USE_IO 1 diff --git a/boards/siyi/n7/init/rc.board_sensors b/boards/siyi/n7/init/rc.board_sensors new file mode 100644 index 0000000000..e790785884 --- /dev/null +++ b/boards/siyi/n7/init/rc.board_sensors @@ -0,0 +1,21 @@ +#!/bin/sh +# +# board specific sensors init +#------------------------------------------------------------------------------ +board_adc start + +# SPI1 +bmi088 -s -b 1 -A -R 2 start +bmi088 -s -b 1 -G -R 2 start + +# SPI1 +icm20689 -s -b 1 -R 2 start + +# I2C1 +ist8310 -X -b 1 -R 10 -a 0xE start + +# I2C3 +ist8310 -I -b 3 -R 10 -a 0xE start + +# SPI4 +ms5611 -s -b 4 start diff --git a/boards/siyi/n7/nuttx-config/Kconfig b/boards/siyi/n7/nuttx-config/Kconfig new file mode 100644 index 0000000000..bb33d3cfda --- /dev/null +++ b/boards/siyi/n7/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/siyi/n7/nuttx-config/bootloader/defconfig b/boards/siyi/n7/nuttx-config/bootloader/defconfig new file mode 100644 index 0000000000..775acfb2a0 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/bootloader/defconfig @@ -0,0 +1,93 @@ +# +# 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/siyi/n7/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=22114 +CONFIG_BOARD_RESET_ON_ASSERT=2 +CONFIG_CDCACM=y +CONFIG_CDCACM_IFLOWCONTROL=y +CONFIG_CDCACM_PRODUCTID=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 BL Siyi N7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Siyi" +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_DMA2=y +CONFIG_STM32H7_OTGFS=y +CONFIG_STM32H7_PROGMEM=y +CONFIG_STM32H7_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32H7_TIM1=y +CONFIG_STM32H7_UART8=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_UART8_RXBUFSIZE=512 +CONFIG_UART8_RXDMA=y +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_TXDMA=y +CONFIG_USBDEV=y +CONFIG_USBDEV_BUSPOWERED=y +CONFIG_USBDEV_MAXPOWER=500 +CONFIG_USEC_PER_TICK=1000 diff --git a/boards/siyi/n7/nuttx-config/include/board.h b/boards/siyi/n7/nuttx-config/include/board.h new file mode 100644 index 0000000000..d6b690c293 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/include/board.h @@ -0,0 +1,381 @@ +/************************************************************************************ + * nuttx-config/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. + * + ************************************************************************************/ +#pragma once + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#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: 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 + */ + + + +#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_ADCSEL 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 + +/* 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 + +/* LED definitions ******************************************************************/ +/* The 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_USART2_RX GPIO_USART2_RX_2 /* PD6 */ +#define GPIO_USART2_TX GPIO_USART2_TX_2 /* PD5 */ +#define GPIO_USART2_RTS GPIO_USART2_RTS_2 /* PD4 */ +#define GPIO_USART2_CTS GPIO_USART2_CTS_NSS_2 /* PD3 */ + + +#define GPIO_UART4_RX GPIO_UART4_RX_5 /* PD0 */ +#define GPIO_UART4_TX GPIO_UART4_TX_5 /* PD1 */ + + + +#define GPIO_UART7_RX GPIO_UART7_RX_4 /* PF6 */ +#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_5 /* PI9 */ +#define GPIO_CAN1_TX GPIO_CAN1_TX_4 /* PH13 */ + +/* SPI + * SPI1 sensors + * SPI2 is FRAM. + * SPI4 is BARO + */ + +#define GPIO_SPI1_MISO GPIO_SPI1_MISO_1 /* PA6 */ +#define GPIO_SPI1_MOSI GPIO_SPI1_MOSI_3 /* PD7 */ +#define GPIO_SPI1_SCK GPIO_SPI1_SCK_3 /* PG11 */ + +#define GPIO_SPI2_MISO GPIO_SPI2_MISO_3 /* PI2 */ +#define GPIO_SPI2_MOSI GPIO_SPI2_MOSI_4 /* PI3 */ +#define GPIO_SPI2_SCK GPIO_SPI2_SCK_6 /* PI1 */ + +#define GPIO_SPI4_MISO GPIO_SPI4_MISO_1 /* PE13 */ +#define GPIO_SPI4_MOSI GPIO_SPI4_MOSI_2 /* PE6 */ +#define GPIO_SPI4_SCK GPIO_SPI4_SCK_2 /* PE2 */ + + + +/* I2C + * + * Each I2C is associated with a U[S]ART + * hence the naming I2C2_SDA_UART4 in FMU USAGE spreadsheet + * + * 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_I2C2_SCL GPIO_I2C2_SCL_2 /* PF1 */ +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_2 /* PF0 */ + +#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_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 new file mode 100644 index 0000000000..6577106c23 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/include/board_dma_map.h @@ -0,0 +1,45 @@ +/**************************************************************************** + * + * 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 + +// DMAMUX1 +#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 /* 3 DMA1:39 FRAM */ +#define DMAMAP_SPI2_TX DMAMAP_DMA12_SPI2TX_0 /* 4 DMA1:40 FRAM */ + +#define DMAMAP_UART8_RX DMAMAP_DMA12_UART8RX_0 /* DMA1:81 */ +#define DMAMAP_UART8_TX DMAMAP_DMA12_UART8TX_0 /* DMA1:82 */ + diff --git a/boards/siyi/n7/nuttx-config/nsh/defconfig b/boards/siyi/n7/nuttx-config/nsh/defconfig new file mode 100644 index 0000000000..f2f14e7cc9 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/nsh/defconfig @@ -0,0 +1,252 @@ +# +# 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/siyi/n7/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=0x0038 +CONFIG_CDCACM_PRODUCTSTR="PX4 N7" +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=12000 +CONFIG_CDCACM_VENDORID=0x3185 +CONFIG_CDCACM_VENDORSTR="Siyi" +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=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_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_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_TIM1=y +CONFIG_STM32H7_TIM3=y +CONFIG_STM32H7_TIM4=y +CONFIG_STM32H7_UART4=y +CONFIG_STM32H7_UART7=y +CONFIG_STM32H7_UART8=y +CONFIG_STM32H7_USART1=y +CONFIG_STM32H7_USART2=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_SERIAL_CONSOLE=y +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_IFLOWCONTROL=y +CONFIG_USART2_OFLOWCONTROL=y +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_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/siyi/n7/nuttx-config/scripts/bootloader_script.ld b/boards/siyi/n7/nuttx-config/scripts/bootloader_script.ld new file mode 100644 index 0000000000..43d36e7dc9 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/scripts/bootloader_script.ld @@ -0,0 +1,215 @@ +/**************************************************************************** + * 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 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 + * + * The board 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 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. + * + * 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/siyi/n7/nuttx-config/scripts/script.ld b/boards/siyi/n7/nuttx-config/scripts/script.ld new file mode 100644 index 0000000000..d6019c0d13 --- /dev/null +++ b/boards/siyi/n7/nuttx-config/scripts/script.ld @@ -0,0 +1,229 @@ +/**************************************************************************** + * 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 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 + * + * The board 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 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) +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/siyi/n7/src/CMakeLists.txt b/boards/siyi/n7/src/CMakeLists.txt new file mode 100644 index 0000000000..c1386c4181 --- /dev/null +++ b/boards/siyi/n7/src/CMakeLists.txt @@ -0,0 +1,70 @@ +############################################################################ +# +# 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_library(drivers_board + bootloader_main.c + usb.c + ) + target_link_libraries(drivers_board + PRIVATE + nuttx_arch # sdio + nuttx_drivers # sdio + 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 + manifest.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 # drv_led_start + nuttx_arch # sdio + nuttx_drivers # sdio + px4_layer + ) +endif() diff --git a/boards/siyi/n7/src/board_config.h b/boards/siyi/n7/src/board_config.h new file mode 100644 index 0000000000..e705c87890 --- /dev/null +++ b/boards/siyi/n7/src/board_config.h @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * 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 board_config.h + * + * Board internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +#include + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* PX4IO connection configuration */ + +#define BOARD_USES_PX4IO_VERSION 2 +#define PX4IO_SERIAL_DEVICE "/dev/ttyS4" +#define PX4IO_SERIAL_TX_GPIO GPIO_UART8_TX +#define PX4IO_SERIAL_RX_GPIO GPIO_UART8_RX +#define PX4IO_SERIAL_BASE STM32_UART8_BASE +#define PX4IO_SERIAL_VECTOR STM32_IRQ_UART8 +#define PX4IO_SERIAL_TX_DMAMAP DMAMAP_UART8_TX +#define PX4IO_SERIAL_RX_DMAMAP DMAMAP_UART8_RX +#define PX4IO_SERIAL_RCC_REG STM32_RCC_APB1LENR +#define PX4IO_SERIAL_RCC_EN RCC_APB1LENR_UART8EN +#define PX4IO_SERIAL_CLOCK STM32_PCLK1_FREQUENCY +#define PX4IO_SERIAL_BITRATE 1500000 /* 1.5Mbps -> max rate for IO */ + +/* LEDs are driven with push open drain to support Anode to 5V or 3.3V */ + +#define GPIO_nLED_RED /* PB1 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1) +#define GPIO_nLED_BLUE /* PC7 */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN7) + +#define BOARD_HAS_CONTROL_STATUS_LEDS 1 +#define BOARD_OVERLOAD_LED LED_RED +#define BOARD_ARMED_STATE_LED LED_BLUE + +/* ADC channels */ + + + +/* Define GPIO pins used as ADC N.B. Channel numbers must match below */ + +#define PX4_ADC_GPIO \ + /* PA0 */ GPIO_ADC1_INP16, \ + /* PA1 */ GPIO_ADC1_INP17, \ + /* PB0 */ GPIO_ADC12_INP9, \ + /* PC0 */ GPIO_ADC123_INP10, \ + /* PC1 */ GPIO_ADC123_INP11, \ + /* PC2 */ GPIO_ADC123_INP12, \ + /* PC3 */ GPIO_ADC12_INP13 + + +/* Define Channel numbers must match above GPIO pin IN(n)*/ + +#define ADC_BATTERY_VOLTAGE_CHANNEL /* PA0 */ 16 +#define ADC_BATTERY_CURRENT_CHANNEL /* PA1 */ 17 +#define ADC_RSSI_IN_CHANNEL /* PB0 */ 9 +#define ADC_SCALED_V5_CHANNEL /* PC0 */ 10 +#define ADC_SCALED_VDD_3V3_SENSORS_CHANNEL /* PC1 */ 11 +#define ADC_HW_VER_SENSE_CHANNEL /* PC2 */ 12 +#define ADC_HW_REV_SENSE_CHANNEL /* PC3 */ 13 + +#define ADC_CHANNELS \ + ((1 << ADC_BATTERY_VOLTAGE_CHANNEL) | \ + (1 << ADC_BATTERY_CURRENT_CHANNEL) | \ + (1 << ADC_RSSI_IN_CHANNEL) | \ + (1 << ADC_SCALED_V5_CHANNEL) | \ + (1 << ADC_SCALED_VDD_3V3_SENSORS_CHANNEL) | \ + (1 << ADC_HW_VER_SENSE_CHANNEL) | \ + (1 << ADC_HW_REV_SENSE_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) + +/* HW Version and Revision drive signals Default to 1 to detect */ + +#define BOARD_HAS_HW_VERSIONING + +#define GPIO_HW_REV_DRIVE /* PH14 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTH|GPIO_PIN14) +#define GPIO_HW_REV_SENSE /* PC3 */ GPIO_ADC12_INP13 +#define GPIO_HW_VER_DRIVE /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0) +#define GPIO_HW_VER_SENSE /* PC2 */ GPIO_ADC123_INP12 +#define HW_INFO_INIT_PREFIX "VD" + + +#define VER00 HW_VER_REV(0x0,0x0) + +/* CAN Silence + * + * Silent mode control \ ESC Mux select + */ + +#define GPIO_CAN1_SILENT_S0 /* PH2 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTH|GPIO_PIN2) + +/* HEATER + * PWM in future + */ +#define GPIO_HEATER_OUTPUT /* PA7 T14CH1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7) +#define HEATER_OUTPUT_EN(on_true) px4_arch_gpiowrite(GPIO_HEATER_OUTPUT, (on_true)) + +/* PWM + */ +#define DIRECT_PWM_OUTPUT_CHANNELS 5 + +#define BOARD_NUM_IO_TIMERS 2 + + +/* Power supply control and monitoring GPIOs */ + +#define GPIO_nPOWER_IN_A /* PG1 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN1) +#define GPIO_nPOWER_IN_B /* PG3 */ (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN3) + +#define GPIO_nVDD_BRICK_VALID GPIO_nPOWER_IN_A /* Brick 1 Is Chosen */ +#define BOARD_NUMBER_BRICKS 1 +#define GPIO_nVDD_USB_VALID GPIO_nPOWER_IN_B /* USB Is Chosen */ + +#define GPIO_VDD_5V_PERIPH_nEN /* PG4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN4) +#define GPIO_VDD_5V_PERIPH_nOC /* PE15 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTE|GPIO_PIN15) +#define GPIO_VDD_5V_HIPOWER_nEN /* PF12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTF|GPIO_PIN12) +#define GPIO_VDD_5V_HIPOWER_nOC /* PG13 */ (GPIO_INPUT |GPIO_FLOAT|GPIO_PORTF|GPIO_PIN13) +#define GPIO_VDD_3V3_SD_CARD_EN /* PG7 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTG|GPIO_PIN7) + + +/* Define True logic Power Control in arch agnostic form */ + +#define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +#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 15 /* timer 15 */ +#define TONE_ALARM_CHANNEL 1 /* PE5 TIM15_CH1 */ + +#define GPIO_BUZZER_1 /* PE5 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN5) + +#define GPIO_TONE_ALARM_IDLE GPIO_BUZZER_1 +#define GPIO_TONE_ALARM GPIO_TIM15_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) + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer8 for the HRT */ +#define HRT_TIMER_CHANNEL 3 /* use capture/compare channel 3 */ + +/* 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 + +#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)) +#define BOARD_ADC_USB_VALID (!px4_arch_gpioread(GPIO_nVDD_USB_VALID)) + +/* Board never powers off the Servo rail */ + +#define BOARD_ADC_SERVO_VALID (1) + +#define BOARD_ADC_BRICK_VALID (!px4_arch_gpioread(GPIO_nVDD_BRICK_VALID)) + + +#define BOARD_ADC_PERIPH_5V_OC (!px4_arch_gpioread(GPIO_VDD_5V_PERIPH_nOC)) +#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 +#define SDMMC_PIN_OFF(def) (((def) & (GPIO_PORT_MASK | GPIO_PIN_MASK)) | (GPIO_INPUT|GPIO_FLOAT|GPIO_SPEED_2MHz)) + +#define PX4_GPIO_INIT_LIST { \ + PX4_ADC_GPIO, \ + GPIO_HW_REV_DRIVE, \ + GPIO_HW_VER_DRIVE, \ + GPIO_CAN1_TX, \ + GPIO_CAN1_RX, \ + GPIO_CAN1_SILENT_S0, \ + GPIO_HEATER_OUTPUT, \ + GPIO_nPOWER_IN_A, \ + GPIO_nPOWER_IN_B, \ + GPIO_VDD_5V_PERIPH_nEN, \ + GPIO_VDD_5V_PERIPH_nOC, \ + GPIO_VDD_5V_HIPOWER_nEN, \ + GPIO_VDD_5V_HIPOWER_nOC, \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D0), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D1), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D2), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_D3), \ + SDMMC_PIN_OFF(GPIO_SDMMC1_CMD), \ + GPIO_VDD_3V3_SD_CARD_EN, \ + GPIO_TONE_ALARM_IDLE, \ + } + +#define BOARD_ENABLE_CONSOLE_BUFFER + +__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/src/modules/ekf2/EKF/utils.hpp b/boards/siyi/n7/src/bootloader_main.c similarity index 65% rename from src/modules/ekf2/EKF/utils.hpp rename to boards/siyi/n7/src/bootloader_main.c index 4d911f6e20..cec6abd467 100644 --- a/src/modules/ekf2/EKF/utils.hpp +++ b/boards/siyi/n7/src/bootloader_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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,45 +31,45 @@ * ****************************************************************************/ -#include +/** + * @file bootloader_main.c + * + * FMU-specific early startup code for bootloader +*/ -#ifndef EKF_UTILS_HPP -#define EKF_UTILS_HPP +#include "board_config.h" +#include "bl.h" -// Use Kahan summation algorithm to get the sum of "sum_previous" and "input". -// This function relies on the caller to be responsible for keeping a copy of -// "accumulator" and passing this value at the next iteration. -// Ref: https://en.wikipedia.org/wiki/Kahan_summation_algorithm -inline float kahanSummation(float sum_previous, float input, float &accumulator) +#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) { - const float y = input - accumulator; - const float t = sum_previous + y; - accumulator = (t - sum_previous) - y; - return t; + /* configure USB interfaces */ + stm32_usbinitialize(); } -namespace ecl +__EXPORT int board_app_initialize(uintptr_t arg) { -inline float powf(float x, int exp) -{ - float ret; - - if (exp > 0) { - ret = x; - - for (int count = 1; count < exp; count++) { - ret *= x; - } - - return ret; - - } else if (exp < 0) { - return 1.0f / ecl::powf(x, -exp); - } - - return 1.0f; + return 0; } -} // namespace ecl +void board_late_initialize(void) +{ + sercon_main(0, NULL); +} -#endif // EKF_UTILS_HPP +extern void sys_tick_handler(void); +void board_timerhook(void) +{ + sys_tick_handler(); +} diff --git a/boards/siyi/n7/src/can.c b/boards/siyi/n7/src/can.c new file mode 100644 index 0000000000..c71b2e8793 --- /dev/null +++ b/boards/siyi/n7/src/can.c @@ -0,0 +1,128 @@ +/**************************************************************************** + * + * 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 px4fmu_can.c + * + * Board-specific CAN functions. + */ + +#ifdef CONFIG_CAN + +#include +#include + +#include +#include + +#include "chip.h" +#include "arm_internal.h" + +#include "chip.h" +#include "stm32_can.h" +#include "board_config.h" + +#ifdef CONFIG_CAN + +/************************************************************************************ + * 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 + +#endif /* CONFIG_CAN */ diff --git a/boards/siyi/n7/src/hw_config.h b/boards/siyi/n7/src/hw_config.h new file mode 100644 index 0000000000..a6b71126e0 --- /dev/null +++ b/boards/siyi/n7/src/hw_config.h @@ -0,0 +1,128 @@ +/* + * 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/ttyS0,115200" +#define BOOT_DELAY_ADDRESS 0x000001a0 +#define BOARD_TYPE 1123 +#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 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/siyi/n7/src/i2c.cpp b/boards/siyi/n7/src/i2c.cpp new file mode 100644 index 0000000000..124fc2375c --- /dev/null +++ b/boards/siyi/n7/src/i2c.cpp @@ -0,0 +1,41 @@ +/**************************************************************************** + * + * 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(2), + initI2CBusInternal(3), + initI2CBusExternal(4), +}; diff --git a/boards/siyi/n7/src/init.c b/boards/siyi/n7/src/init.c new file mode 100644 index 0000000000..46cf4ae4aa --- /dev/null +++ b/boards/siyi/n7/src/init.c @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2019, 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 + * + * PX4FMU-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. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include "board_config.h" + +#include +#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 + +/**************************************************************************** + * 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_PERIPH_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 */ + board_control_spi_sensors_power(true, 0xffff); + VDD_5V_PERIPH_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)); + + board_control_spi_sensors_power_configgpio(); + + /* 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_PERIPH_EN(true); + VDD_5V_HIPOWER_EN(true); + board_control_spi_sensors_power(true, 0xffff); + + /* Need hrt running before using the ADC */ + + px4_platform_init(); + + // Use the default HW_VER_REV(0x0,0x0) for Ramtron + + stm32_spiinitialize(); + + /* 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(); + + /* 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); + } + +#endif /* CONFIG_MMCSD */ + + return OK; +} diff --git a/boards/siyi/n7/src/led.c b/boards/siyi/n7/src/led.c new file mode 100644 index 0000000000..df40a23dca --- /dev/null +++ b/boards/siyi/n7/src/led.c @@ -0,0 +1,235 @@ +/**************************************************************************** + * + * 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 px4fmu2_led.c + * + * PX4FMU 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 + GPIO_nSAFETY_SWITCH_LED_OUT, // Indexed by LED_SAFETY by xlatpx4 +}; + +#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) + 0, // 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/siyi/n7/src/manifest.c b/boards/siyi/n7/src/manifest.c new file mode 100644 index 0000000000..4bf33cab90 --- /dev/null +++ b/boards/siyi/n7/src/manifest.c @@ -0,0 +1,132 @@ +/**************************************************************************** + * + * 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_board[] = { + { + .present = 1, + .mandatory = 1, + .connection = px4_hw_con_onboard, + }, +}; + +static px4_hw_mft_list_entry_t mft_lists[] = { +// ver_rev + {VER00, hw_mft_list_board, arraySize(hw_mft_list_board)}, +}; + +/************************************************************************************ + * 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/siyi/n7/src/sdio.c b/boards/siyi/n7/src/sdio.c new file mode 100644 index 0000000000..869d757756 --- /dev/null +++ b/boards/siyi/n7/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/siyi/n7/src/spi.cpp b/boards/siyi/n7/src/spi.cpp new file mode 100644 index 0000000000..c78c7bb250 --- /dev/null +++ b/boards/siyi/n7/src/spi.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { + initSPIBus(SPI::Bus::SPI1, { + initSPIDevice(DRV_IMU_DEVTYPE_ICM20689, SPI::CS{GPIO::PortF, GPIO::Pin2}, SPI::DRDY{GPIO::PortB, GPIO::Pin4}), + initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin4}, SPI::DRDY{GPIO::PortB, GPIO::Pin14}), + initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin10}, SPI::DRDY{GPIO::PortB, GPIO::Pin15}), + initSPIDevice(DRV_DEVTYPE_UNUSED, SPI::CS{GPIO::PortH, GPIO::Pin5}), + }, {GPIO::PortE, GPIO::Pin3}), + initSPIBus(SPI::Bus::SPI2, { + initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortF, GPIO::Pin5}) + }), + initSPIBus(SPI::Bus::SPI4, { + initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortF, GPIO::Pin10}), + }), +}; + +static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/boards/siyi/n7/src/timer_config.cpp b/boards/siyi/n7/src/timer_config.cpp new file mode 100644 index 0000000000..c9bc0acd46 --- /dev/null +++ b/boards/siyi/n7/src/timer_config.cpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 + +constexpr io_timers_t io_timers[MAX_IO_TIMERS] = { + initIOTimer(Timer::Timer1, 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::PortA, GPIO::Pin10}), + 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}), +}; + +constexpr io_timers_channel_mapping_t io_timers_channel_mapping = + initIOTimerChannelMapping(io_timers, timer_io_channels); diff --git a/boards/siyi/n7/src/usb.c b/boards/siyi/n7/src/usb.c new file mode 100644 index 0000000000..6d42476b71 --- /dev/null +++ b/boards/siyi/n7/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 px4fmu_usb.c + * + * Board-specific USB functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#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 PX4FMU 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/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig index 3efa0c136f..25c5120d57 100644 --- a/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig +++ b/boards/sky-drones/smartap-airlink/nuttx-config/nsh/defconfig @@ -115,7 +115,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_IOB_NBUFFERS=24 CONFIG_IOB_THROTTLE=0 CONFIG_IPCFG_BINARY=y @@ -181,6 +181,8 @@ 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 diff --git a/boards/sky-drones/smartap-airlink/src/mtd.cpp b/boards/sky-drones/smartap-airlink/src/mtd.cpp index 8b74a4617c..742b463041 100644 --- a/boards/sky-drones/smartap-airlink/src/mtd.cpp +++ b/boards/sky-drones/smartap-airlink/src/mtd.cpp @@ -34,7 +34,7 @@ #include #include // KiB BS nB -static const px4_mft_device_t spi5 = { // FM25V02A on FMUM 32K 512 X 64 +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) }; @@ -45,18 +45,12 @@ static const px4_mft_device_t i2c3 = { // 24LC64T on Base 8K 32 X 2 static const px4_mtd_entry_t fmum_fram = { .device = &spi5, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/boards/spracing/h7extreme/init/rc.board_defaults b/boards/spracing/h7extreme/init/rc.board_defaults index 34177ec437..d2b9948883 100644 --- a/boards/spracing/h7extreme/init/rc.board_defaults +++ b/boards/spracing/h7extreme/init/rc.board_defaults @@ -14,8 +14,7 @@ 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_ACC_COMP 0 -param set-default ATT_W_ACC 0.4000 -param set-default ATT_W_GYRO_BIAS 0.0000 +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/thepeach/k1/nuttx-config/nsh/defconfig b/boards/thepeach/k1/nuttx-config/nsh/defconfig index 3a63ed05bb..78c6bde8df 100644 --- a/boards/thepeach/k1/nuttx-config/nsh/defconfig +++ b/boards/thepeach/k1/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/thepeach/r1/nuttx-config/nsh/defconfig b/boards/thepeach/r1/nuttx-config/nsh/defconfig index 51a9be25a1..75c975f76f 100644 --- a/boards/thepeach/r1/nuttx-config/nsh/defconfig +++ b/boards/thepeach/r1/nuttx-config/nsh/defconfig @@ -108,7 +108,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 CONFIG_LIBC_STRERROR=y @@ -141,6 +141,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/boards/uvify/core/nuttx-config/nsh/defconfig b/boards/uvify/core/nuttx-config/nsh/defconfig index 0dcb89b2b3..6d8e82e9f5 100644 --- a/boards/uvify/core/nuttx-config/nsh/defconfig +++ b/boards/uvify/core/nuttx-config/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_I2C=y CONFIG_I2C_RESET=y CONFIG_IDLETHREAD_STACKSIZE=750 CONFIG_INIT_ENTRYPOINT="nsh_main" -CONFIG_INIT_STACKSIZE=2944 +CONFIG_INIT_STACKSIZE=3194 CONFIG_LIBC_FLOATINGPOINT=y CONFIG_LIBC_LONG_LONG=y CONFIG_LIBC_MAX_EXITFUNS=1 @@ -140,6 +140,8 @@ 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=262144 CONFIG_RAM_START=0x20000000 diff --git a/cmake/gtest/px4_add_gtest.cmake b/cmake/gtest/px4_add_gtest.cmake index 1a54456194..c93b15734a 100644 --- a/cmake/gtest/px4_add_gtest.cmake +++ b/cmake/gtest/px4_add_gtest.cmake @@ -107,6 +107,7 @@ function(px4_add_functional_gtest) px4_daemon work_queue parameters + events perf tinybson uorb_msgs diff --git a/cmake/kconfig.cmake b/cmake/kconfig.cmake index ea5e48800e..f96a62b67b 100644 --- a/cmake/kconfig.cmake +++ b/cmake/kconfig.cmake @@ -216,15 +216,17 @@ if(EXISTS ${BOARD_DEFCONFIG}) endforeach() - # Put every module not in userspace also to kernel list - foreach(modpath ${config_module_list}) + if (CONFIG_BOARD_PROTECTED) + # Put every module not in userspace also to kernel list + foreach(modpath ${config_module_list}) get_filename_component(module ${modpath} NAME) list(FIND config_user_list ${module} _index) if (${_index} EQUAL -1) list(APPEND config_kernel_list ${modpath}) endif() - endforeach() + endforeach() + endif() if(PLATFORM) # set OS, and append specific platform module path diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index 5c0ee1153d..19d2c41da1 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -168,10 +168,11 @@ function(px4_add_module) if(NOT DYNAMIC) target_link_libraries(${MODULE} PRIVATE prebuild_targets px4_platform systemlib perf) if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT AND KERNEL) - target_link_libraries(${MODULE} PRIVATE kernel_parameters_interface px4_kernel_layer uORB_kernel) + target_link_libraries(${MODULE} PRIVATE + kernel_events_interface kernel_parameters_interface px4_kernel_layer uORB_kernel) set_property(GLOBAL APPEND PROPERTY PX4_KERNEL_MODULE_LIBRARIES ${MODULE}) else() - target_link_libraries(${MODULE} PRIVATE parameters_interface px4_layer uORB) + target_link_libraries(${MODULE} PRIVATE events_interface parameters_interface px4_layer uORB) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES ${MODULE}) endif() set_property(GLOBAL APPEND PROPERTY PX4_MODULE_PATHS ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py index fac011eb7d..8d7f6e6478 100755 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ b/integrationtests/python_src/px4_it/mavros/mission_test.py @@ -303,9 +303,12 @@ class MavrosMissionTest(MavrosTestCommon): self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res)) self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res)) + self.assertTrue(res['roll_error_std'] < 5.0, str(res)) self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) - self.assertTrue(res['yaw_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)) if __name__ == '__main__': diff --git a/msg/ActuatorTest.msg b/msg/ActuatorTest.msg index f880720da0..221258f906 100644 --- a/msg/ActuatorTest.msg +++ b/msg/ActuatorTest.msg @@ -18,4 +18,4 @@ float32 value # range: [-1, 1], where 1 means maximum positive output, # and NaN maps to disarmed (stop the motors) uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) -uint8 ORB_QUEUE_LENGTH = 12 # same as MAX_NUM_MOTORS to support code in esc_calibration +uint8 ORB_QUEUE_LENGTH = 16 # >= MAX_NUM_MOTORS to support code in esc_calibration diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 611fea9e85..70b09f9cfe 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -60,6 +60,8 @@ set(msg_files CollisionReport.msg ControlAllocatorStatus.msg Cpuload.msg + DatamanRequest.msg + DatamanResponse.msg DebugArray.msg DebugKeyValue.msg DebugValue.msg @@ -90,6 +92,7 @@ set(msg_files FollowTargetStatus.msg GeneratorStatus.msg GeofenceResult.msg + GimbalControls.msg GimbalDeviceAttitudeStatus.msg GimbalDeviceInformation.msg GimbalDeviceSetAttitude.msg @@ -97,9 +100,12 @@ set(msg_files GimbalManagerSetAttitude.msg GimbalManagerSetManualControl.msg GimbalManagerStatus.msg + GpioConfig.msg + GpioIn.msg + GpioOut.msg + GpioRequest.msg GpsDump.msg GpsInjectData.msg - GimbalControls.msg Gripper.msg HealthReport.msg HeaterStatus.msg @@ -174,6 +180,7 @@ set(msg_files SensorSelection.msg SensorsStatus.msg SensorsStatusImu.msg + SensorUwb.msg SystemPower.msg TakeoffStatus.msg TaskStackInfo.msg @@ -190,8 +197,6 @@ set(msg_files UavcanParameterValue.msg UlogStream.msg UlogStreamAck.msg - UwbDistance.msg - UwbGrid.msg VehicleAcceleration.msg VehicleAirData.msg VehicleAngularAccelerationSetpoint.msg diff --git a/msg/DatamanRequest.msg b/msg/DatamanRequest.msg new file mode 100644 index 0000000000..f819771a45 --- /dev/null +++ b/msg/DatamanRequest.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data +uint32 data_length \ No newline at end of file diff --git a/msg/DatamanResponse.msg b/msg/DatamanResponse.msg new file mode 100644 index 0000000000..ebf752db50 --- /dev/null +++ b/msg/DatamanResponse.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data + +uint8 STATUS_SUCCESS = 0 +uint8 STATUS_FAILURE_ID_ERR = 1 +uint8 STATUS_FAILURE_NO_DATA = 2 +uint8 STATUS_FAILURE_READ_FAILED = 3 +uint8 STATUS_FAILURE_WRITE_FAILED = 4 +uint8 STATUS_FAILURE_CLEAR_FAILED = 5 +uint8 status diff --git a/msg/EstimatorAidSource1d.msg b/msg/EstimatorAidSource1d.msg index 46faddafac..78273d6b06 100644 --- a/msg/EstimatorAidSource1d.msg +++ b/msg/EstimatorAidSource1d.msg @@ -14,7 +14,6 @@ float32 innovation float32 innovation_variance float32 test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused @@ -22,3 +21,4 @@ bool fused # true if the sample was successfully fused # 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 diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 9fdba8fefa..2d33fc361d 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -14,7 +14,6 @@ float32[2] innovation float32[2] innovation_variance float32[2] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/msg/EstimatorAidSource3d.msg b/msg/EstimatorAidSource3d.msg index 5d143bc04d..deb4c05bd0 100644 --- a/msg/EstimatorAidSource3d.msg +++ b/msg/EstimatorAidSource3d.msg @@ -14,7 +14,6 @@ float32[3] innovation float32[3] innovation_variance float32[3] test_ratio -bool fusion_enabled # true when measurements are being fused bool innovation_rejected # true if the observation has been rejected bool fused # true if the sample was successfully fused diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index f3b76f18ed..5491734b1d 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -127,3 +127,8 @@ uint32 mag_device_id # legacy local position estimator (LPE) flags uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) + +float32 mag_inclination_deg +float32 mag_inclination_ref_deg +float32 mag_strength_gs +float32 mag_strength_ref_gs diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 9d1f77403a..4d747d8268 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -39,6 +39,9 @@ bool cs_rng_kin_consistent # 31 - true when the range finder kinematic cons bool cs_fake_pos # 32 - true when fake position measurements are being fused bool cs_fake_hgt # 33 - true when fake height measurements are being fused bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused +bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended +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 # fault status uint32 fault_status_changes # number of filter fault status (fs) changes diff --git a/msg/GpioConfig.msg b/msg/GpioConfig.msg new file mode 100644 index 0000000000..0ff393ec87 --- /dev/null +++ b/msg/GpioConfig.msg @@ -0,0 +1,28 @@ +# GPIO configuration + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # Pin mask +uint32 state # Initial pin output state + +# Configuration Mask +# Bit 0-3: Direction: 0=Input, 1=Output +# Bit 4-7: Input Config: 0=Floating, 1=PullUp, 2=PullDown +# Bit 8-12: Output Config: 0=PushPull, 1=OpenDrain +# Bit 13-31: Reserved +uint32 INPUT = 0 # 0x0000 +uint32 OUTPUT = 1 # 0x0001 +uint32 PULLUP = 16 # 0x0010 +uint32 PULLDOWN = 32 # 0x0020 +uint32 OPENDRAIN = 256 # 0x0100 + +uint32 INPUT_FLOATING = 0 # 0x0000 +uint32 INPUT_PULLUP = 16 # 0x0010 +uint32 INPUT_PULLDOWN = 32 # 0x0020 + +uint32 OUTPUT_PUSHPULL = 0 # 0x0000 +uint32 OUTPUT_OPENDRAIN = 256 # 0x0100 +uint32 OUTPUT_OPENDRAIN_PULLUP = 272 # 0x0110 + +uint32 config diff --git a/msg/GpioIn.msg b/msg/GpioIn.msg new file mode 100644 index 0000000000..0482a2188e --- /dev/null +++ b/msg/GpioIn.msg @@ -0,0 +1,6 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 state # pin state mask diff --git a/msg/GpioOut.msg b/msg/GpioOut.msg new file mode 100644 index 0000000000..3865bbf2e9 --- /dev/null +++ b/msg/GpioOut.msg @@ -0,0 +1,7 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # pin mask +uint32 state # pin state mask diff --git a/msg/GpioRequest.msg b/msg/GpioRequest.msg new file mode 100644 index 0000000000..3328b00145 --- /dev/null +++ b/msg/GpioRequest.msg @@ -0,0 +1,4 @@ +# Request GPIO mask to be read + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id diff --git a/msg/Mission.msg b/msg/Mission.msg index 70fc68ccf2..546959a3b3 100644 --- a/msg/Mission.msg +++ b/msg/Mission.msg @@ -3,3 +3,10 @@ uint8 dataman_id # default 0, there are two offboard storage places in the datam uint16 count # count of the missions stored in the dataman int32 current_seq # default -1, start at the one changed latest + +int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise +int32 land_index # Index of the land item, -1 otherwise + +uint16 mission_update_counter # indicates updates to the mission, reload from dataman if increased +uint16 geofence_update_counter # indicates updates to the geofence, reload from dataman if increased +uint16 safe_points_update_counter # indicates updates to the safe points, reload from dataman if increased diff --git a/msg/SensorGps.msg b/msg/SensorGps.msg index 4404dd524f..db18899cb1 100644 --- a/msg/SensorGps.msg +++ b/msg/SensorGps.msg @@ -5,10 +5,10 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -int32 lat # Latitude in 1E-7 degrees -int32 lon # Longitude in 1E-7 degrees -int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) -int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) +float64 latitude_deg # Latitude in degrees, allows centimeter level RTK precision +float64 longitude_deg # Longitude in degrees, allows centimeter level RTK precision +float64 altitude_msl_m # Altitude above MSL, meters +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) @@ -55,4 +55,11 @@ float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) float32 rtcm_injection_rate # RTCM message injection rate Hz uint8 selected_rtcm_instance # uorb instance that is being used for RTCM corrections +bool rtcm_crc_failed # RTCM message CRC failure detected + +uint8 RTCM_MSG_USED_UNKNOWN = 0 +uint8 RTCM_MSG_USED_NOT_USED = 1 +uint8 RTCM_MSG_USED_USED = 2 +uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver + # TOPICS sensor_gps vehicle_gps_position diff --git a/msg/SensorUwb.msg b/msg/SensorUwb.msg new file mode 100644 index 0000000000..ae889a8bdc --- /dev/null +++ b/msg/SensorUwb.msg @@ -0,0 +1,34 @@ +# UWB distance contains the distance information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) + +uint32 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint16 mac # MAC adress of Initiator (controller) + +uint16 mac_dest # MAC adress of Responder (Controlee) +uint16 status # status feedback # +uint8 nlos # None line of site condition y/n +float32 distance # distance in m to the UWB receiver + + +#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees +float32 aoa_azimuth_dev # Angle of arrival of first incomming RX msg +float32 aoa_elevation_dev # Angle of arrival of first incomming RX msg +float32 aoa_azimuth_resp # Angle of arrival of first incomming RX msg at the responder +float32 aoa_elevation_resp # Angle of arrival of first incomming RX msg at the responder + +# Figure of merit for the angle measurements +uint8 aoa_azimuth_fom # AOA Azimuth FOM +uint8 aoa_elevation_fom # AOA Elevation FOM +uint8 aoa_dest_azimuth_fom # AOA Azimuth FOM +uint8 aoa_dest_elevation_fom # AOA Elevation FOM + +# Initiator physical configuration +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + # Standard configuration is Antennas facing down and azimuth aligened in forward direction +float32 offset_x # UWB initiator offset in X axis (NED drone frame) +float32 offset_y # UWB initiator offset in Y axis (NED drone frame) +float32 offset_z # UWB initiator offset in Z axis (NED drone frame) diff --git a/msg/UwbDistance.msg b/msg/UwbDistance.msg deleted file mode 100644 index e496a72396..0000000000 --- a/msg/UwbDistance.msg +++ /dev/null @@ -1,15 +0,0 @@ -# UWB distance contains the distance information measured by an ultra-wideband positioning system, -# such as Pozyx or NXP Rddrone. - -uint64 timestamp # time since system start (microseconds) -uint32 time_uwb_ms # Time of UWB device in ms -uint32 counter # Number of Ranges since last Start of Ranging -uint32 sessionid # UWB SessionID -uint32 time_offset # Time between Ranging Rounds in ms -uint16 status # status feedback # - -uint16[12] anchor_distance # distance in cm to each UWB Anchor (UWB Device which takes part in Ranging) -bool[12] nlos # Visual line of sight yes/no -float32[12] aoafirst # Angle of arrival of first incoming RX msg - -float32[3] position # Position of the Landing point in relation to the Drone (x,y,z in Meters NED) diff --git a/msg/UwbGrid.msg b/msg/UwbGrid.msg deleted file mode 100644 index 0862f84330..0000000000 --- a/msg/UwbGrid.msg +++ /dev/null @@ -1,25 +0,0 @@ -# UWB report message contains the grid information measured by an ultra-wideband positioning system, -# such as Pozyx or NXP Rddrone. - -uint64 timestamp # time since system start (microseconds) -uint16 initator_time # time to synchronize clocks (microseconds) -uint8 num_anchors # Number of anchors - -float64[4] target_gps # GPS position of target of the UWB system (Lat / Lon / Alt / Yaw Offset to true North) - -# Grid position information -# Position in x,y,z in (x,y,z in centimeters NED) -# staring with POI and Anchor 0 up to Anchor 11 -int16[3] target_pos # Point of Interest, mostly landing Target x,y,z -int16[3] anchor_pos_0 -int16[3] anchor_pos_1 -int16[3] anchor_pos_2 -int16[3] anchor_pos_3 -int16[3] anchor_pos_4 -int16[3] anchor_pos_5 -int16[3] anchor_pos_6 -int16[3] anchor_pos_7 -int16[3] anchor_pos_8 -int16[3] anchor_pos_9 -int16[3] anchor_pos_10 -int16[3] anchor_pos_11 diff --git a/msg/VehicleControlMode.msg b/msg/VehicleControlMode.msg index 1f4034369d..92324cf03f 100644 --- a/msg/VehicleControlMode.msg +++ b/msg/VehicleControlMode.msg @@ -14,3 +14,4 @@ bool flag_control_position_enabled # true if position is controlled bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_allocation_enabled # true if control allocation is enabled diff --git a/msg/VehicleLocalPosition.msg b/msg/VehicleLocalPosition.msg index 4f9238123c..5f17cde407 100644 --- a/msg/VehicleLocalPosition.msg +++ b/msg/VehicleLocalPosition.msg @@ -39,6 +39,7 @@ float32 ay # East velocity derivative in NED earth-fixed frame, (metres/s float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only float32 delta_heading uint8 heading_reset_counter bool heading_good_for_control diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 878b4b04ed..bf545ea02b 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -6,13 +6,8 @@ uint64 armed_time # Arming timestamp (microseconds) uint64 takeoff_time # Takeoff timestamp (microseconds) uint8 arming_state -uint8 ARMING_STATE_INIT = 0 -uint8 ARMING_STATE_STANDBY = 1 -uint8 ARMING_STATE_ARMED = 2 -uint8 ARMING_STATE_STANDBY_ERROR = 3 -uint8 ARMING_STATE_SHUTDOWN = 4 -uint8 ARMING_STATE_IN_AIR_RESTORE = 5 -uint8 ARMING_STATE_MAX = 6 +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 uint8 latest_arming_reason uint8 latest_disarming_reason @@ -121,4 +116,3 @@ bool rc_calibration_in_progress bool calibration_enabled bool pre_flight_checks_pass # true if all checks necessary to arm pass - diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt index a6e06b9d47..d2e08a405b 100644 --- a/platforms/common/CMakeLists.txt +++ b/platforms/common/CMakeLists.txt @@ -43,7 +43,6 @@ endif() add_library(px4_platform STATIC board_common.c board_identity.c - events.cpp external_reset_lockout.cpp i2c.cpp i2c_spi_buses.cpp diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h index 832b0dbd6a..7a6af777c4 100644 --- a/platforms/common/include/px4_platform_common/board_common.h +++ b/platforms/common/include/px4_platform_common/board_common.h @@ -440,6 +440,8 @@ __BEGIN_DECLS #if defined(RC_SERIAL_SINGLEWIRE) static inline bool board_rc_singlewire(const char *device) { return strcmp(device, RC_SERIAL_PORT) == 0; } +#elif defined(RC_SERIAL_SINGLEWIRE_FORCE) +static inline bool board_rc_singlewire(const char *device) { return true; } #else static inline bool board_rc_singlewire(const char *device) { return false; } #endif 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 9bee390323..4df96fa5a9 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", 1900, -18}; +static constexpr wq_config_t hp_default{"wq:hp_default", 2392, -18}; static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19}; diff --git a/platforms/common/uORB/uORB.h b/platforms/common/uORB/uORB.h index 7fdbb699d1..0e2abb2bcf 100644 --- a/platforms/common/uORB/uORB.h +++ b/platforms/common/uORB/uORB.h @@ -42,16 +42,17 @@ #include #include +typedef uint16_t orb_id_size_t; /** * Object metadata. */ struct orb_metadata { - const char *o_name; /**< unique object name */ - const uint16_t o_size; /**< object size */ - const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ - const char *o_fields; /**< semicolon separated list of fields (with type) */ - uint8_t o_id; /**< ORB_ID enum */ + const char *o_name; /**< unique object name */ + const uint16_t o_size; /**< object size */ + const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */ + const char *o_fields; /**< semicolon separated list of fields (with type) */ + orb_id_size_t o_id; /**< ORB_ID enum */ }; typedef const struct orb_metadata *orb_id_t; diff --git a/platforms/common/uORB/uORBDeviceMaster.cpp b/platforms/common/uORB/uORBDeviceMaster.cpp index c39e4572db..bd3877f13b 100644 --- a/platforms/common/uORB/uORBDeviceMaster.cpp +++ b/platforms/common/uORB/uORBDeviceMaster.cpp @@ -150,7 +150,7 @@ int uORB::DeviceMaster::advertise(const struct orb_metadata *meta, bool is_adver // add to the node map. _node_list.add(node); - _node_exists[node->get_instance()].set((uint8_t)node->id(), true); + _node_exists[node->get_instance()].set((orb_id_size_t)node->id(), true); } group_tries++; diff --git a/platforms/common/uORB/uORBDeviceMaster.hpp b/platforms/common/uORB/uORBDeviceMaster.hpp index 94c97e61df..ccda58f8d3 100644 --- a/platforms/common/uORB/uORBDeviceMaster.hpp +++ b/platforms/common/uORB/uORBDeviceMaster.hpp @@ -98,7 +98,7 @@ public: return false; } - return _node_exists[instance][(uint8_t)id]; + return _node_exists[instance][(orb_id_size_t)id]; } /** diff --git a/platforms/common/uORB/uORBDeviceNode.cpp b/platforms/common/uORB/uORBDeviceNode.cpp index 7cc5c9bd3e..112170de6b 100644 --- a/platforms/common/uORB/uORBDeviceNode.cpp +++ b/platforms/common/uORB/uORBDeviceNode.cpp @@ -188,7 +188,10 @@ uORB::DeviceNode::write(cdev::file_t *filp, const char *buffer, size_t buflen) if (nullptr == _data) { const size_t data_size = _meta->o_size * _queue_size; _data = (uint8_t *) px4_cache_aligned_alloc(data_size); - memset(_data, 0, data_size); + + if (_data) { + memset(_data, 0, data_size); + } } unlock(); diff --git a/platforms/nuttx/NuttX/apps b/platforms/nuttx/NuttX/apps index a489381b49..616f7024a4 160000 --- a/platforms/nuttx/NuttX/apps +++ b/platforms/nuttx/NuttX/apps @@ -1 +1 @@ -Subproject commit a489381b49835ecba6f3b873b5071d882a18152f +Subproject commit 616f7024a479bf209eadce133bba5dc8820a7f99 diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index 3f77354c0d..c23b72dffe 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit 3f77354c0dc88793a47ff3b57595195ab45f7ba9 +Subproject commit c23b72dffeb0de0d1a836ab561eb9169c4a9e58e diff --git a/platforms/nuttx/init/kinetis/rc.board_arch_defaults b/platforms/nuttx/init/kinetis/rc.board_arch_defaults index 1cf9c919db..7a7bbb3db4 100644 --- a/platforms/nuttx/init/kinetis/rc.board_arch_defaults +++ b/platforms/nuttx/init/kinetis/rc.board_arch_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # Multi-EKF (off by default) -param set-default EKF2_MULTI_IMU 0 +param set-default -s EKF2_MULTI_IMU 0 param set-default SENS_IMU_MODE 1 set LOGGER_BUF 8 diff --git a/platforms/nuttx/init/stm32/rc.board_arch_defaults b/platforms/nuttx/init/stm32/rc.board_arch_defaults index cda40b9a40..edef4e1e63 100644 --- a/platforms/nuttx/init/stm32/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32/rc.board_arch_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # Multi-EKF (off by default) -param set-default EKF2_MULTI_IMU 0 +param set-default -s EKF2_MULTI_IMU 0 param set-default SENS_IMU_MODE 1 set LOGGER_BUF 8 diff --git a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults index 17bae04429..2c71f7470c 100644 --- a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # Multi-EKF (across IMUs only) -param set-default EKF2_MULTI_IMU 3 +param set-default -s EKF2_MULTI_IMU 3 param set-default SENS_IMU_MODE 0 param set-default -s IMU_GYRO_FFT_EN 1 diff --git a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults index 31d23de1ec..60979fa61b 100644 --- a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults +++ b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults @@ -4,7 +4,7 @@ #------------------------------------------------------------------------------ # Multi-EKF -param set-default EKF2_MULTI_IMU 3 +param set-default -s EKF2_MULTI_IMU 3 param set-default SENS_IMU_MODE 0 param set-default -s IMU_GYRO_FFT_EN 1 diff --git a/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp b/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp index ede9c2c172..63a60e5484 100644 --- a/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp +++ b/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009.cpp @@ -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 @@ -31,182 +31,142 @@ * ****************************************************************************/ +#include #include -#include -#include "mcp23009_registers.hpp" +#include +#include +#include +#include +#include +#include +#include -using namespace Microchip_MCP23009; - -const struct gpio_operations_s MCP23009::gpio_ops = { -go_read : MCP23009::go_read, -go_write : MCP23009::go_write, -go_attach : nullptr, -go_enable : nullptr, -go_setpintype : MCP23009::go_setpintype, +static uint32_t DEVID{0}; +struct mcp23009_gpio_dev_s { + struct gpio_dev_s gpio; + uint8_t mask; }; -MCP23009::MCP23009(int bus, int address, int first_minor, int bus_frequency) : - I2C(DRV_GPIO_DEVTYPE_MCP23009, "MCP23009", bus, address, bus_frequency), - _first_minor(first_minor) +/* Copy the read input data */ +class ReadCallback : public uORB::SubscriptionCallback { -} +public: + using SubscriptionCallback::SubscriptionCallback; -MCP23009::~MCP23009() -{ - /* set all as input & unregister */ - for (int i = 0; i < num_gpios; ++i) { - go_setpintype(i, GPIO_INPUT_PIN); - gpio_pin_unregister(&_gpio[i].gpio, _first_minor + i); - } -} + void call() override + { + px4::msg::GpioIn new_input; -int MCP23009::go_read(struct gpio_dev_s *dev, bool *value) -{ - mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; - return gpio->obj->go_read(gpio->id, value); -} + if (update(&new_input) && new_input.device_id == DEVID) { + input = new_input.state; + } -int MCP23009::go_write(struct gpio_dev_s *dev, bool value) -{ - mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; - return gpio->obj->go_write(gpio->id, value); -} - -int MCP23009::go_setpintype(struct gpio_dev_s *dev, enum gpio_pintype_e pintype) -{ - mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; - return gpio->obj->go_setpintype(gpio->id, pintype); -} - - -int MCP23009::read_reg(Register address, uint8_t &data) -{ - return transfer((uint8_t *)&address, 1, &data, 1); -} - -int MCP23009::write_reg(Register address, uint8_t value) -{ - uint8_t data[2] = {(uint8_t)address, value}; - return transfer(data, sizeof(data), nullptr, 0); -} - -int MCP23009::init(uint8_t direction, uint8_t intital, uint8_t pull_up) -{ - /* do I2C init (and probe) first */ - int ret = I2C::init(); - - if (ret != PX4_OK) { - return ret; } - /* Use this state as the out puts */ + uint8_t input; +}; - ret = write_reg(Register::OLAT, intital); - ret |= write_reg(Register::IODIR, direction); - ret |= write_reg(Register::GPPU, pull_up); +static uORB::Publication toGpioRequest{ORB_ID(gpio_request)}; +static ReadCallback fromGpioIn{ORB_ID(gpio_in)}; +static int mcp23009_read(struct gpio_dev_s *dev, bool *value) +{ + mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; + *value = fromGpioIn.input & gpio->mask; + return OK; +} - if (ret != PX4_OK) { - return ret; +static uORB::Publication toGpioOut{ORB_ID(gpio_out)}; +static int mcp23009_write(struct gpio_dev_s *dev, bool value) +{ + mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; + gpio_out_s msg{ + hrt_absolute_time(), + DEVID, + gpio->mask, // clear mask + value ? gpio->mask : 0u, // set mask + }; + return toGpioOut.publish(msg) ? OK : -ETIMEDOUT; +} + +static uORB::Publication toGpioConfig{ORB_ID(gpio_config)}; +static int mcp23009_setpintype(struct gpio_dev_s *dev, enum gpio_pintype_e pintype) +{ + mcp23009_gpio_dev_s *gpio = (struct mcp23009_gpio_dev_s *)dev; + gpio_config_s msg{ + hrt_absolute_time(), + DEVID, + gpio->mask, + }; + + switch (pintype) { + case GPIO_INPUT_PIN: + msg.config = gpio_config_s::INPUT; + break; + + case GPIO_INPUT_PIN_PULLUP: + msg.config = gpio_config_s::INPUT_PULLUP; + break; + + case GPIO_OUTPUT_PIN: + msg.config = gpio_config_s::OUTPUT; + break; + + default: + return -ENOTSUP; } - /* register the pins */ - for (int i = 0; i < num_gpios; ++i) { - _gpio[i].gpio.gp_pintype = GPIO_INPUT_PIN; - _gpio[i].gpio.gp_ops = &gpio_ops; - _gpio[i].id = i; - _gpio[i].obj = this; - ret = gpio_pin_register(&_gpio[i].gpio, _first_minor + i); + return toGpioConfig.publish(msg) ? OK : -ETIMEDOUT; +} - if (ret != PX4_OK) { + + +// ---------------------------------------------------------------------------- +static const struct gpio_operations_s mcp23009_gpio_ops { + mcp23009_read, + mcp23009_write, + nullptr, + nullptr, + mcp23009_setpintype, +}; + +static constexpr uint8_t NUM_GPIOS = 8; +static mcp23009_gpio_dev_s _gpio[NUM_GPIOS] { + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 0) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 1) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 2) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 3) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 4) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 5) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 6) }, + { {GPIO_INPUT_PIN, {}, &mcp23009_gpio_ops}, (1u << 7) } +}; + +// ---------------------------------------------------------------------------- +int mcp23009_register_gpios(uint8_t i2c_bus, uint8_t i2c_addr, int first_minor) +{ + const auto device_id = device::Device::DeviceId{ + device::Device::DeviceBusType_I2C, i2c_bus, i2c_addr, DRV_GPIO_DEVTYPE_MCP23009}; + DEVID = device_id.devid; + + for (int i = 0; i < NUM_GPIOS; ++i) { + int ret = gpio_pin_register(&_gpio[i].gpio, first_minor + i); + + if (ret != OK) { return ret; } } - return ret; + fromGpioIn.registerCallback(); + return OK; } -int MCP23009::probe() +int mcp23009_unregister_gpios(int first_minor) { - // no whoami, try to read IOCON - uint8_t data; - return read_reg(Register::IOCON, data); -} - -int MCP23009::go_read(int id, bool *value) -{ - - uint8_t data; - int ret = read_reg(Register::GPIO, data); - - if (ret != 0) { - return ret; - } - - *value = data & (1 << id); - return 0; -} - -int MCP23009::go_write(int id, bool value) -{ - uint8_t data; - int ret = read_reg(Register::GPIO, data); - - if (ret != 0) { - return ret; - } - - if (value) { - data |= (1 << id); - - } else { - data &= ~(1 << id); - } - - return write_reg(Register::GPIO, data); -} - -int MCP23009::go_setpintype(int id, enum gpio_pintype_e pintype) -{ - uint8_t direction; - int ret = read_reg(Register::IODIR, direction); - - if (ret != 0) { - return ret; - } - - uint8_t pullup; - ret = read_reg(Register::GPPU, pullup); - - if (ret != 0) { - return ret; - } - - switch (pintype) { - case GPIO_INPUT_PIN: - direction |= (1 << id); - pullup &= ~(1 << id); - break; - - case GPIO_INPUT_PIN_PULLUP: - direction |= (1 << id); - pullup |= (1 << id); - break; - - case GPIO_OUTPUT_PIN: - direction &= ~(1 << id); - break; - - default: - return -EINVAL; - } - - _gpio[id].gpio.gp_pintype = pintype; - - ret = write_reg(Register::GPPU, pullup); - - if (ret != 0) { - return ret; - } - - return write_reg(Register::IODIR, direction); + for (int i = 0; i < NUM_GPIOS; ++i) { + mcp23009_setpintype(&_gpio[i].gpio, GPIO_INPUT_PIN); + gpio_pin_unregister(&_gpio[i].gpio, first_minor + i); + } + + fromGpioIn.unregisterCallback(); + return OK; } diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h index 9e1654451c..b85da67e07 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_ctrl.h @@ -53,7 +53,8 @@ #define _CRYPTOIOCBASE IOCTL_IDX_TO_BASE(2) #define _PARAMIOCBASE IOCTL_IDX_TO_BASE(3) #define _PLATFORMIOCBASE IOCTL_IDX_TO_BASE(4) -#define MAX_IOCTL_PTRS 5 +#define _EVENTSIOCBASE IOCTL_IDX_TO_BASE(5) +#define MAX_IOCTL_PTRS 6 /* The PLATFORMIOCLAUNCH IOCTL is used to launch kernel side modules * from the user side code diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/gpio/mcp23009.hpp b/platforms/nuttx/src/px4/common/include/px4_platform/gpio/mcp23009.hpp index 3a12aaaca7..481f5b51d7 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/gpio/mcp23009.hpp +++ b/platforms/nuttx/src/px4/common/include/px4_platform/gpio/mcp23009.hpp @@ -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 @@ -33,49 +33,7 @@ #pragma once -#include +#include -#include - -using namespace time_literals; - -namespace Microchip_MCP23009 -{ -enum class Register : uint8_t; -} - -class MCP23009 : public device::I2C -{ -public: - MCP23009(int bus, int address, int first_minor = 0, int bus_frequency = 400000); - virtual ~MCP23009(); - - int init(uint8_t direction, uint8_t intital = 0, uint8_t pull_up = 0); - -protected: - int probe() override; - -private: - static constexpr int num_gpios = 8; - static const gpio_operations_s gpio_ops; - - struct mcp23009_gpio_dev_s { - struct gpio_dev_s gpio; - uint8_t id; - MCP23009 *obj; - }; - - static int go_read(struct gpio_dev_s *dev, bool *value); - static int go_write(struct gpio_dev_s *dev, bool value); - static int go_setpintype(struct gpio_dev_s *dev, enum gpio_pintype_e pintype); - - int go_read(int id, bool *value); - int go_write(int id, bool value); - int go_setpintype(int id, enum gpio_pintype_e pintype); - - int read_reg(Microchip_MCP23009::Register address, uint8_t &data); - int write_reg(Microchip_MCP23009::Register address, uint8_t data); - - const int _first_minor; - mcp23009_gpio_dev_s _gpio[num_gpios] {}; -}; +int mcp23009_register_gpios(uint8_t i2c_bus, uint8_t i2c_addr, int first_minor = 0); +int mcp23009_unregister_gpios(int first_minor = 0); diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 3331eab90e..d22905cd38 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -125,6 +126,7 @@ int px4_platform_init() #if !defined(CONFIG_BUILD_FLAT) hrt_ioctl_init(); + events_ioctl_init(); #endif /* configure CPU load estimation */ @@ -163,7 +165,7 @@ int px4_platform_init() #if defined(CONFIG_FS_PROCFS) int ret_mount_procfs = mount(nullptr, "/proc", "procfs", 0, nullptr); - if (ret < 0) { + if (ret_mount_procfs < 0) { syslog(LOG_ERR, "ERROR: Failed to mount procfs at /proc: %d\n", ret_mount_procfs); } diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index 8f45bd58e3..f4b3476722 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -249,25 +249,19 @@ static const px4_mtd_manifest_t default_mtd_config = { #else -const px4_mft_device_t spifram = { // FM25V02A on FMUM 32K 512 X 64 +const px4_mft_device_t spifram = { // FM25V02A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32) .bus_type = px4_mft_device_t::SPI, .devid = SPIDEV_FLASH(0) }; const px4_mtd_entry_t fram = { .device = &spifram, - .npart = 2, + .npart = 1, .partd = { { .type = MTD_PARAMETERS, .path = "/fs/mtd_params", - .nblocks = 32 - }, - { - .type = MTD_WAYPOINTS, - .path = "/fs/mtd_waypoints", - .nblocks = 32 - + .nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_SECTOR_SHIFT)) } }, }; diff --git a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake index 7f76502e68..9c57b488b6 100644 --- a/platforms/nuttx/src/px4/common/px4_protected_layers.cmake +++ b/platforms/nuttx/src/px4/common/px4_protected_layers.cmake @@ -52,6 +52,8 @@ target_link_libraries(px4_kernel_layer nuttx_kc nuttx_karch nuttx_kmm + PRIVATE + kernel_events_interface # events_ioctl_init ) target_link_libraries(px4_kernel_layer diff --git a/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp index 2a3356fb54..92918be161 100644 --- a/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k3xx/adc/adc.cpp @@ -38,144 +38,76 @@ #include -#include - -//todo S32K add ADC fior now steal the kinetis one -#include -#include - - -#define _REG(_addr) (*(volatile uint32_t *)(_addr)) - -/* ADC register accessors */ - -#define REG(a, _reg) _REG(KINETIS_ADC##a##_BASE + (_reg)) - -#define rSC1A(adc) REG(adc, KINETIS_ADC_SC1A_OFFSET) /* ADC status and control registers 1 */ -#define rSC1B(adc) REG(adc, KINETIS_ADC_SC1B_OFFSET) /* ADC status and control registers 1 */ -#define rCFG1(adc) REG(adc, KINETIS_ADC_CFG1_OFFSET) /* ADC configuration register 1 */ -#define rCFG2(adc) REG(adc, KINETIS_ADC_CFG2_OFFSET) /* Configuration register 2 */ -#define rRA(adc) REG(adc, KINETIS_ADC_RA_OFFSET) /* ADC data result register */ -#define rRB(adc) REG(adc, KINETIS_ADC_RB_OFFSET) /* ADC data result register */ -#define rCV1(adc) REG(adc, KINETIS_ADC_CV1_OFFSET) /* Compare value registers */ -#define rCV2(adc) REG(adc, KINETIS_ADC_CV2_OFFSET) /* Compare value registers */ -#define rSC2(adc) REG(adc, KINETIS_ADC_SC2_OFFSET) /* Status and control register 2 */ -#define rSC3(adc) REG(adc, KINETIS_ADC_SC3_OFFSET) /* Status and control register 3 */ -#define rOFS(adc) REG(adc, KINETIS_ADC_OFS_OFFSET) /* ADC offset correction register */ -#define rPG(adc) REG(adc, KINETIS_ADC_PG_OFFSET) /* ADC plus-side gain register */ -#define rMG(adc) REG(adc, KINETIS_ADC_MG_OFFSET) /* ADC minus-side gain register */ -#define rCLPD(adc) REG(adc, KINETIS_ADC_CLPD_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLPS(adc) REG(adc, KINETIS_ADC_CLPS_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP4(adc) REG(adc, KINETIS_ADC_CLP4_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP3(adc) REG(adc, KINETIS_ADC_CLP3_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP2(adc) REG(adc, KINETIS_ADC_CLP2_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP1(adc) REG(adc, KINETIS_ADC_CLP1_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLP0(adc) REG(adc, KINETIS_ADC_CLP0_OFFSET) /* ADC plus-side general calibration value register */ -#define rCLMD(adc) REG(adc, KINETIS_ADC_CLMD_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLMS(adc) REG(adc, KINETIS_ADC_CLMS_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM4(adc) REG(adc, KINETIS_ADC_CLM4_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM3(adc) REG(adc, KINETIS_ADC_CLM3_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM2(adc) REG(adc, KINETIS_ADC_CLM2_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM1(adc) REG(adc, KINETIS_ADC_CLM1_OFFSET) /* ADC minus-side general calibration value register */ -#define rCLM0(adc) REG(adc, KINETIS_ADC_CLM0_OFFSET) /* ADC minus-side general calibration value register */ +#include +#include int px4_arch_adc_init(uint32_t base_address) { - /* Input is Buss Clock 56 Mhz We will use /8 for 7 Mhz */ + uint32_t regval; - irqstate_t flags = px4_enter_critical_section(); + /* Configure and perform calibration */ + putreg32(ADC_MCR_ADCLKSEL_DIV4, S32K3XX_ADC2_MCR); - _REG(KINETIS_SIM_SCGC3) |= SIM_SCGC3_ADC1; - rCFG1(1) = ADC_CFG1_ADICLK_BUSCLK | ADC_CFG1_MODE_1213BIT | ADC_CFG1_ADIV_DIV8; - rCFG2(1) = 0; - rSC2(1) = ADC_SC2_REFSEL_DEFAULT; + regval = getreg32(S32K3XX_ADC2_AMSIO); + regval |= ADC_AMSIO_HSEN_MASK; + putreg32(regval, S32K3XX_ADC2_AMSIO); - px4_leave_critical_section(flags); + regval = getreg32(S32K3XX_ADC2_CAL2); + regval &= ~ADC_CAL2_ENX; + putreg32(regval, S32K3XX_ADC2_CAL2); - /* Clear the CALF and begin the calibration */ + regval = getreg32(S32K3XX_ADC2_CALBISTREG); + regval &= ~(ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_MASK | + ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_TSAMP_MASK | ADC_CALBISTREG_RESN_MASK); + regval |= ADC_CALBISTREG_TEST_EN | ADC_CALBISTREG_AVG_EN | ADC_CALBISTREG_NR_SMPL_4SMPL | + ADC_CALBISTREG_CALSTFUL | ADC_CALBISTREG_RESN_14BIT; + putreg32(regval, S32K3XX_ADC2_CALBISTREG); - rSC3(1) = ADC_SC3_CAL | ADC_SC3_CALF; + while (getreg32(S32K3XX_ADC2_CALBISTREG) & ADC_CALBISTREG_C_T_BUSY) {}; - while ((rSC1A(1) & ADC_SC1_COCO) == 0) { - usleep(100); + putreg32(ADC_MCR_PWDN, S32K3XX_ADC2_MCR); - if (rSC3(1) & ADC_SC3_CALF) { - return -1; - } - } + putreg32(22, S32K3XX_ADC2_CTR0); - /* dummy read to clear COCO of calibration */ + putreg32(22, S32K3XX_ADC2_CTR1); - int32_t r = rRA(1); + putreg32(0, S32K3XX_ADC2_DMAE); - /* Check the state of CALF at the end of calibration */ + putreg32(ADC_MCR_ADCLKSEL_DIV4 | ADC_MCR_AVGS_32CONV | ADC_MCR_AVGEN | ADC_MCR_BCTU_MODE | ADC_MCR_MODE, + S32K3XX_ADC2_MCR); - if (rSC3(1) & ADC_SC3_CALF) { - return -1; - } + putreg32(0x10, S32K3XX_ADC2_NCMR0); - /* Calculate the calibration values for single ended positive */ + putreg32(0x10, S32K3XX_ADC2_NCMR1); - r = rCLP0(1) + rCLP1(1) + rCLP2(1) + rCLP3(1) + rCLP4(1) + rCLPS(1) ; - r = 0x8000U | (r >> 1U); - rPG(1) = r; + regval = getreg32(S32K3XX_ADC2_MCR); - /* Calculate the calibration values for double ended Negitive */ + regval |= ADC_MCR_NSTART; - r = rCLM0(1) + rCLM1(1) + rCLM2(1) + rCLM3(1) + rCLM4(1) + rCLMS(1) ; - r = 0x8000U | (r >> 1U); - rMG(1) = r; - - /* kick off a sample and wait for it to complete */ - hrt_abstime now = hrt_absolute_time(); - - rSC1A(1) = ADC_SC1_ADCH(ADC_SC1_ADCH_TEMP); - - while (!(rSC1A(1) & ADC_SC1_COCO)) { - - /* don't wait for more than 500us, since that means something broke - should reset here if we see this */ - if ((hrt_absolute_time() - now) > 500) { - return -1; - } - } + putreg32(regval, S32K3XX_ADC2_MCR); return 0; } void px4_arch_adc_uninit(uint32_t base_address) { - irqstate_t flags = px4_enter_critical_section(); - _REG(KINETIS_SIM_SCGC3) &= ~SIM_SCGC3_ADC1; - px4_leave_critical_section(flags); } uint32_t px4_arch_adc_sample(uint32_t base_address, unsigned channel) { - irqstate_t flags = px4_enter_critical_section(); + uint32_t result = 0; - /* clear any previous COCC */ - rRA(1); + if (channel == 0) { + result = getreg32(S32K3XX_ADC2_PCDR4); - /* run a single conversion right now - should take about 35 cycles (5 microseconds) max */ - rSC1A(1) = ADC_SC1_ADCH(channel); + if ((result & ADC_PCDR_VALID) == ADC_PCDR_VALID) { + result = result & 0xFFFF; - /* wait for the conversion to complete */ - const hrt_abstime now = hrt_absolute_time(); - - while (!(rSC1A(1) & ADC_SC1_COCO)) { - - /* don't wait for more than 10us, since that means something broke - should reset here if we see this */ - if ((hrt_absolute_time() - now) > 10) { - px4_leave_critical_section(flags); - return 0xffff; + } else { + result = 0; } } - /* read the result and clear EOC */ - uint32_t result = rRA(1); - - px4_leave_critical_section(flags); - return result; } @@ -186,10 +118,10 @@ float px4_arch_adc_reference_v() uint32_t px4_arch_adc_temp_sensor_mask() { - return 1 << (ADC_SC1_ADCH_TEMP >> ADC_SC1_ADCH_SHIFT); + return 0; // No temp sensor } uint32_t px4_arch_adc_dn_fullcount() { - return 1 << 12; // 12 bit ADC + return 1 << 15; // 15 bit conversion data } diff --git a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp index c17bbda267..9bcf0268ad 100644 --- a/platforms/posix/src/px4/common/px4_daemon/pxh.cpp +++ b/platforms/posix/src/px4/common/px4_daemon/pxh.cpp @@ -40,6 +40,7 @@ * @author Julian Oes */ +#include #include #include #include diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/modal_io/modal_io.cpp index 6a9c6e0a24..1e5a2bc08c 100644 --- a/src/drivers/actuators/modal_io/modal_io.cpp +++ b/src/drivers/actuators/modal_io/modal_io.cpp @@ -48,7 +48,6 @@ const char *_device; ModalIo::ModalIo() : OutputModuleInterface(MODULE_NAME, px4::serial_port_to_wq(MODAL_IO_DEFAULT_PORT)), - _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}, _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _output_update_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": output update interval")) { @@ -76,6 +75,8 @@ ModalIo::ModalIo() : _esc_status.esc[i].esc_power = 0; } + _esc_status_pub.advertise(); + qc_esc_packet_init(&_fb_packet); qc_esc_packet_init(&_uart_bridge_packet); diff --git a/src/drivers/actuators/modal_io/modal_io.hpp b/src/drivers/actuators/modal_io/modal_io.hpp index a61c269e6b..196728aab7 100644 --- a/src/drivers/actuators/modal_io/modal_io.hpp +++ b/src/drivers/actuators/modal_io/modal_io.hpp @@ -174,7 +174,7 @@ private: } led_rsc_t; ch_assign_t _output_map[MODAL_IO_OUTPUT_CHANNELS] {{1, 1}, {2, 1}, {3, 1}, {4, 1}}; - MixingOutput _mixing_output; + MixingOutput _mixing_output{"MODAL_IO", MODAL_IO_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; perf_counter_t _cycle_perf; perf_counter_t _output_update_perf; diff --git a/src/drivers/adc/ads1115/ADS1115.h b/src/drivers/adc/ads1115/ADS1115.h index 6f3c1e92be..79a31198c9 100644 --- a/src/drivers/adc/ads1115/ADS1115.h +++ b/src/drivers/adc/ads1115/ADS1115.h @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -127,7 +127,7 @@ protected: private: - uORB::Publication _to_adc_report{ORB_ID(adc_report)}; + uORB::PublicationMulti _to_adc_report{ORB_ID(adc_report)}; static const hrt_abstime SAMPLE_INTERVAL{50_ms}; diff --git a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp index a863ac975b..6db6bb76d0 100755 --- a/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp +++ b/src/drivers/barometer/invensense/icp201xx/ICP201XX.cpp @@ -250,7 +250,7 @@ ICP201XX::RunImpl() case STATE::CONFIG: { if (configure()) { _state = STATE::WAIT_READ; - ScheduleDelayed(10_ms); + ScheduleDelayed(30_ms); } else { if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp index 91ab266311..7bf3813542 100644 --- a/src/drivers/camera_capture/camera_capture.cpp +++ b/src/drivers/camera_capture/camera_capture.cpp @@ -226,7 +226,9 @@ void CameraCapture::capture_trampoline(void *context, uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow) { - camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); + if (camera_capture::g_camera_capture) { + camera_capture::g_camera_capture->capture_callback(chan_index, edge_time, edge_state, overflow); + } } void @@ -359,6 +361,11 @@ CameraCapture::stop() work_cancel(HPWORK, &_work_publisher); + if (_capture_channel >= 0) { + up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr); + } + + if (camera_capture::g_camera_capture != nullptr) { delete (camera_capture::g_camera_capture); } diff --git a/src/drivers/cyphal/Publishers/udral/Gnss.hpp b/src/drivers/cyphal/Publishers/udral/Gnss.hpp index 168b1e437f..ef592c7855 100644 --- a/src/drivers/cyphal/Publishers/udral/Gnss.hpp +++ b/src/drivers/cyphal/Publishers/udral/Gnss.hpp @@ -66,9 +66,9 @@ public: size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; - geo.latitude = gps.lat; - geo.longitude = gps.lon; - geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast(gps.alt) }; + geo.latitude = (int64_t)(gps.latitude_deg / 1e7); + geo.longitude = (int64_t)(gps.longitude_deg / 1e7); + geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = gps.altitude_msl_m }; uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; diff --git a/src/drivers/differential_pressure/asp5033/ASP5033.cpp b/src/drivers/differential_pressure/asp5033/ASP5033.cpp new file mode 100644 index 0000000000..9a3a263bbd --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/ASP5033.cpp @@ -0,0 +1,258 @@ +/**************************************************************************** + * + * + * 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 ASP5033.cpp + * + *@author Denislav Petrov + */ + + + +#include "ASP5033.hpp" + +ASP5033::ASP5033(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config) +{ +} + +ASP5033::~ASP5033() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); + perf_free(_fault_perf); +} + +int ASP5033::probe() +{ + int ret = sensor_id_check(); + return ret; +} + +int ASP5033::sensor_id_check() +{ + uint8_t id[1]; + uint8_t cmd_1 = REG_ID_SET_ASP5033; + uint8_t cmd_2 = REG_WHOAMI_RECHECK_ID_ASP5033; + uint8_t cmd_3 = REG_ID_ASP5033; + uint8_t cmd_1_2[2]; + cmd_1_2[0] = static_cast(cmd_1); + cmd_1_2[1] = static_cast(cmd_2); + + + if ((transfer(&cmd_1, 1, &id[0], sizeof(id)) != PX4_OK) || (*id != REG_WHOAMI_DEFAULT_ID_ASP5033)) { return 0; } + + if (transfer(&cmd_1_2[0], 2, nullptr, 0) != PX4_OK) { return 0; } + + if ((transfer(&cmd_3, 1, &id[0], sizeof(id)) != PX4_OK) || (*id != REG_WHOAMI_RECHECK_ID_ASP5033)) { return 0; } + + return 1; + + + +} + +int ASP5033::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + ScheduleNow(); + return ret; +} + + +/** + * @brief calculation of the differential pressure in this way: + * it collect all measured pressure and store it into press_sum, + * count the value of collected times-press_count, then divide both + * and get the actual value of differential pressure - _pressure + * + * @return true if pressure is valid and no errors, false if not + */ +bool ASP5033::get_differential_pressure() +{ + if (hrt_elapsed_time(&last_sample_time) > 200_ms) { + return false; + } + + if (press_count == 0) { + return false; + } + + //calculation differential pressure + _pressure = press_sum / press_count; + + press_sum = 0.; + press_count = 0; + return true; + +} + + +void ASP5033::print_status() +{ + + I2CSPIDriverBase::print_status(); + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_fault_perf); +} + +void ASP5033::RunImpl() +{ + int ret = PX4_ERROR; + + // collection phase + if (_collect_phase) { + // perform collection + ret = collect(); + + if (OK != ret) { + perf_count(_comms_errors); + /* restart the measurement state machine */ + _collect_phase = false; + _sensor_ok = false; + ScheduleNow(); + return; + } + + // next phase is measurement + _collect_phase = false; + + // is there a collect->measure gap? + if (_measure_interval > CONVERSION_INTERVAL) { + + // schedule a fresh cycle call when we are ready to measure again + ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL); + + return; + } + } + + /* measurement phase */ + ret = measure(); + + if (OK != ret) { + DEVICE_DEBUG("measure error"); + } + + _sensor_ok = (ret == OK); + + // next phase is collection + _collect_phase = true; + + // schedule a fresh cycle call when the measurement is done + ScheduleDelayed(CONVERSION_INTERVAL); +} + + +int ASP5033::measure() +{ + // Send the command to begin a measurement. + uint8_t cmd_1 = CMD_MEASURE_ASP5033; + uint8_t cmd_2 = REG_CMD_ASP5033;; + + //write to driver to start + uint8_t cmd[2]; + cmd[0] = static_cast(cmd_2); + cmd[1] = static_cast(cmd_1); + int ret = transfer(&cmd[0], 2, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + } + + return ret; +} + +int ASP5033::collect() +{ + perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + + + // Read pressure and temperature as one block + uint8_t val[5] {0, 0, 0, 0, 0}; + uint8_t cmd = REG_PRESS_DATA_ASP5033; + transfer(&cmd, 1, &val[0], sizeof(val)); + + //Pressure is a signed 24-bit value + int32_t press = (val[0] << 24) | (val[1] << 16) | (val[2] << 8); + // convert back to 24 bit + press >>= 8; + + // k is a shift based on the pressure range of the device. See + // table in the datasheet + constexpr uint8_t k = 7; + constexpr float press_scale = 1.0f / (1U << k); //= 1.0f / (1U << k); + press_sum += press * press_scale; + press_count++; + + // temperature is 16 bit signed in units of 1/256 C + const int16_t temp = (val[3] << 8) | val[4]; + constexpr float temp_scale = 1.0f / 256; + _temperature = temp * temp_scale; + last_sample_time = hrt_absolute_time(); + bool status = get_differential_pressure(); + + if (status == true && (int)_temperature != 0) { + // publish values + differential_pressure_s differential_pressure{}; + differential_pressure.timestamp_sample = timestamp_sample; + differential_pressure.device_id = get_device_id(); + differential_pressure.differential_pressure_pa = _pressure; + differential_pressure.temperature = _temperature ; + differential_pressure.error_count = perf_event_count(_comms_errors); + differential_pressure.timestamp = timestamp_sample; + _differential_pressure_pub.publish(differential_pressure); + + } + + perf_end(_sample_perf); + + return PX4_OK; +} + + + + + diff --git a/src/drivers/differential_pressure/asp5033/ASP5033.hpp b/src/drivers/differential_pressure/asp5033/ASP5033.hpp new file mode 100644 index 0000000000..37a9fdeb8f --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/ASP5033.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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 ASP5033.hpp + * + * Driver for ASP5033 connected via I2C. + * + * Supported sensors: + * + * - ASP5033 + * + * Interface application notes: + * + * + *@author Denislav Petrov + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include + +#include +#include + + + + +/* Measurement rate is 100Hz */ +#define MEAS_RATE 100 +#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ + + +/* Configuration Constants */ +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x6D; /* 0x6D 0xE4 */ +static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface + + +#define REG_CMD_ASP5033 0x30 +#define REG_PRESS_DATA_ASP5033 0X06 +#define REG_TEMP_DATA_ASP5033 0X09 +#define CMD_MEASURE_ASP5033 0X0A +#define REG_WHOAMI_DEFAULT_ID_ASP5033 0X00 +#define REG_WHOAMI_RECHECK_ID_ASP5033 0X66 +#define REG_ID_ASP5033 0x01 +#define REG_ID_SET_ASP5033 0xa4 + +using namespace time_literals; + + +class ASP5033 : public device::I2C, public I2CSPIDriver +{ +public: + ASP5033(const I2CSPIDriverConfig &config); + ~ASP5033() override; + + static void print_usage(); + void print_status() override; + + + void RunImpl(); + + int init() override; + + + + float press_sum; + uint32_t press_count; + + +private: + + float _pressure = 0.f; + float _temperature = 0.f; + float _pressure_prev = 0.f; + float _temperaute_prev = 0.f; + + int probe() override; + + int measure(); + int collect(); + int sensor_id_check(); + + bool get_differential_pressure(); + hrt_abstime last_sample_time = hrt_absolute_time(); + orb_advert_t _mavlink_log_pub {nullptr}; //log send to + + + uint32_t _measure_interval{CONVERSION_INTERVAL}; + uint32_t _conversion_interval{CONVERSION_INTERVAL}; + + bool _sensor_ok{false}; + bool _collect_phase{false}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + 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/differential_pressure/asp5033/CMakeLists.txt b/src/drivers/differential_pressure/asp5033/CMakeLists.txt new file mode 100644 index 0000000000..3ca04b9d67 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# 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__differential_pressure__asp5033 + MAIN asp5033 + COMPILE_FLAGS + SRCS + asp5033_main.cpp + ASP5033.cpp + ASP5033.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/differential_pressure/asp5033/Kconfig b/src/drivers/differential_pressure/asp5033/Kconfig new file mode 100644 index 0000000000..6bf415af2e --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033 + bool "asp5033" + default n + ---help--- + Enable support for asp5033 diff --git a/src/drivers/differential_pressure/asp5033/asp5033_main.cpp b/src/drivers/differential_pressure/asp5033/asp5033_main.cpp new file mode 100644 index 0000000000..f34597ef37 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/asp5033_main.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * 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 "ASP5033.hpp" +#include +#include + + + + +void ASP5033::print_usage() +{ + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver to enable an external [ASP5033] +(https://www.qio-tek.com/index.php/product/qiotek-asp5033-dronecan-airspeed-and-compass-module/) +TE connected via I2C. +This is not included by default in firmware. It can be included with terminal command: "make boardconfig" +or in default.px4board with adding the line: "CONFIG_DRIVERS_DIFFERENTIAL_PRESSURE_ASP5033=y" +It can be enabled with the "SENS_EN_ASP5033" parameter set to 1. +)DESCR_STR"); + PRINT_MODULE_USAGE_NAME("asp5033", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x6D); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int asp5033_main(int argc, char *argv[]) +{ + + using ThisDriver = ASP5033; + BusCLIArguments cli{true, false}; + cli.i2c_address = I2C_ADDRESS_DEFAULT; + cli.default_i2c_frequency = I2C_SPEED; + + const char *verb = cli.parseDefaultArguments(argc, argv); + + + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_ASP5033); + + + if (!strcmp(verb, "start")) { + return ThisDriver::module_start(cli, iterator); + + } else if (!strcmp(verb, "stop")) { + return ThisDriver::module_stop(iterator); + + } else if (!strcmp(verb, "status")) { + return ThisDriver::module_status(iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/drivers/differential_pressure/asp5033/parameters.c b/src/drivers/differential_pressure/asp5033/parameters.c new file mode 100644 index 0000000000..5b50f628f6 --- /dev/null +++ b/src/drivers/differential_pressure/asp5033/parameters.c @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + + +/** + * ASP5033 differential pressure sensor (external I2C) + * + * @reboot_required true + * @group Sensors + * @boolean + */ +PARAM_DEFINE_INT32(SENS_EN_ASP5033, 0); + + + + diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp index b66eeeba3d..4419530515 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.cpp @@ -33,6 +33,7 @@ /* Include Files */ #include "AFBRS50.hpp" +#include "argus_hal_test.h" #include @@ -42,9 +43,6 @@ /*! Define the SPI baud rate (to be used in the SPI module). */ #define SPI_BAUD_RATE 5000000 -#define LONG_RANGE_MODE_HZ 25 -#define SHORT_RANGE_MODE_HZ 50 - #include "s2pi.h" #include "timer.h" #include "argus_hal_test.h" @@ -52,6 +50,7 @@ AFBRS50 *g_dev{nullptr}; AFBRS50::AFBRS50(uint8_t device_orientation): + ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default), _px4_rangefinder(0, device_orientation) { @@ -61,6 +60,7 @@ AFBRS50::AFBRS50(uint8_t device_orientation): device_id.devid_s.devtype = DRV_DIST_DEVTYPE_AFBRS50; _px4_rangefinder.set_device_id(device_id.devid); + _px4_rangefinder.set_device_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); } AFBRS50::~AFBRS50() @@ -70,12 +70,12 @@ AFBRS50::~AFBRS50() perf_free(_sample_perf); } -status_t AFBRS50::measurement_ready_callback(status_t status, void *data) +status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd) { if (!up_interrupt_context()) { if (status == STATUS_OK) { if (g_dev) { - g_dev->ProcessMeasurement(data); + g_dev->ProcessMeasurement(hnd); } } else { @@ -86,35 +86,33 @@ status_t AFBRS50::measurement_ready_callback(status_t status, void *data) return status; } -void AFBRS50::ProcessMeasurement(void *data) +void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd) { - if (data != nullptr) { - perf_count(_sample_perf); + perf_count(_sample_perf); - argus_results_t res{}; - status_t evaluate_status = Argus_EvaluateData(_hnd, &res, data); + argus_results_t res{}; + status_t evaluate_status = Argus_EvaluateData(hnd, &res); - if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) { - uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); - float result_m = static_cast(result_mm) / 1000.f; - int8_t quality = res.Bin.SignalQuality; + if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) { + uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000); + float result_m = static_cast(result_mm) / 1000.f; + int8_t quality = res.Bin.SignalQuality; - // Signal quality indicates 100% for good signals, 50% and lower for weak signals. - // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. - if (quality == 1) { - quality = 0; - } - - // distance quality check - if (result_m > _max_distance) { - result_m = 0.0; - quality = 0; - } - - _current_distance = result_m; - _current_quality = quality; - _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); + // Signal quality indicates 100% for good signals, 50% and lower for weak signals. + // 1% is an errored signal (not reliable). Signal Quality of 0% is unknown. + if (quality == 1) { + quality = 0; } + + // distance quality check + if (result_m > _max_distance) { + result_m = 0.0; + quality = 0; + } + + _current_distance = result_m; + _current_quality = quality; + _px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality); } } @@ -137,7 +135,37 @@ int AFBRS50::init() // Initialize the S2PI hardware required by the API. S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE); - status_t status = Argus_Init(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS); + 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); if (status == STATUS_OK) { uint32_t id = Argus_GetChipID(_hnd); @@ -148,7 +176,6 @@ int AFBRS50::init() 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); - argus_laser_type_t lt = Argus_GetLaserType(_hnd); switch (mv) { case AFBR_S50MV85G_V1: @@ -168,19 +195,20 @@ int AFBRS50::init() case AFBR_S50LV85D_V1: _min_distance = 0.08f; - - if (lt == LASER_H_V2X) { - _max_distance = 50.f; - PX4_INFO_RAW("AFBR-S50LX85D (v2)\n"); - - } else { - _max_distance = 30.f; - PX4_INFO_RAW("AFBR-S50LV85D (v1)\n"); - } - + _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.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"); break; case AFBR_S50MV68B_V1: @@ -223,6 +251,9 @@ int AFBRS50::init() ScheduleDelayed(_measure_interval); return PX4_OK; + + } else { + PX4_ERR("Argus_InitMode failed: %ld", status); } return PX4_ERROR; @@ -230,6 +261,15 @@ int AFBRS50::init() void AFBRS50::Run() { + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // update parameters from storage + ModuleParams::updateParams(); + } + switch (_state) { case STATE::TEST: { if (_testing) { @@ -243,7 +283,8 @@ void AFBRS50::Run() break; case STATE::CONFIGURE: { - status_t status = set_rate(SHORT_RANGE_MODE_HZ); + _current_rate = (uint32_t)_p_sens_afbr_s_rate.get(); + status_t status = set_rate(_current_rate); if (status != STATUS_OK) { PX4_ERR("CONFIGURE status not okay: %i", (int)status); @@ -251,24 +292,18 @@ void AFBRS50::Run() ScheduleNow(); } - status = Argus_SetConfigurationDFMMode(_hnd, ARGUS_MODE_B, DFM_MODE_8X); + 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_SetConfigurationDFMMode(_hnd, ARGUS_MODE_A, DFM_MODE_8X); + status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false); if (status != STATUS_OK) { - PX4_ERR("Argus_SetConfigurationDFMMode status not okay: %i", (int)status); - } - - // start in short range mode - _mode = ARGUS_MODE_B; - set_mode(_mode); - - if (status != STATUS_OK) { - PX4_ERR("CONFIGURE status not okay: %i", (int)status); + PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status); ScheduleNow(); } else { @@ -288,7 +323,7 @@ void AFBRS50::Run() } } - UpdateMode(); + Evaluate_rate(); } break; @@ -306,49 +341,41 @@ void AFBRS50::Run() ScheduleDelayed(_measure_interval); } -void AFBRS50::UpdateMode() +void AFBRS50::Evaluate_rate() { - // only update mode if _current_distance is a valid measurement - if ((_current_distance > 0) && (_current_quality > 0)) { + // only update mode if _current_distance is a valid measurement and if the last rate switch was more than 1 second ago + if ((_current_distance > 0) && (_current_quality > 0) && ((hrt_absolute_time() - _last_rate_switch) > 1_s)) { - if ((_current_distance >= _long_range_threshold) && (_mode != ARGUS_MODE_A)) { - // change to long range mode - argus_mode_t mode = ARGUS_MODE_A; - status_t status = set_mode(mode); + status_t status = STATUS_OK; - if (status != STATUS_OK) { - PX4_ERR("set_mode status not okay: %i", (int)status); - } + if ((_current_distance >= (_p_sens_afbr_thresh.get() + _p_sens_afbr_hyster.get())) + && (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) { - status = set_rate(LONG_RANGE_MODE_HZ); + _current_rate = (uint32_t)_p_sens_afbr_l_rate.get(); + status = set_rate(_current_rate); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); + + } else { + PX4_INFO("switched to long range rate: %i", (int)_current_rate); + _last_rate_switch = hrt_absolute_time(); } - status = set_rate(LONG_RANGE_MODE_HZ); + } else if ((_current_distance <= (_p_sens_afbr_thresh.get() - _p_sens_afbr_hyster.get())) + && (_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); if (status != STATUS_OK) { PX4_ERR("set_rate status not okay: %i", (int)status); - } - } else if ((_current_distance <= _short_range_threshold) && (_mode != ARGUS_MODE_B)) { - // change to short range mode - argus_mode_t mode = ARGUS_MODE_B; - status_t status = set_mode(mode); - - if (status != STATUS_OK) { - PX4_ERR("set_mode status not okay: %i", (int)status); - } - - status = set_rate(SHORT_RANGE_MODE_HZ); - - if (status != STATUS_OK) { - PX4_ERR("set_rate status not okay: %i", (int)status); + } else { + PX4_INFO("switched to short range rate: %i", (int)_current_rate); + _last_rate_switch = hrt_absolute_time(); } } - - ScheduleDelayed(1000_ms); // don't switch again for at least 1 second } } @@ -373,33 +400,6 @@ void AFBRS50::print_info() get_info(); } -status_t AFBRS50::set_mode(argus_mode_t mode) -{ - while (Argus_GetStatus(_hnd) != STATUS_IDLE) { - px4_usleep(1_ms); - } - - status_t status = Argus_SetConfigurationMeasurementMode(_hnd, mode); - - if (status != STATUS_OK) { - PX4_ERR("Argus_SetConfigurationMeasurementMode status not okay: %i", (int)status); - return status; - } - - argus_mode_t current_mode; - status = Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); - - if (status != STATUS_OK) { - PX4_ERR("Argus_GetConfigurationMeasurementMode status not okay: %i", (int)status); - return status; - - } else { - _mode = current_mode; - } - - return status; -} - status_t AFBRS50::set_rate(uint32_t rate_hz) { while (Argus_GetStatus(_hnd) != STATUS_IDLE) { @@ -429,13 +429,10 @@ status_t AFBRS50::set_rate(uint32_t rate_hz) void AFBRS50::get_info() { - argus_mode_t current_mode; argus_dfm_mode_t dfm_mode; - Argus_GetConfigurationMeasurementMode(_hnd, ¤t_mode); - Argus_GetConfigurationDFMMode(_hnd, current_mode, &dfm_mode); + Argus_GetConfigurationDFMMode(_hnd, &dfm_mode); PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance); - PX4_INFO_RAW("mode: %d\n", current_mode); PX4_INFO_RAW("dfm mode: %d\n", dfm_mode); PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval)); } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp index f7503b321b..2ad767b2fa 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp +++ b/src/drivers/distance_sensor/broadcom/afbrs50/AFBRS50.hpp @@ -44,13 +44,16 @@ #include #include #include -#include #include +#include +#include #include +#include +#include using namespace time_literals; -class AFBRS50 : public px4::ScheduledWorkItem +class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem { public: AFBRS50(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING); @@ -75,18 +78,16 @@ public: private: void Run() override; - void UpdateMode(); + void Evaluate_rate(); - void ProcessMeasurement(void *data); + void ProcessMeasurement(argus_hnd_t *hnd); - static status_t measurement_ready_callback(status_t status, void *data); + static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd); void get_info(); - status_t set_mode(argus_mode_t mode); status_t set_rate(uint32_t rate_hz); argus_hnd_t *_hnd{nullptr}; - argus_mode_t _mode{ARGUS_MODE_B}; // Short-Range enum class STATE : uint8_t { TEST, @@ -98,14 +99,24 @@ private: PX4Rangefinder _px4_rangefinder; hrt_abstime _measurement_time{0}; + hrt_abstime _last_rate_switch{0}; perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")}; uint32_t _measure_interval{1000000 / 50}; // 50Hz float _current_distance{0}; int8_t _current_quality{0}; - const float _short_range_threshold = 4.0; //meters - const float _long_range_threshold = 6.0; //meters float _max_distance; float _min_distance; + uint32_t _current_rate{0}; + + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + DEFINE_PARAMETERS( + (ParamInt) _p_sens_afbr_mode, + (ParamInt) _p_sens_afbr_s_rate, + (ParamInt) _p_sens_afbr_l_rate, + (ParamInt) _p_sens_afbr_thresh, + (ParamInt) _p_sens_afbr_hyster + ); }; 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 61950a12b8..015249f859 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/API/Src/s2pi.c @@ -85,6 +85,8 @@ static int gpio_falling_edge(int irq, void *context, void *arg) status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps) { + (void)defaultSlave; + px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_CS); s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS); @@ -107,11 +109,24 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps) * - #STATUS_BUSY: An SPI transfer is in progress. * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. *****************************************************************************/ -status_t S2PI_GetStatus(void) +status_t S2PI_GetStatus(s2pi_slave_t slave) { + (void)slave; + return s2pi_.Status; } +status_t S2PI_TryGetMutex(s2pi_slave_t slave) +{ + (void) slave; + return STATUS_OK; +} + +void S2PI_ReleaseMutex(s2pi_slave_t slave) +{ + (void) slave; +} + /*!*************************************************************************** * @brief Sets the SPI baud rate in bps. * @param baudRate_Bps The default SPI baud rate in bauds-per-second. @@ -135,8 +150,10 @@ status_t S2PI_SetBaudRate(uint32_t baudRate_Bps) * switch back to ordinary SPI functionality. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_CaptureGpioControl(void) +status_t S2PI_CaptureGpioControl(s2pi_slave_t slave) { + (void)slave; + /* Check if something is ongoing. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -165,8 +182,10 @@ status_t S2PI_CaptureGpioControl(void) * the #S2PI_CaptureGpioControl function. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_ReleaseGpioControl(void) +status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave) { + (void)slave; + /* Check if something is ongoing. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -202,6 +221,8 @@ status_t S2PI_ReleaseGpioControl(void) *****************************************************************************/ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value) { + (void)slave; + /* Check if pin is valid. */ if (pin > S2PI_IRQ || value > 1) { return ERROR_INVALID_ARGUMENT; @@ -228,6 +249,8 @@ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value) *****************************************************************************/ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value) { + (void)slave; + /* Check if pin is valid. */ if (pin > S2PI_IRQ || !value) { return ERROR_INVALID_ARGUMENT; @@ -255,6 +278,8 @@ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value) *****************************************************************************/ status_t S2PI_CycleCsPin(s2pi_slave_t slave) { + (void)slave; + /* Check the driver status. */ IRQ_LOCK(); status_t status = s2pi_.Status; @@ -372,8 +397,10 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8 * invoked with the #ERROR_ABORTED error byte. * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_Abort(void) +status_t S2PI_Abort(s2pi_slave_t slave) { + (void)slave; + status_t status = s2pi_.Status; /* Check if something is ongoing. */ @@ -405,6 +432,8 @@ status_t S2PI_Abort(void) *****************************************************************************/ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData) { + (void)slave; + s2pi_.IrqCallback = callback; s2pi_.IrqCallbackData = callbackData; @@ -430,5 +459,7 @@ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, v *****************************************************************************/ uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave) { + (void)slave; + return px4_arch_gpioread(s2pi_.GPIOs[S2PI_IRQ]); } diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h index a933976370..44cdc92168 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_api.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides generic functionality belonging to all - * devices from the AFBR-S50 product family. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides generic functionality belonging to all + * devices from the AFBR-S50 product family. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,21 +37,21 @@ #ifndef ARGUS_API_H #define ARGUS_API_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argusapi AFBR-S50 API + * @defgroup argus_api AFBR-S50 API + * @ingroup argus * - * @brief The main module of the API from the AFBR-S50 SDK. + * @brief The main module of the API from the AFBR-S50 SDK. * - * @details General API for the AFBR-S50 time-of-flight sensor device family.\n - * See the \ref getting_started Guide for a detailed description - * on how to use the module/API. + * @details General API for the AFBR-S50 time-of-flight sensor device family.\n + * See the \ref getting_started Guide for a detailed description + * on how to use the module/API. * - * @addtogroup argusapi + * @addtogroup argus_api * @{ *****************************************************************************/ @@ -61,213 +61,301 @@ extern "C" { #include "argus_dfm.h" #include "argus_snm.h" #include "argus_xtalk.h" - -/*! The data structure for the API representing a AFBR-S50 device instance. */ -typedef void argus_hnd_t; +#include "argus_offset.h" /*! The S2PI slave identifier. */ typedef int32_t s2pi_slave_t; /*!*************************************************************************** - * @brief Initializes the API modules and the device with default parameters. + * @brief Initializes the device with default measurement mode. * * @details The function that needs to be called once after power up to - * initialize the modules state (i.e. the corresponding handle) and the - * dedicated Time-of-Flight device. In order to obtain a handle, - * reference the #Argus_CreateHandle method. + * initialize the modules state (i.e. the corresponding handle) and the + * dedicated Time-of-Flight device. In order to obtain a handle, + * reference the #Argus_CreateHandle method. * - * Prior to calling the function, the required peripherals (i.e. S2PI, - * GPIO w/ IRQ and Timers) must be initialized and ready to use. + * Prior to calling the function, the required peripherals (i.e. S2PI, + * GPIO w/ IRQ and Timers) must be initialized and ready to use. * - * The function executes the following tasks: - * - Initialization of the internal state represented by the handle - * object. - * - Setup the device such that an safe configuration is present in - * the registers. - * - Initialize sub modules such as calibration or measurement modules. - * . + * The function executes the following tasks: + * - Initialization of the internal state represented by the handle + * object. + * - Setup the device such that an safe configuration is present in + * the registers. + * - Initialize sub modules such as calibration or measurement modules. + * . * - * The modules configuration is initialized with reasonable default values. + * The modules configuration is initialized with reasonable default + * values. Note that the default measurement mode depends on the + * given device. * - * @param hnd The API handle; contains all internal states and data. + * Also refer to #Argus_InitMode, which uses an specified measurement + * mode instead of the dedicated default measurement mode. * - * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ - * lines. This is actually just a number that is passed - * to the SPI interface to distinct for multiple SPI slave - * devices. Note that the slave must be not equal to 0, - * since is reserved for error handling. + * @param hnd The API handle; contains all internal states and data. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Init(argus_hnd_t *hnd, s2pi_slave_t spi_slave); /*!*************************************************************************** - * @brief Reinitializes the API modules and the device with default parameters. + * @brief Initializes the device with specified measurement mode. * - * @details The function reinitializes the device with default configuration. - * Can be used as reset sequence for the device. See #Argus_Init for - * more information on the initialization. + * @details The function that needs to be called once after power up to + * initialize the modules state (i.e. the corresponding handle) and the + * dedicated Time-of-Flight device. In order to obtain a handle, + * reference the #Argus_CreateHandle method. * - * Note that the #Argus_Init function must be called first! Otherwise, - * the function will return an error if it is called for an yet - * uninitialized device/handle. + * Prior to calling the function, the required peripherals (i.e. S2PI, + * GPIO w/ IRQ and Timers) must be initialized and ready to use. * - * @param hnd The API handle; contains all internal states and data. + * The function executes the following tasks: + * - Initialization of the internal state represented by the handle + * object. + * - Setup the device such that an safe configuration is present in + * the registers. + * - Initialize sub modules such as calibration or measurement modules. + * . * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * The modules configuration is initialized with reasonable default values. + * + * Also refer to #Argus_Init, which uses the dedicated default measurement + * mode instead of an user specified measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @param mode The specified measurement mode to be initialized. + * Pass 0 as special value to select default measurement mode + * (see #Argus_Init). + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_InitMode(argus_hnd_t *hnd, s2pi_slave_t spi_slave, argus_mode_t mode); + +/*!*************************************************************************** + * @brief Reinitializes the device with the current measurement mode. + * + * @details The function reinitializes the device with the currently active + * measurement mode. + * + * This can be used as a soft reset for the device and API. + * See #Argus_Init for more information on the initialization. + * + * Note that the #Argus_Init or #Argus_InitMode function must be called + * first! Otherwise, the function will return an error if it is called + * for an yet uninitialized device/handle. + * + * Also refer to #Argus_ReinitMode, which uses a specified measurement + * mode instead of the currently active measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Reinit(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Deinitializes the API modules and the device. + * @brief Reinitializes the device with a specified measurement mode. + * + * @details The function reinitializes the device with a specified (/p mode) + * measurement mode. + * + * This can be used as a soft reset for the device and API. + * See #Argus_InitMode for more information on the initialization. + * + * Note that the #Argus_Init or #Argus_InitMode function must be called + * first! Otherwise, the function will return an error if it is called + * for an yet uninitialized device/handle. + * + * Also refer to #Argus_Reinit, which re-uses the currently active + * measurement mode instead of an user specified measurement mode. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param mode The specified measurement mode to be initialized. + * Pass 0 as special value to select the current measurement mode + * (see #Argus_Init). + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ReinitMode(argus_hnd_t *hnd, argus_mode_t mode); + +/*!*************************************************************************** + * @brief Deinitializes the API modules and the device. * * @details The function deinitializes the device and clear all internal states. - * Can be used to cleanup before releasing the memory. The device - * can not be used any more and must be initialized again prior to next - * usage. + * Can be used to cleanup before releasing the memory. The device + * can not be used any more and must be initialized again prior to next + * usage. * - * Note that the #Argus_Init function must be called first! Otherwise, - * the function will return an error if it is called for an yet - * uninitialized device/handle. + * Note that the #Argus_Init function must be called first! Otherwise, + * the function will return an error if it is called for an yet + * uninitialized device/handle. * - * @param hnd The API handle; contains all internal states and data. + * @param hnd The API handle; contains all internal states and data. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Deinit(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Creates a new device data handle object to store all internal states. + * @brief Creates a new device data handle object to store all internal states. * * @details The function must be called to obtain a new device handle object. - * The handle is basically an abstract object in memory that contains - * all the internal states and settings of the API module. The handle - * is passed to all the API methods in order to address the specified - * device. This allows to use the API with more than a single measurement - * device. + * The handle is basically an abstract object in memory that contains + * all the internal states and settings of the API module. The handle + * is passed to all the API methods in order to address the specified + * device. This allows to use the API with more than a single measurement + * device. * - * The handler is created by calling the memory allocation method from - * the standard library: @code void * malloc(size_t size) @endcode - * In order to implement an individual memory allocation method, - * define and implement the following weakly binded method and return - * a pointer to the newly allocated memory. * + * The handler is created by calling the memory allocation method from + * the standard library: @code void * malloc(size_t size) @endcode + * In order to implement an individual memory allocation method, + * define and implement the following weakly binded method and return + * a pointer to the newly allocated memory. * * - * @code void * Argus_Malloc (size_t size) @endcode + * @code void * Argus_Malloc (size_t size) @endcode * - * Also see the #Argus_DestroyHandle method for the corresponding - * deallocation of the allocated memory. + * Also see the #Argus_DestroyHandle method for the corresponding + * deallocation of the allocated memory. * - * @note Although the method is using memory allocated on the heap, it - * is eventually no dynamic memory allocation, since the block of - * memory is kept all the time and no memory blocks are dynamically - * freed and re-allocated. If the usage of heap must be avoided, one - * can always implement its own version of the `Argus_Malloc` function - * to create the memory elsewhere. + * @note Although the method is using memory allocated on the heap, it + * is eventually no dynamic memory allocation, since the block of + * memory is kept all the time and no memory blocks are dynamically + * freed and re-allocated. If the usage of heap must be avoided, one + * can always implement its own version of the `Argus_Malloc` function + * to create the memory elsewhere. * - * @return Returns a pointer to the newly allocated device handler object. - * Returns a null pointer if the allocation failed! + * @return Returns a pointer to the newly allocated device handler object. + * Returns a null pointer if the allocation failed! *****************************************************************************/ argus_hnd_t *Argus_CreateHandle(void); /*!*************************************************************************** - * @brief Destroys a given device data handle object. + * @brief Destroys a given device data handle object. * * @details The function can be called to free the previously created device - * data handle object in order to save memory when the device is not - * used any more. + * data handle object in order to save memory when the device is not + * used any more. * - * Please refer to the #Argus_CreateHandle method for the corresponding - * allocation of the memory. + * Note that the handle must be deinitialized before it can be + * destroyed. The function returns #ERROR_FAIL if the handle is not + * yet deinitialized. * - * The handler is destroyed by freeing the corresponding memory with the - * method from the standard library, @code void free(void * ptr) @endcode. - * In order to implement an individual memory deallocation method, define - * and implement the following weakly binded method and free the memory - * object passed to the method by a pointer. + * Please refer to the #Argus_CreateHandle method for the corresponding + * allocation of the memory. * - * @code void Argus_Free (void * ptr) @endcode + * The handler is destroyed by freeing the corresponding memory with the + * method from the standard library, @code void free(void * ptr) @endcode. + * In order to implement an individual memory deallocation method, define + * and implement the following weakly binded method and free the memory + * object passed to the method by a pointer. * - * Also see the #Argus_CreateHandle method for the corresponding - * allocation of the required memory. + * @code void Argus_Free (void * ptr) @endcode * - * @param hnd The device handle object to be deallocated. + * Also see the #Argus_CreateHandle method for the corresponding + * allocation of the required memory. + * + * @param hnd The device handle object to be deallocated. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -void Argus_DestroyHandle(argus_hnd_t *hnd); +status_t Argus_DestroyHandle(argus_hnd_t *hnd); /*!************************************************************************** * Generic API ****************************************************************************/ /*!*************************************************************************** - * @brief Gets the version number of the current API library. + * @brief Gets the version number of the current API library. * - * @details The version is compiled of a major (a), minor (b) and bugfix (c) - * number: a.b.c. + * @details The version is compiled of a major (a), minor (b) and bugfix (c) + * number: a.b.c. * - * The values are encoded into a 32-bit value: + * The values are encoded into a 32-bit value: * - * - [ 31 .. 24 ] - Major Version Number - * - [ 23 .. 16 ] - Minor Version Number - * - [ 15 .. 0 ] - Bugfix Version Number - * . + * - [ 31 .. 24 ] - Major Version Number + * - [ 23 .. 16 ] - Minor Version Number + * - [ 15 .. 0 ] - Bugfix Version Number + * . * - * To obtain the parts from the returned uin32_t value: + * To obtain the parts from the returned uin32_t value: * - * @code - * uint32_t value = Argus_GetAPIVersion(); - * uint8_t a = (value >> 24) & 0xFFU; - * uint8_t b = (value >> 16) & 0xFFU; - * uint8_t c = value & 0xFFFFU; - * @endcode + * @code + * uint32_t value = Argus_GetAPIVersion(); + * uint8_t a = (value >> 24) & 0xFFU; + * uint8_t b = (value >> 16) & 0xFFU; + * uint8_t c = value & 0xFFFFU; + * @endcode * - * @return Returns the current version number. + * @return Returns the current version number. *****************************************************************************/ uint32_t Argus_GetAPIVersion(void); /*!*************************************************************************** - * @brief Gets the build number of the current API library. + * @brief Gets the build number of the current API library. * - * @return Returns the current build number as a C-string. + * @return Returns the current build number as a C-string. *****************************************************************************/ char const *Argus_GetBuildNumber(void); /*!*************************************************************************** - * @brief Gets the version/variant of the module. + * @brief Gets the version/variant of the module. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current module number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current module number. *****************************************************************************/ argus_module_version_t Argus_GetModuleVersion(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the version number of the chip. + * @brief Gets the name string of the module. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current version number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current module name. + *****************************************************************************/ +char const *Argus_GetModuleName(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the version number of the chip. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current version number. *****************************************************************************/ argus_chip_version_t Argus_GetChipVersion(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the type number of the device laser. + * @brief Gets the type number of the device laser. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the current device laser type number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the current device laser type number. *****************************************************************************/ argus_laser_type_t Argus_GetLaserType(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the unique identification number of the chip. + * @brief Gets the unique identification number of the chip. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the unique identification number. + * @param hnd The API handle; contains all internal states and data. + * @return Returns the unique identification number. *****************************************************************************/ uint32_t Argus_GetChipID(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Gets the SPI hardware slave identifier. + * @brief Gets the SPI hardware slave identifier. * - * @param hnd The API handle; contains all internal states and data. - * @return The SPI hardware slave identifier. + * @param hnd The API handle; contains all internal states and data. + * @return The SPI hardware slave identifier. *****************************************************************************/ s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd); @@ -276,323 +364,425 @@ s2pi_slave_t Argus_GetSPISlave(argus_hnd_t *hnd); /*!************************************************************************** * Measurement/Device Operation **************************************************************************** - * @addtogroup argusmeas + * @addtogroup argus_meas * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Starts the timer based measurement cycle asynchronously. + * @brief Starts the timer based measurement cycle asynchronously. * * @details This function starts a timer based measurement cycle asynchronously - * in the background. A periodic timer interrupt triggers the measurement - * frames on the ASIC and the data readout afterwards. + * in the background. A periodic timer interrupt triggers the measurement + * frames on the ASIC and the data readout afterwards. * - * When the measurement has finished, a callback (which is passed as - * a parameter to the function) is invoked in order to inform the - * main thread to call the \link #Argus_EvaluateData data evaluation - * method\endlink. This call is mandatory to release the data buffer - * for the next measurement and it must not be invoked directly from - * the callback since it is currently within an interrupt service - * routine. Rather a flag should inform the main thread or task - * scheduler to invoke the evaluation as soon as possible in order - * to not introduce any unwanted delays to the next measurement frame. + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation as soon as possible in order + * to not introduce any unwanted delays to the next measurement frame. * - * The next measurement frame will be started as soon as the pre- - * conditions are meet. These are: - * 1. timer flag set (i.e. a certain time has passed since the last - * measurement in order to fulfill eye-safety), - * 2. device idle (i.e. no measurement currently ongoing) and - * 3. data buffer ready (i.e. the previous data has been evaluated). + * The next measurement frame will be started as soon as the pre- + * conditions are meet. These are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). * - * Usually, the device idle and data buffer ready conditions are met - * before the timer tick occurs and thus the timer dictates the frame - * rate. + * Usually, the device idle and data buffer ready conditions are met + * before the timer tick occurs and thus the timer dictates the frame + * rate. * - * The callback function pointer will be invoked when the measurement - * frame has finished successfully or whenever an error, that cannot - * be handled internally, occurs. + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. * - * The periodic timer interrupts are used to check the measurement status - * for timeouts. An error is invoked when a measurement cycle have not - * finished within the specified time. + * The periodic timer interrupts are used to check the measurement status + * for timeouts. An error is invoked when a measurement cycle have not + * finished within the specified time. * - * Use #Argus_StopMeasurementTimer to stop the measurements. + * Use #Argus_StopMeasurementTimer to stop the measurements. * - * @note In order to use this function, the periodic interrupt timer module - * (see @ref argus_timer) must be implemented! + * @note In order to use this function, the periodic interrupt timer module + * (see @ref argus_timer) must be implemented! * - * @param hnd The API handle; contains all internal states and data. - * @param cb Callback function that will be invoked when the measurement - * is completed. Its parameters are the \link #status_t status - * \endlink and a pointer to the \link #argus_results_t results - * \endlink structure. If an error occurred, the status differs - * from #STATUS_OK and the second parameter is null. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink of the finished measurement cycle and the pointer to + * the calling \link #argus_hnd_t API handle\endlink, i.e. the + * /p hnd value. The latter must be passed to the + * #Argus_EvaluateData function. + * If an error occurred, the status differs from #STATUS_OK. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, argus_callback_t cb); +status_t Argus_StartMeasurementTimer(argus_hnd_t *hnd, + argus_measurement_ready_callback_t cb); /*!*************************************************************************** - * @brief Stops the timer based measurement cycle. + * @brief Stops the timer based measurement cycle. * * @details This function stops the ongoing timer based measurement cycles - * that have been started using the #Argus_StartMeasurementTimer - * function. + * that have been started using the #Argus_StartMeasurementTimer + * function. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_StopMeasurementTimer(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Triggers a single measurement frame asynchronously. + * @brief Triggers a single measurement frame asynchronously. * * @details This function immediately triggers a single measurement frame - * asynchronously if all the pre-conditions are met. Otherwise it - * returns with a corresponding status (e.g. #STATUS_BUSY or - * #STATUS_ARGUS_POWERLIMIT). + * asynchronously if all the pre-conditions are met. Otherwise it + * returns with a corresponding status (e.g. #STATUS_BUSY or + * #STATUS_ARGUS_POWERLIMIT). * - * When the measurement has finished, a callback (which is passed as - * a parameter to the function) is invoked in order to inform the - * main thread to call the \link #Argus_EvaluateData data evaluation - * method\endlink. This call is mandatory to release the data buffer - * for the next measurement and it must not be invoked directly from - * the callback since it is currently within an interrupt service - * routine. Rather a flag should inform the main thread or task - * scheduler to invoke the evaluation task. + * When the measurement has finished, a callback (which is passed as + * a parameter to the function) is invoked in order to inform the + * main thread to call the \link #Argus_EvaluateData data evaluation + * method\endlink. This call is mandatory to release the data buffer + * for the next measurement and it must not be invoked directly from + * the callback since it is currently within an interrupt service + * routine. Rather a flag should inform the main thread or task + * scheduler to invoke the evaluation task. * - * The pre-conditions for starting a measurement frame are: - * 1. timer flag set (i.e. a certain time has passed since the last - * measurement in order to fulfill eye-safety), - * 2. device idle (i.e. no measurement currently ongoing) and - * 3. data buffer ready (i.e. the previous data has been evaluated). + * The pre-conditions for starting a measurement frame are: + * 1. timer flag set (i.e. a certain time has passed since the last + * measurement in order to fulfill eye-safety), + * 2. device idle (i.e. no measurement currently ongoing) and + * 3. data buffer ready (i.e. the previous data has been evaluated). * - * The callback function pointer will be invoked when the measurement - * frame has finished successfully or whenever an error, that cannot - * be handled internally, occurs. + * The callback function pointer will be invoked when the measurement + * frame has finished successfully or whenever an error, that cannot + * be handled internally, occurs. * - * The successful finishing of the measurement frame is not checked - * for timeouts! Instead, the user can call the #Argus_GetStatus() - * function on a regular function to do so. + * The successful finishing of the measurement frame is not checked + * for timeouts! Instead, the user can call the #Argus_GetStatus() + * function on a regular function to do so. * - * @param hnd The API handle; contains all internal states and data. - * @param cb Callback function that will be invoked when the measurement - * is completed. Its parameters are the \link #status_t status - * \endlink and a pointer to the \link #argus_results_t results - * \endlink structure. If an error occurred, the status differs - * from #STATUS_OK and the second parameter is null. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @note Despite this function triggers a new measurement cycle upon its + * invocation, the frame time parameter is still active for this + * measurement mode. Basically, the first pre-condition mentioned + * above is controlled via the frame time parameter. This means + * that measurements cannot be triggered faster than the frame + * timer parameters specifies. The maximum integration time (i.e. + * exposure time) is also determined by the frame time such that + * new measurements are finished with the specified frame time and + * the device is ready to trigger a new measurement after the + * frame time has elapse. + * See #Argus_SetConfigurationFrameTime function for more information + * on the frame time. + * + * @param hnd The API handle; contains all internal states and data. + * + * @param cb Callback function that will be invoked when the measurement + * is completed. Its parameters are the \link #status_t status + * \endlink of the finished measurement cycle and the pointer to + * the calling \link #argus_hnd_t API handle\endlink, i.e. the + * /p hnd value. The latter must be passed to the + * #Argus_EvaluateData function. + * If an error occurred, the status differs from #STATUS_OK. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_TriggerMeasurement(argus_hnd_t *hnd, argus_callback_t cb); +status_t Argus_TriggerMeasurement(argus_hnd_t *hnd, + argus_measurement_ready_callback_t cb); /*!*************************************************************************** - * @brief Stops the currently ongoing measurements and SPI activity immediately. + * @brief Determines whether a data evaluation is pending. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details If the function returns true, a raw buffer is required to be + * evaluated to the #Argus_EvaluateData function. The raw data buffer + * is filled with raw data from the measurement tasks which need to + * be evaluated and the buffer must be freed in order to restart a + * new measurement task. + * + * Note that no configuration parameters can be update until all raw + * buffers are evaluated. + * + * @note See also the #Argus_GetStatus function to obtain the current device + * status and error code if any. + * + * @param hnd The API handle; contains all internal states and data. + * @return True if any raw buffer is filled with data that must be evaluated. + *****************************************************************************/ +bool Argus_IsDataEvaluationPending(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Determines if the device if active with timer based measurements. + * @details If the function returns true, the device is active with timer + * scheduled measurements that have been started via the + * #Argus_StartMeasurementTimer. + * + * Note that the active state is independent of the busy state that + * is set when the device is actually busy. The active state is also + * true if the device is currently idle but waits for the next timer + * event to trigger a new measurement cycle. + * + * @note See also the #Argus_GetStatus function to obtain the current device + * status and error code if any. + * + * @param hnd The API handle; contains all internal states and data. + * @return True if the device is operating in timer triggered measurement mode. + *****************************************************************************/ +bool Argus_IsTimerMeasurementActive(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Stops the currently ongoing measurements and SPI activity immediately. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_Abort(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Checks the state of the device/driver. + * @brief Checks the state of the device/driver. * - * @details Returns the current module status or error if any. - * See the following for a list of errors: + * @details Returns the current module status or error if any. * - * Status: - * - Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE). - * - Busy: Device or SPI interface are busy (== #STATUS_BUSY). - * - Initializing: The modules and devices are currently initializing - * (== #STATUS_INITIALIZING). - * . + * See the following for a list of errors: * - * Error: - * - Not Initialized: The modules (or any submodule) has not been - * initialized yet (== #ERROR_NOT_INITIALIZED). - * - Not Connected: No device has been connected (or connection errors - * have occurred) (== #ERROR_ARGUS_NOT_CONNECTED). - * - Timeout: A previous frame measurement has not finished within a - * specified time (== #ERROR_TIMEOUT). - * . + * Status: + * - Idle/OK: Device and SPI interface are idle (== #STATUS_IDLE). + * - Busy: Device or SPI interface are busy (== #STATUS_BUSY). + * - Initializing: The modules and devices are currently initializing + * (== #STATUS_INITIALIZING). + * . * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * Error: + * - Not Initialized: The modules (or any submodule) has not been + * initialized yet (== #ERROR_NOT_INITIALIZED). + * - Not Connected: No device has been connected (or connection errors + * have occurred) (== #ERROR_ARGUS_NOT_CONNECTED). + * - Timeout: A previous frame measurement has not finished within a + * specified time (== #ERROR_TIMEOUT). + * . + * + * @note Note that this function returns the actual busy state. This means + * that it will return #STATUS_IDLE during the pause between two + * consecutive measurement frames. If the device is active with timer + * based measurements (i.e. started via the #Argus_StartMeasurementTimer + * function), the return state switches from idle to busy and back + * periodically. Use the #Argus_IsTimerMeasurementActive function in + * order to determine if the device is active with timer based + * measurements. + * + * @note Note also that the device might reject configuration parameter + * update despite the status is #STATUS_IDLE. This is due to the fact + * that the internal raw data buffers are still busy and require to + * be freed by passing them to the #Argus_EvaluateData function. Use + * the #Argus_IsDataEvaluationPending function to see whether any of + * the raw data buffers is busy or the configuration can be changed. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetStatus(argus_hnd_t *hnd); /*!***************************************************************************** - * @brief Tests the connection to the device by sending a ping message. + * @brief Tests the connection to the device by sending a ping message. * - * @details A ping is transfered to the device in order to check the device and - * SPI connection status. Returns #STATUS_OK on success and - * #ERROR_ARGUS_NOT_CONNECTED elsewise. + * @details A ping is transferred to the device in order to check the device and + * SPI connection status. Returns #STATUS_OK on success and + * #ERROR_ARGUS_NOT_CONNECTED else-wise. * - * @param hnd The API handle; contains all internal states and data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). ******************************************************************************/ status_t Argus_Ping(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Evaluate useful information from the raw measurement data. + * @brief Evaluates measurement data from the raw sensor readout data. * - * @details This function is called with a pointer to the raw results obtained - * from the measurement cycle. It evaluates this data and creates - * useful information from it. Furthermore, calibration is applied to - * the data. Finally, the results are used in order to adapt the device - * configuration to the ambient conditions in order to achieve optimal - * device performance. + * @details This function must be called after each completion of a measurement + * cycle. The completion of a measurement cycle is communicated by the + * API via the invocation of the measurement data ready callback. The + * callback is installed in the API when new measurements are started + * either via the #Argus_TriggerMeasurement or via the + * #Argus_StartMeasurementTimer functions. * - * Therefore, it consists of the following sub-functions: - * - Apply pre-calibration: Applies calibration steps before evaluating - * the data, i.e. calculations that are to the integration results - * directly. - * - Evaluate data: Calculates measurement parameters such as range, - * amplitude or ambient light intensity, depending on the configurations. - * - Apply post-calibration: Applies calibrations after evaluation of - * measurement data, i.e. calibrations applied to the calculated - * values such as range. - * - Dynamic Configuration Adaption: checks if the configuration needs - * to be adjusted before the next measurement cycle in order to - * achieve optimum performance. Note that the configuration might not - * applied directly but before the next measurement starts. This is - * due to the fact that the device could be busy measuring already - * the next frame and thus no SPI activity is allowed. - * . - * However, if the device is idle, the configuration will be written - * immediately. + * This function evaluates measurement values like distances, amplitudes + * states and auxiliary values like temperature or voltage values from + * the raw sensor readout data obtained from the device during the + * measurement cycle. A pointer to a #argus_results_t data structure + * must be passed where all the evaluated values will be written to. + * The structure must persist during the whole execution of the + * #Argus_EvaluateData function. * - * @param hnd The API handle; contains all internal states and data. - * @param res A pointer to the results structure that will be populated - * with evaluated data. - * @param raw The pointer to the raw data that has been obtained by the - * measurement finished callback. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * In addition to the evaluation of measurement data, the function + * feeds back the obtained information to the device in order to + * optimize its performance with respect to the ambient conditions, + * utilizing the so called Dynamic Configuration Adaption (DCA) + * feature. + * + * Furthermore, several calibration algorithm are applied to the data. + * + * If the function is called without any data ready to be evaluated + * from the measurement module, the error code #ERROR_ARGUS_BUFFER_EMPTY + * is returned and not data is written to the passed #argus_results_t + * data structure. + * + * @note The call to this function is mandatory for each finished measurement + * cycle, i.e. for each call to the measurement data ready callback. + * If the function is not called, the data is not evaluated and the + * internal raw data buffers are not freed. In that case, they can not + * be reused for the next measurement and the API can not start new + * measurements. + * There are up to two internal buffers available, the to callback + * is called twice before the API must wait for the data evaluation + * to finish. + * + * @param hnd The API handle; contains all internal states and data. + * @param res A pointer to the results structure that will be populated + * with evaluated data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_EvaluateData(argus_hnd_t *hnd, argus_results_t *res, void *raw); +status_t Argus_EvaluateData(argus_hnd_t *hnd, argus_results_t *res); /*!*************************************************************************** - * @brief Executes a crosstalk calibration measurement. + * @brief Evaluates measurement data from the raw sensor readout data. + * + * @details This function enhances the #Argus_EvaluateData by adding additional + * debug data into a specified debug data structure (\p dbg). If the + * \p dbg is null, the function is eqivalent to the #Argus_EvaluateData + * function. This, see #Argus_EvaluateData for reference. + * + * @param hnd The API handle; contains all internal states and data. + * @param res A pointer to the results structure that will be populated + * with evaluated data. + * @param dbg An optional pointer (can be null) to the debug data structure. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_EvaluateDataDebug(argus_hnd_t *hnd, argus_results_t *res, + argus_results_debug_t *dbg); + +/*!*************************************************************************** + * @brief Executes a crosstalk calibration measurement. * * @details This function immediately triggers a crosstalk vector calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a crosstalk calibration, the reflection of the - * transmitted signal must be kept from the receiver side, by either - * covering the TX completely (or RX respectively) or by setting up - * an absorbing target at far distance. + * In order to perform a crosstalk calibration, the reflection of the + * transmitted signal must be kept from the receiver side, by either + * covering the TX completely (or RX respectively) or by setting up + * an absorbing target at far distance. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationCrosstalkVectorTable function. + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationCrosstalkVectorTable function. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd, argus_mode_t mode); - +status_t Argus_ExecuteXtalkCalibrationSequence(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Executes a relative range offset calibration measurement. + * @brief Executes a relative range offset calibration measurement. * * @details This function immediately triggers a relative range offset calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a relative range offset calibration, a flat - * calibration target must be setup perpendicular to the sensors - * field-of-view. + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. * - * \code + * \code * AFBR-S50 ToF Sensor - * #| - * #| | - * #|-----+ | - * #| Rx | | - * Reference #|----++ | Calibration - * Plane #| Tx | | Target - * #|----+ | - * #| | - * #| <------- targetRange -----------------> | - * \endcode + * #| + * #| | + * #|-----+ | + * #| RX | | + * Reference #|----++ | Calibration + * Plane #| TX | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode * - * There are two options to run the offset calibration: relative and - * absolute. - * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): - * when the absolute distance is not essential or the distance to - * the calibration target is not known, the relative method can be - * used to compensate the relative pixel range offset w.r.t. the - * average range. The absolute or global range offset is not changed. - * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): - * when the absolute distance is essential and the distance to the - * calibration target is known, the absolute method can be used to - * calibrate the absolute measured distance. Additionally, the - * relative pixel offset w.r.t. the average range is also compensated. - * . + * There are two options to run the offset calibration: relative and + * absolute. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationPixelRangeOffsets or - * #Argus_GetCalibrationGlobalRangeOffset function. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd, - argus_mode_t mode); +status_t Argus_ExecuteRelativeRangeOffsetCalibrationSequence(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Executes an absolute range offset calibration measurement. + * @brief Executes an absolute range offset calibration measurement. * * @details This function immediately triggers an absolute range offset calibration - * measurement sequence. The ordinary measurement activity is suspended - * while the calibration is ongoing. + * measurement sequence. The ordinary measurement activity is suspended + * while the calibration is ongoing. * - * In order to perform a relative range offset calibration, a flat - * calibration target must be setup perpendicular to the sensors - * field-of-view. + * In order to perform a relative range offset calibration, a flat + * calibration target must be setup perpendicular to the sensors + * field-of-view. * - * \code + * \code * AFBR-S50 ToF Sensor - * #| - * #| | - * #|-----+ | - * #| Rx | | - * Reference #|----++ | Calibration - * Plane #| Tx | | Target - * #|----+ | - * #| | - * #| <------- targetRange -----------------> | - * \endcode + * #| + * #| | + * #|-----+ | + * #| RX | | + * Reference #|----++ | Calibration + * Plane #| TX | | Target + * #|----+ | + * #| | + * #| <------- targetRange -----------------> | + * \endcode * - * There are two options to run the offset calibration: relative and - * absolute. - * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): - * when the absolute distance is not essential or the distance to - * the calibration target is not known, the relative method can be - * used to compensate the relative pixel range offset w.r.t. the - * average range. The absolute or global range offset is not changed. - * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): - * when the absolute distance is essential and the distance to the - * calibration target is known, the absolute method can be used to - * calibrate the absolute measured distance. Additionally, the - * relative pixel offset w.r.t. the average range is also compensated. - * . + * There are two options to run the offset calibration: relative and + * absolute. * - * After calibration has finished successfully, the obtained data is - * applied immediately and can be read from the API using the - * #Argus_GetCalibrationPixelRangeOffsets or - * #Argus_GetCalibrationGlobalRangeOffset function. + * - Relative (#Argus_ExecuteRelativeRangeOffsetCalibrationSequence): + * when the absolute distance is not essential or the distance to + * the calibration target is not known, the relative method can be + * used to compensate the relative pixel range offset w.r.t. the + * average range. The absolute or global range offset is not changed. + * - Absolute (#Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence): + * when the absolute distance is essential and the distance to the + * calibration target is known, the absolute method can be used to + * calibrate the absolute measured distance. Additionally, the + * relative pixel offset w.r.t. the average range is also compensated. + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param targetRange The absolute range between the reference plane and the - * calibration target in meter an Q9.22 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * After calibration has finished successfully, the obtained data is + * applied immediately and can be read from the API using the + * #Argus_GetCalibrationPixelRangeOffsets or + * #Argus_GetCalibrationGlobalRangeOffset function. + * + * @param hnd The API handle; contains all internal states and data. + * @param targetRange The absolute range between the reference plane and the + * calibration target in meter an Q9.22 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence(argus_hnd_t *hnd, - argus_mode_t mode, q9_22_t targetRange); /*! @} */ @@ -600,188 +790,245 @@ status_t Argus_ExecuteAbsoluteRangeOffsetCalibrationSequence(argus_hnd_t *hnd, /*!************************************************************************** * Configuration API **************************************************************************** - * @addtogroup arguscfg + * @addtogroup argus_cfg * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Sets the measurement mode to a specified device. + * @brief Gets the default measurement mode for a specified module type. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param module The specified module type. + * @return Returns the default measurement mode for the specified module type. *****************************************************************************/ -status_t Argus_SetConfigurationMeasurementMode(argus_hnd_t *hnd, - argus_mode_t value); +argus_mode_t Argus_GetDefaultMeasurementMode(argus_module_version_t module); /*!*************************************************************************** - * @brief Gets the measurement mode from a specified device. + * @brief Sets the measurement mode to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details This generates a new default configuration and calibration for the + * specified measurement mode and applies it to the device. + * + * See #argus_mode_t for a list of all available measurement modes. + * + * @warning The function overwrites all made changes to the configuration or + * calibration parameters with the default values. So this function + * must be called before any other changes to the configuration or + * calibration parameters are made! + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The new measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetConfigurationMeasurementMode(argus_hnd_t *hnd, - argus_mode_t *value); +status_t Argus_SetMeasurementMode(argus_hnd_t *hnd, argus_mode_t mode); /*!*************************************************************************** - * @brief Sets the frame time to a specified device. + * @brief Resets the measurement mode to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The measurement frame time in microseconds. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details This generates a new default configuration and calibration for the + * current measurement mode and applies it to the device. + * + * @warning The function overwrites all made changes to the configuration or + * calibration parameters with the default values. So this function + * must be called before any other changes to the configuration or + * calibration parameters are made! + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_ResetMeasurementMode(argus_hnd_t *hnd); + +/*!*************************************************************************** + * @brief Gets the measurement mode from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param mode The current measurement mode. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetMeasurementMode(argus_hnd_t *hnd, argus_mode_t *mode); + +/*!*************************************************************************** + * @brief Sets the frame time to a specified device. + * + * @details The frame time determines the measurement rate of the device. + * Usually, this controller the periodicity of measurements to be + * triggered via the timer based measurement mode that can be started + * via the #Argus_StartMeasurementTimer function. But also the + * behavior of the #Argus_TriggerMeasurement function is influenced + * by the frame rate parameter. + * + * The frame time parameter handles the maximum frame rate by limiting + * the trigger of a new measurement frame to the specified value. + * On the other hand, the accuracy of measurement results it also + * influenced since the frame time specifies the maximum integration + * depth (i.e. exposure time) along with the laser safety limitations. + * This means, the measurement speed can be increased by decreasing + * the frame time parameter and the accuracy can be improved by + * increasing the frame time parameter. + * + * Note the additional factor will limit the maximum frame rate on the + * one hand and the accuracy on the other hand: + * - High CPU load (or slow CPU in general) will lead to delays due + * to long data evaluation task (#Argus_EvaluateData) or long user + * application code. Reduce CPU load or increase CPU power to + * increase maximum frame rate. + * - The dual frequency mode (DFM, see #Argus_SetConfigurationDFMMode) + * will additionally limit the maximum frame rate to approximately + * 100 frames per second. Disable the DFM to increase maximum frame + * rates. + * - The smart power save (SPS, see + * #Argus_SetConfigurationSmartPowerSaveEnabled) mode will decrease + * the maximum possible frame rate slightly. Disable it to increase + * the maximum frame rate. + * - The dynamic configuration adaption with its specific power saving + * ratio parameter (see #Argus_SetConfigurationDynamicAdaption) + * will limit the maximum integration depth along with the laser + * safety limitations. Increase the power saving ratio to increase + * accuracy. Note that laser safety limitations might already limit + * the maximum integration depth such that the power saving ratio + * is ineffective. + * . + * + * @param hnd The API handle; contains all internal states and data. + * @param value The measurement frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t value); /*!*************************************************************************** - * @brief Gets the frame time from a specified device. + * @brief Gets the frame time from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current frame time in microseconds. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current frame time in microseconds. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationFrameTime(argus_hnd_t *hnd, uint32_t *value); /*!*************************************************************************** - * @brief Sets the smart power save enabled flag to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new smart power save enabled flag. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the smart power save enabled flag to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, - argus_mode_t mode, bool value); /*!*************************************************************************** - * @brief Gets the smart power save enabled flag from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current smart power save enabled flag. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the smart power save enabled flag from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current smart power save enabled flag. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationSmartPowerSaveEnabled(argus_hnd_t *hnd, - argus_mode_t mode, bool *value); /*!*************************************************************************** - * @brief Sets the Dual Frequency Mode (DFM) to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new DFM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the Dual Frequency Mode (DFM) to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationDFMMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_dfm_mode_t value); /*!*************************************************************************** - * @brief Gets the Dual Frequency Mode (DFM) from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current DFM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the Dual Frequency Mode (DFM) from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current DFM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationDFMMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_dfm_mode_t *value); /*!*************************************************************************** - * @brief Sets the Shot Noise Monitor (SNM) mode to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new SNM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the Shot Noise Monitor (SNM) mode to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_snm_mode_t value); /*!*************************************************************************** - * @brief Gets the Shot Noise Montor (SNM) mode from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current SNM mode value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the Shot Noise Monitor (SNM) mode from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current SNM mode value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationShotNoiseMonitorMode(argus_hnd_t *hnd, - argus_mode_t mode, argus_snm_mode_t *value); -#if 0 -///*!*************************************************************************** -// * @brief Sets the Crosstalk Monitor (XTM) mode to a specified device. -// * @param hnd The API handle; contains all internal states and data. -// * @param mode The targeted measurement mode. -// * @param value The new XTM mode value (true: enabled; false: disabled). -// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). -// *****************************************************************************/ -//status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, -// argus_mode_t mode, -// bool value); -// -///*!*************************************************************************** -// * @brief Gets the Crosstalk Monitor (XTM) mode from a specified device. -// * @param hnd The API handle; contains all internal states and data. -// * @param mode The targeted measurement mode. -// * @param value The current XTM mode value (true: enabled; false: disabled). -// * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). -// *****************************************************************************/ -//status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t * hnd, -// argus_mode_t mode, -// bool * value); -#endif /*!*************************************************************************** - * @brief Sets the full DCA module configuration to a specified device. +* @brief Sets the Crosstalk Monitor (XTM) mode to a specified device. +* +* @param hnd The API handle; contains all internal states and data. +* @param value The new XTM mode value (true: enabled; false: disabled). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Argus_SetConfigurationCrosstalkMonitorMode(argus_hnd_t *hnd, + bool value); + +/*!*************************************************************************** +* @brief Gets the Crosstalk Monitor (XTM) mode from a specified device. +* +* @param hnd The API handle; contains all internal states and data. +* @param value The current XTM mode value (true: enabled; false: disabled). +* @return Returns the \link #status_t status\endlink (#STATUS_OK on success). +*****************************************************************************/ +status_t Argus_GetConfigurationCrosstalkMonitorMode(argus_hnd_t *hnd, + bool *value); + +/*!*************************************************************************** + * @brief Sets the full DCA module configuration to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new DCA configuration set. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new DCA configuration set. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationDynamicAdaption(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_dca_t const *value); /*!*************************************************************************** - * @brief Gets the # from a specified device. + * @brief Gets the # from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current DCA configuration set value. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current DCA configuration set value. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationDynamicAdaption(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_dca_t *value); /*!*************************************************************************** - * @brief Sets the pixel binning configuration parameters to a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new pixel binning configuration parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Sets the pixel binning configuration parameters to a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The new pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetConfigurationPixelBinning(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_pba_t const *value); /*!*************************************************************************** - * @brief Gets the pixel binning configuration parameters from a specified device. - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current pixel binning configuration parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the pixel binning configuration parameters from a specified device. + * + * @param hnd The API handle; contains all internal states and data. + * @param value The current pixel binning configuration parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationPixelBinning(argus_hnd_t *hnd, - argus_mode_t mode, argus_cfg_pba_t *value); /*!*************************************************************************** - * @brief Gets the current unambiguous range in mm. - * @param hnd The API handle; contains all internal states and data. - * @param range_mm The returned range in mm. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Gets the current unambiguous range in mm. + * + * @param hnd The API handle; contains all internal states and data. + * @param range_mm The returned range in mm. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, uint32_t *range_mm); @@ -791,458 +1038,398 @@ status_t Argus_GetConfigurationUnambiguousRange(argus_hnd_t *hnd, /*!************************************************************************** * Calibration API **************************************************************************** - * @addtogroup arguscal + * @addtogroup argus_cal * @{ ****************************************************************************/ /*!*************************************************************************** - * @brief Sets the global range offset value to a specified device. + * @brief Sets the global range offset value to a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offset is subtracted from the raw range values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new global range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, - argus_mode_t mode, q0_15_t value); /*!*************************************************************************** - * @brief Gets the global range offset value from a specified device. + * @brief Gets the global range offset value from a specified device. * - * @details The global range offset is subtracted from the raw range values. + * @details The global range offset is subtracted from the raw range values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current global range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current global range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationGlobalRangeOffset(argus_hnd_t *hnd, - argus_mode_t mode, q0_15_t *value); /*!*************************************************************************** - * @brief Sets the relative pixel offset table to a specified device. + * @brief Sets the relative pixel offset table to a specified device. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t, wrapped within the #argus_cal_offset_table_t structure. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * Its recommended to use the built-in pixel offset calibration - * sequence (see #Argus_ExecuteRelativeRangeOffsetCalibrationSequence) - * to determine the offset table for the current device. + * Its recommended to use the built-in pixel offset calibration + * sequence (see #Argus_ExecuteRelativeRangeOffsetCalibrationSequence) + * to determine the offset table for the current device. * - * If a constant offset table for all device needs to be incorporated - * into the sources, the #Argus_GetExternalPixelRangeOffsets_Callback - * should be used. + * If a constant offset table for all device needs to be incorporated + * into the sources, the #Argus_GetPixelRangeOffsets_Callback + * should be used. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_SetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); +status_t Argus_SetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, + argus_cal_offset_table_t const *value); /*!*************************************************************************** - * @brief Gets the relative pixel offset table from a specified device. + * @brief Gets the relative pixel offset table from a specified device. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. The relative pixel offset values are meant + * to be with respect to the average range of all pixels, i.e. the + * average of all relative offsets should be 0! * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The crosstalk vector table is a two dimensional array of type + * #q0_15_t, wrapped within the #argus_cal_offset_table_t structure. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current relative range offset in meter and Q0.15 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); +status_t Argus_GetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, + argus_cal_offset_table_t *value); /*!*************************************************************************** - * @brief Gets the relative pixel offset table from a specified device. + * @brief Resets the relative pixel offset values for the specified device to + * the factory calibrated default values. * * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. The relative pixel offset values are meant - * to be with respect to the average range of all pixels, i.e. the - * average of all relative offsets should be 0! + * values for each individual pixel. Note that a global range offset + * is applied additionally. * - * The crosstalk vector table is a two dimensional array of type - * #q0_15_t. + * The factory defaults are device specific values. * - * The dimensions are: - * - size(0) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(1) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * - * The total offset table consists of the custom pixel offset values - * (set via #Argus_SetCalibrationPixelRangeOffsets) and the internal, - * factory calibrated device specific offset values. - * This is informational only! - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current total relative range offset in meter and Q0.15 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationTotalPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode, - q0_15_t value[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); - +status_t Argus_ResetCalibrationPixelRangeOffsets(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Resets the relative pixel offset values for the specified device to - * the factory calibrated default values. - * - * @details The relative pixel offset values are subtracted from the raw range - * values for each individual pixel. Note that a global range offset - * is applied additionally. - * - * The factory defaults are device specific values. - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_ResetCalibrationPixelRangeOffsets(argus_hnd_t *hnd, argus_mode_t mode); - -/*!*************************************************************************** - * @brief A callback that returns the external pixel range offsets. + * @brief A callback that returns the external pixel range offsets. * * @details The function needs to be implemented by the host application in - * order to set the external pixel range offsets values upon system - * initialization. If not defined in user code, the default - * implementation will return an all zero offset table, assuming there - * is no (additional) external pixel range offset values. + * order to set the external pixel range offsets values upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero offset table, assuming there + * is no (additional) external pixel range offset values. * - * If defined in user code, the function must fill all offset values - * in the provided \par offsets parameter with external range offset - * values. - * The values can be obtained by the calibration routine. + * If defined in user code, the function must fill all offset values + * in the provided \par offsets parameter with external range offset + * values. + * The values can be obtained by the calibration routine. * - * Example usage: + * Example usage: * - * @code - * status_t Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - * argus_mode_t mode) - * { - * (void) mode; // Ignore mode; use same values for all modes. - * memset(offsets, 0, sizeof(q0_15_t) * ARGUS_PIXELS); + * @code + * status_t Argus_GetPixelRangeOffsets_Callback(argus_cal_offset_table_t offsets) + * { + * memset(offsets, 0, sizeof(argus_cal_offset_t)); * - * // Set offset values in meter and Q0.15 format. - * offsets[0][0].dS = -16384; offsets[0][0].dC = -32768; - * offsets[0][1].dS = -32768; offsets[0][1].dC = 0; - * offsets[0][2].dS = 16384; offsets[0][2].dC = -16384; - * // etc. - * } - * @endcode + * // Set offset values in meter and Q0.15 format. + * offsets.Table[0][0] = -3542; + * offsets.Table[0][1] = -4385; + * offsets.Table[0][2] = 2953; + * // etc. + * } + * @endcode * - * @param offsets The pixel range offsets in meter and Q0.15 format; to be - * filled with data. - * @param mode Determines the current measurement mode; can be ignored if - * only a single measurement mode is utilized. + * @param offsets The pixel range offsets in meter and Q0.15 format; to be + * filled with data. + * @param mode The current measurement mode. *****************************************************************************/ -void Argus_GetExternalPixelRangeOffsets_Callback(q0_15_t offsets[ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - argus_mode_t mode); +void Argus_GetPixelRangeOffsets_Callback(argus_cal_offset_table_t *offsets, + argus_mode_t const mode); /*!*************************************************************************** - * @brief Sets the sample time for the range offset calibration sequence. + * @brief Sets the sample time for the range offset calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * range offset calibration sequence and generate the offset data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the offset data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new range offset calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t value); /*!*************************************************************************** - * @brief Gets the sample time for the range offset calibration sequence. + * @brief Gets the sample time for the range offset calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * range offset calibration sequence and generate the ooffset data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * range offset calibration sequence and generate the offset data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current range offset calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current range offset calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationRangeOffsetSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value); /*!*************************************************************************** - * @brief Sets the pixel-to-pixel crosstalk compensation parameters to a specified device. + * @brief Sets the pixel-to-pixel crosstalk compensation parameters to a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new pixel-to-pixel crosstalk compensation parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, - argus_mode_t mode, argus_cal_p2pxtalk_t const *value); /*!*************************************************************************** - * @brief Gets the pixel-to-pixel crosstalk compensation parameters from a specified device. + * @brief Gets the pixel-to-pixel crosstalk compensation parameters from a specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current pixel-to-pixel crosstalk compensation parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current pixel-to-pixel crosstalk compensation parameters. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkPixel2Pixel(argus_hnd_t *hnd, - argus_mode_t mode, argus_cal_p2pxtalk_t *value); /*!*************************************************************************** - * @brief Sets the custom crosstalk vector table to a specified device. + * @brief Sets the custom crosstalk vector table to a specified device. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. The #argus_cal_xtalk_table_t is the corresponding + * typedef for the required data. * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * Its recommended to use the built-in crosstalk calibration sequence - * (see #Argus_ExecuteXtalkCalibrationSequence) to determine the - * crosstalk vector table. + * Its recommended to use the built-in crosstalk calibration sequence + * (see #Argus_ExecuteXtalkCalibrationSequence) to determine the + * crosstalk vector table. * - * If a constant table for all device needs to be incorporated into - * the sources, the #Argus_GetExternalCrosstalkVectorTable_Callback - * should be used. + * If a constant table for all device needs to be incorporated into + * the sources, the #Argus_GetCrosstalkVectorTable_Callback + * should be used. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The new crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + argus_cal_xtalk_table_t const *value); /*!*************************************************************************** - * @brief Gets the custom crosstalk vector table from a specified device. + * @brief Gets the custom crosstalk vector table from a specified device. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. + * The crosstalk vector table is a three dimensional array of type + * #xtalk_t. The #argus_cal_xtalk_table_t is the corresponding + * typedef for the required data. * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . + * The dimensions are: + * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) + * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) + * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) + * . * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current crosstalk vector table. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); + argus_cal_xtalk_table_t *value); /*!*************************************************************************** - * @brief Gets the factory calibrated default crosstalk vector table for the - * specified device. + * @brief Resets the crosstalk vector table for the specified device to the + * factory calibrated default values. * * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. - * - * The crosstalk vector table is a three dimensional array of type - * #xtalk_t. - * - * The dimensions are: - * - size(0) = #ARGUS_DFM_FRAME_COUNT (Dual-frequency mode A- or B-frame) - * - size(1) = #ARGUS_PIXELS_X (Pixel count in x-direction) - * - size(2) = #ARGUS_PIXELS_Y (Pixel count in y-direction) - * . - * - * The total vector table consists of the custom crosstalk vector - * table (set via #Argus_SetCalibrationCrosstalkVectorTable) and - * an internal, factory calibrated device specific vector table. - * This is informational only! - * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @param value The current total crosstalk vector table. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_GetCalibrationTotalCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode, - xtalk_t value[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]); - -/*!*************************************************************************** - * @brief Resets the crosstalk vector table for the specified device to the - * factory calibrated default values. - * - * @details The crosstalk vectors are subtracted from the raw sampling data - * in the data evaluation phase. + * in the data evaluation phase. * * - * The factory defaults are device specific calibrated values. + * The factory defaults are device specific calibrated values. * - * @param hnd The API handle; contains all internal states and data. - * @param mode The targeted measurement mode. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd, - argus_mode_t mode); +status_t Argus_ResetCalibrationCrosstalkVectorTable(argus_hnd_t *hnd); /*!*************************************************************************** - * @brief Sets the sample time for the crosstalk calibration sequence. + * @brief Sets the sample time for the crosstalk calibration sequence. * - * @details Sets the measurement sample acquisition time for executing the - * crosstalk calibration sequence and generate the crosstalk data.\n - * Units: msec. + * @details Sets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new crosstalk calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, uint16_t value); /*!*************************************************************************** - * @brief Gets the sample time for the crosstalk calibration sequence. + * @brief Gets the sample time for the crosstalk calibration sequence. * - * @details Gets the measurement sample acquisition time for executing the - * crosstalk calibration sequence and generate the crosstalk data.\n - * Units: msec. + * @details Gets the measurement sample acquisition time for executing the + * crosstalk calibration sequence and generate the crosstalk data.\n + * Units: msec. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current crosstalk calibration sequence sample time. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current crosstalk calibration sequence sample time. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkSequenceSampleTime(argus_hnd_t *hnd, uint16_t *value); /*!*************************************************************************** - * @brief Sets the max. amplitude threshold for the crosstalk calibration sequence. + * @brief Sets the max. amplitude threshold for the crosstalk calibration sequence. * - * @details The maximum amplitude threshold defines a maximum crosstalk vector - * amplitude before causing an error message. If the crosstalk is - * too high, there is usually an issue with the measurement setup, i.e. - * there is still a measurement signal detected. + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. * - * @param hnd The API handle; contains all internal states and data. - * @param value The new crosstalk calibration sequence maximum amplitude - * threshold value in UQ12.4 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The new crosstalk calibration sequence maximum amplitude + * threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_SetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, uq12_4_t value); /*!*************************************************************************** - * @brief Gets the max. amplitude threshold for the crosstalk calibration sequence. + * @brief Gets the max. amplitude threshold for the crosstalk calibration sequence. * - * @details The maximum amplitude threshold defines a maximum crosstalk vector - * amplitude before causing an error message. If the crosstalk is - * too high, there is usually an issue with the measurement setup, i.e. - * there is still a measurement signal detected. + * @details The maximum amplitude threshold defines a maximum crosstalk vector + * amplitude before causing an error message. If the crosstalk is + * too high, there is usually an issue with the measurement setup, i.e. + * there is still a measurement signal detected. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current max. amplitude threshold value in UQ12.4 format. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param hnd The API handle; contains all internal states and data. + * @param value The current max. amplitude threshold value in UQ12.4 format. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_GetCalibrationCrosstalkSequenceAmplitudeThreshold(argus_hnd_t *hnd, uq12_4_t *value); -/*!*************************************************************************** - * @brief Sets the sample count for the substrate voltage calibration sequence. - * - * @param hnd The API handle; contains all internal states and data. - * @param value The new substrate voltage calibration sequence sample count. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t Argus_SetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, - uint16_t value); /*!*************************************************************************** - * @brief Gets the sample count for the substrate voltage calibration sequence. + * @brief Clears all user calibration values from NVM for the specified device. * - * @param hnd The API handle; contains all internal states and data. - * @param value The current substrate voltage calibration sequence sample count. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @details The user calibration values are stored in the non-volatile memory + * (NVM) if corresponding \link #argus_nvm NVM hardware layer\endlink + * is implemented. This method clears the user calibration data from + * the non-volatile memory. + * + * @warning This does not reset the currently set calibration values to + * factory defaults! + * + * @param hnd The API handle; contains all internal states and data. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t Argus_GetCalibrationVsubSequenceSampleCount(argus_hnd_t *hnd, - uint16_t *value); +status_t Argus_ClearUserCalibration(argus_hnd_t *hnd); + /*!*************************************************************************** - * @brief A callback that returns the external crosstalk vector table. + * @brief A callback that returns the external crosstalk vector table. * * @details The function needs to be implemented by the host application in - * order to set the external crosstalk vector table upon system - * initialization. If not defined in user code, the default - * implementation will return an all zero vector table, assuming there - * is no (additional) external crosstalk. + * order to set the external crosstalk vector table upon system + * initialization. If not defined in user code, the default + * implementation will return an all zero vector table, assuming there + * is no (additional) external crosstalk. * - * If defined in user code, the function must fill all vector values - * in the provided \par xtalk parameter with external crosstalk values. - * The values can be obtained by the calibration routine. + * If defined in user code, the function must fill all vector values + * in the provided \par crosstalk parameter with external crosstalk + * values. The values can be obtained by the calibration routine. * - * Example usage: + * Example usage: * - * @code - * status_t Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - * argus_mode_t mode) - * { - * (void) mode; // Ignore mode; use same values for all modes. - * memset(&xtalk, 0, sizeof(xtalk)); + * @code + * status_t Argus_GetCrosstalkVectorTable_Callback( + * argus_cal_xtalk_table_t * xtalk) + * { + * memset(xtalk, 0, sizeof(argus_cal_xtalk_table_t)); * - * // Set crosstalk vectors in Q11.4 format. - * // Note on dual-frequency frame index: 0 = A-Frame; 1 = B-Frame - * xtalk[0][0][0].dS = -9; xtalk[0][0][0].dC = -11; - * xtalk[0][0][1].dS = -13; xtalk[0][0][1].dC = -16; - * xtalk[0][0][2].dS = 6; xtalk[0][0][2].dC = -18; - * // etc. - * } - * @endcode + * // Set crosstalk vectors in Q11.4 format. + * // Note on dual-frequency frame index: 0 = A-Frame; 1 = B-Frame + * xtalk.FrameA[0][0].dS = -9; xtalk.FrameB[0][0].dC = -11; + * xtalk.FrameA[0][1].dS = -13; xtalk.FrameB[0][1].dC = -16; + * xtalk.FrameA[0][2].dS = 6; xtalk.FrameB[0][2].dC = -18; + * // etc. + * } + * @endcode * - * @param xtalk The crosstalk vector array; to be filled with data. - * @param mode Determines the current measurement mode; can be ignored if - * only a single measurement mode is utilized. + * @param xtalk The crosstalk vector array; to be filled with data. + * @param mode The current measurement mode. *****************************************************************************/ -void Argus_GetExternalCrosstalkVectorTable_Callback(xtalk_t - xtalk[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y], - argus_mode_t mode); +void Argus_GetCrosstalkVectorTable_Callback(argus_cal_xtalk_table_t *xtalk, + argus_mode_t const mode); -#ifdef __cplusplus -} -#endif +/*!*************************************************************************** + * @brief Gets the currently calibrated Golden Pixel coordinates. + * + * @details The Golden Pixel is the pixel that is located at the center of the + * receiving light beam. Thus it it the one that receives the most + * signal and plays a central role in 1D measurement systems. + * + * The function fills the provided \p x and \p y parameters with + * the Golden Pixel coordinates. Typical values are x = 5 and y = 1 + * or 2. But the actual values depend on the specific sensor. + * + * Please also note the utility functions provided in the \ref argus_map + * module to convert between pixel coordinates and channel numbers or + * shift pixel maps by a position offset (#ShiftSelectedPixels) or + * generate pixel masks centered around the Golden Pixel + * (#FillPixelMask). + * + * @param hnd The API handle; contains all internal states and data. + * @param x The Golden Pixel x-coordinate. + * @param y The Golden Pixel y-coordinate. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +status_t Argus_GetCalibrationGoldenPixel(argus_hnd_t const *hnd, uint8_t *x, uint8_t *y); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_API_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h index f23d117648..8f6b40bdc5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dca.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the dynamic configuration adaption (DCA) setup parameters - * and data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dynamic configuration adaption (DCA) setup parameters + * and data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,34 +37,37 @@ #ifndef ARGUS_DCA_H #define ARGUS_DCA_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusdca Dynamic Configuration Adaption - * @ingroup argusapi + * @defgroup argus_dca Dynamic Configuration Adaption + * @ingroup argus_api * - * @brief Dynamic Configuration Adaption (DCA) parameter definitions and API functions. + * @brief Dynamic Configuration Adaption (DCA) parameter definitions and API functions. * - * @details The DCA contains an algorithms that detect ambient conditions - * and adopt the device configuration to the changing parameters - * dynamically while operating the sensor. This is achieved by - * rating the currently received signal quality and changing the - * device configuration accordingly to the gathered information - * from the current measurement frame results before the next - * integration cycle starts. + * @details The DCA contains an algorithms that detect ambient conditions + * and adopt the device configuration to the changing parameters + * dynamically while operating the sensor. This is achieved by + * rating the currently received signal quality and changing the + * device configuration accordingly to the gathered information + * from the current measurement frame results before the next + * integration cycle starts. * - * The DCA consists of the following features: - * - Static or Dynamic mode. The first is utilizing the nominal - * values while the latter is dynamically adopting between min. - * and max. value and starting from the nominal values. - * - Analog Integration Depth Adaption (from multiple patterns down to single pulses) - * - Optical Output Power Adaption - * - Pixel Input Gain Adaption (w/ ambient light rejection) - * - ADC Sensitivity (i.e. ADC Range) Adaption - * - Power Saving Ratio (to decrease the average output power and thus the current consumption) - * - All that features are heeding the Laser Safety limits. - * . + * The DCA consists of the following features: + * - Static or Dynamic mode. The first is utilizing the nominal + * values while the latter is dynamically adopting between min. + * and max. value and starting from the nominal values. + * - Analog Integration Depth Adaption (from multiple patterns down to single pulses) + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption (w/ ambient light rejection) + * - ADC Sensitivity (i.e. ADC Range) Adaption + * - Power Saving Ratio (to decrease the average output power and thus the current consumption) + * - All that features are heeding the Laser Safety limits. + * . * - * @addtogroup argusdca + * @addtogroup argus_dca * @{ *****************************************************************************/ @@ -73,39 +76,26 @@ /*! The minimum amplitude threshold value. */ -#define ARGUS_CFG_DCA_ATH_MIN (1U << 6U) +#define ARGUS_CFG_DCA_ATH_MIN (1U << 6U) /*! The maximum amplitude threshold value. */ -#define ARGUS_CFG_DCA_ATH_MAX (0xFFFFU) +#define ARGUS_CFG_DCA_ATH_MAX (0xFFFFU) /*! The minimum saturated pixel threshold value. */ -#define ARGUS_CFG_DCA_PXTH_MIN (1U) +#define ARGUS_CFG_DCA_PXTH_MIN (1U) /*! The maximum saturated pixel threshold value. */ -#define ARGUS_CFG_DCA_PXTH_MAX (33U) +#define ARGUS_CFG_DCA_PXTH_MAX (33U) /*! The maximum analog integration depth in UQ10.6 format, * i.e. the maximum pattern count per sample. */ -#define ARGUS_CFG_DCA_DEPTH_MAX ((uq10_6_t)(ADS_SEQCT_N_MASK << (6U - ADS_SEQCT_N_SHIFT))) +#define ARGUS_CFG_DCA_DEPTH_MAX ((uq10_6_t)(0xFFC0U)) /*! The minimum analog integration depth in UQ10.6 format, * i.e. the minimum pattern count per sample. */ -#define ARGUS_CFG_DCA_DEPTH_MIN ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble - - -/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ -#define ARGUS_CFG_DCA_POWER_MAX_LSB (ADS_LASET_VCSEL_HC1_MASK >> ADS_LASET_VCSEL_HC1_SHIFT) - -/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ -#define ARGUS_CFG_DCA_POWER_MIN_LSB (1) - -/*! The maximum optical output power, i.e. the maximum VCSEL high current in LSB. */ -#define ARGUS_CFG_DCA_POWER_MAX (ADS0032_HIGH_CURRENT_LSB2MA(ARGUS_CFG_DCA_POWER_MAX_LSB + 1)) - -/*! The minimum optical output power, i.e. the minimum VCSEL high current in mA. */ -#define ARGUS_CFG_DCA_POWER_MIN (1) +#define ARGUS_CFG_DCA_DEPTH_MIN ((uq10_6_t)(1U)) // 1/64, i.e. 1/2 nibble /*! The dynamic configuration algorithm Pixel Input Gain stage count. */ @@ -139,9 +129,9 @@ /*!*************************************************************************** - * @brief The dynamic configuration algorithm enable flags. + * @brief The dynamic configuration algorithm enable flags. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_enable_t { /*! @internal * * DCA is disabled and will be completely skipped. @@ -160,9 +150,9 @@ typedef enum { } argus_dca_enable_t; /*!*************************************************************************** - * @brief The DCA amplitude evaluation method. + * @brief The DCA amplitude evaluation method. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_amplitude_mode_t { /*! Evaluate the DCA amplitude as the maximum of all valid amplitudes. */ DCA_AMPLITUDE_MAX = 1U, @@ -172,9 +162,9 @@ typedef enum { } argus_dca_amplitude_mode_t; /*!*************************************************************************** - * @brief The dynamic configuration algorithm Optical Output Power stages enumerator. + * @brief The dynamic configuration algorithm Optical Output Power stages enumerator. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_power_t { /*! Use low output power stage. */ DCA_POWER_LOW = 0, @@ -187,9 +177,9 @@ typedef enum { } argus_dca_power_t; /*!*************************************************************************** - * @brief The dynamic configuration algorithm Pixel Input Gain stages enumerator. + * @brief The dynamic configuration algorithm Pixel Input Gain stages enumerator. *****************************************************************************/ -typedef enum { +typedef enum argus_dca_gain_t { /*! Low gain stage. */ DCA_GAIN_LOW = 0, @@ -206,113 +196,113 @@ typedef enum { /*!*************************************************************************** - * @brief State flags for the current frame. - * @details State flags determine the current state of the measurement frame: - * - [0]: #ARGUS_STATE_MEASUREMENT_MODE - * - [1]: #ARGUS_STATE_DUAL_FREQ_MODE - * - [2]: #ARGUS_STATE_MEASUREMENT_FREQ - * - [3]: #ARGUS_STATE_DEBUG_MODE - * - [4]: #ARGUS_STATE_WEAK_SIGNAL - * - [5]: #ARGUS_STATE_BGL_WARNING - * - [6]: #ARGUS_STATE_BGL_ERROR - * - [7]: #ARGUS_STATE_PLL_LOCKED - * - [8]: #ARGUS_STATE_LASER_WARNING - * - [9]: #ARGUS_STATE_LASER_ERROR - * - [10]: #ARGUS_STATE_HAS_DATA - * - [11]: #ARGUS_STATE_HAS_AUX_DATA - * - [12]: #ARGUS_STATE_DCA_MAX - * - [13]: DCA Power Stage - * - [14-15]: DCA Gain Stages - * . + * @brief State flags for the current frame. + * @details State flags determine the current state of the measurement frame: + * - [0]: #ARGUS_STATE_XTALK_MONITOR_ACTIVE + * - [1]: #ARGUS_STATE_DUAL_FREQ_MODE + * - [2]: #ARGUS_STATE_MEASUREMENT_FREQ + * - [3]: #ARGUS_STATE_DEBUG_MODE + * - [4]: #ARGUS_STATE_WEAK_SIGNAL + * - [5]: #ARGUS_STATE_BGL_WARNING + * - [6]: #ARGUS_STATE_BGL_ERROR + * - [7]: #ARGUS_STATE_PLL_LOCKED + * - [8]: #ARGUS_STATE_LASER_WARNING + * - [9]: #ARGUS_STATE_LASER_ERROR + * - [10]: #ARGUS_STATE_HAS_DATA + * - [11]: #ARGUS_STATE_HAS_AUX_DATA + * - [12]: #ARGUS_STATE_DCA_MAX + * - [13]: DCA Power Stage + * - [14-15]: DCA Gain Stages + * . *****************************************************************************/ -typedef enum { +typedef enum argus_state_t { /*! No state flag set. */ ARGUS_STATE_NONE = 0, - /*! 0x0001: Measurement Mode. - * - 0: Mode A: Long Range / Medium Precision - * - 1: Mode B: Short Range / High Precision */ - ARGUS_STATE_MEASUREMENT_MODE = 1U << 0U, + /*! 0x0001: Crosstalk Monitor is enabled and updating. + * - 0: Inactive: crosstalk monitor values are not updated, + * - 1: Active: crosstalk monitor values are updated. */ + ARGUS_STATE_XTALK_MONITOR_ACTIVE = 1U << 0U, /*! 0x0002: Dual Frequency Mode Enabled. - * - 0: Disabled: measurements with base frequency, - * - 1: Enabled: measurement with detuned frequency. */ + * - 0: Disabled: measurements with base frequency, + * - 1: Enabled: measurement with detuned frequency. */ ARGUS_STATE_DUAL_FREQ_MODE = 1U << 1U, /*! 0x0004: Measurement Frequency for Dual Frequency Mode * (only if #ARGUS_STATE_DUAL_FREQ_MODE flag is set). - * - 0: A-Frame w/ detuned frequency, - * - 1: B-Frame w/ detuned frequency */ + * - 0: A-Frame w/ detuned frequency, + * - 1: B-Frame w/ detuned frequency */ ARGUS_STATE_MEASUREMENT_FREQ = 1U << 2U, /*! 0x0008: Debug Mode. If set, the range value of erroneous pixels - * are not cleared or reset. - * - 0: Disabled (default). - * - 1: Enabled. */ + * are not cleared or reset. + * - 0: Disabled (default). + * - 1: Enabled. */ ARGUS_STATE_DEBUG_MODE = 1U << 3U, /*! 0x0010: Weak Signal Flag. - * Set whenever the Pixel Binning Algorithm is detecting a - * weak signal, i.e. if the amplitude dies not reach its - * (absolute) threshold. If the Golden Pixel is enabled, - * this also indicates that the Pixel Binning Algorithm - * falls back to the Golden Pixel. - * - 0: Normal Signal. - * - 1: Weak Signal or Golden Pixel Mode. */ + * Set whenever the Pixel Binning Algorithm is detecting a + * weak signal, i.e. if the amplitude dies not reach its + * (absolute) threshold. If the Golden Pixel is enabled, + * this also indicates that the Pixel Binning Algorithm + * falls back to the Golden Pixel. + * - 0: Normal Signal. + * - 1: Weak Signal or Golden Pixel Mode. */ ARGUS_STATE_WEAK_SIGNAL = 1U << 4U, /*! 0x0020: Background Light Warning Flag. - * Set whenever the background light is very high and the - * measurement data might be unreliable. - * - 0: No Warning: Background Light is within valid range. - * - 1: Warning: Background Light is very high. */ + * Set whenever the background light is very high and the + * measurement data might be unreliable. + * - 0: No Warning: Background Light is within valid range. + * - 1: Warning: Background Light is very high. */ ARGUS_STATE_BGL_WARNING = 1U << 5U, /*! 0x0040: Background Light Error Flag. - * Set whenever the background light is too high and the - * measurement data is unreliable or invalid. - * - 0: No Error: Background Light is within valid range. - * - 1: Error: Background Light is too high. */ + * Set whenever the background light is too high and the + * measurement data is unreliable or invalid. + * - 0: No Error: Background Light is within valid range. + * - 1: Error: Background Light is too high. */ ARGUS_STATE_BGL_ERROR = 1U << 6U, /*! 0x0080: PLL_LOCKED bit. - * - 0: PLL not locked at start of integration. - * - 1: PLL locked at start of integration. */ + * - 0: PLL not locked at start of integration. + * - 1: PLL locked at start of integration. */ ARGUS_STATE_PLL_LOCKED = 1U << 7U, /*! 0x0100: Laser Failure Warning Flag. - * Set whenever the an invalid system condition is detected. - * (i.e. DCA at max state but no amplitude on any (incl. reference) - * pixel, not amplitude but any saturated pixel). - * - 0: No Warning: Laser is operating properly. - * - 1: Warning: Invalid laser conditions detected. If the invalid - * condition stays, a laser malfunction error is raised. */ + * Set whenever the an invalid system condition is detected. + * (i.e. DCA at max state but no amplitude on any (incl. reference) + * pixel, not amplitude but any saturated pixel). + * - 0: No Warning: Laser is operating properly. + * - 1: Warning: Invalid laser conditions detected. If the invalid + * condition stays, a laser malfunction error is raised. */ ARGUS_STATE_LASER_WARNING = 1U << 8U, /*! 0x0200: Laser Failure Error Flag. - * Set whenever a laser malfunction error is raised and the - * system is put into a safe state. - * - 0: No Error: Laser is operating properly. - * - 1: Error: Invalid laser conditions are detected for a certain - * soak time and the system is put into a safe state. */ + * Set whenever a laser malfunction error is raised and the + * system is put into a safe state. + * - 0: No Error: Laser is operating properly. + * - 1: Error: Invalid laser conditions are detected for a certain + * soak time and the system is put into a safe state. */ ARGUS_STATE_LASER_ERROR = 1U << 9U, /*! 0x0400: Set if current frame has distance measurement data available. - * - 0: No measurement data available, all values are 0 or stalled. - * - 1: Measurement data is available and correctly evaluated. */ + * - 0: No measurement data available, all values are 0 or stalled. + * - 1: Measurement data is available and correctly evaluated. */ ARGUS_STATE_HAS_DATA = 1U << 10U, /*! 0x0800: Set if current frame has auxiliary measurement data available. - * - 0: No auxiliary data available, all values are 0 or stalled. - * - 1: Auxiliary data is available and correctly evaluated. */ + * - 0: No auxiliary data available, all values are 0 or stalled. + * - 1: Auxiliary data is available and correctly evaluated. */ ARGUS_STATE_HAS_AUX_DATA = 1U << 11U, /*! 0x1000: DCA Maximum State Flag. - * Set whenever the DCA has extended all its parameters to their - * maximum values and can not increase the integration energy any - * further. - * - 0: DCA has not yet reached its maximum state. - * - 1: DCA has reached its maximum state and can not increase any further. */ + * Set whenever the DCA has extended all its parameters to their + * maximum values and can not increase the integration energy any + * further. + * - 0: DCA has not yet reached its maximum state. + * - 1: DCA has reached its maximum state and can not increase any further. */ ARGUS_STATE_DCA_MAX = 1U << 12U, /*! 0x2000: DCA is in high Optical Output Power stage. */ @@ -333,20 +323,20 @@ typedef enum { } argus_state_t; /*!*************************************************************************** - * @brief Dynamic Configuration Adaption (DCA) Parameters. - * @details DCA contains: - * - Static or dynamic mode. The first is utilizing the nominal values - * while the latter is dynamically adopting between min. and max. - * value and starting form the nominal values. - * - Analog Integration Depth Adaption down to single pulses. - * - Optical Output Power Adaption - * - Pixel Input Gain Adaption - * - Digital Integration Depth Adaption - * - Dynamic Global Phase Shift Injection. - * - All that features are heeding the Laser Safety limits. - * . + * @brief Dynamic Configuration Adaption (DCA) Parameters. + * @details DCA contains: + * - Static or dynamic mode. The first is utilizing the nominal values + * while the latter is dynamically adopting between min. and max. + * value and starting form the nominal values. + * - Analog Integration Depth Adaption down to single pulses. + * - Optical Output Power Adaption + * - Pixel Input Gain Adaption + * - Digital Integration Depth Adaption + * - Dynamic Global Phase Shift Injection. + * - All that features are heeding the Laser Safety limits. + * . *****************************************************************************/ -typedef struct { +typedef struct argus_cfg_dca_t { /*! Enables the automatic configuration adaption features. * Enables the dynamic part if #DCA_ENABLE_DYNAMIC and the static only if * #DCA_ENABLE_STATIC. */ @@ -494,4 +484,7 @@ typedef struct { } argus_cfg_dca_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DCA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h index c639922a7d..6d9dd3e9d3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_def.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details This file provides generic definitions belonging to all - * devices from the AFBR-S50 product family. + * @brief This file is part of the AFBR-S50 hardware API. + * @details This file provides generic definitions belonging to all + * devices from the AFBR-S50 product family. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,6 +37,9 @@ #ifndef ARGUS_DEF_H #define ARGUS_DEF_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** * Include files @@ -52,36 +55,41 @@ #include /*!*************************************************************************** - * @addtogroup argusapi + * @addtogroup argus_api * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Maximum number of phases per measurement cycle. - * @details The actual phase number is defined in the register configuration. - * However the software does only support a fixed value of 4 yet. + * @brief Maximum number of phases per measurement cycle. + * @details The actual phase number is defined in the register configuration. + * However the software does only support a fixed value of 4 yet. *****************************************************************************/ -#define ARGUS_PHASECOUNT 4U +#define ARGUS_PHASECOUNT 4 /*!*************************************************************************** - * @brief The device pixel field size in x direction (long edge). + * @brief The device pixel field size in x direction (long edge). *****************************************************************************/ -#define ARGUS_PIXELS_X 8U +#define ARGUS_PIXELS_X 8 /*!*************************************************************************** - * @brief The device pixel field size in y direction (short edge). + * @brief The device pixel field size in y direction (short edge). *****************************************************************************/ -#define ARGUS_PIXELS_Y 4U +#define ARGUS_PIXELS_Y 4 /*!*************************************************************************** - * @brief The total device pixel count. + * @brief The total device pixel count. *****************************************************************************/ -#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) +#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) /*!*************************************************************************** - * @brief The AFBR-S50 module types. + * @brief A flag indicating that the device is a extended range device. *****************************************************************************/ -typedef enum { +#define MODULE_EXTENDED_FLAG (0x40U) + +/*!*************************************************************************** + * @brief The AFBR-S50 module types. + *****************************************************************************/ +typedef enum argus_module_version_t { /*! No device connected or not recognized. */ MODULE_NONE = 0, @@ -89,54 +97,80 @@ typedef enum { * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 1 - legacy version! */ - AFBR_S50MV85G_V1 = 1, + AFBR_S50MV85G_V1 = 0x01, /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 2 - legacy version! */ - AFBR_S50MV85G_V2 = 2, - - /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device - * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for - * medium range 3D applications. - * Version 7 - current version! */ - AFBR_S50MV85G_V3 = 7, + AFBR_S50MV85G_V2 = 0x02, /*! AFBR-S50LV85D: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * long range 1D applications. * Version 1 - current version! */ - AFBR_S50LV85D_V1 = 3, + AFBR_S50LV85D_V1 = 0x03, /*! AFBR-S50MV68B: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and red, 680 nm, laser source for * medium range 1D applications. * Version 1 - current version! */ - AFBR_S50MV68B_V1 = 4, + AFBR_S50MV68B_V1 = 0x04, /*! AFBR-S50MV85I: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * medium range 3D applications. * Version 1 - current version! */ - AFBR_S50MV85I_V1 = 5, + AFBR_S50MV85I_V1 = 0x05, /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for * short range 3D applications. * Version 1 - current version! */ - AFBR_S50SV85K_V1 = 6, + AFBR_S50SV85K_V1 = 0x06, + /*! AFBR-S50MV85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * medium range 3D applications. + * Version 3 - current version! */ + AFBR_S50MV85G_V3 = 0x07, - /*! Reserved for future extensions. */ - Reserved = 0x3F + /*! AFBR-S50LX85D: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended long range 1D applications. + * Version 1 - current version! */ + AFBR_S50LX85D_V1 = AFBR_S50LV85D_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX68B: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and red, 680 nm, laser source for + * extended medium range 1D applications. + * Version 1 - current version! */ + AFBR_S50MX68B_V1 = AFBR_S50MV68B_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85I: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended medium range 3D applications. + * Version 1 - current version! */ + AFBR_S50MX85I_V1 = AFBR_S50MV85I_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended short range 3D applications. + * Version 1 - current version! */ + AFBR_S50SX85K_V1 = AFBR_S50SV85K_V1 | MODULE_EXTENDED_FLAG, + + /*! AFBR-S50MX85G: an ADS0032 based multi-pixel range finder device + * w/ 4x8 pixel matrix and infra-red, 850 nm, laser source for + * extended medium range 3D applications. + * Version 1 - current version! */ + AFBR_S50MX85G_V1 = AFBR_S50MV85G_V3 | MODULE_EXTENDED_FLAG, } argus_module_version_t; /*!*************************************************************************** - * @brief The AFBR-S50 laser configurations. + * @brief The AFBR-S50 laser configurations. *****************************************************************************/ -typedef enum { +typedef enum argus_laser_type_t { /*! No laser connected. */ LASER_NONE = 0, @@ -152,12 +186,15 @@ typedef enum { /*! 850nm Infra-Red VCSEL v2 /w extended mode. */ LASER_H_V2X = 4, + /*! 680nm Red VCSEL v1 w/ extended mode. */ + LASER_R_V1X = 5, + } argus_laser_type_t; /*!*************************************************************************** - * @brief The AFBR-S50 chip versions. + * @brief The AFBR-S50 chip versions. *****************************************************************************/ -typedef enum { +typedef enum argus_chip_version_t { /*! No device connected or not recognized. */ ADS0032_NONE = 0, @@ -178,37 +215,102 @@ typedef enum { } argus_chip_version_t; -/*!*************************************************************************** - * @brief The number of measurement modes with distinct configuration and - * calibration records. - *****************************************************************************/ -#define ARGUS_MODE_COUNT (2) /*!*************************************************************************** - * @brief The measurement modes. + * @brief The measurement mode flags. + * @details The measurement mode flags that can be combined to a measurement + * mode, e.g. high speed short range mode. See #argus_mode_t for + * a complete list of available measurement modes. + * + * - Bit 0: Short Range Mode + * - Bit 1: Long Range Mode + * - Bit 2: High Speed Mode + * + * Note that the Long and Short Range Flags are mutual exclusive but + * any of those 2 must be set. Thus the value 0 is invalid! + * All other flags enhance the base configurations, e.g. the High + * Speed flag create the high speed mode of the selected base + * measurement mode. *****************************************************************************/ -typedef enum { - /*! Measurement Mode A: Long Range Mode. */ - ARGUS_MODE_A = 1, +typedef enum argus_mode_flags_t { + /*! Measurement Mode Flag for Short Range Base Mode. */ + ARGUS_MODE_FLAG_SHORT_RANGE = 0x01 << 0, - /*! Measurement Mode B: Short Range Mode. */ - ARGUS_MODE_B = 2, + /*! Measurement Mode Flag for Long Range Base Mode. */ + ARGUS_MODE_FLAG_LONG_RANGE = 0x01 << 1, + + /*! Measurement Mode Flag for High Speed Mode. */ + ARGUS_MODE_FLAG_HIGH_SPEED = 0x01 << 2 + +} argus_mode_flags_t; + +/*!*************************************************************************** + * @brief The measurement modes. + * @details The measurement modes are composed in binary from of the flags + * define in #argus_mode_flags_t, i.e. each bit has a special meaning: + * + * - Bit 0: Short Range Mode + * - Bit 1: Long Range Mode + * - Bit 2: High Speed Mode + * + * Note that the Long and Short Range Bits are mutual exclusive but any + * of those 2 must be set. Thus the value 0 is invalid! + *****************************************************************************/ +typedef enum argus_mode_t { + /*! Measurement Mode: Short Range Mode. */ + ARGUS_MODE_SHORT_RANGE = // = 0x01 = 0b0001 + ARGUS_MODE_FLAG_SHORT_RANGE, + + /*! Measurement Mode: Long Range Mode. */ + ARGUS_MODE_LONG_RANGE = // = 0x02 = 0b0010 + ARGUS_MODE_FLAG_LONG_RANGE, + + /*! Measurement Mode: High Speed Short Range Mode. */ + ARGUS_MODE_HIGH_SPEED_SHORT_RANGE = // = 0x05 = 0b0101 + ARGUS_MODE_FLAG_SHORT_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED, + + /*! Measurement Mode: High Speed Long Range Mode. */ + ARGUS_MODE_HIGH_SPEED_LONG_RANGE = // = 0x06 = 0b0110 + ARGUS_MODE_FLAG_LONG_RANGE | ARGUS_MODE_FLAG_HIGH_SPEED, } argus_mode_t; +/*! The data structure for the API representing a AFBR-S50 device instance. */ +typedef struct argus_hnd_t argus_hnd_t; + /*!*************************************************************************** - * @brief Generic API callback function. - * @details Invoked by the API. The content of the abstract data pointer - * depends upon the context. - * @param status The module status that caused the callback. #STATUS_OK if - * everything was as expected. - * @param data An abstract pointer to an user defined data. This will usually - * be passed to the function that also takes the callback as an - * parameter. Otherwise it has a special meaning such as - * configuration or calibration data. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @brief Measurement Ready API callback function. + * + * @details Invoked by the API whenever a measurement cycle is finished and + * new data is ready to be evaluated via the #Argus_EvaluateData API + * function. + * The callback is passed to the API via the #Argus_TriggerMeasurement + * or #Argus_StartMeasurementTimer API functions. + * The API passes the status of the currently finished measurement + * cycle to the callback as first parameters. The second parameter is + * a pointer the API handle structure. The latter is used to identify + * the calling instance of the API in case of multiple devices. + * Further it can be passed to the #Argus_EvaluateData function. + * + * @warning Since the callback is called from an interrupt context, the + * callback execution must return as fast as possible. The usual task + * in the callback is to post an event to the main thread to inform it + * about the new data and that is must call the #Argus_EvaluateData + * function. + * + * @param status The module status that caused the callback. #STATUS_OK if + * everything was as expected. + * + * @param hnd The API handle pointer to the calling instance. Identifies the + * instance of the API that was invoking the callback and thus the + * instance that must call the #Argus_EvaluateData for. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -typedef status_t (*argus_callback_t)(status_t status, void *data); +typedef status_t (*argus_measurement_ready_callback_t)(status_t status, argus_hnd_t *hnd); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h index b2517182f8..4c0182a0b3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_dfm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the dual frequency mode (DFM) setup parameters. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the dual frequency mode (DFM) setup parameters. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,24 +36,27 @@ #ifndef ARGUS_DFM_H #define ARGUS_DFM_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusdfm Dual Frequency Mode - * @ingroup argusapi + * @defgroup argus_dfm Dual Frequency Mode + * @ingroup argus_api * - * @brief Dual Frequency Mode (DFM) parameter definitions and API functions. + * @brief Dual Frequency Mode (DFM) parameter definitions and API functions. * - * @details The DFM is an algorithm to extend the unambiguous range of the - * sensor by utilizing two detuned measurement frequencies. + * @details The DFM is an algorithm to extend the unambiguous range of the + * sensor by utilizing two detuned measurement frequencies. * - * The AFBR-S50 API provides three measurement modes: - * - 1X: Single Frequency Measurement - * - 4X: Dual Frequency Measurement w/ 4 times the unambiguous - * range of the Single Frequency Measurement - * - 8X: Dual Frequency Measurement w/ 8 times the unambiguous - * range of the Single Frequency Measurement + * The AFBR-S50 API provides three measurement modes: + * - 1X: Single Frequency Measurement + * - 4X: Dual Frequency Measurement w/ 4 times the unambiguous + * range of the Single Frequency Measurement + * - 8X: Dual Frequency Measurement w/ 8 times the unambiguous + * range of the Single Frequency Measurement * - * @addtogroup argusdfm + * @addtogroup argus_dfm * @{ *****************************************************************************/ @@ -61,10 +64,10 @@ #define ARGUS_DFM_FRAME_COUNT (2U) /*! The Dual Frequency Mode measurement modes count. Excluding the disabled mode. */ -#define ARGUS_DFM_MODE_COUNT (2U) // expect off-mode! +#define ARGUS_DFM_MODE_COUNT (2U) // except off-mode! /*! The Dual Frequency Mode measurement modes enumeration. */ -typedef enum { +typedef enum argus_dfm_mode_t { /*! Single Frequency Measurement Mode (w/ 1x Unambiguous Range). */ DFM_MODE_OFF = 0U, @@ -78,4 +81,7 @@ typedef enum { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_DFM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h index 64588d25f1..4ffa55656b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_map.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines macros to work with pixel and ADC channel masks. + * @brief This file is part of the AFBR-S50 API. + * @details Defines macros to work with pixel and ADC channel masks. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,217 +37,409 @@ #ifndef ARGUS_MAP_H #define ARGUS_MAP_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusmap ADC Channel Mapping - * @ingroup argusres + * @defgroup argus_map Pixel Channel Mapping + * @ingroup argus_api * - * @brief Pixel ADC Channel Mapping + * @brief Pixel Channel Mapping * - * @details The ADC Channels of each pixel or auxiliary channel on the device - * are numbered in a way that is convenient on the chip architecture. - * The macros in this module are defined in order to map between the - * chip internal channel number (ch) to the two-dimensional - * x-y-indices or one-dimensional n-index representation. + * @details The ADC Channels of each pixel or auxiliary channel on the device + * are numbered in a way that is convenient on the chip architecture. + * The macros in this module are defined in order to map between the + * chip internal channel number (ch) to the two-dimensional + * x-y-indices or one-dimensional n-index representation. * - * @addtogroup argusmap + * @addtogroup argus_map * @{ *****************************************************************************/ -#include "api/argus_def.h" #include "utility/int_math.h" +#include +/*!*************************************************************************** + * @brief The device pixel field size in x direction (long edge). + *****************************************************************************/ +#define ARGUS_PIXELS_X 8 + +/*!*************************************************************************** + * @brief The device pixel field size in y direction (short edge). + *****************************************************************************/ +#define ARGUS_PIXELS_Y 4 + +/*!*************************************************************************** + * @brief The total device pixel count. + *****************************************************************************/ +#define ARGUS_PIXELS ((ARGUS_PIXELS_X)*(ARGUS_PIXELS_Y)) /*!***************************************************************************** - * @brief Macro to determine the pixel ADC channel number from the x-z-indices. - * @param x The x-index of the pixel. - * @param y The y-index of the pixel. - * @return The ADC channel number of the pixel. + * @brief Macro to determine the pixel ADC channel number from the x-z-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The ADC channel number of the pixel. ******************************************************************************/ #define PIXEL_XY2CH(x, y) ((((y) << 3U) & 0x10U) | (((x) ^ 0x07U) << 1U) | ((y) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the pixel x-index from the ADC channel number. - * @param c The ADC channel number of the pixel. - * @return The x-index of the pixel. + * @brief Macro to determine the pixel x-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The x-index of the pixel. ******************************************************************************/ #define PIXEL_CH2X(c) ((((c) >> 1U) ^ 0x07U) & 0x07U) /*!***************************************************************************** - * @brief Macro to determine the pixel y-index from the ADC channel number. - * @param c The ADC channel number of the pixel. - * @return The y-index of the pixel. + * @brief Macro to determine the pixel y-index from the ADC channel number. + * @param c The ADC channel number of the pixel. + * @return The y-index of the pixel. ******************************************************************************/ #define PIXEL_CH2Y(c) ((((c) >> 3U) & 0x02U) | ((c) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the n-index from the x-y-indices. - * @param x The x-index of the pixel. - * @param y The y-index of the pixel. - * @return The n-index of the pixel. + * @brief Macro to determine the n-index from the x-y-indices. + * @param x The x-index of the pixel. + * @param y The y-index of the pixel. + * @return The n-index of the pixel. ******************************************************************************/ #define PIXEL_XY2N(x, y) (((x) << 2U) | (y)) /*!***************************************************************************** - * @brief Macro to determine the pixel x-index from the n-index. - * @param n The n-index of the pixel. - * @return The x-index number of the pixel. + * @brief Macro to determine the pixel x-index from the n-index. + * @param n The n-index of the pixel. + * @return The x-index number of the pixel. ******************************************************************************/ #define PIXEL_N2X(n) ((n) >> 2U) /*!***************************************************************************** - * @brief Macro to determine the pixel y-index from the n-index. - * @param n The n-index of the pixel. - * @return The y-index number of the pixel. + * @brief Macro to determine the pixel y-index from the n-index. + * @param n The n-index of the pixel. + * @return The y-index number of the pixel. ******************************************************************************/ #define PIXEL_N2Y(n) ((n) & 0x03U) /*!***************************************************************************** - * @brief Macro to determine the pixel n-index from the ADC channel number. - * @param n The n-index of the pixel. - * @return The ADC channel number of the pixel. + * @brief Macro to determine the pixel n-index from the ADC channel number. + * @param n The n-index of the pixel. + * @return The ADC channel number of the pixel. ******************************************************************************/ #define PIXEL_N2CH(n) ((((n) << 3U) & 0x10U) | ((((n) >> 1U) ^ 0x0EU) & 0x0EU) | ((n) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine the pixel - * @param c The ADC channel number of the pixel. - * @return The n-index of the pixel. + * @brief Macro to determine the pixel + * @param c The ADC channel number of the pixel. + * @return The n-index of the pixel. ******************************************************************************/ #define PIXEL_CH2N(c) (((((c) << 1U) ^ 0x1CU) & 0x1CU) | (((c) >> 3U) & 0x02U) | ((c) & 0x01U)) /*!***************************************************************************** - * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel. - * @return True if the pixel (n) is enabled. + * @brief Macro to determine if a pixel given by the n-index is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel. + * @return True if the pixel (n) is enabled. ******************************************************************************/ #define PIXELN_ISENABLED(msk, n) (((msk) >> (n)) & 0x01U) /*!***************************************************************************** - * @brief Macro to enable a pixel given by the n-index in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel to enable. + * @brief Macro to enable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to enable. ******************************************************************************/ #define PIXELN_ENABLE(msk, n) ((msk) |= (0x01U << (n))) /*!***************************************************************************** - * @brief Macro disable a pixel given by the n-index in a pixel mask. - * @param msk 32-bit pixel mask - * @param n n-index of the pixel to disable. + * @brief Macro disable a pixel given by the n-index in a pixel mask. + * @param msk 32-bit pixel mask + * @param n n-index of the pixel to disable. ******************************************************************************/ #define PIXELN_DISABLE(msk, n) ((msk) &= (~(0x01U << (n)))) /*!***************************************************************************** - * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The ADC channel number of the pixel. - * @return True if the specified pixel ADC channel is enabled. + * @brief Macro to determine if an ADC pixel channel is enabled from a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The ADC channel number of the pixel. + * @return True if the specified pixel ADC channel is enabled. ******************************************************************************/ #define PIXELCH_ISENABLED(msk, c) (PIXELN_ISENABLED(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to enable an ADC pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The pixel ADC channel number to enable. + * @brief Macro to enable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to enable. ******************************************************************************/ #define PIXELCH_ENABLE(msk, c) (PIXELN_ENABLE(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to disable an ADC pixel channel in a pixel mask. - * @param msk The 32-bit pixel mask - * @param c The pixel ADC channel number to disable. + * @brief Macro to disable an ADC pixel channel in a pixel mask. + * @param msk The 32-bit pixel mask + * @param c The pixel ADC channel number to disable. ******************************************************************************/ #define PIXELCH_DISABLE(msk, c) (PIXELN_DISABLE(msk, PIXEL_CH2N(c))) /*!***************************************************************************** - * @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel. - * @param y y-index of the pixel. - * @return True if the pixel (x,y) is enabled. + * @brief Macro to determine if a pixel given by the x-y-indices is enabled in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel. + * @param y y-index of the pixel. + * @return True if the pixel (x,y) is enabled. ******************************************************************************/ #define PIXELXY_ISENABLED(msk, x, y) (PIXELN_ISENABLED(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro to enable a pixel given by the x-y-indices in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel to enable. - * @param y y-index of the pixel to enable. + * @brief Macro to enable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to enable. + * @param y y-index of the pixel to enable. ******************************************************************************/ #define PIXELXY_ENABLE(msk, x, y) (PIXELN_ENABLE(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro disable a pixel given by the x-y-indices in a pixel mask. - * @param msk 32-bit pixel mask - * @param x x-index of the pixel to disable. - * @param y y-index of the pixel to disable. + * @brief Macro disable a pixel given by the x-y-indices in a pixel mask. + * @param msk 32-bit pixel mask + * @param x x-index of the pixel to disable. + * @param y y-index of the pixel to disable. ******************************************************************************/ #define PIXELXY_DISABLE(msk, x, y) (PIXELN_DISABLE(msk, PIXEL_XY2N(x, y))) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is enabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel. - * @return True if the ADC channel is enabled. + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel. + * @return True if the ADC channel is enabled. ******************************************************************************/ #define CHANNELN_ISENABLED(msk, ch) (((msk) >> ((ch) - 32U)) & 0x01U) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is enabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel to enabled. + * @brief Macro to determine if an ADC channel is enabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to enabled. ******************************************************************************/ #define CHANNELN_ENABLE(msk, ch) ((msk) |= (0x01U << ((ch) - 32U))) /*!***************************************************************************** - * @brief Macro to determine if an ADC channel is disabled in a channel mask. - * @param msk 32-bit channel mask - * @param ch channel number of the ADC channel to disable. + * @brief Macro to determine if an ADC channel is disabled in a channel mask. + * @param msk 32-bit channel mask + * @param ch channel number of the ADC channel to disable. ******************************************************************************/ #define CHANNELN_DISABLE(msk, ch) ((msk) &= (~(0x01U << ((ch) - 32U)))) /*!***************************************************************************** - * @brief Macro to determine the number of enabled pixel/channels in a mask - * via a popcount algorithm. - * @param pxmsk 32-bit pixel mask - * @return The count of enabled pixel channels. + * @brief Macro to determine the number of enabled pixel/channels in a mask + * via a popcount algorithm. + * @param pxmsk 32-bit pixel mask + * @return The count of enabled pixel channels. ******************************************************************************/ #define PIXEL_COUNT(pxmsk) popcount(pxmsk) /*!***************************************************************************** - * @brief Macro to determine the number of enabled channels via a popcount - * algorithm. - * @param pxmsk 32-bit pixel mask - * @param chmsk 32-bit channel mask - * @return The count of enabled ADC channels. + * @brief Macro to determine the number of enabled channels via a popcount + * algorithm. + * @param pxmsk 32-bit pixel mask + * @param chmsk 32-bit channel mask + * @return The count of enabled ADC channels. ******************************************************************************/ #define CHANNEL_COUNT(pxmsk, chmsk) (popcount(pxmsk) + popcount(chmsk)) /*!***************************************************************************** - * @brief Converts a raw ADC channel mask to a x-y-sorted pixel mask. - * @param msk The raw ADC channel mask to be converted. - * @return The converted x-y-sorted pixel mask. + * @brief Converts a raw ADC channel mask to a x-y-sorted pixel mask. + * @param msk The raw ADC channel mask to be converted. + * @return The converted x-y-sorted pixel mask. ******************************************************************************/ static inline uint32_t ChannelToPixelMask(uint32_t msk) { uint32_t res = 0; for (uint_fast8_t n = 0; n < 32; n += 2) { - res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; + res |= ((msk >> PIXEL_N2CH(n)) & 0x3U) << n; // sets 2 bits at once } return res; } +/*!***************************************************************************** + * @brief Converts a x-y-sorted pixel mask to a raw ADC channel mask. + * @param msk The x-y-sorted pixel channel mask to be converted. + * @return The converted raw ADC channel mask. + ******************************************************************************/ +static inline uint32_t PixelToChannelMask(uint32_t msk) +{ + uint32_t res = 0; + + for (uint_fast8_t ch = 0; ch < 32; ch += 2) { + res |= ((msk >> PIXEL_CH2N(ch)) & 0x3U) << ch; // sets 2 bits at once + } + + return res; +} + + +/*!***************************************************************************** + * @brief Shifts a pixel mask by a given offset. + * + * @details This moves the selected pixel pattern by a specified number of + * pixels in x and y direction. + * If the shift in y direction is odd (e.g +1), the pattern will be + * shifted by +0.5 or -0.5 in x direction due to the hexagonal shape + * of the pixel field. Thus, a center pixel (usually the Golden Pixel) + * is determined that is used to determine if the pattern is shifted + * by +0.5 or -0.5 pixels in x direction. The center pixel is then + * always shifted without changing the x index and the surrounding + * pixels are adopting its x index accordingly. + * + * Example: Consider the flower pattern, i.e. the Golden Pixel (e.g. + * 5/2) is selected and all is direct neighbors (i.e. 5/1, 6/1, 6/2, + * 6/3, 5/3, 4/2). If the pattern is shifted by -1 in y direction, the + * new Golden Pixel would be 5/1. Now all surrounding pixels are + * selected, namely 4/0, 4/1, 4/2, 5/0, 5/2, 6/1). This yields again + * the flower around the Golden Pixel. + * + * Thus, the pixels can not all be shifted by the same dx/dy values due + * to the hexagonal shape of the pixel field, e.g. the upper right + * neighbor of 5/2 is 5/1 but the upper right neighbor of 5/1 is NOT + * 5/0 but 4/0! + * This happens only if the shift in y direction is an odd number. + * The algorithm to determine new indices is as follows: + * - If the shift in y direction is even (e.g. +2, -2), no compensation + * of the hexagonal shape is needed; skip compensation, simply + * add/subtract indices. + * - If the center pixel y index is even, pixels that will have even y + * index after the shift will be additionally shifted by -1 in x + * direction. + * - If the center pixel y index is odd, pixel that will have odd y + * index after the shift will be additionally shifted by +1 in x + * direction. + * + * @see Please also refer to the function #Argus_GetCalibrationGoldenPixel + * to obtain the current Golden Pixel location. + * + * @param pixel_mask The x-y-sorted pixel mask to be shifted. + * @param dx The number of pixel to shift in x direction. + * @param dy The number of pixel to shift in y direction. + * @param center_y The center y index of the pattern that is shifted. + * @return The shifted pixel mask. + ******************************************************************************/ +static inline uint32_t ShiftSelectedPixels(const uint32_t pixel_mask, + const int8_t dx, + const int8_t dy, + const uint8_t center_y) +{ + if (dx == 0 && dy == 0) { return pixel_mask; } + + uint32_t shifted_mask = 0; + + for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { + int8_t x_src = x - dx; + int8_t y_src = y - dy; + + if (dy & 0x1) { + /* Compensate for hexagonal pixel shape. */ + if ((center_y & 0x1) && (y & 0x1)) { + x_src--; + } + + if (!(center_y & 0x1) && !(y & 0x1)) { + x_src++; + } + } + + if (x_src < 0 || x_src >= ARGUS_PIXELS_X) { continue; } + + if (y_src < 0 || y_src >= ARGUS_PIXELS_Y) { continue; } + + if (PIXELXY_ISENABLED(pixel_mask, x_src, y_src)) { + PIXELXY_ENABLE(shifted_mask, x, y); + } + } + } + + return shifted_mask; +} + +/*!***************************************************************************** + * @brief Fills a pixel mask to a specified number of pixels around a center pixel. + * + * @details The pixel mask is iteratively filled with the nearest pixel to a + * specified center pixel until a specified number of pixels is achieved. + * The distance between two pixel is determined via a quadratic metric, + * i.e. dx^2 + dy^2. Pixels towards the lower x indices are preferred. + * + * Note that the distance of only calculated approximately, e.g. the + * y distance of pixels is considered to be 2 instead of cos(60)*2. + * + * Nothing is done if the number of pixels already exceeds the specified + * /p pixel_count parameter. + * + * @see Please also refer to the function #Argus_GetCalibrationGoldenPixel + * to obtain the current Golden Pixel location. + * + * @param pixel_mask The x-y-sorted pixel mask to be filled with pixels. + * @param pixel_count The final number of pixels in the pixel mask. + * @param center_x The center pixel x-index. + * @param center_y The center pixel y-index. + * @return The filled pixel mask with at least /p pixel_count pixels selected. + ******************************************************************************/ +static inline uint32_t FillPixelMask(uint32_t pixel_mask, + const uint8_t pixel_count, + const uint8_t center_x, + const uint8_t center_y) +{ + assert(pixel_count <= ARGUS_PIXELS); + assert(center_x < ARGUS_PIXELS_X); + assert(center_y < ARGUS_PIXELS_Y); + + if (pixel_count == ARGUS_PIXELS) { return 0xFFFFFFFFU; } + + /* If the pattern was shifted towards boundaries, the pixel count may have + * decreased. In this case, the pixels closest to the reference pixel are + * selected. Pixel towards lower x index are prioritized. */ + while (pixel_count > PIXEL_COUNT(pixel_mask)) { + int32_t min_dist = INT32_MAX; + int8_t min_x = -1; + int8_t min_y = -1; + + /* Find nearest not selected pixel. */ + for (uint8_t x = 0; x < ARGUS_PIXELS_X; ++x) { + for (uint8_t y = 0; y < ARGUS_PIXELS_Y; ++y) { + if (!PIXELXY_ISENABLED(pixel_mask, x, y)) { + int32_t distx = (x - center_x) << 1; + + if (!(y & 0x1)) { distx++; } + + if (!(center_y & 0x1)) { distx--; } + + const int32_t disty = (y - center_y) << 1; + int32_t dist = distx * distx + disty * disty; + + if (dist < min_dist) { + min_dist = dist; + min_x = x; + min_y = y; + } + } + } + } + + assert(min_x >= 0 && min_x < ARGUS_PIXELS_X); + assert(min_y >= 0 && min_y < ARGUS_PIXELS_Y); + assert(!PIXELXY_ISENABLED(pixel_mask, min_x, min_y)); + PIXELXY_ENABLE(pixel_mask, min_x, min_y); + } + + return pixel_mask; +} /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_MAP_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h index 0e074c6a8b..7a0fa31aa5 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_meas.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details Defines the generic measurement parameters and data structures. + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic measurement parameters and data structures. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,17 +36,20 @@ #ifndef ARGUS_MEAS_H #define ARGUS_MEAS_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusmeas Measurement/Device Control - * @ingroup argusapi + * @defgroup argus_meas Measurement/Device Control + * @ingroup argus_api * - * @brief Measurement/Device control module + * @brief Measurement/Device control module * - * @details This module contains measurement and device control specific - * definitions and methods. + * @details This module contains measurement and device control specific + * definitions and methods. * - * @addtogroup argusmeas + * @addtogroup argus_meas * @{ *****************************************************************************/ @@ -66,11 +69,11 @@ #define ARGUS_AUX_DATA_SIZE (3U * ARGUS_AUX_CHANNEL_COUNT) // 3 bytes * x channels * 1 phase /*!*************************************************************************** - * @brief The device measurement configuration structure. - * @details The portion of the configuration data that belongs to the - * measurement cycle. I.e. the data that defines a measurement frame. + * @brief The device measurement configuration structure. + * @details The portion of the configuration data that belongs to the + * measurement cycle. I.e. the data that defines a measurement frame. *****************************************************************************/ -typedef struct { +typedef struct argus_meas_frame_t { /*! Frame integration time in microseconds. * The integration time determines the measured time between * the start signal and the IRQ. Note that this value will be @@ -82,13 +85,13 @@ typedef struct { /*! Pixel enabled mask for the 32 pixels sorted * by x-y-indices. - * See [pixel mapping](@ref argusmap) for more + * See [pixel mapping](@ref argus_map) for more * details on the pixel mask. */ uint32_t PxEnMask; /*! ADS channel enabled mask for the remaining * channels 31 .. 63 (miscellaneous values). - * See [pixel mapping](@ref argusmap) for more + * See [pixel mapping](@ref argus_map) for more * details on the ADC channel mask. */ uint32_t ChEnMask; @@ -113,9 +116,6 @@ typedef struct { * Determines the optical output power. */ uq12_4_t OutputPower; - /*! The amplitude that is evaluated and used in the DCA module. */ - uq12_4_t DCAAmplitude; - /*! Laser Bias Current Settings in LSB. */ uint8_t BiasCurrent; @@ -133,4 +133,7 @@ typedef struct { } argus_meas_frame_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_MEAS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h new file mode 100644 index 0000000000..7a41440f39 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_offset.h @@ -0,0 +1,59 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic device calibration API. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef ARGUS_OFFSET_H +#define ARGUS_OFFSET_H + +/*!*************************************************************************** + * @addtogroup argus_cal + * @{ + *****************************************************************************/ + +#include "argus_def.h" + +/*!*************************************************************************** + * @brief Pixel Range Offset Table. + * @details Contains pixel range offset values for all 32 active pixels. + *****************************************************************************/ +typedef struct argus_cal_offset_table_t { + /*! The offset values per pixel in meter and Q0.15 format. */ + q0_15_t Table[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + +} argus_cal_offset_table_t; + + +/*! @} */ +#endif /* ARGUS_OFFSET_T */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h index 07b4853bda..f28576500d 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_pba.h @@ -1,12 +1,12 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the pixel binning algorithm (PBA) setup parameters and - * data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the pixel binning algorithm (PBA) setup parameters and + * data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -37,86 +37,98 @@ #ifndef ARGUS_PBA_H #define ARGUS_PBA_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup arguspba Pixel Binning Algorithm - * @ingroup argusapi + * @defgroup argus_pba Pixel Binning Algorithm + * @ingroup argus_api * - * @brief Pixel Binning Algorithm (PBA) parameter definitions and API functions. + * @brief Pixel Binning Algorithm (PBA) parameter definitions and API functions. * - * @details Defines the generic pixel binning algorithm (PBA) setup parameters - * and data structure. + * @details Defines the generic Pixel Binning Algorithm (PBA) setup parameters + * and data structure. * - * The PBA module contains filter algorithms that determine the - * pixels with the best signal quality and extract an 1d distance - * information from the filtered pixels. + * The PBA module contains filter algorithms that determine the + * pixels with the best signal quality and extract an 1D distance + * information from the filtered pixels by averaging them in a + * specified way. * - * The pixel filter algorithm is a three-stage filter with a - * fallback value: + * The Pixel Binning Algorithm is a three-stage filter with a + * fallback value: * - * -# A fixed pre-filter mask is applied to statically disable - * specified pixels. - * -# A relative and absolute amplitude filter is applied in the - * second stage. The relative filter is determined by a ratio - * of the maximum amplitude off all available (i.e. not filtered - * in stage 1) pixels. Pixels that have an amplitude below the - * relative threshold are dismissed. The same holds true for - * the absolute amplitude threshold. All pixel with smaller - * amplitude are dismissed.\n - * The relative threshold is useful to setup a distance - * measurement scenario. All well illuminated pixels are - * selected and considered for the final 1d distance. The - * absolute threshold is used to dismiss pixels that are below - * the noise level. The latter would be considered for the 1d - * result if the maximum amplitude is already very low. - * -# A distance filter is used to distinguish pixels that target - * the actual object from pixels that see the brighter background, - * e.g. white walls. Thus, the pixel with the minimum distance - * is referenced and all pixel that have a distance between - * the minimum and the given minimum distance scope are selected - * for the 1d distance result. The minimum distance scope is - * determined by an relative (to the current minimum distance) - * and an absolute value. The larger scope value is the - * relevant one, i.e. the relative distance scope can be used - * to heed the increasing noise at larger distances. - * -# If all of the above filters fail to determine a single valid - * pixel, the golden pixel is used as a fallback value. The - * golden pixel is the pixel that sits right at the focus point - * of the optics at large distances. - * . + * -# A fixed pre-filter mask is applied to statically disable + * specified pixels. + * -# A relative and absolute amplitude filter is applied in the + * second stage. The relative filter is determined by a ratio + * of the maximum amplitude off all available (i.e. not filtered + * in stage 1) pixels. Pixels that have an amplitude below the + * relative threshold are dismissed. The same holds true for + * the absolute amplitude threshold. All pixel with smaller + * amplitude are dismissed.\n + * Note that the absolute amplitude threshold is disabled if + * the Golden Pixel (see below) is also disabled in order to + * prevent invalid filtering for multi-pixel devices.\n + * The relative threshold is useful to setup a distance + * measurement scenario. All well illuminated pixels are + * selected and considered for the final 1D distance. The + * absolute threshold is used to dismiss pixels that are below + * the noise level. The latter would be considered for the 1D + * result if the maximum amplitude is already very low. + * -# An absolute minimum distance filter is applied in addition + * to the amplitude filter. This removes all pixel that have + * a lower distance than the specified threshold. This is used + * to remove invalid pixels that can be detected by a physically + * not correct negative distance. + * -# A distance filter is used to distinguish pixels that target + * the actual object from pixels that see the brighter background, + * e.g. white walls. Thus, the pixel with the minimum distance + * is referenced and all pixel that have a distance between + * the minimum and the given minimum distance scope are selected + * for the 1D distance result. The minimum distance scope is + * determined by an relative (to the current minimum distance) + * and an absolute value. The larger scope value is the + * relevant one, i.e. the relative distance scope can be used + * to heed the increasing noise at larger distances. + * -# If all of the above filters fail to determine a single valid + * pixel, the Golden Pixel is used as a fallback value. The + * Golden Pixel is the pixel that sits right at the focus point + * of the optics at large distances. + * . * - * After filtering is done, there may be more than a single pixel - * left to determine the 1d signal. Therefore several averaging - * methods are implemented to obtain the best 1d result from many - * pixels. See #argus_pba_averaging_mode_t for details. + * After filtering is done, there may be more than a single pixel + * left to determine the 1D signal. Therefore several averaging + * methods are implemented to obtain the best 1D result from many + * pixels. See #argus_pba_averaging_mode_t for details. * * - * @addtogroup arguspba + * @addtogroup argus_pba * @{ *****************************************************************************/ #include "argus_def.h" /*!*************************************************************************** - * @brief Enable flags for the pixel binning algorithm. + * @brief Enable flags for the pixel binning algorithm. * * @details Determines the pixel binning algorithm feature enable status. - * - [0]: #PBA_ENABLE: Enables the pixel binning feature. - * - [1]: reserved - * - [2]: reserved - * - [3]: reserved - * - [4]: reserved - * - [5]: #PBA_ENABLE_GOLDPX: Enables the golden pixel feature. - * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope - * feature. - * - [7]: reserved - * . + * - [0]: #PBA_ENABLE: Enables the pixel binning feature. + * - [1]: reserved + * - [2]: reserved + * - [3]: reserved + * - [4]: reserved + * - [5]: #PBA_ENABLE_GOLDPX: Enables the Golden Pixel feature. + * - [6]: #PBA_ENABLE_MIN_DIST_SCOPE: Enables the minimum distance scope + * feature. + * - [7]: reserved + * . *****************************************************************************/ -typedef enum { +typedef enum argus_pba_flags_t { /*! Enables the pixel binning feature. */ PBA_ENABLE = 1U << 0U, - /*! Enables the golden pixel. */ + /*! Enables the Golden Pixel. */ PBA_ENABLE_GOLDPX = 1U << 5U, /*! Enables the minimum distance scope filter. */ @@ -125,9 +137,9 @@ typedef enum { } argus_pba_flags_t; /*!*************************************************************************** - * @brief The averaging modes for the pixel binning algorithm. + * @brief The averaging modes for the pixel binning algorithm. *****************************************************************************/ -typedef enum { +typedef enum argus_pba_averaging_mode_t { /*! Evaluate the 1D range from all available pixels using * a simple average. */ PBA_SIMPLE_AVG = 1U, @@ -140,11 +152,12 @@ typedef enum { } argus_pba_averaging_mode_t; /*!*************************************************************************** - * @brief The pixel binning algorithm settings data structure. - * @details Describes the pixel binning algorithm settings. + * @brief The pixel binning algorithm settings data structure. + * @details Describes the pixel binning algorithm settings. *****************************************************************************/ typedef struct { - /*! Enables the pixel binning features. + /*! Enables the Pixel Binning Algorithm. + * * Each bit may enable a different feature. See #argus_pba_flags_t * for details about the enabled flags. */ argus_pba_flags_t Enabled; @@ -156,6 +169,7 @@ typedef struct { argus_pba_averaging_mode_t AveragingMode; /*! The Relative amplitude threshold value (in %) of the max. amplitude. + * * Pixels with amplitude below this threshold value are dismissed. * * All available values from the 8-bit representation are valid. @@ -165,22 +179,27 @@ typedef struct { uq0_8_t RelAmplThreshold; /*! The relative minimum distance scope value in %. - * Pixels that have a range value within [x0, x0 + dx] are considered - * for the pixel binning, where x0 is the minimum distance of all - * amplitude picked pixels and dx is the minimum distance scope value. - * The minimum distance scope value will be the maximum of relative - * and absolute value. + * + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. * * All available values from the 8-bit representation are valid. * The actual percentage value is determined by 100%/256*x. * - * Special values: + * Special values: * - 0: Use 0 for absolute value only or to choose the pixel with the - * minimum distance only (of also the absolute value is 0)! */ + * minimum distance only (of also the absolute value is 0)! */ uq0_8_t RelMinDistanceScope; - /*! The Absolute amplitude threshold value in LSB. - * Pixels with amplitude below this threshold value are dismissed. + /*! The absolute amplitude threshold value in LSB. + * + * Pixels with amplitude below this threshold value are dismissed. + * + * The absolute amplitude threshold is only valid if the Golden Pixel + * mode is enabled. Otherwise, the threshold is set to 0 LSB internally. * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/16. @@ -189,33 +208,42 @@ typedef struct { uq12_4_t AbsAmplThreshold; /*! The absolute minimum distance scope value in m. - * Pixels that have a range value within [x0, x0 + dx] are considered - * for the pixel binning, where x0 is the minimum distance of all - * amplitude picked pixels and dx is the minimum distance scope value. - * The minimum distance scope value will be the maximum of relative - * and absolute value. + * + * Pixels that have a range value within [x0, x0 + dx] are considered + * for the pixel binning, where x0 is the minimum distance of all + * amplitude picked pixels and dx is the minimum distance scope value. + * The minimum distance scope value will be the maximum of relative + * and absolute value. * * All available values from the 16-bit representation are valid. * The actual LSB value is determined by x/2^15. * - * Special values: + * Special values: * - 0: Use 0 for relative value only or to choose the pixel with the - * minimum distance only (of also the relative value is 0)! */ + * minimum distance only (of also the relative value is 0)! */ uq1_15_t AbsMinDistanceScope; - /*! The pre-filter pixel mask determines the pixel channels that are - * statically excluded from the pixel binning (i.e. 1D distance) result. + /*! The absolute minimum distance threshold value in m. * - * The pixel enabled mask is an 32-bit mask that determines the - * device internal channel number. It is recommended to use the - * - #PIXELXY_ISENABLED(msk, x, y) - * - #PIXELXY_ENABLE(msk, x, y) - * - #PIXELXY_DISABLE(msk, x, y) - * . - * macros to work with the pixel enable masks. */ + * Pixels with distance below this threshold value are dismissed. */ + q9_22_t AbsMinDistanceThreshold; + + /*! The pre-filter pixel mask determines the pixel channels that are + * statically excluded from the pixel binning (i.e. 1D distance) result. + * + * The pixel enabled mask is an 32-bit mask that determines the + * device internal channel number. It is recommended to use the + * - #PIXELXY_ISENABLED(msk, x, y) + * - #PIXELXY_ENABLE(msk, x, y) + * - #PIXELXY_DISABLE(msk, x, y) + * . + * macros to work with the pixel enable masks. */ uint32_t PrefilterMask; } argus_cfg_pba_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PBA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h index faa031aeb7..a739cea7f3 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_px.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the device pixel measurement results data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the device pixel measurement results data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,34 +36,40 @@ #ifndef ARGUS_PX_H #define ARGUS_PX_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @addtogroup argusres + * @addtogroup argus_res * @{ *****************************************************************************/ +#include "argus_def.h" + + /*! Maximum amplitude value in UQ12.4 format. */ -#define ARGUS_AMPLITUDE_MAX (0xFFF0U) +#define ARGUS_AMPLITUDE_MAX (0xFFF0U) /*! Maximum range value in Q9.22 format. * Also used as a special value to determine no object detected or infinity range. */ #define ARGUS_RANGE_MAX (Q9_22_MAX) /*!*************************************************************************** - * @brief Status flags for the evaluated pixel structure. + * @brief Status flags for the evaluated pixel structure. * * @details Determines the pixel status. 0 means OK (#PIXEL_OK). - * - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device. - * - [1]: #PIXEL_SAT: The pixel was saturated. - * - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result. - * - [3]: #PIXEL_AMPL_MIN: The pixel amplitude has evaluated to 0. - * - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask. - * - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal. - * - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace. - * - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors. - * . + * - [0]: #PIXEL_OFF: Pixel was disabled and not read from the device. + * - [1]: #PIXEL_SAT: The pixel was saturated. + * - [2]: #PIXEL_BIN_EXCL: The pixel was excluded from the 1D result. + * - [3]: #PIXEL_INVALID: The pixel data is invalid. + * - [4]: #PIXEL_PREFILTERED: The was pre-filtered by static mask. + * - [5]: #PIXEL_NO_SIGNAL: The pixel has no valid signal. + * - [6]: #PIXEL_OUT_OF_SYNC: The pixel has lost signal trace. + * - [7]: #PIXEL_STALLED: The pixel value is stalled due to errors. + * . *****************************************************************************/ -typedef enum { +typedef enum argus_px_status_t { /*! 0x00: Pixel status OK. */ PIXEL_OK = 0, @@ -77,43 +83,45 @@ typedef enum { /*! 0x04: Pixel is excluded from the pixel binning (1d) result. */ PIXEL_BIN_EXCL = 1U << 2U, - /*! 0x08: Pixel amplitude minimum underrun - * (i.e. the amplitude calculation yields 0). */ - PIXEL_AMPL_MIN = 1U << 3U, + /*! 0x08: Pixel has invalid data due to miscellaneous reasons, e.g. + * - Amplitude calculates to 0 (i.e. division by 0) + * - Golden Pixel is invalid due to other saturated pixel. + * - Range/distance is negative. */ + PIXEL_INVALID = 1U << 3U, /*! 0x10: Pixel is pre-filtered by the static pixel binning pre-filter mask, - * i.e. the pixel is disabled by software. */ + * i.e. the pixel is disabled by software. */ PIXEL_PREFILTERED = 1U << 4U, /*! 0x20: Pixel amplitude is below its threshold value. The received signal - * strength is too low to evaluate a valid signal. The range value is - * set to the maximum possible value (approx. 512 m). */ + * strength is too low to evaluate a valid signal. The range value is + * set to the maximum possible value (approx. 512 m). */ PIXEL_NO_SIGNAL = 1U << 5U, /*! 0x40: Pixel is not in sync with respect to the dual frequency algorithm. - * I.e. the pixel may have a correct value but is estimated into the - * wrong unambiguous window. */ + * I.e. the pixel may have a correct value but is estimated into the + * wrong unambiguous window. */ PIXEL_OUT_OF_SYNC = 1U << 6U, /*! 0x80: Pixel is stalled due to one of the following reasons: - * - #PIXEL_SAT - * - #PIXEL_AMPL_MIN - * - #PIXEL_OUT_OF_SYNC - * - Global Measurement Error - * . - * A stalled pixel does not update its measurement data and keeps the - * previous values. If the issue is resolved, the stall disappears and - * the pixel is updating again. */ + * - #PIXEL_SAT + * - #PIXEL_INVALID + * - #PIXEL_OUT_OF_SYNC + * - Global Measurement Error + * . + * A stalled pixel does not update its measurement data and keeps the + * previous values. If the issue is resolved, the stall disappears and + * the pixel is updating again. */ PIXEL_STALLED = 1U << 7U } argus_px_status_t; /*!*************************************************************************** - * @brief The evaluated measurement results per pixel. - * @details This structure contains the evaluated data for a single pixel.\n - * If the amplitude is 0, the pixel is turned off or has invalid data. + * @brief The evaluated measurement results per pixel. + * @details This structure contains the evaluated data for a single pixel.\n + * If the amplitude is 0, the pixel is turned off or has invalid data. *****************************************************************************/ -typedef struct { +typedef struct argus_pixel_t { /*! Range Values from the device in meter. It is the actual distance before * software adjustments/calibrations. */ q9_22_t Range; @@ -141,14 +149,23 @@ typedef struct { /*!*************************************************************************** * @brief Representation of a correlation vector containing sine/cosine components. *****************************************************************************/ -typedef struct { - /*! The sine component. */ - q15_16_t S; +typedef struct argus_vector_t { + union { + /*! The sine [0] and cosine [1] components. */ + q15_16_t SC[2]; - /*! The cosine component. */ - q15_16_t C; + struct { + /*! The sine component. */ + q15_16_t S; + /*! The cosine component. */ + q15_16_t C; + }; + }; } argus_vector_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PX_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h index f59d817634..7cb81bfcf1 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_res.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the generic measurement results data structure. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the generic measurement results data structure. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,35 +36,39 @@ #ifndef ARGUS_RES_H #define ARGUS_RES_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argusres Measurement Data - * @ingroup argusapi + * @defgroup argus_res Measurement Data + * @ingroup argus_api * - * @brief Measurement results data structures. + * @brief Measurement results data structures. * - * @details The interface defines all data structures that correspond to - * the AFBR-S50 measurement results, e.g. - * - 1D distance and amplitude values, - * - 3D distance and amplitude values (i.e. per pixel), - * - Auxiliary channel measurement results (VDD, IAPD, temperature, ...) - * - Device and result status - * - ... - * . + * @details The interface defines all data structures that correspond to + * the AFBR-S50 measurement results, e.g. + * - 1D distance and amplitude values, + * - 3D distance and amplitude values (i.e. per pixel), + * - Auxiliary channel measurement results (VDD, IAPD, temperature, ...) + * - Device and result status + * - ... + * . * - * @addtogroup argusres + * @addtogroup argus_res * @{ *****************************************************************************/ -#include "argus_def.h" #include "argus_px.h" +#include "argus_def.h" #include "argus_meas.h" +#include "argus_xtalk.h" /*!*************************************************************************** - * @brief The 1d measurement results data structure. + * @brief The 1d measurement results data structure. * @details The 1d measurement results obtained by the Pixel Binning Algorithm. *****************************************************************************/ -typedef struct { +typedef struct argus_results_bin_t { /*! Raw 1D range value in meter (Q9.22 format). The distance obtained by * the Pixel Binning Algorithm from the current measurement frame. */ q9_22_t Range; @@ -83,11 +87,11 @@ typedef struct { } argus_results_bin_t; /*!*************************************************************************** - * @brief The auxiliary measurement results data structure. - * @details The auxiliary measurement results obtained by the auxiliary task.\n - * Special values, i.e. 0xFFFFU, indicate no readout value available. + * @brief The auxiliary measurement results data structure. + * @details The auxiliary measurement results obtained by the auxiliary task.\n + * Special values, i.e. 0xFFFFU, indicate no readout value available. *****************************************************************************/ -typedef struct { +typedef struct argus_results_aux_t { /*! VDD ADC channel readout value.\n * Special Value if no value has been measured:\n * Invalid/NotAvailable = 0xFFFFU (UQ12_4_MAX) */ @@ -129,32 +133,66 @@ typedef struct { } argus_results_aux_t; /*!*************************************************************************** - * @brief The measurement results data structure. + * @brief The debug data of measurement results data structure. + * @details This data structure will be filled with API internal data for + * debugging purposes. + *****************************************************************************/ +typedef struct argus_results_debug_t { + /*! The amplitude that is evaluated and used in the DCA module. */ + uq12_4_t DCAAmplitude; + + /*! Raw x-y-sorted ADC results from the device.\n + * Data is arranged as 32-bit values in following order: + * index > phase; where index is pixel number n and auxiliary ADC channel.\n + * Note that disabled pixels are skipped.\n + * e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */ + uint32_t Data[ARGUS_RAW_DATA_VALUES]; + + /*! The current crosstalk correction values as determined by the + * crosstalk predictor algorithm. This is basically the temperature + * dependent portion of the crosstalk correction.\n + * Note that there are two values for the upper and lower two rows + * respectively. */ + xtalk_t XtalkPredictor[ARGUS_PIXELS_Y / 2U]; + + /*! The current crosstalk correction values as determined by the + * crosstalk monitor algorithm. This is a dynamic portion of the + * crosstalk correction that is determined by monitoring passive + * pixels.\n + * Note that the values are valid row-wise. */ + xtalk_t XtalkMonitor[ARGUS_PIXELS_Y]; + +} argus_results_debug_t; + +/*!*************************************************************************** + * @brief The measurement results data structure. * @details This structure contains all information obtained by a single - * distance measurement on the device: - * - The measurement status can be read from the #Status. - * - A timing information is given via the #TimeStamp. - * - Information about the frame state is in the #Frame structure. - * - The 1D distance results are gathered under #Bin. - * - The 3D distance results for each pixel is at #Pixels or #Pixel. - * - Auxiliary values such as temperature can be found at #Auxiliary. - * - Raw data from the device is stored in the #Data array. - * . + * distance measurement on the device: + * - The measurement status can be read from the #Status. + * - A timing information is given via the #TimeStamp. + * - Information about the frame state is in the #Frame structure. + * - The 1D distance results are gathered under #Bin. + * - The 3D distance results for each pixel is at #Pixels or #Pixel. + * - Auxiliary values such as temperature can be found at #Auxiliary. + * - Raw data and debug information from the device and API is stored + * in the optional #Debug data structure. Note that this points to + * an optional structure and can be null! + * . * - * The pixel x-y orientation is sketched in the following graph. Note that - * the laser source would be on the right side beyond the reference pixel. - * See also \link argusmap ADC Channel Mapping\endlink + * The pixel x-y orientation is sketched in the following graph. Note that + * the laser source would be on the right side beyond the reference pixel. + * See also \link argus_map ADC Channel Mapping\endlink * @code - * // Pixel Field: Pixel[x][y] - * // - * // 0 -----------> x - * // | O O O O O O O O - * // | O O O O O O O O - * // | O O O O O O O O O (ref. Px) - * // y O O O O O O O O + * // Pixel Field: Pixel[x][y] + * // + * // 0 -----------> x + * // | O O O O O O O O + * // | O O O O O O O O + * // | O O O O O O O O O (ref. Px) + * // y O O O O O O O O * @endcode *****************************************************************************/ -typedef struct { +typedef struct argus_results_t { /*! The \link #status_t status\endlink of the current measurement frame. * - 0 (i.e. #STATUS_OK) for a good measurement signal. * - > 0 for warnings and weak measurement signal. @@ -168,13 +206,6 @@ typedef struct { /*! The configuration for the current measurement frame. */ argus_meas_frame_t Frame; - /*! Raw x-y-sorted ADC results from the device.\n - * Data is arranged as 32-bit values in following order: - * index > phase; where index is pixel number n and auxiliary ADC channel.\n - * Note that disabled pixels are skipped.\n - * e.g. [n=0,p=0][n=0,p=1]..[n=0,p=3][n=1,p=0]...[n=1,p=3]...[n=31,p=3] */ - uint32_t Data[ARGUS_RAW_DATA_VALUES]; - union { /*! Pixel data indexed by channel number n.\n * Contains calibrated range, amplitude and pixel status among others. @@ -183,14 +214,14 @@ typedef struct { * - 0..31: active pixels * - 32: reference pixel * - * See also \link argusmap ADC Channel Mapping\endlink */ + * See also \link argus_map ADC Channel Mapping\endlink */ argus_pixel_t Pixels[ARGUS_PIXELS + 1U]; struct { /*! Pixel data indexed by x-y-indices.\n * The pixels are ordered in a two dimensional array that represent * the x and y indices of the pixel.\n - * See also \link argusmap ADC Channel Mapping\endlink + * See also \link argus_map ADC Channel Mapping\endlink * * Contains calibrated range, amplitude and pixel status among others. */ argus_pixel_t Pixel[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; @@ -213,8 +244,17 @@ typedef struct { /*! The auxiliary ADC channel data, e.g. sensor temperature. */ argus_results_aux_t Auxiliary; + /*! Optional Debug Data. + * If the pointer is set to a #argus_results_debug_t data structure before + * passing it to the #Argus_EvaluateData function, the data structure is + * filled with internal parameters for debugging purposes. */ + argus_results_debug_t *Debug; + } argus_results_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_RES_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h index 2776308143..2b77965bb0 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_snm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Defines the Shot Noise Monitor (SNM) setup parameters. + * @brief This file is part of the AFBR-S50 API. + * @details Defines the Shot Noise Monitor (SNM) setup parameters. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,30 +36,33 @@ #ifndef ARGUS_SNM_H #define ARGUS_SNM_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argussnm Shot Noise Monitor - * @ingroup argusapi + * @defgroup argus_snm Shot Noise Monitor + * @ingroup argus_api * - * @brief Shot Noise Monitor (SNM) parameter definitions and API functions. + * @brief Shot Noise Monitor (SNM) parameter definitions and API functions. * - * @details The SNM is an algorithm to monitor and react on shot noise - * induced by harsh environment conditions like high ambient - * light. + * @details The SNM is an algorithm to monitor and react on shot noise + * induced by harsh environment conditions like high ambient + * light. * - * The AFBR-S50 API provides three modes: - * - Dynamic: Automatic mode, automatically adopts to current - * ambient conditions. - * - Static (Outdoor): Static mode, optimized for outdoor applications. - * - Static (Indoor): Static mode, optimized for indoor applications. - * . + * The AFBR-S50 API provides three modes: + * - Dynamic: Automatic mode, automatically adopts to current + * ambient conditions. + * - Static (Outdoor): Static mode, optimized for outdoor applications. + * - Static (Indoor): Static mode, optimized for indoor applications. + * . * - * @addtogroup argussnm + * @addtogroup argus_snm * @{ *****************************************************************************/ /*! The Shot Noise Monitor modes enumeration. */ -typedef enum { +typedef enum argus_snm_mode_t { /*! Static Shot Noise Monitoring Mode, optimized for indoor applications. * Assumes the best case scenario, i.e. no bad influence from ambient conditions. * Thus it uses a fixed setting that will result in the best performance. @@ -79,4 +82,7 @@ typedef enum { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_SNM_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h index 244ad1beec..77cd856413 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_status.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Provides status codes for the AFBR-S50 API. + * @brief This file is part of the AFBR-S50 API. + * @details Provides status codes for the AFBR-S50 API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,25 +36,32 @@ #ifndef ARGUS_STATUS_H #define ARGUS_STATUS_H +#ifdef __cplusplus +extern "C" { +#endif #include /*!*************************************************************************** - * @defgroup status Status Codes + * @defgroup argus_status Status Codes + * @ingroup argus + * * @brief Status and Error Code Definitions + * * @details Defines status and error codes for function return values. * Basic status number structure: * - 0 is OK or no error. * - negative values determine errors. * - positive values determine warnings or status information. * . - * @addtogroup status + * + * @addtogroup argus_status * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Type used for all status and error return values. - * @details Basic status number structure: + * @brief Type used for all status and error return values. + * @details Basic status number structure: * - 0 is OK or no error. * - negative values determine errors. * - positive values determine warnings or status information. @@ -138,8 +145,8 @@ enum Status { ********** NVM / Flash Layer Status ********************************************************* *********************************************************************************************/ - /*! -98: Flash Error: The version of the settings in the flash memory is not compatible. */ - ERROR_NVM_INVALID_FILE_VERSION = -98, + /*! -98: Flash Error: The read memory block was not written previously and contains no data. */ + ERROR_NVM_EMPTY = -98, /*! -99: Flash Error: The memory is out of range. */ ERROR_NVM_OUT_OF_RANGE = -99, @@ -183,6 +190,13 @@ enum Status { /*! -102: AFBR-S50 Error: Inconsistent configuration parameters. */ ERROR_ARGUS_INVALID_CFG = -102, + /*! -103: AFBR-S50 Error: The evaluation function has been called but no + * raw data is available yet. + * See also #Argus_EvaluateData for more information. */ + ERROR_ARGUS_BUFFER_EMPTY = -103, + + /*! -104: AFBR-S50 Error: Invalid slave identifier is passed to the module. */ + ERROR_ARGUS_INVALID_SLAVE = -104, /*! -105: AFBR-S50 Error: Invalid measurement mode configuration parameter. */ ERROR_ARGUS_INVALID_MODE = -105, @@ -191,7 +205,6 @@ enum Status { * The current measurement data set is invalid! */ ERROR_ARGUS_BIAS_VOLTAGE_REINIT = -107, - /*! -109: AFBR-S50 Error: The EEPROM readout has failed. The failure is detected * by three distinct read attempts, each resulting in invalid data. * Note: this state differs from that #STATUS_ARGUS_EEPROM_BIT_ERROR @@ -224,7 +237,6 @@ enum Status { * requested command. */ ERROR_ARGUS_BUSY = -191, - /*! -199: AFBR-S50 Error: Unknown module number. */ ERROR_ARGUS_UNKNOWN_MODULE = -199, @@ -235,24 +247,22 @@ enum Status { ERROR_ARGUS_UNKNOWN_LASER = -197, + /*! 191: AFBR-S50 Status (internal): The device is currently busy with testing the + * SPI connection to the device. */ + STATUS_ARGUS_BUSY_TEST = 191, - /*! 193: AFBR-S50 Status (internal): The device is currently busy with updating the - * configuration (i.e. with writing register values). */ - STATUS_ARGUS_BUSY_CFG_UPDATE = 193, - - /*! 194: AFBR-S50 Status (internal): The device is currently busy with updating the - * calibration data (i.e. writing to register values). */ - STATUS_ARGUS_BUSY_CAL_UPDATE = 194, + /*! 192: AFBR-S50 Status (internal): The device is currently busy with updating the + * settings parameter (i.e. with writing register values). */ + STATUS_ARGUS_BUSY_UPDATE = 192, /*! 195: AFBR-S50 Status (internal): The device is currently executing a calibration - * sequence. */ + * sequence. */ STATUS_ARGUS_BUSY_CAL_SEQ = 195, /*! 196: AFBR-S50 Status (internal): The device is currently executing a measurement * cycle. */ STATUS_ARGUS_BUSY_MEAS = 196, - /*! 100: AFBR-S50 Status (internal): The ASIC is initializing a new measurement, i.e. * a register value is written that starts an integration cycle on the ASIC. */ STATUS_ARGUS_STARTING = 100, @@ -260,9 +270,10 @@ enum Status { /*! 103: AFBR-S50 Status (internal): The ASIC is performing an integration cycle. */ STATUS_ARGUS_ACTIVE = 103, - - }; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_STATUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h index ea16342c84..a1a2d878ac 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_version.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file contains the current API version number. + * @brief This file is part of the AFBR-S50 API. + * @details This file contains the current API version number. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,16 +36,19 @@ #ifndef ARGUS_VERSION_H #define ARGUS_VERSION_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup version API Version - * @ingroup argusapi + * @defgroup argus_version API Version + * @ingroup argus_api * - * @brief API and library core version number + * @brief API and library core version number * - * @details Contains the AFBR-S50 API and Library Core Version Number. + * @details Contains the AFBR-S50 API and Library Core Version Number. * - * @addtogroup version + * @addtogroup argus_version * @{ *****************************************************************************/ @@ -53,13 +56,13 @@ #define ARGUS_API_VERSION_MAJOR 1 /*! Minor version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_MINOR 3 +#define ARGUS_API_VERSION_MINOR 4 /*! Bugfix version number of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUGFIX 5 +#define ARGUS_API_VERSION_BUGFIX 4 -/*! Build version nunber of the AFBR-S50 API. */ -#define ARGUS_API_VERSION_BUILD "20210812171515" +/*! Build version number of the AFBR-S50 API. */ +#define ARGUS_API_VERSION_BUILD "20230327150535" /*****************************************************************************/ @@ -73,4 +76,7 @@ (ARGUS_API_VERSION_BUGFIX)) /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_VERSION_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h index 5613706267..6f3d40b49a 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/api/argus_xtalk.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 hardware API. - * @details Defines the generic device calibration API. + * @brief This file is part of the AFBR-S50 hardware API. + * @details Defines the generic device calibration API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,44 +36,71 @@ #ifndef ARGUS_XTALK_H #define ARGUS_XTALK_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @addtogroup arguscal + * @addtogroup argus_cal * @{ *****************************************************************************/ -#include "api/argus_def.h" +#include "argus_def.h" +#include "argus_dfm.h" /*!*************************************************************************** - * @brief Pixel Crosstalk Compensation Vector. - * @details Contains calibration data (per pixel) that belongs to the - * RX-TX-Crosstalk compensation feature. + * @brief Pixel Crosstalk Compensation Vector. + * @details Contains calibration data (per pixel) that belongs to the + * RX-TX-Crosstalk compensation feature. + * The crosstalk vector consists of a Sine and Cosine component in LSB. *****************************************************************************/ - -/*! Pixel Crosstalk Vector */ -typedef struct { +typedef struct xtalk_t { /*! Crosstalk Vector - Sine component. + * Units: LSB * Special Value: Q11_4_MIN == not available */ q11_4_t dS; /*! Crosstalk Vector - Cosine component. + * Units: LSB * Special Value: Q11_4_MIN == not available */ q11_4_t dC; } xtalk_t; /*!*************************************************************************** - * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. - * @details Contains calibration data that belongs to the pixel-to-pixel - * crosstalk compensation feature. + * @brief Pixel Crosstalk Vector Table. + * @details Contains crosstalk vector values for all 32 active pixels, + * separated for A/B-Frames. *****************************************************************************/ -typedef struct { +typedef struct argus_cal_xtalk_table_t { + union { + struct { + /*! The crosstalk vector table for A-Frames. */ + xtalk_t FrameA[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + + /*! The crosstalk vector table for B-Frames. */ + xtalk_t FrameB[ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + }; + + /*! The crosstalk vector table for A/B-Frames of all 32 pixels.*/ + xtalk_t Table[ARGUS_DFM_FRAME_COUNT][ARGUS_PIXELS_X][ARGUS_PIXELS_Y]; + }; + +} argus_cal_xtalk_table_t; + + +/*!*************************************************************************** + * @brief Pixel-To-Pixel Crosstalk Compensation Parameters. + * @details Contains calibration data that belongs to the pixel-to-pixel + * crosstalk compensation feature. + *****************************************************************************/ +typedef struct argus_cal_p2pxtalk_t { /*! Pixel-To-Pixel Compensation on/off. */ bool Enabled; /*! The relative threshold determines when the compensation is active for * each individual pixel. The value determines the ratio of the individual - * pixel signal is with respect to the overall average signal. If the + * pixel signal with respect to the overall average signal. If the * ratio is smaller than the value, the compensation is active. Absolute * and relative conditions are connected with AND logic. */ uq0_8_t RelativeThreshold; @@ -111,4 +138,7 @@ typedef struct { /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_XTALK_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h index dcea881d02..79cf0ede58 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/argus.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file the main header of the AFBR-S50 API. + * @brief This file is part of the AFBR-S50 API. + * @details This file the main header of the AFBR-S50 API. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,7 +36,6 @@ #ifndef ARGUS_H #define ARGUS_H - #ifdef __cplusplus extern "C" { #endif @@ -44,7 +43,6 @@ extern "C" { #include "api/argus_api.h" #ifdef __cplusplus -} +} // extern "C" #endif - #endif /* ARGUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h index 3eb7d2cfd4..d55097cd8a 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_irq.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for enabling/disabling interrupts. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for enabling/disabling interrupts. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,96 +36,94 @@ #ifndef ARGUS_IRQ_H #define ARGUS_IRQ_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_irq IRQ: Global Interrupt Control Layer - * @ingroup argus_platform + * @defgroup argus_irq IRQ: Global Interrupt Control Layer + * @ingroup argus_hal * - * @brief Global Interrupt Control Layer + * @brief Global Interrupt Control Layer * - * @details This module provides functionality to globally enable/disable - * interrupts in a nested way. + * @details This module provides functionality to globally enable/disable + * interrupts in a nested way. * - * Here is a simple example implementation using the CMSIS functions - * "__enable_irq()" and "__disable_irq()". An integer counter is - * used to achieve nested interrupt disabling: + * Here is a simple example implementation using the CMSIS functions + * "__enable_irq()" and "__disable_irq()". An integer counter is + * used to achieve nested interrupt disabling: * - * @code + * @code * - * // Global lock level counter value. - * static volatile int g_irq_lock_ct; + * // Global lock level counter value. + * static volatile int g_irq_lock_ct; * - * // Global unlock all interrupts using CMSIS function "__enable_irq()". - * void IRQ_UNLOCK(void) - * { - * assert(g_irq_lock_ct > 0); - * if (--g_irq_lock_ct <= 0) - * { - * g_irq_lock_ct = 0; - * __enable_irq(); - * } - * } + * // Global unlock all interrupts using CMSIS function "__enable_irq()". + * void IRQ_UNLOCK(void) + * { + * assert(g_irq_lock_ct > 0); + * if (--g_irq_lock_ct <= 0) + * { + * g_irq_lock_ct = 0; + * __enable_irq(); + * } + * } * - * // Global lock all interrupts using CMSIS function "__disable_irq()". - * void IRQ_LOCK(void) - * { - * __disable_irq(); - * g_irq_lock_ct++; - * } + * // Global lock all interrupts using CMSIS function "__disable_irq()". + * void IRQ_LOCK(void) + * { + * __disable_irq(); + * g_irq_lock_ct++; + * } * - * @endcode + * @endcode * - * @note The IRQ locking mechanism is used to create atomic sections - * (within the scope of the AFBR-S50 API) that are very few processor - * instruction only. It does NOT lock interrupts for considerable - * amounts of time. + * @note The IRQ locking mechanism is used to create atomic sections + * (within the scope of the AFBR-S50 API) that are very few processor + * instruction only. It does NOT lock interrupts for considerable + * amounts of time. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. * - * @note The interrupts utilized by the AFBR-S50 API can be interrupted - * by other, higher prioritized interrupts, e.g. some system - * critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK - * mechanism can be implemented such that only the interrupts - * required for the AFBR-S50 API are locked. The above example is - * dedicated to a ARM Corex-M0 architecture, where interrupts - * can only disabled at a global scope. Other architectures like - * ARM Cortex-M4 allow selective disabling of interrupts. + * @note The interrupts utilized by the AFBR-S50 API can be interrupted + * by other, higher prioritized interrupts, e.g. some system + * critical interrupts. In this case, the IRQ_LOCK/IRQ_UNLOCK + * mechanism can be implemented such that only the interrupts + * required for the AFBR-S50 API are locked. The above example is + * dedicated to a ARM Corex-M0 architecture, where interrupts + * can only disabled at a global scope. Other architectures like + * ARM Cortex-M4 allow selective disabling of interrupts. * - * @addtogroup argus_irq + * @addtogroup argus_irq * @{ *****************************************************************************/ /*!*************************************************************************** - * @brief Enable IRQ Interrupts + * @brief Enable IRQ Interrupts * - * @details Enables IRQ interrupts and enters an atomic or critical section. + * @details Enables IRQ interrupts and enters an atomic or critical section. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. *****************************************************************************/ void IRQ_UNLOCK(void); /*!*************************************************************************** - * @brief Disable IRQ Interrupts + * @brief Disable IRQ Interrupts * - * @details Disables IRQ interrupts and leaves the atomic or critical section. + * @details Disables IRQ interrupts and leaves the atomic or critical section. * - * @note The IRQ_LOCK might get called multiple times. Therefore, the - * API expects that the IRQ_UNLOCK must be called as many times as - * the IRQ_LOCK was called before the interrupts are enabled. + * @note The IRQ_LOCK might get called multiple times. Therefore, the + * API expects that the IRQ_UNLOCK must be called as many times as + * the IRQ_LOCK was called before the interrupts are enabled. *****************************************************************************/ void IRQ_LOCK(void); -#ifdef __cplusplus -} -#endif - /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_IRQ_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h index 69939b7759..b8150f0281 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_nvm.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the optional non-volatile memory. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional non-volatile memory. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,99 +36,113 @@ #ifndef ARGUS_NVM_H #define ARGUS_NVM_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_nvm NVM: Non-Volatile Memory Layer - * @ingroup argus_platform + * @defgroup argus_nvm NVM: Non-Volatile Memory Layer + * @ingroup argus_hal * - * @brief Non-Volatile Memory Layer + * @brief Non-Volatile Memory Layer * - * @details This module provides functionality to access the non-volatile - * memory (e.g. flash) on the underlying platform. + * @details This module provides functionality to access the non-volatile + * memory (e.g. flash) on the underlying platform. * - * This module is optional and only required if calibration data - * needs to be stored within the API. + * This module is optional and only required if calibration data + * needs to be stored within the API. * - * @note The implementation of this module is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * @note The implementation of this module is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. * - * @addtogroup argus_nvm + * @addtogroup argus_nvm * @{ *****************************************************************************/ -#include "argus.h" +#include "api/argus_def.h" + +/*! The NVM block size in the non-volatile memory. */ +#define ARGUS_NVM_BLOCK_SIZE 0x300 // 768 bytes /*!*************************************************************************** - * @brief Initializes the non-volatile memory unit and reserves a chunk of memory. + * @brief Write a block of data to the non-volatile memory. * - * @details The function is called upon API initialization sequence. If available, - * the non-volatile memory module reserves a chunk of memory with the - * provides number of bytes (size) and returns with #STATUS_OK. + * @details The function is called whenever the API wants to write data into + * non-volatile memory, e.g. flash. Later, the API reads the written + * data via the #NVM_ReadBlock function. * - * If not implemented, the function should return #ERROR_NOT_IMPLEMENTED - * in oder to inform the API to not use the NVM module. + * The data shall be written to a specified memory block that is + * uniquely dedicated to each individual device. The /p id parameter + * is passed to the function that identifies the device. The /p id + * is composed of the device ID and module type, i.e. it is unique + * among all devices. If only a single device is used anyway, the + * /p id parameter can be ignored. * - * After initialization, the API calls the #NVM_Write and #NVM_Read - * methods to write within the reserved chunk of memory. + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED and the API ignores the NVM. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * If write fails, e.g. due to lack of memory, a negative status + * must be returned, e.g. #ERROR_NVM_OUT_OF_RANGE. * - * @param size The required size of NVM to store all parameters. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * The block size is fixed for a single device. The actual block size + * is defined with #ARGUS_NVM_BLOCK_SIZE. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param id The 32-bit ID number to identify the corresponding memory block. + * @param block_size The number of bytes to be written. Note that this value + * is fixed, i.e. the API always writes the same data size. + * The size is defined here: #ARGUS_NVM_BLOCK_SIZE. + * @param buf The pointer to the data buffer of size /p block_size that needs + * to be written to the NVM. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t NVM_Init(uint32_t size); +status_t NVM_WriteBlock(uint32_t id, uint32_t block_size, uint8_t const *buf); /*!*************************************************************************** - * @brief Write a block of data to the non-volatile memory. + * @brief Reads a block of data from the non-volatile memory. * - * @details The function is called whenever the API wants to write data into - * the previously reserved (#NVM_Init) memory block. The data shall - * be written at a given offset and with a given size. + * @details The function is called whenever the API wants to read data from + * non-volatile memory, e.g. flash. The data will be previously + * stored using the #NVM_WriteBlock function. Otherwise, the function + * must return a corresponding error code, namely #ERROR_NVM_EMPTY. * - * If no NVM module is available, the function can return with error - * #ERROR_NOT_IMPLEMENTED. + * The data shall be read from a specified memory block that is + * uniquely dedicated to each individual device. The /p id parameter + * is passed to the function that identifies the device. The /p id + * is composed of the device ID and module type, i.e. it is unique + * among all devices. If only a single device is used anyway, the + * /p id parameter can be ignored. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. + * If no NVM module is available, the function can return with error + * #ERROR_NOT_IMPLEMENTED and the API ignores the NVM. * - * @param offset The index offset where the first byte needs to be written. - * @param size The number of bytes to be written. - * @param buf The pointer to the data buffer with the data to be written. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * If read fails, e.g. if data has not been written previously, + * a negative status must be returned, e.g. #ERROR_NVM_EMPTY if no + * data has been written yet or any other negative error else-wise. + * + * The block size is fixed for a single device. The actual block size + * is defined with #ARGUS_NVM_BLOCK_SIZE. + * + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disables the NVM feature. + * + * @param id The 32-bit ID number to identify the corresponding memory block. + * @param block_size The number of bytes to be read. Note that this value + * is fixed, i.e. the API always reads the same data size. + * The size is defined here: #ARGUS_NVM_BLOCK_SIZE. + * @param buf The pointer to the data buffer of size /p block_size to copy + * the data to. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t NVM_Write(uint32_t offset, uint32_t size, uint8_t const *buf); - -/*!*************************************************************************** - * @brief Reads a block of data from the non-volatile memory. - * - * @details The function is called whenever the API wants to read data from - * the previously reserved (#NVM_Init) memory block. The data shall - * be read at a given offset and with a given size. - * - * If no NVM module is available, the function can return with error - * #ERROR_NOT_IMPLEMENTED. - * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disables the NVM feature. - * - * @param offset The index offset where the first byte needs to be read. - * @param size The number of bytes to be read. - * @param buf The pointer to the data buffer to copy the data to. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). - *****************************************************************************/ -status_t NVM_Read(uint32_t offset, uint32_t size, uint8_t *buf); -#ifdef __cplusplus -} -#endif +status_t NVM_ReadBlock(uint32_t id, uint32_t block_size, uint8_t *buf); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_NVM_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h index 0ca49f858c..9a76dbe485 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_print.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the optional debug module. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the optional debug module. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,48 +36,55 @@ #ifndef ARGUS_PRINT_H #define ARGUS_PRINT_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup argus_log Debug: Logging Interface - * @ingroup argus_platform + * @defgroup argus_log Debug: Logging Interface + * @ingroup argus_hal * - * @brief Logging interface for the AFBR-S50 API. + * @brief Logging interface for the AFBR-S50 API. * - * @details This interface provides logging utility functions. - * Defines a printf-like function that is used to print error and - * log messages. + * @details This interface provides logging utility functions. + * Defines a printf-like function that is used to print error and + * log messages. * - * @addtogroup argus_log + * @addtogroup argus_log * @{ *****************************************************************************/ #include "api/argus_def.h" /*!*************************************************************************** - * @brief A printf-like function to print formatted data to an debugging interface. + * @brief A printf-like function to print formatted data to an debugging interface. * * @details Writes the C string pointed by fmt_t to an output. If format - * includes format specifiers (subsequences beginning with %), the - * additional arguments following fmt_t are formatted and inserted in - * the resulting string replacing their respective specifiers. + * includes format specifiers (subsequences beginning with %), the + * additional arguments following fmt_t are formatted and inserted in + * the resulting string replacing their respective specifiers. * - * To enable the print functionality, an implementation of the function - * must be provided that maps the output to an interface like UART or - * a debugging console, e.g. by forwarding to standard printf() method. + * To enable the print functionality, an implementation of the function + * must be provided that maps the output to an interface like UART or + * a debugging console, e.g. by forwarding to standard printf() method. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that does nothing. This will improve - * the performance but no error messages are logged. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that does nothing. This will improve + * the performance but no error messages are logged. * - * @note The naming is different from the standard printf() on purpose to - * prevent builtin compiler optimizations. + * @note The naming is different from the standard printf() on purpose to + * prevent builtin compiler optimizations. * - * @param fmt_s The usual print() format string. - * @param ... The usual print() parameters. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param fmt_s The usual print() format string. + * @param ... The usual print() parameters. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t print(const char *fmt_s, ...); /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_PRINT_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h index 4b8f5331b5..8d912afc22 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_s2pi.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the required S2PI module. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required S2PI module. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,95 +36,94 @@ #ifndef ARGUS_S2PI_H #define ARGUS_S2PI_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_s2pi S2PI: Serial Peripheral Interface - * @ingroup argus_platform + * @defgroup argus_s2pi S2PI: Serial Peripheral Interface + * @ingroup argus_hal * - * @brief S2PI: SPI incl. GPIO Hardware Layer Module + * @brief S2PI: SPI incl. GPIO Hardware Layer Module * - * @details The S2PI module consists of a standard SPI interface plus a - * single GPIO interrupt line. Furthermore, the SPI pins are - * accessible via GPIO control to allow a software emulation of - * additional protocols using the same pins. + * @details The S2PI module consists of a standard SPI interface plus a + * single GPIO interrupt line. Furthermore, the SPI pins are + * accessible via GPIO control to allow a software emulation of + * additional protocols using the same pins. * - * **SPI interface:** + * **SPI interface:** * - * The SPI interface is based around a single functionality: + * The SPI interface is based around a single functionality: * - * #S2PI_TransferFrame. This function transfers a specified number - * of bytes via the interfaces MOSI line and simultaneously reads - * the incoming data on the MOSI line. The read can also be skipped. - * The transfer happen asynchronously, e.g. via a DMA request. After - * finishing the transfer, the provided callback is invoked with - * the status of the transfer and the provided abstract parameter. - * Furthermore, the functions receives a slave parameter that can - * be used to connect multiple slaves, each with its individual - * chip select line. + * #S2PI_TransferFrame. This function transfers a specified number + * of bytes via the interfaces MOSI line and simultaneously reads + * the incoming data on the MOSI line. The read can also be skipped. + * The transfer happen asynchronously, e.g. via a DMA request. After + * finishing the transfer, the provided callback is invoked with + * the status of the transfer and the provided abstract parameter. + * Furthermore, the functions receives a slave parameter that can + * be used to connect multiple slaves, each with its individual + * chip select line. * - * The interface also provides functionality to change the SPI - * baud rate. An additional abort method is used to cancel the - * ongoing transfer. + * The interface also provides functionality to change the SPI + * baud rate. An additional abort method is used to cancel the + * ongoing transfer. * - * **GPIO interface:** + * **GPIO interface:** * - * The GPIO part of the S2PI interface has two distinct concerns: + * The GPIO part of the S2PI interface has two distinct concerns: * - * First, the GPIO interface handles the measurement finished interrupt - * from the device. When the device invokes the interrupt, it pulls - * the interrupt line to low. Thus the interrupt must trigger when - * a transition from high to low occurs on the interrupt line. + * First, the GPIO interface handles the measurement finished interrupt + * from the device. When the device invokes the interrupt, it pulls + * the interrupt line to low. Thus the interrupt must trigger when + * a transition from high to low occurs on the interrupt line. * - * The module simply invokes a callback when this interrupt occurs. - * The #S2PI_SetIrqCallback method is used to install the callback - * for a specified slave. Each slave will have its own interrupt - * line. An additional callback parameter can be set that would be - * passed to the callback function. + * The module simply invokes a callback when this interrupt occurs. + * The #S2PI_SetIrqCallback method is used to install the callback + * for a specified slave. Each slave will have its own interrupt + * line. An additional callback parameter can be set that would be + * passed to the callback function. * - * In addition to the interrupt, all SPI pins need to be accessible - * as GPIO pins through this interface. This is required to read - * the EEPROM memory on the device hat is connected to the SPI - * pins but requires a different protocol that is not compatible - * to any standard SPI interface. Therefore, the interface provides - * the possibility to switch to GPIO control mode that allows to - * emulate the EEPROM protocol via software bit banging. + * In addition to the interrupt, all SPI pins need to be accessible + * as GPIO pins through this interface. This is required to read + * the EEPROM memory on the device hat is connected to the SPI + * pins but requires a different protocol that is not compatible + * to any standard SPI interface. Therefore, the interface provides + * the possibility to switch to GPIO control mode that allows to + * emulate the EEPROM protocol via software bit banging. * - * Two methods are provided to switch forth and back between SPI - * and GPIO control. In GPIO mode, several functions are used to - * read and write the individual GPIO pins. + * Two methods are provided to switch forth and back between SPI + * and GPIO control. In GPIO mode, several functions are used to + * read and write the individual GPIO pins. * - * Note that the GPIO mode is only required to readout the EEPROM - * upon initialization of the device, i.e. during execution of the - * #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used - * during measurements. + * Note that the GPIO mode is only required to readout the EEPROM + * upon initialization of the device, i.e. during execution of the + * #Argus_Init or #Argus_Reinit methods. The GPIO mode is not used + * during measurements. * * - * @addtogroup argus_s2pi + * @addtogroup argus_s2pi * @{ *****************************************************************************/ #include "api/argus_def.h" /*!*************************************************************************** - * @brief S2PI layer callback function type for the SPI transfer completed event. + * @brief S2PI layer callback function type for the SPI transfer completed event. * - * @param status The \link #status_t status\endlink of the completed + * @param status The \link #status_t status\endlink of the completed * transfer (#STATUS_OK on success). * - * @param param The provided (optional, can be null) callback parameter. + * @param param The provided (optional, can be null) callback parameter. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ typedef status_t (*s2pi_callback_t)(status_t status, void *param); /*!*************************************************************************** - * @brief S2PI layer callback function type for the GPIO interrupt event. + * @brief S2PI layer callback function type for the GPIO interrupt event. * - * @param param The provided (optional, can be null) callback parameter. + * @param param The provided (optional, can be null) callback parameter. *****************************************************************************/ typedef void (*s2pi_irq_callback_t)(void *param); @@ -132,8 +131,8 @@ typedef void (*s2pi_irq_callback_t)(void *param); * can be used to identify the slave within the SPI module. */ typedef int32_t s2pi_slave_t; -/*! The enumeration of S2PI pins. */ -typedef enum { +/*! The enumeration of S2PI pins. */ +typedef enum s2pi_pin_t { /*! The SPI clock pin. */ S2PI_CLK, @@ -153,64 +152,141 @@ typedef enum { /*!*************************************************************************** - * @brief Returns the status of the SPI module. + * @brief Returns the status of the SPI module. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. - * - #STATUS_BUSY: An SPI transfer is in progress. - * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_IDLE: No SPI transfer or GPIO access is ongoing. + * - #STATUS_BUSY: An SPI transfer is in progress. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. *****************************************************************************/ -status_t S2PI_GetStatus(void); +status_t S2PI_GetStatus(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Transfers a single SPI frame asynchronously. + * @brief Tries to grab the SPI interface mutex for the next transfer. + * + * @details This mutex prevents new asynchronous SPI requests to interfere + * with transfers already in progress for this interface. + * + * Note that this is only required if multiple device are connected to + * a single SPI interface. If only operating a single device per SPI, + * the function can simply always return #STATUS_OK. + * + * There must be a dedicated mutex object per SPI interface if + * multiple SPI interfaces are used. + * + * The mutex will be released in the #S2PI_ReleaseMutex function. + * See #S2PI_ReleaseMutex for additional information. + * + * Here is a simple example implementation for the multiple devices on + * a single SPI interface case. Note that the SpiMutexBlocked must be + * defined per SPI interface if multiple SPI interfaces are used. + * + * @code + * static volatile bool SpiMutexBlocked = false; + * status_t S2PI_TryGetMutex(s2pi_slave_t slave) + * { + * (void) slave; // not used in this implementation as all + * // SPI slaves are on the same SPI interface + * + * status_t status = STATUS_BUSY; + * IRQ_LOCK(); + * if (!SpiMutexBlocked) + * { + * SpiMutexBlocked = true; + * status = STATUS_OK; + * } + * IRQ_UNLOCK(); + * return status; + * } + * void S2PI_ReleaseMutex(s2pi_slave_t slave) + * { + * (void) slave; // not used in this implementation + * SpiMutexBlocked = false; + * } + * @endcode + * + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: the SPI interface was successfully reserved for the caller + * - #STATUS_BUSY: another transfer is ongoing, the caller must not access the bus + *****************************************************************************/ +status_t S2PI_TryGetMutex(s2pi_slave_t slave); + +/*!*************************************************************************** + * @brief Releases the SPI interface mutex. + * + * @details Once the mutex is captured, only a single thread (the one that + * captured it) will call this release function, so there is no + * need for any test or thread safe barriers. Also there is no + * side effect of calling this function when the Mutex is not + * taken so this function can be really simple and doesn't need + * to return anything. + * + * See #S2PI_TryGetMutex on more information and an example + * implementation for the single SPI interface case. + * + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + *****************************************************************************/ +void S2PI_ReleaseMutex(s2pi_slave_t slave); + +/*!*************************************************************************** + * @brief Transfers a single SPI frame asynchronously. * * @details Transfers a single SPI frame in asynchronous manner. The Tx data - * buffer is written to the device via the MOSI line. - * Optionally, the data on the MISO line is written to the provided - * Rx data buffer. If null, the read data is dismissed. Note that - * Rx and Tx buffer can be identical. I.e. the same buffer is used - * for writing and reading data. First, a byte is transmitted and - * the received byte overwrites the previously send value. + * buffer is written to the device via the MOSI line. + * Optionally, the data on the MISO line is written to the provided + * Rx data buffer. If null, the read data is dismissed. Note that + * Rx and Tx buffer can be identical. I.e. the same buffer is used + * for writing and reading data. First, a byte is transmitted and + * the received byte overwrites the previously send value. * - * The transfer of a single frame requires to not toggle the chip - * select line to high in between the data frame. The maximum - * number of bytes transfered in a single SPI transfer is given by - * the data value register of the device, which is 396 data bytes - * plus a single address byte: 397 bytes. + * The transfer of a single frame requires to not toggle the chip + * select line to high in between the data frame. The maximum + * number of bytes transferred in a single SPI transfer is given by + * the data value register of the device, which is 396 data bytes + * plus a single address byte: 397 bytes. * - * An optional callback is invoked when the asynchronous transfer - * is finished. If the \p callback parameter is a null pointer, - * no callback is provided. Note that the provided buffer must not - * change while the transfer is ongoing. + * An optional callback is invoked when the asynchronous transfer + * is finished. If the \p callback parameter is a null pointer, + * no callback is provided. Note that the provided buffer must not + * change while the transfer is ongoing. * - * Use the slave parameter to determine the corresponding slave via the - * given chip select line. + * Use the slave parameter to determine the corresponding slave via the + * given chip select line. * - * Usually, two distinct interrupts are required to handle the RX and - * TX ready events. The callback must be invoked from whichever - * interrupt comes after the SPI transfer has been finished. Note - * that new SPI transfers are invoked from within the callback function - * (i.e. from within the interrupt service routine of same priority). + * Usually, two distinct interrupts are required to handle the RX and + * TX ready events. The callback must be invoked from whichever + * interrupt comes after the SPI transfer has been finished. Note + * that new SPI transfers are invoked from within the callback function + * (i.e. from within the interrupt service routine of same priority). * - * @param slave The specified S2PI slave. - * @param txData The 8-bit values to write to the SPI bus MOSI line. - * @param rxData The 8-bit values received from the SPI bus MISO line + * @param slave The specified S2PI slave. + * @param txData The 8-bit values to write to the SPI bus MOSI line. + * @param rxData The 8-bit values received from the SPI bus MISO line * (pass a null pointer if the data don't need to be read). - * @param frameSize The number of 8-bit values to be sent/received. - * @param callback A callback function to be invoked when the transfer is - * finished. Pass a null pointer if no callback is required. - * @param callbackData A pointer to a state that will be passed to the + * @param frameSize The number of 8-bit values to be sent/received. + * @param callback A callback function to be invoked when the transfer is + * finished. Pass a null pointer if no callback is required. + * @param callbackData A pointer to a state that will be passed to the * callback. Pass a null pointer if not used. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK: Successfully invoked the transfer. - * - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. - * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. - * - #STATUS_BUSY: An SPI transfer is already in progress. The - * transfer was not started. - * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer - * was not started. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully invoked the transfer. + * - #ERROR_INVALID_ARGUMENT: An invalid parameter has been passed. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + * - #STATUS_BUSY: An SPI transfer is already in progress. The + * transfer was not started. + * - #STATUS_S2PI_GPIO_MODE: The module is in GPIO mode. The transfer + * was not started. *****************************************************************************/ status_t S2PI_TransferFrame(s2pi_slave_t slave, uint8_t const *txData, @@ -220,136 +296,158 @@ status_t S2PI_TransferFrame(s2pi_slave_t slave, void *callbackData); /*!*************************************************************************** - * @brief Terminates a currently ongoing asynchronous SPI transfer. + * @brief Terminates a currently ongoing asynchronous SPI transfer. * - * @details When a callback is set for the current ongoing activity, it is - * invoked with the #ERROR_ABORTED error byte. + * @details When a callback is set for the current ongoing activity, it is + * invoked with the #ERROR_ABORTED error byte. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_Abort(void); +status_t S2PI_Abort(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Set a callback for the GPIO IRQ for a specified S2PI slave. + * @brief Set a callback for the GPIO IRQ for a specified S2PI slave. * - * @param slave The specified S2PI slave. - * @param callback A callback function to be invoked when the specified - * S2PI slave IRQ occurs. Pass a null pointer to disable - * the callback. - * @param callbackData A pointer to a state that will be passed to the + * @param slave The specified S2PI slave. + * @param callback A callback function to be invoked when the specified + * S2PI slave IRQ occurs. Pass a null pointer to disable + * the callback. + * @param callbackData A pointer to a state that will be passed to the * callback. Pass a null pointer if not used. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK: Successfully installation of the callback. - * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK: Successfully installation of the callback. + * - #ERROR_S2PI_INVALID_SLAVE: A wrong slave identifier is provided. *****************************************************************************/ status_t S2PI_SetIrqCallback(s2pi_slave_t slave, s2pi_irq_callback_t callback, void *callbackData); /*!*************************************************************************** - * @brief Reads the current status of the IRQ pin. + * @brief Reads the current interrupt pending status of the IRQ pin. * * @details In order to keep a low priority for GPIO IRQs, the state of the - * IRQ pin must be read in order to reliable check for chip timeouts. + * IRQ pin must be read in order to reliable check for chip timeouts. * - * The execution of the interrupt service routine for the data-ready - * interrupt from the corresponding GPIO pin might be delayed due to - * priority issues. The delayed execution might disable the timeout - * for the eye-safety checker too late causing false error messages. - * In order to overcome the issue, the state of the IRQ GPIO input - * pin is read before raising a timeout error in order to check if - * the device has already finished but the IRQ is still pending to be - * executed! + * The execution of the interrupt service routine for the data-ready + * interrupt from the corresponding GPIO pin might be delayed due to + * priority issues. The delayed execution might disable the timeout + * for the eye-safety checker too late causing false error messages. + * In order to overcome the issue, the interrupt state of the IRQ + * GPIO input pin is read before raising a timeout error in order to + * check if the device has already finished and the IRQ is still + * pending to be executed! + * + * Note: an easy implementation is to simply return the state of the + * IRQ line, i.e. 0 if there is a low input state and 1 if there is + * a high input state on the IRQ input pin. However, this + * implementation is not fully reliable since the GPIO interrupt + * (triggered on the falling edge) might be missed and the callback + * is never invoked while the IRQ line is correctly asserted to low + * state by the device. In that case, the API is waiting forever + * until the callback is invoked which might never happen. Therefore, + * it is better if the implementation checks the state of the IRQ + * pending status register or even combines both variations. - * @param slave The specified S2PI slave. - * @return Returns 1U if the IRQ pin is high (IRQ not pending) and 0U if the - * devices pulls the pin to low state (IRQ pending). + * @param slave The specified S2PI slave. + * + * @return Returns 1U if the IRQ is NOT pending (pin is in high state) and + * 0U if the IRQ is pending (pin is pulled to low state by the device). *****************************************************************************/ uint32_t S2PI_ReadIrqPin(s2pi_slave_t slave); /*!*************************************************************************** - * @brief Cycles the chip select line. + * @brief Cycles the chip select line. * * @details In order to cancel the integration on the ASIC, a fast toggling - * of the chip select pin of the corresponding SPI slave is required. - * Therefore, this function toggles the CS from high to low and back. - * The SPI instance for the specified S2PI slave must be idle, - * otherwise the status #STATUS_BUSY is returned. + * of the chip select pin of the corresponding SPI slave is required. + * Therefore, this function toggles the CS from high to low and back. + * The SPI instance for the specified S2PI slave must be idle, + * otherwise the status #STATUS_BUSY is returned. * - * @param slave The specified S2PI slave. - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_CycleCsPin(s2pi_slave_t slave); - - /*!***************************************************************************** - * @brief Captures the S2PI pins for GPIO usage. + * @brief Captures the S2PI pins for GPIO usage. * - * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the - * pins are configured for GPIO operation. The GPIO control must be - * release with the #S2PI_ReleaseGpioControl function in order to - * switch back to ordinary SPI functionality. + * @details The SPI is disabled (module status: #STATUS_S2PI_GPIO_MODE) and the + * pins are configured for GPIO operation. The GPIO control must be + * release with the #S2PI_ReleaseGpioControl function in order to + * switch back to ordinary SPI functionality. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_CaptureGpioControl(void); +status_t S2PI_CaptureGpioControl(s2pi_slave_t slave); /*!***************************************************************************** - * @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. + * @brief Releases the S2PI pins from GPIO usage and switches back to SPI mode. * - * @details The GPIO pins are configured for SPI operation and the GPIO mode is - * left. Must be called if the pins are captured for GPIO operation via - * the #S2PI_CaptureGpioControl function. + * @details The GPIO pins are configured for SPI operation and the GPIO mode is + * left. Must be called if the pins are captured for GPIO operation via + * the #S2PI_CaptureGpioControl function. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. Note that the slave information is + * only required if multiple SPI instances are used in order to + * map to the correct SPI instance. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ -status_t S2PI_ReleaseGpioControl(void); +status_t S2PI_ReleaseGpioControl(s2pi_slave_t slave); /*!***************************************************************************** - * @brief Writes the output for a specified SPI pin in GPIO mode. + * @brief Writes the output for a specified SPI pin in GPIO mode. * * @details This function writes the value of an SPI pin if the SPI pins are - * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. * - * @note Since some GPIO peripherals switch the GPIO pins very fast a delay - * must be added after each GBIO access (i.e. right before returning - * from the #S2PI_WriteGpioPin method) in order to decrease the baud - * rate of the software EEPROM protocol. Increase the delay if timing - * issues occur while reading the EERPOM. For example: - * Delay = 10 µsec => Baud Rate < 100 kHz + * @note Since some GPIO peripherals switch the GPIO pins very fast a delay + * must be added after each GBIO access (i.e. right before returning + * from the #S2PI_WriteGpioPin method) in order to decrease the baud + * rate of the software EEPROM protocol. Increase the delay if timing + * issues occur while reading the EERPOM. For example: + * Delay = 10 µsec => Baud Rate < 100 kHz * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @param slave The specified S2PI slave. - * @param pin The specified S2PI pin. - * @param value The GPIO pin state to write (0 = low, 1 = high). - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to write (0 = low, 1 = high). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_WriteGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t value); /*!***************************************************************************** - * @brief Reads the input from a specified SPI pin in GPIO mode. + * @brief Reads the input from a specified SPI pin in GPIO mode. * * @details This function reads the value of an SPI pin if the SPI pins are - * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. + * captured for GPIO operation via the #S2PI_CaptureGpioControl previously. * - * @note This function is only called during device initialization! + * @note This function is only called during device initialization! * - * @param slave The specified S2PI slave. - * @param pin The specified S2PI pin. - * @param value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level). - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param slave The specified S2PI slave. + * @param pin The specified S2PI pin. + * @param value The GPIO pin state to read (0 = low, GND level, 1 = high, VCC level). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t S2PI_ReadGpioPin(s2pi_slave_t slave, s2pi_pin_t pin, uint32_t *value); -#ifdef __cplusplus -} -#endif /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif // ARGUS_S2PI_H diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h index 80deb42016..b736010682 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/platform/argus_timer.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides an interface for the required timer modules. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an interface for the required timer modules. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,118 +36,117 @@ #ifndef ARGUS_TIMER_H #define ARGUS_TIMER_H - #ifdef __cplusplus extern "C" { #endif /*!*************************************************************************** - * @defgroup argus_timer Timer: Hardware Timer Interface - * @ingroup argus_platform + * @defgroup argus_timer Timer: Hardware Timer Interface + * @ingroup argus_hal * - * @brief Timer implementations for lifetime counting as well as periodic - * callback. + * @brief Timer implementations for lifetime counting as well as periodic + * callback. * - * @details The module provides an interface to the timing utilities that - * are required by the AFBR-S50 time-of-flight sensor API. + * @details The module provides an interface to the timing utilities that + * are required by the AFBR-S50 time-of-flight sensor API. * - * Two essential features have to be provided by the user code: - * 1. Time Measurement Capability: In order to keep track of outgoing - * signals, the API needs to measure elapsed time. In order to - * provide optimum device performance, the granularity should be - * around 10 to 100 microseconds. - * 2. Periodic Callback: The API provides an automatic starting of - * measurement cycles at a fixed frame rate via a periodic - * interrupt timer. If this feature is not used, implementation - * of the periodic interrupts can be skipped. An weak default - * implementation is provide in the API. - * . + * Two essential features have to be provided by the user code: + * 1. Time Measurement Capability: In order to keep track of outgoing + * signals, the API needs to measure elapsed time. In order to + * provide optimum device performance, the granularity should be + * around 10 to 100 microseconds. + * 2. Periodic Callback: The API provides an automatic starting of + * measurement cycles at a fixed frame rate via a periodic + * interrupt timer. If this feature is not used, implementation + * of the periodic interrupts can be skipped. An weak default + * implementation is provide in the API. + * . * - * The time measurement feature is simply implemented by the function - * #Timer_GetCounterValue. Whenever the function is called, the - * provided counter values must be written with the values obtained - * by the current time. + * The time measurement feature is simply implemented by the function + * #Timer_GetCounterValue. Whenever the function is called, the + * provided counter values must be written with the values obtained + * by the current time. * - * The periodic interrupt timer is a simple callback interface. - * After installing the callback function pointer via #Timer_SetCallback, - * the timer can be started by setting interval via #Timer_SetInterval. - * From then, the callback is invoked periodically as the corresponding - * interval may specify. The timer is stopped by setting the interval - * to 0 using the #Timer_SetInterval function. The interval can be - * updated at any time by updating the interval via the #Timer_SetInterval - * function. To any of these functions, an abstract parameter pointer - * must be passed. This parameter is passed back to the callback any - * time it is invoked. + * The periodic interrupt timer is a simple callback interface. + * After installing the callback function pointer via #Timer_SetCallback, + * the timer can be started by setting interval via #Timer_SetInterval. + * From then, the callback is invoked periodically as the corresponding + * interval may specify. The timer is stopped by setting the interval + * to 0 using the #Timer_SetInterval function. The interval can be + * updated at any time by updating the interval via the #Timer_SetInterval + * function. To any of these functions, an abstract parameter pointer + * must be passed. This parameter is passed back to the callback any + * time it is invoked. * - * In order to provide the usage of multiple devices, an mechanism is - * introduced to allow the installation of multiple callback interval - * at the same time. Therefore, the abstract parameter pointer is used - * to identify the corresponding callback interval. For example, there - * are two callbacks for two intervals, t1 and t2, required. The user - * can start two timers by calling the #Timer_SetInterval method twice, - * but with an individual parameter pointer, ptr1 and ptr2, each: - * \code - * Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1 - * Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1 - * \endcode + * In order to provide the usage of multiple devices, an mechanism is + * introduced to allow the installation of multiple callback interval + * at the same time. Therefore, the abstract parameter pointer is used + * to identify the corresponding callback interval. For example, there + * are two callbacks for two intervals, t1 and t2, required. The user + * can start two timers by calling the #Timer_SetInterval method twice, + * but with an individual parameter pointer, ptr1 and ptr2, each: + * \code + * Timer_SetInterval(100000, ptr1); // 10 ms callback w/ parameter ptr1 + * Timer_SetInterval(200000, ptr2); // 20 ms callback w/ parameter ptr1 + * \endcode * - * Note that the implemented timer module must therefore support - * as many different intervals as instances of the AFBR-S50 device are - * used. + * Note that the implemented timer module must therefore support + * as many different intervals as instances of the AFBR-S50 device are + * used. * - * @addtogroup argus_timer + * @addtogroup argus_timer * @{ *****************************************************************************/ -#include "api/argus_def.h" +#include "utility/status.h" /******************************************************************************* * Lifetime Counter Timer Interface ******************************************************************************/ /*!*************************************************************************** - * @brief Obtains the lifetime counter value from the timers. + * @brief Obtains the lifetime counter value from the timers. * * @details The function is required to get the current time relative to any - * point in time, e.g. the startup time. The returned values \p hct and - * \p lct are given in seconds and microseconds respectively. The current - * elapsed time since the reference time is then calculated from: + * point in time, e.g. the startup time. The returned values \p hct and + * \p lct are given in seconds and microseconds respectively. The current + * elapsed time since the reference time is then calculated from: * - * t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec + * t_now [µsec] = hct * 1000000 µsec + lct * 1 µsec * - * Note that the accuracy/granularity of the lifetime counter does - * not need to be 1 µsec. Usually, a granularity of approximately - * 100 µsec is sufficient. However, in case of very high frame rates - * (above 1000 frames per second), it is recommended to implement - * an even lower granularity (somewhere in the 10 µsec regime). + * Note that the accuracy/granularity of the lifetime counter does + * not need to be 1 µsec. Usually, a granularity of approximately + * 100 µsec is sufficient. However, in case of very high frame rates + * (above 1000 frames per second), it is recommended to implement + * an even lower granularity (somewhere in the 10 µsec regime). * - * It must be guaranteed, that each call of the #Timer_GetCounterValue - * function must provide a value that is greater or equal, but never lower, - * than the value returned from the previous call. + * It must be guaranteed, that each call of the #Timer_GetCounterValue + * function must provide a value that is greater or equal, but never lower, + * than the value returned from the previous call. * - * A hardware based implementation of the lifetime counter functionality - * would be to chain two distinct timers such that counter 2 increases - * its value when counter 1 wraps to 0. The easiest way is to setup - * counter 1 to wrap exactly every second. Counter 1 would than count - * the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds - * (\p hct) value. A 16-bit counter is sufficient in case of counter 1 - * while counter 2 must be a 32-bit version. + * A hardware based implementation of the lifetime counter functionality + * would be to chain two distinct timers such that counter 2 increases + * its value when counter 1 wraps to 0. The easiest way is to setup + * counter 1 to wrap exactly every second. Counter 1 would than count + * the sub-seconds (i.e. µsec) value (\p lct) and counter 2 the seconds + * (\p hct) value. A 16-bit counter is sufficient in case of counter 1 + * while counter 2 must be a 32-bit version. * - * In case of a lack of available hardware timers, a software solution - * can be used that requires only a 16-bit timer. In a simple scenario, - * the timer is configured to wrap around every second and increase - * a software counter value in its interrupt service routine (triggered - * with the wrap around event) every time the wrap around occurs. + * In case of a lack of available hardware timers, a software solution + * can be used that requires only a 16-bit timer. In a simple scenario, + * the timer is configured to wrap around every second and increase + * a software counter value in its interrupt service routine (triggered + * with the wrap around event) every time the wrap around occurs. * * - * @note The implementation of this function is mandatory for the correct - * execution of the API. + * @note The implementation of this function is mandatory for the correct + * execution of the API. * - * @param hct A pointer to the high counter value bits representing current - * time in seconds. + * @param hct A pointer to the high counter value bits representing current + * time in seconds. * - * @param lct A pointer to the low counter value bits representing current - * time in microseconds. Range: 0, .., 999999 µsec + * @param lct A pointer to the low counter value bits representing current + * time in microseconds. Range: 0, .., 999999 µsec *****************************************************************************/ void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct); @@ -156,68 +155,70 @@ void Timer_GetCounterValue(uint32_t *hct, uint32_t *lct); ******************************************************************************/ /*!*************************************************************************** - * @brief The callback function type for periodic interrupt timer. + * @brief The callback function type for periodic interrupt timer. * - * @details The function that is invoked every time a specified interval elapses. - * An abstract parameter is passed to the function whenever it is called. + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. *****************************************************************************/ typedef void (*timer_cb_t)(void *param); /*!*************************************************************************** - * @brief Installs an periodic timer callback function. + * @brief Installs an periodic timer callback function. * - * @details Installs an periodic timer callback function that is invoked whenever - * an interval elapses. The callback is the same for any interval, - * however, the single intervals can be identified by the passed - * parameter. - * Passing a zero-pointer removes and disables the callback. + * @details Installs an periodic timer callback function that is invoked whenever + * an interval elapses. The callback is the same for any interval, + * however, the single intervals can be identified by the passed + * parameter. + * Passing a zero-pointer removes and disables the callback. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disable the periodic timer callback - * and thus the automatic starting of measurements from the background. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. * - * @param f The timer callback function. + * @param f The timer callback function. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Timer_SetCallback(timer_cb_t f); /*!*************************************************************************** - * @brief Sets the timer interval for a specified callback parameter. + * @brief Sets the timer interval for a specified callback parameter. * - * @details Sets the callback interval for the specified parameter and starts - * the timer with a new interval. If there is already an interval with - * the given parameter, the timer is restarted with the given interval. - * If the same time interval as already set is passed, nothing happens. - * Passing a interval of 0 disables the timer. + * @details Sets the callback interval for the specified parameter and starts + * the timer with a new interval. If there is already an interval with + * the given parameter, the timer is restarted with the given interval. + * If the same time interval as already set is passed, nothing happens. + * Passing a interval of 0 disables the timer. * - * Note that a microsecond granularity for the timer interrupt period is - * not required. Usually a microseconds granularity is sufficient. - * The required granularity depends on the targeted frame rate, e.g. in - * case of more than 1 kHz measurement rate, a granularity of less than - * a microsecond is required to achieve the given frame rate. + * When enabling the timer (or resetting by applying another interval), + * the first timer interrupt must happen after the specified interval. * - * @note The implementation of this function is optional for the correct - * execution of the API. If not implemented, a weak implementation - * within the API will be used that disable the periodic timer callback - * and thus the automatic starting of measurements from the background. + * Note that a microsecond granularity for the timer interrupt period is + * not required. Usually a milliseconds granularity is sufficient. + * The required granularity depends on the targeted frame rate, e.g. in + * case of more than 1 kHz measurement rate, a granularity of less than + * a millisecond is required to achieve the given frame rate. * - * @param dt_microseconds The callback interval in microseconds. + * @note The implementation of this function is optional for the correct + * execution of the API. If not implemented, a weak implementation + * within the API will be used that disable the periodic timer callback + * and thus the automatic starting of measurements from the background. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param dt_microseconds The callback interval in microseconds. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Timer_SetInterval(uint32_t dt_microseconds, void *param); -#ifdef __cplusplus -} -#endif - /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* ARGUS_TIMER_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h index ae7422ada5..8d5406360b 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_def.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details Provides definitions and basic macros for fixed point data types. + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,23 +36,30 @@ #ifndef FP_DEF_H #define FP_DEF_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup fixedpoint Fixed Point Math - * @ingroup argusutil - * @brief A basic math library for fixed point number in the Qx.y fomat. - * @details This module contains common fixed point type definitions as - * well as some basic math algorithms. All types are based on - * integer types. The number are defined with the Q number format. + * @defgroup argus_fp Fixed Point Math + * @ingroup argus_util * - * - For a description of the Q number format refer to: - * https://en.wikipedia.org/wiki/Q_(number_format) - * - Another resource for fixed point math in C might be found at - * http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491 - * . - * @warning This definitions are not portable and work only with - * little-endian systems! - * @addtogroup fixedpoint + * @brief A basic math library for fixed point number in the Qx.y fomat. + * + * @details This module contains common fixed point type definitions as + * well as some basic math algorithms. All types are based on + * integer types. The number are defined with the Q number format. + * + * - For a description of the Q number format refer to: + * https://en.wikipedia.org/wiki/Q_(number_format) + * - Another resource for fixed point math in C might be found at + * http://www.eetimes.com/author.asp?section_id=36&doc_id=1287491 + * . + * + * @warning This definitions are not portable and work only with + * little-endian systems! + * + * @addtogroup argus_fp * @{ *****************************************************************************/ @@ -66,11 +73,11 @@ ***** UQ6.2 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ6.2 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 6 integer and 2 fractional bits. - * - Range: 0 .. 63.75 - * - Granularity: 0.25 + * @brief Unsigned fixed point number: UQ6.2 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 6 integer and 2 fractional bits. + * - Range: 0 .. 63.75 + * - Granularity: 0.25 *****************************************************************************/ typedef uint8_t uq6_2_t; @@ -86,11 +93,11 @@ typedef uint8_t uq6_2_t; ***** UQ4.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ4.4 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 4 integer and 4 fractional bits. - * - Range: 0 .. 15.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ4.4 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 4 integer and 4 fractional bits. + * - Range: 0 .. 15.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint8_t uq4_4_t; @@ -106,11 +113,11 @@ typedef uint8_t uq4_4_t; ***** UQ2.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ2.6 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 2 integer and 6 fractional bits. - * - Range: 0 .. 3.984375 - * - Granularity: 0.015625 + * @brief Unsigned fixed point number: UQ2.6 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 2 integer and 6 fractional bits. + * - Range: 0 .. 3.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef uint8_t uq2_6_t; @@ -126,11 +133,11 @@ typedef uint8_t uq2_6_t; ***** UQ1.7 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ1.7 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 1 integer and 7 fractional bits. - * - Range: 0 .. 1.9921875 - * - Granularity: 0.0078125 + * @brief Unsigned fixed point number: UQ1.7 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 1.9921875 + * - Granularity: 0.0078125 *****************************************************************************/ typedef uint8_t uq1_7_t; @@ -146,11 +153,11 @@ typedef uint8_t uq1_7_t; ***** UQ0.8 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ0.8 - * @details An unsigned fixed point number format based on the 8-bit unsigned - * integer type with 1 integer and 7 fractional bits. - * - Range: 0 .. 0.99609375 - * - Granularity: 0.00390625 + * @brief Unsigned fixed point number: UQ0.8 + * @details An unsigned fixed point number format based on the 8-bit unsigned + * integer type with 1 integer and 7 fractional bits. + * - Range: 0 .. 0.99609375 + * - Granularity: 0.00390625 *****************************************************************************/ typedef uint8_t uq0_8_t; @@ -167,11 +174,11 @@ typedef uint8_t uq0_8_t; ***** Q3.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q3.4 - * @details A signed fixed point number format based on the 8-bit signed - * integer type with 3 integer and 4 fractional bits. - * - Range: -8 ... 7.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q3.4 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 3 integer and 4 fractional bits. + * - Range: -8 ... 7.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int8_t q3_4_t; @@ -189,11 +196,11 @@ typedef int8_t q3_4_t; ***** Q1.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q1.6 - * @details A signed fixed point number format based on the 8-bit signed - * integer type with 1 integer and 6 fractional bits. - * - Range: -2 ... 1.984375 - * - Granularity: 0.015625 + * @brief Signed fixed point number: Q1.6 + * @details A signed fixed point number format based on the 8-bit signed + * integer type with 1 integer and 6 fractional bits. + * - Range: -2 ... 1.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef int8_t q1_6_t; @@ -215,11 +222,11 @@ typedef int8_t q1_6_t; ***** UQ12.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ12.4 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 12 integer and 4 fractional bits. - * - Range: 0 ... 4095.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ12.4 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 12 integer and 4 fractional bits. + * - Range: 0 ... 4095.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint16_t uq12_4_t; @@ -235,11 +242,11 @@ typedef uint16_t uq12_4_t; ***** UQ10.6 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ10.6 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 10 integer and 6 fractional bits. - * - Range: 0 ... 1023.984375 - * - Granularity: 0.015625 + * @brief Unsigned fixed point number: UQ10.6 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 10 integer and 6 fractional bits. + * - Range: 0 ... 1023.984375 + * - Granularity: 0.015625 *****************************************************************************/ typedef uint16_t uq10_6_t; @@ -255,11 +262,11 @@ typedef uint16_t uq10_6_t; ***** UQ1.15 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ1.15 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 1 integer and 15 fractional bits. - * - Range: 0 .. 1.999969 - * - Granularity: 0.000031 + * @brief Unsigned fixed point number: UQ1.15 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 1 integer and 15 fractional bits. + * - Range: 0 .. 1.999969 + * - Granularity: 0.000031 *****************************************************************************/ typedef uint16_t uq1_15_t; @@ -275,11 +282,11 @@ typedef uint16_t uq1_15_t; ***** UQ0.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ0.16 - * @details An unsigned fixed point number format based on the 16-bit unsigned - * integer type with 0 integer and 16 fractional bits. - * - Range: 0 .. 0.9999847412109375 - * - Granularity: 1.52587890625e-5 + * @brief Unsigned fixed point number: UQ0.16 + * @details An unsigned fixed point number format based on the 16-bit unsigned + * integer type with 0 integer and 16 fractional bits. + * - Range: 0 .. 0.9999847412109375 + * - Granularity: 1.52587890625e-5 *****************************************************************************/ typedef uint16_t uq0_16_t; @@ -296,11 +303,11 @@ typedef uint16_t uq0_16_t; ***** Q11.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q11.4 - * @details A signed fixed point number format based on the 16-bit signed - * integer type with 11 integer and 4 fractional bits. - * - Range: -2048 ... 2047.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q11.4 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 11 integer and 4 fractional bits. + * - Range: -2048 ... 2047.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int16_t q11_4_t; @@ -319,11 +326,11 @@ typedef int16_t q11_4_t; ***** Q7.8 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q7.8 - * @details A signed fixed point number format based on the 16-bit signed - * integer type with 7 integer and 8 fractional bits. - * - Range: -128 .. 127.99609375 - * - Granularity: 0.00390625 + * @brief Signed fixed point number: Q7.8 + * @details A signed fixed point number format based on the 16-bit signed + * integer type with 7 integer and 8 fractional bits. + * - Range: -128 .. 127.99609375 + * - Granularity: 0.00390625 *****************************************************************************/ typedef int16_t q7_8_t; @@ -342,11 +349,11 @@ typedef int16_t q7_8_t; ***** Q3.12 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q3.12 - * @details A signed fixed point number format based on the 16-bit integer - * type with 3 integer and 12 fractional bits. - * - Range: -8 .. 7.99975586 - * - Granularity: 0.00024414 + * @brief Signed fixed point number: Q3.12 + * @details A signed fixed point number format based on the 16-bit integer + * type with 3 integer and 12 fractional bits. + * - Range: -8 .. 7.99975586 + * - Granularity: 0.00024414 *****************************************************************************/ typedef int16_t q3_12_t; @@ -365,11 +372,11 @@ typedef int16_t q3_12_t; ***** Q0.15 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q0.15 - * @details A signed fixed point number format based on the 16-bit integer - * type with 0 integer and 15 fractional bits. - * - Range: -1 .. 0.999969482 - * - Granularity: 0.000030518 + * @brief Signed fixed point number: Q0.15 + * @details A signed fixed point number format based on the 16-bit integer + * type with 0 integer and 15 fractional bits. + * - Range: -1 .. 0.999969482 + * - Granularity: 0.000030518 *****************************************************************************/ typedef int16_t q0_15_t; @@ -389,11 +396,11 @@ typedef int16_t q0_15_t; ***** UQ28.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ28.4 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 28 integer and 4 fractional bits. - * - Range: 0 ... 268435455.9375 - * - Granularity: 0.0625 + * @brief Unsigned fixed point number: UQ28.4 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 28 integer and 4 fractional bits. + * - Range: 0 ... 268435455.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef uint32_t uq28_4_t; @@ -409,11 +416,11 @@ typedef uint32_t uq28_4_t; ***** UQ16.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ16.16 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 16 integer and 16 fractional bits. - * - Range: 0 ... 65535.999984741 - * - Granularity: 0.000015259 + * @brief Unsigned fixed point number: UQ16.16 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 16 integer and 16 fractional bits. + * - Range: 0 ... 65535.999984741 + * - Granularity: 0.000015259 *****************************************************************************/ typedef uint32_t uq16_16_t; @@ -432,11 +439,11 @@ typedef uint32_t uq16_16_t; ***** UQ10.22 ******************************************************************************/ /*!*************************************************************************** - * @brief Unsigned fixed point number: UQ10.22 - * @details An unsigned fixed point number format based on the 32-bit unsigned - * integer type with 10 integer and 22 fractional bits. - * - Range: 0 ... 1023.99999976158 - * - Granularity: 2.38418579101562E-07 + * @brief Unsigned fixed point number: UQ10.22 + * @details An unsigned fixed point number format based on the 32-bit unsigned + * integer type with 10 integer and 22 fractional bits. + * - Range: 0 ... 1023.99999976158 + * - Granularity: 2.38418579101562E-07 *****************************************************************************/ typedef uint32_t uq10_22_t; @@ -456,11 +463,11 @@ typedef uint32_t uq10_22_t; ***** Q27.4 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q27.4 - * @details A signed fixed point number format based on the 32-bit signed - * integer type with 27 integer and 4 fractional bits. - * - Range: -134217728 ... 134217727.9375 - * - Granularity: 0.0625 + * @brief Signed fixed point number: Q27.4 + * @details A signed fixed point number format based on the 32-bit signed + * integer type with 27 integer and 4 fractional bits. + * - Range: -134217728 ... 134217727.9375 + * - Granularity: 0.0625 *****************************************************************************/ typedef int32_t q27_4_t; @@ -475,15 +482,35 @@ typedef int32_t q27_4_t; +/******************************************************************************* + ***** Q16.15 + ******************************************************************************/ +/*!*************************************************************************** + * @brief Signed fixed point number: Q16.15 + * @details A signed fixed point number format based on the 32-bit integer + * type with 16 integer and 15 fractional bits. + * - Range: -65536 .. 65536.999969482 + * - Granularity: 0.000030518 + *****************************************************************************/ +typedef int32_t q16_15_t; + +/*! Minimum value of Q16.15 number format. */ +#define Q16_15_MIN ((q16_15_t)INT32_MIN) + +/*! Maximum value of Q16.15 number format. */ +#define Q16_15_MAX ((q16_15_t)INT32_MAX) + + + /******************************************************************************* ***** Q15.16 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q15.16 - * @details A signed fixed point number format based on the 32-bit integer - * type with 15 integer and 16 fractional bits. - * - Range: -32768 .. 32767.99998 - * - Granularity: 1.52588E-05 + * @brief Signed fixed point number: Q15.16 + * @details A signed fixed point number format based on the 32-bit integer + * type with 15 integer and 16 fractional bits. + * - Range: -32768 .. 32767.99998 + * - Granularity: 1.52588E-05 *****************************************************************************/ typedef int32_t q15_16_t; @@ -502,11 +529,11 @@ typedef int32_t q15_16_t; ***** Q9.22 ******************************************************************************/ /*!*************************************************************************** - * @brief Signed fixed point number: Q9.22 - * @details A signed fixed point number format based on the 32-bit integer - * type with 9 integer and 22 fractional bits. - * - Range: -512 ... 511.9999998 - * - Granularity: 2.38418579101562E-07 + * @brief Signed fixed point number: Q9.22 + * @details A signed fixed point number format based on the 32-bit integer + * type with 9 integer and 22 fractional bits. + * - Range: -512 ... 511.9999998 + * - Granularity: 2.38418579101562E-07 *****************************************************************************/ typedef int32_t q9_22_t; @@ -522,4 +549,7 @@ typedef int32_t q9_22_t; /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* FP_DEF_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h new file mode 100644 index 0000000000..60b75a164a --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_div.h @@ -0,0 +1,173 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_DIV_H +#define FP_DIV_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include "int_math.h" + +/*!*************************************************************************** + * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1). + *****************************************************************************/ +#ifndef USE_HW_DIV +#define USE_HW_DIV 0 +#endif + +/*!*************************************************************************** + * @brief 32-bit implementation of an Q15.16 division. + * + * @details Algorithm to evaluate a/b, where b is in Q15.16 format, on a 32-bit + * architecture with maximum precision. + * The result is correctly rounded and given as the input format. + * Division by 0 yields max. values determined by signa of numerator. + * Too high/low results are truncated to max/min values. + * + * Depending on the architecture, the division is implemented with a 64-bit + * division and shifting (Cortex-M3/4) or as a fast software algorithm + * (Cortex-M0/1) wich runs fast on processors without hardware division. + * + * @see https://code.google.com/archive/p/libfixmath + * + * @param a Numerator in any Qx.y format + * @param b Denominator in Q15.16 format + * @return Result = a/b in the same Qx.y format as the input parameter a. + *****************************************************************************/ +inline int32_t fp_div16(int32_t a, q15_16_t b) +{ + //assert(b); + if (b == 0) { return a < 0 ? INT32_MIN : INT32_MAX; } + +#if USE_HW_DIV + // Tested on Cortex-M4, it takes approx. 75% of the + // software algorithm below. + int64_t c = ((int64_t) a) << 30U; + + if ((uint32_t)(a ^ b) & 0x80000000U) { + c = (((-c) / b) + (1 << 13U)) >> 14U; + + if (c > 0x80000000U) { return INT32_MIN; } + + return -c; + + } else { + c = ((c / b) + (1 << 13U)) >> 14U; + + if (c > (int64_t)INT32_MAX) { return INT32_MAX; } + + return c; + } + +#else + // This uses the basic binary restoring division algorithm. + // It appears to be faster to do the whole division manually than + // trying to compose a 64-bit divide out of 32-bit divisions on + // platforms without hardware divide. + // Tested on Cortex-M0, it takes approx. 33% of the time of the + // 64-bit version above. + + uint32_t remainder = absval(a); + uint32_t divider = absval(b); + + uint32_t quotient = 0; + uint32_t bit = 0x10000U; + + /* The algorithm requires D >= R */ + while (divider < remainder) { + divider <<= 1U; + bit <<= 1U; + } + + if (!bit) { + if ((uint32_t)(a ^ b) & 0x80000000U) { // return truncated values + return INT32_MIN; + + } else { + return INT32_MAX; + } + } + + if (divider & 0x80000000U) { + // Perform one step manually to avoid overflows later. + // We know that divider's bottom bit is 0 here. + if (remainder >= divider) { + quotient |= bit; + remainder -= divider; + } + + divider >>= 1U; + bit >>= 1U; + } + + /* Main division loop */ + while (bit && remainder) { + if (remainder >= divider) { + quotient |= bit; + remainder -= divider; + } + + remainder <<= 1U; + bit >>= 1U; + } + + if (remainder >= divider) { + quotient++; + } + + uint32_t result = quotient; + + /* Figure out the sign of result */ + if ((uint32_t)(a ^ b) & 0x80000000U) { + result = -result; + } + + return (int32_t)result; +#endif +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h new file mode 100644 index 0000000000..f8c494a630 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_ema.h @@ -0,0 +1,204 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides averaging algorithms for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_EMA_H +#define FP_EMA_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +#include "utility/fp_rnd.h" +#include "utility/fp_mul.h" + +/*!*************************************************************************** + * @brief Circular exponentially weighted moving average using UQ1.15 format. + * + * @details Evaluates the moving average (exponentially weighted) for circular + * data in UQ1.15 format. + * Circular data is that MAX_VALUE + 1 == MIN_VALUE. For example the + * usual phase information. + * + * Problem: Due to circularity of phase values, i.e. 0+x and 2PI+x are + * the same, the usual EMA has issues with the wrap around effect. + * Especially for vectors with phase around 0 (or 2PI), two values + * like 0 + x and PI - y are averaged to something around PI instead + * of 0 which would be more correct. + * + * Solution: Assume that phase jumps of more than PI are not allowed + * or possible. If a deviation of the new value to the smoothed signal + * occurs, it is clear that this stems from the wrap around effect and + * can be caught and correctly handled by the smoothing algorithm. + * + * Caution: If a target comes immediately into the field of view, phase + * jumps of > PI are indeed possible and volitional. However, the + * averaging break there anyway since the smoothed signal approaches + * only with delay to the correct values. The error made here is, that + * the smoothed signal approaches from the opposite direction. However, + * is approaches even faster since it always takes the shortest + * direction. + * + * @param mean The previous mean value in UQ1.15 format. + * @param x The current value to be added to the average UQ1.15 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in UQ1.15 format. + *****************************************************************************/ +inline uq1_15_t fp_ema15c(uq1_15_t mean, uq1_15_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + // Heeds the wrap around effect by casting dx to int16: + const int16_t dx = (int16_t)(x - mean); + const int32_t diff = weight * dx; + return (uq1_15_t)fp_rnds((mean << 8U) + diff, 8U); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q11.4 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q11.4 format. + * + * @param mean The previous mean value in Q11.4 format. + * @param x The current value to be added to the average Q11.4 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q11.4 format. + *****************************************************************************/ +inline q11_4_t fp_ema4(q11_4_t mean, q11_4_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + const int32_t dx = x - mean; + const int32_t diff = weight * dx; + return (q11_4_t)fp_rnds((mean << 8U) + diff, 8U); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q7.8 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q7.8 format. + * + * @param mean The previous mean value in Q7.8 format. + * @param x The current value to be added to the average Q7.8 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q7.8 format. + *****************************************************************************/ +inline q7_8_t fp_ema8(q7_8_t mean, q7_8_t x, uq0_8_t weight) +{ + return (q7_8_t)fp_ema4(mean, x, weight); +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline uint32_t uint_ema32(uint32_t mean, uint32_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + if (x > mean) { + const uint32_t dx = x - mean; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean + diff; + + } else { + const uint32_t dx = mean - x; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean - diff; + } +} +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline int32_t int_ema32(int32_t mean, int32_t x, uq0_8_t weight) +{ + if (weight == 0) { return x; } + + if (x > mean) { + const uint32_t dx = x - mean; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean + diff; + + } else { + const uint32_t dx = mean - x; + const uint32_t diff = fp_mulu(weight, dx, 8U); + return mean - diff; + } +} + +/*!*************************************************************************** + * @brief Exponentially weighted moving average using the Q15.16 format. + * + * @details Evaluates the moving average (exponentially weighted) for data in + * Q15.16 format. + * + * @param mean The previous mean value in Q15.16 format. + * @param x The current value to be added to the average Q15.16 format. + * @param weight The EMA weight in UQ0.7 format. + * @return The new mean value in Q15.16 format. + *****************************************************************************/ +inline q15_16_t fp_ema16(q15_16_t mean, q15_16_t x, uq0_8_t weight) +{ + return (q15_16_t)int_ema32(mean, x, weight); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_EMA_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h new file mode 100644 index 0000000000..f845ca6e00 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_exp.h @@ -0,0 +1,69 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an exponential function for fixed point type. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_EXP_H +#define FP_EXP_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +/*!*************************************************************************** + * @brief Calculates the exponential of an fixed point number Q15.16 format. + * + * @details Calculates y = exp(x) in fixed point representation. + * + * Note that the result might not be 100 % accurate and might contain + * a small error! + * + * @see https://www.quinapalus.com/efunc.html + * + * @param x The input parameter in unsigned fixed point format Q15.16. + * @return Result y = exp(x) in the UQ16.16 format. + *****************************************************************************/ +uq16_16_t fp_exp16(q15_16_t x); + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h new file mode 100644 index 0000000000..6bc42b2ec7 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_log.h @@ -0,0 +1,69 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file provides an logarithm function for fixed point type. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_LOG_H +#define FP_LOG_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" + +/*!*************************************************************************** + * @brief Calculates the natural logarithm (base e) of an fixed point number. + * + * @details Calculates y = ln(x) = log_e(x) in fixed point representation. + * + * Note that the result might not be 100 % accurate and might contain + * a small error! + * + * @see https://www.quinapalus.com/efunc.html + * + * @param x The input parameter in unsigned fixed point format Q15.16. + * @return Result y = ln(x) in the UQ16.16 format. + *****************************************************************************/ +q15_16_t fp_log16(uq16_16_t x); + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_DIV_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h new file mode 100644 index 0000000000..78db582644 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_mul.h @@ -0,0 +1,235 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_MUL_H +#define FP_MUL_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include "utility/fp_rnd.h" + +/*!*************************************************************************** + * Set to use hardware division (Cortex-M3/4) over software division (Cortex-M0/1). + *****************************************************************************/ +#ifndef USE_64BIT_MUL +#define USE_64BIT_MUL 0 +#endif + +#if !USE_64BIT_MUL +/*!*************************************************************************** + * @brief Long multiplication of two unsigned 32-bit into an 64-bit value on + * 32-bit architecture. + * + * @details w (two words) gets the product of u and v (one word each). + * w[0] is the most significant word of the result, w[1] the least. + * (The words are in big-endian order). + * It is Knuth's Algorithm M from [Knu2] section 4.3.1. + * * + * @see http://www.hackersdelight.org/hdcodetxt/muldwu.c.txt + * + * @param w The result (u * v) value given as two unsigned 32-bit numbers: + * w[0] is the most significant word of the result, w[1] the least. + * (The words are in big-endian order). + * @param u Left hand side of the multiplication. + * @param v Right hand side of the multiplication. + *****************************************************************************/ +inline void muldwu(uint32_t w[], uint32_t u, uint32_t v) +{ + const uint32_t u0 = u >> 16U; + const uint32_t u1 = u & 0xFFFFU; + const uint32_t v0 = v >> 16U; + const uint32_t v1 = v & 0xFFFFU; + + uint32_t t = u1 * v1; + const uint32_t w3 = t & 0xFFFFU; + uint32_t k = t >> 16U; + + t = u0 * v1 + k; + const uint32_t w2 = t & 0xFFFFU; + const uint32_t w1 = t >> 16U; + + t = u1 * v0 + w2; + k = t >> 16U; + + w[0] = u0 * v0 + w1 + k; + w[1] = (t << 16U) + w3; +} +#endif + +/*!*************************************************************************** + * @brief 64-bit implementation of an unsigned multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * number of 32-bit width. The multiplication is done in 64-bit and + * the result is shifted down by the passed shift parameter in order + * to return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in UQx1.y1 format + * @param v The right parameter in UQx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in UQx.(y1+y2-shift) format. + *****************************************************************************/ +inline uint32_t fp_mulu(uint32_t u, uint32_t v, uint_fast8_t shift) +{ + assert(shift <= 32); +#if USE_64BIT_MUL + const uint64_t w = (uint64_t)u * (uint64_t)v; + return (w >> shift) + ((w >> (shift - 1)) & 1U); +#else + uint32_t tmp[2] = { 0 }; + muldwu(tmp, u, v); + + assert(shift ? tmp[0] <= (UINT32_MAX >> (32 - shift)) : tmp[0] == 0); + + if (32 - shift) { + return ((tmp[0] << (32 - shift)) + fp_rndu(tmp[1], shift)); + + } else { + return tmp[1] > (UINT32_MAX >> 1) ? tmp[0] + 1 : tmp[0]; + } + +#endif +} + +/*!*************************************************************************** + * @brief 64-bit implementation of a signed multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * number of 32-bit width. The multiplication is done in 64-bit and + * the result is shifted down by the passed shift parameter in order + * to return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline int32_t fp_muls(int32_t u, int32_t v, uint_fast8_t shift) +{ + int_fast8_t sign = 1; + + uint32_t u2, v2; + + if (u < 0) { u2 = -u; sign = -sign; } else { u2 = u; } + + if (v < 0) { v2 = -v; sign = -sign; } else { v2 = v; } + + const uint32_t res = fp_mulu(u2, v2, shift); + + assert(sign > 0 ? res <= 0x7FFFFFFFU : res <= 0x80000000U); + + return sign > 0 ? res : -res; +} + + +/*!*************************************************************************** + * @brief 48-bit implementation of a unsigned multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * numbers with 32-bit unsigned and 16-bit unsigned format respectively. + * The multiplication is done in two 16x16-bit operations and the + * result is shifted down by the passed shift parameter in order to + * return a 32-bit value. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline uint32_t fp_mul_u32_u16(uint32_t u, uint16_t v, uint_fast8_t shift) +{ + assert(shift <= 48); + + if (shift > 16) { + uint32_t msk = 0xFFFFU; + uint32_t a = (u >> 16U) * v; + uint32_t b = (msk & u) * v; + return fp_rndu(a, shift - 16) + fp_rndu(b, shift); + + } else { + uint32_t msk = ~(0xFFFFFFFFU << shift); + uint32_t a = (u >> shift) * v; + uint32_t b = fp_rndu((msk & u) * v, shift); + return a + b; + } +} + +/*!*************************************************************************** + * @brief 48-bit implementation of an unsigned/signed multiplication with fixed point format. + * + * @details Algorithm to evaluate a*b, where a and b are arbitrary fixed point + * numbers with 32-bit signed and 16-bit unsigned format respectively. + * The multiplication is done in two 16x16-bit operations and the + * result is shifted down by the passed shift parameter in order to + * return a 32-bit value. + * The shift is executed with correct rounding. + * + * Note that the result must fit into the 32-bit value. An assertion + * error occurs otherwise (or undefined behavior of no assert available). + * + * @param u The left parameter in Qx1.y1 format + * @param v The right parameter in Qx2.y2 format + * @param shift The final right shift (rounding) value. + * @return Result = (a*b)>>shift in Qx.(y1+y2-shift) format. + *****************************************************************************/ +inline int32_t fp_mul_s32_u16(int32_t u, uint16_t v, uint_fast8_t shift) +{ + return u >= 0 ? fp_mul_u32_u16(u, v, shift) : - fp_mul_u32_u16(-u, v, shift); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_MUL_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h new file mode 100644 index 0000000000..ad6f71e09c --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/fp_rnd.h @@ -0,0 +1,118 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides definitions and basic macros for fixed point data types. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef FP_RND_H +#define FP_RND_H +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_fp + * @{ + *****************************************************************************/ + +#include "fp_def.h" +#include + +/*!*************************************************************************** + * @brief Converting with rounding from UQx.n1 to UQx.n2. + * @details Equivalent to dividing by 2^n with correct rounding to unsigned + * integer values. + * @param Q The number in (U)Qx.n1 fixed point format to be rounded. + * @param n The number of bits to be rounded, + * e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4. + * @return The rounded value in (U)Qx.n2 format. + *****************************************************************************/ +inline uint32_t fp_rndu(uint32_t Q, uint_fast8_t n) +{ + if (n == 0) { return Q; } + + else if (n > 32U) { return 0; } + + // Shift by n>=32 yields undefined behavior! Thus, this extra first + // step is essential to prevent issues. + Q >>= n - 1; + return (Q >> 1) + (Q & 1U); +} + +/*!*************************************************************************** + * @brief Converting with rounding from Qx.n1 to Qx.n2. + * @details Equivalent to dividing by 2^n with correct rounding to integer + * values. + * @param Q The number in (U)Qx.n1 fixed point format to be rounded. + * @param n The number of bits to be rounded, + * e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4. + * @return The rounded value in (U)Qx.n2 format. + *****************************************************************************/ +inline int32_t fp_rnds(int32_t Q, uint_fast8_t n) +{ + return (Q < 0) ? -fp_rndu(-Q, n) : fp_rndu(Q, n); +} + +/*!*************************************************************************** + * @brief Converting with truncation from UQx.n1 to UQx.n2. + * @details Equivalent to dividing by 2^n with truncating (throw away) the + * fractional part, resulting in an unsigned integer/fixed-point value. + * @param Q The number in (U)Qx.n1 fixed point format to be truncated. + * @param n The number of bits to be truncated, + * e.g. UQ8.8 -> UQ12.4 => n = 8 - 4 = 4. + * @return The truncated value in (U)Qx.n2 format. + *****************************************************************************/ +inline uint32_t fp_truncu(uint32_t Q, uint_fast8_t n) +{ + return (n < 32U) ? (Q >> n) : 0; +} + +/*!*************************************************************************** + * @brief Converting with truncation from Qx.n1 to Qx.n2. + * @details Equivalent to dividing by 2^n with truncating (throw away) the + * fractional part, resulting in a signed integer/fixed-point value. + * @param Q The number in (U)Qx.n1 fixed point format to be truncated. + * @param n The number of bits to be truncated, + * e.g. Q7.8 -> Q11.4 => n = 8 - 4 = 4. + * @return The truncated value in (U)Qx.n2 format. + *****************************************************************************/ +inline int32_t fp_truncs(int32_t Q, uint_fast8_t n) +{ + return (Q < 0) ? -fp_truncu(-Q, n) : fp_truncu(Q, n); +} + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* FP_RND_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h new file mode 100644 index 0000000000..27de8cc688 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/int_math.h @@ -0,0 +1,281 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details Provides algorithms applied to integer values. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef INT_MATH +#define INT_MATH +#ifdef __cplusplus +extern "C" { +#endif + +/*!*************************************************************************** + * @addtogroup argus_misc + * @{ + *****************************************************************************/ + +#include +#include + +/*! Enables the integer square root function. */ +#ifndef INT_SQRT +#define INT_SQRT 0 +#endif + +/*!*************************************************************************** + * @brief Integer Base-2 Logarithm. + * + * @details Calculates the base-2 logarithm for unsigned integer values. The + * result is the integer equivalent of floor(log2(x)). + * + * @param x Input parameter. + * @return The floor of the base-2 logarithm. + *****************************************************************************/ +inline uint32_t log2i(uint32_t x) +{ + assert(x != 0); +#if 1 + return 31 - __builtin_clz(x); +#else +#define S(k) if (x >= (1 << k)) { i += k; x >>= k; } + int i = 0; S(16); S(8); S(4); S(2); S(1); return i; +#undef S +#endif +} + +/*!*************************************************************************** + * @brief Integer Base-2 Logarithm with rounded result. + * + * @details Calculates the base-2 logarithm for unsigned integer values and + * returns the rounded result. The result is the integer equivalent + * of round(log2(x)). + * + * It is finding the nearest power-of-two value s.t. |x - 2^n| becomes + * minimum for all n. + * + * @param x Input parameter. + * @return The rounded value of the base-2 logarithm. + *****************************************************************************/ +inline uint32_t log2_round(uint32_t x) +{ + assert(x != 0); +#if 0 + const uint32_t y = x; + const uint32_t i = 0; + + while (y >>= 1) { i++; } + +#else + const uint32_t i = log2i(x); +#endif + return (i + ((x >> (i - 1U)) == 3U)); +} + +/*!*************************************************************************** + * @brief Finding the nearest power-of-two value. + * + * @details Implemented s.t. |x - 2^n| becomes minimum for all n. + * Special case 0: returns 0; + * Maximum input: 3037000499; higher number result in overflow! (returns 0) + * + * @param x Input parameter. + * @return Nearest power-of-two number, i.e. 2^n. + *****************************************************************************/ +inline uint32_t binary_round(uint32_t x) +{ + assert(x != 0); + const uint32_t shift = log2_round(x); + return (shift > 31U) ? 0 : 1U << shift; +} + +/*!*************************************************************************** + * @brief Counting bits set in a 32-bit unsigned integer. + * + * @details @see http://graphics.stanford.edu/~seander/bithacks.html + * + * @param x Input parameter. + * @return Number of bits set in input value. + *****************************************************************************/ +inline uint32_t popcount(uint32_t x) +{ + // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel + x = x - ((x >> 1) & 0x55555555); + x = (x & 0x33333333) + ((x >> 2) & 0x33333333); + return (((x + (x >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24; +} + +/*!*************************************************************************** + * @brief Determining if an integer is a power of 2 + * + * @details @see http://graphics.stanford.edu/~seander/bithacks.html + * + * @param x Input parameter. + * @return True if integer is power of 2. + *****************************************************************************/ +inline uint32_t ispowoftwo(uint32_t x) +{ + return x && !(x & (x - 1)); +} + +/*!*************************************************************************** + * @brief Calculates the absolute value. + * + * @param x Input parameter. + * @return The absolute value of x. + *****************************************************************************/ +inline uint32_t absval(int32_t x) +{ + // Note: special case of INT32_MIN must be handled correctly: + return x < 0 ? ((~(uint32_t)(x)) + 1) : (uint32_t)x; + + /* alternative with equal performance:*/ +// int32_t y = x >> 31; +// return (x ^ y) - y; + /* wrong implementation: + * does not correctly return abs(INT32_MIN) on 32-bit platform */ +// return x < 0 ? (uint32_t)(-x) : (uint32_t)x; +} + +/*!*************************************************************************** + * @brief Calculates the floor division by a factor of 2: floor(x / 2^n). + * + * @param x Input parameter. + * @param n The shift value, maximum is 31. + * @return The floor division by 2^n result. + *****************************************************************************/ +inline uint32_t floor2(uint32_t x, uint_fast8_t n) +{ + assert(n < 32); + return x >> n; +} + +/*!*************************************************************************** + * @brief Calculates the ceildiv division by a factor of 2: ceildiv(x / 2^n). + * + * @param x Input parameter. + * @param n The shift value, maximum is 31. + * @return The ceildiv division by 2^n result. + *****************************************************************************/ +inline uint32_t ceiling2(uint32_t x, uint_fast8_t n) +{ + assert(n < 32); + return x ? (1 + ((x - 1) >> n)) : 0; +} + +/*!*************************************************************************** + * @brief Calculates the ceildiv division: ceildiv(x / y). + * + * @param x Numerator + * @param y Denominator + * @return The result of the ceildiv division ceildiv(x / y). + *****************************************************************************/ +inline uint32_t ceildiv(uint32_t x, uint32_t y) +{ + assert(y != 0); + return x ? (1 + ((x - 1) / y)) : 0; +} + +/*!*************************************************************************** + * @brief Calculates the maximum of two values. + * + * @param a Input parameter. + * @param b Input parameter. + * @return The maximum value of the input parameters. + *****************************************************************************/ +#define MAX(a, b) ((a) > (b) ? (a) : (b)) + +/*!*************************************************************************** + * @brief Calculates the minimum of two values. + * + * @param a Input parameter. + * @param b Input parameter. + * @return The minimum value of the input parameters. + *****************************************************************************/ +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +/*!*************************************************************************** + * @brief Clamps a value between a minimum and maximum boundary. + * + * @details Clamps the values such that the condition min <= x <= max is true. + * + * @note The condition \p min <= \p max must hold!!! + * + * @param x The input parameter to be clamped. + * @param min The minimum or lower boundary. + * @param max The maximum or upper boundary. + * @return The clamped value of the input parameter within [min,max]. + *****************************************************************************/ +#define CLAMP(x, min, max) (MIN(MAX((x), (min)), (max))) + +#if INT_SQRT +/*!*************************************************************************** + * @brief Calculates the integer square root of x. + * + * @details The integer square root is defined as: + * isqrt(x) = (int)sqrt(x) + * + * @see https://en.wikipedia.org/wiki/Integer_square_root + * @see https://github.com/chmike/fpsqrt/blob/master/fpsqrt.c + * + * @param x Input parameter. + * @return isqrt(x) + *****************************************************************************/ +inline uint32_t isqrt(uint32_t v) +{ + unsigned t, q, b, r; + r = v; // r = v - x² + b = 0x40000000; // a² + q = 0; // 2ax + + while (b > 0) { + t = q + b; // t = 2ax + a² + q >>= 1; // if a' = a/2, then q' = q/2 + + if (r >= t) { // if (v - x²) >= 2ax + a² + r -= t; // r' = (v - x²) - (2ax + a²) + q += b; // if x' = (x + a) then ax' = ax + a², thus q' = q' + b + } + + b >>= 2; // if a' = a/2, then b' = b / 4 + } + + return q; +} +#endif // INT_SQRT + +/*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* INT_MATH */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h new file mode 100644 index 0000000000..ae9a4648f5 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/status.h @@ -0,0 +1,49 @@ +/*************************************************************************//** + * @file + * @brief This file is part of the AFBR-S50 API. + * @details This file contains status codes for all platform specific + * functions. + * + * @copyright + * + * Copyright (c) 2023, Broadcom 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 of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *****************************************************************************/ + +#ifndef STATUS_H +#define STATUS_H +#ifdef __cplusplus +extern "C" { +#endif + +#include "api/argus_status.h" + +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* STATUS_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h index 9c5f930351..1c2fe480e8 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/Inc/utility/time.h @@ -1,11 +1,11 @@ /*************************************************************************//** * @file - * @brief This file is part of the AFBR-S50 API. - * @details This file provides utility functions for timing necessities. + * @brief This file is part of the AFBR-S50 API. + * @details This file provides utility functions for timing necessities. * * @copyright * - * Copyright (c) 2021, Broadcom Inc + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,255 +36,563 @@ #ifndef TIME_H #define TIME_H +#ifdef __cplusplus +extern "C" { +#endif /*!*************************************************************************** - * @defgroup time Time Utility - * @ingroup argusutil - * @brief Timer utilities for time measurement duties. - * @details This module provides time measurement utility functions like - * delay or time measurement methods, or time math functions. - * @addtogroup time + * @defgroup argus_time Time Utility + * @ingroup argus_util + * + * @brief Timer utilities for time measurement duties. + * + * @details This module provides time measurement utility functions like + * delay or time measurement methods, or time math functions. + * + * @addtogroup argus_time * @{ *****************************************************************************/ +#include "platform/argus_timer.h" +#include #include #include /*!*************************************************************************** - * @brief A data structure to represent current time. + * @brief A data structure to represent current time. * - * @details Value is obtained from the PIT time which must be configured as - * lifetime counter. + * @details Value is obtained from the PIT time which must be configured as + * lifetime counter. + * + * Range: [0.000000, 4294967296.999999] seconds *****************************************************************************/ -typedef struct { - /*! Seconds. */ +typedef struct ltc_t { + /*! Seconds; + * Range: [0, UINT32_MAX] seconds */ uint32_t sec; - /*! Microseconds. */ + /*! Microseconds; + * Range: [0, 999999] microseconds */ uint32_t usec; } ltc_t; /*!*************************************************************************** - * @brief Obtains the elapsed time since MCU startup. - * @param t_now returned current time + * @brief Converts #ltc_t to microseconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to microseconds. + * The value is truncated to UINT32_MAX value if the result would + * exceed UINT32_MAX microseconds. + * @param t Input #ltc_t structure. + * @return Time value in microseconds. *****************************************************************************/ -void Time_GetNow(ltc_t *t_now); +inline uint32_t Time_ToUSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294.967295 sec (UINT32_MAX/1000000) + return ((t->sec < 4294U) || (t->sec == 4294U && t->usec < 967295U)) ? + t->usec + t->sec * 1000000U : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed microseconds since MCU startup. - * @details Wrap around effect due to uint32_t result format!! - * @param - - * @return Elapsed microseconds since MCU startup as uint32_t. + * @brief Converts #ltc_t to milliseconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to milliseconds. + * The value is truncated to UINT32_MAX value if the result would + * exceed UINT32_MAX milliseconds. + * The returned value is correctly rounded to the nearest value. + * @param t Input #ltc_t structure. + * @return Time value in milliseconds. *****************************************************************************/ -uint32_t Time_GetNowUSec(void); +inline uint32_t Time_ToMSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294967.295499 sec (UINT32_MAX/1000) + return ((t->sec < 4294967U) || (t->sec == 4294967U && t->usec < 295500U)) ? + (t->usec + 500U) / 1000U + t->sec * 1000U : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed milliseconds since MCU startup. - * @details Wrap around effect due to uint32_t result format!! - * @param - - * @return Elapsed milliseconds since MCU startup as uint32_t. + * @brief Converts #ltc_t to seconds (uint32_t). + * @details The specified time value (type #ltc_t) is converted to seconds. + * The returned value is correctly rounded to the nearest value. + * @param t Input #ltc_t structure. + * @return Time value in seconds. *****************************************************************************/ -uint32_t Time_GetNowMSec(void); +inline uint32_t Time_ToSec(ltc_t const *t) +{ + assert(t != 0); + + // max. value to convert correctly is 4294967295.499999 sec (UINT32_MAX/1000) + return (t->sec < 4294967295U || t->usec < 500000U) ? + (t->usec + 500000U) / 1000000U + t->sec : UINT32_MAX; +} /*!*************************************************************************** - * @brief Obtains the elapsed seconds since MCU startup. - * @param - - * @return Elapsed seconds since MCU startup as uint32_t. + * @brief Converts microseconds (uint32_t) to #ltc_t. + * @details The specified time value in microseconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_usec Input time in microseconds. *****************************************************************************/ -uint32_t Time_GetNowSec(void); +inline void Time_FromUSec(ltc_t *t, uint32_t t_usec) +{ + assert(t != 0); + t->sec = t_usec / 1000000U; + t->usec = t_usec % 1000000U; +} /*!*************************************************************************** - * @brief Obtains the elapsed time since a given time point. - * @param t_elapsed Returns the elapsed time since t_start. - * @param t_start Start time point. + * @brief Converts milliseconds (uint32_t) to #ltc_t. + * @details The specified time value in milliseconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_msec Input time in milliseconds. *****************************************************************************/ -void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start); +inline void Time_FromMSec(ltc_t *t, uint32_t t_msec) +{ + assert(t != 0); + t->sec = t_msec / 1000U; + t->usec = (t_msec % 1000U) * 1000U; +} /*!*************************************************************************** - * @brief Obtains the elapsed microseconds since a given time point. - * @details Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @return Elapsed microseconds since t_start as uint32_t. + * @brief Converts seconds (uint32_t) to #ltc_t. + * @details The specified time value in seconds is converted to type #ltc_t. + * @param t Output #ltc_t structure. + * @param t_sec Input time in seconds. *****************************************************************************/ -uint32_t Time_GetElapsedUSec(ltc_t const *t_start); +inline void Time_FromSec(ltc_t *t, uint32_t t_sec) +{ + assert(t != 0); + t->usec = 0; + t->sec = t_sec; +} + /*!*************************************************************************** - * @brief Obtains the elapsed milliseconds since a given time point. - * @details Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @return Elapsed milliseconds since t_start as uint32_t. + * @brief Checks if /p t1 is greater or equal that /p t2. + * @details Handles overflow. + * @param t1 1st operand. + * @param t2 2nd operand. + * @return Returns (t1 >= t2); *****************************************************************************/ -uint32_t Time_GetElapsedMSec(ltc_t const *t_start); +inline bool Time_GreaterEqual(ltc_t const *t1, ltc_t const *t2) +{ + assert(t1 != 0); + assert(t2 != 0); + return (t1->sec == t2->sec) ? (t1->usec >= t2->usec) : (t1->sec > t2->sec); +} + /*!*************************************************************************** - * @brief Obtains the elapsed seconds since a given time point. - * @param t_start Start time point. - * @return Elapsed seconds since t_start as uint32_t. + * @brief Obtains the elapsed time since MCU startup. + * @param t_now returned current time *****************************************************************************/ -uint32_t Time_GetElapsedSec(ltc_t const *t_start); +inline void Time_GetNow(ltc_t *t_now) +{ + assert(t_now != 0); + Timer_GetCounterValue(&(t_now->sec), &(t_now->usec)); + assert(t_now->usec < 1000000U); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points. - * @details Result is defined as t_diff = t_end - t_start. - * Note: since no negative time differences are supported, t_end has - * to be later/larger than t_start. Otherwise, the result won't be - * a negative time span but given by max value. - * @param t_diff Returned time difference. - * @param t_start Start time point. - * @param t_end End time point. + * @brief Obtains the elapsed time since MCU startup. + * @return Returns the current time. *****************************************************************************/ -void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end); +inline ltc_t Time_Now(void) +{ + ltc_t t_now; + Time_GetNow(&t_now); + return t_now; +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * microseconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow such that to large - * values are limited by 0xFFFFFFFF µs. - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in microseconds. + * @brief Obtains the elapsed microseconds since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @return Elapsed microseconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowUSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToUSec(&t_now); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * milliseconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow. - * Wrap around effect due to uint32_t result format!! - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in milliseconds. + * @brief Obtains the elapsed milliseconds (rounded) since MCU startup. + * @details Wrap around effect due to uint32_t result format!! + * @return Elapsed milliseconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowMSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToMSec(&t_now); +} /*!*************************************************************************** - * @brief Obtains the time difference between two given time points in - * seconds. - * @details Result is defined as t_diff = t_end - t_start. - * Refers to Time_Diff() and handles overflow. - * @param t_start Start time point. - * @param t_end End time point. - * @return Time difference in seconds. + * @brief Obtains the elapsed seconds (rounded) since MCU startup. + * @return Elapsed seconds since MCU startup as uint32_t. *****************************************************************************/ -uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end); +inline uint32_t Time_GetNowSec(void) +{ + ltc_t t_now = Time_Now(); + return Time_ToSec(&t_now); +} + /*!*************************************************************************** - * @brief Time delay for a given time period. - * @param dt Delay time. + * @brief Obtains the time difference between two given time points. + * @details Result is defined as t_diff = t_end - t_start. + * Note: since no negative time differences are supported, t_end has + * to be later/larger than t_start. Otherwise, the result is undefined! + * @param t_diff Returned time difference. + * @param t_start Start time point. + * @param t_end End time point. *****************************************************************************/ -void Time_Delay(ltc_t const *dt); +inline void Time_Diff(ltc_t *t_diff, ltc_t const *t_start, ltc_t const *t_end) +{ + assert(t_diff != 0); + assert(t_start != 0); + assert(t_end != 0); + assert(t_diff != t_start); + assert(t_diff != t_end); + assert(Time_GreaterEqual(t_end, t_start)); + + if (t_start->usec <= t_end->usec) { // no carry over + t_diff->sec = t_end->sec - t_start->sec; + t_diff->usec = t_end->usec - t_start->usec; + + } else { // with carry over + t_diff->sec = t_end->sec - 1 - t_start->sec; + t_diff->usec = (1000000U - t_start->usec) + t_end->usec; + } +} /*!*************************************************************************** - * @brief Time delay for a given time period in microseconds. - * @param dt_usec Delay time in microseconds. + * @brief Obtains the time difference between two given time points in + * microseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow such that to large + * values are limited by 0xFFFFFFFF µs. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in microseconds. *****************************************************************************/ -void Time_DelayUSec(uint32_t dt_usec); +inline uint32_t Time_DiffUSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToUSec(&t_diff); +} /*!*************************************************************************** - * @brief Time delay for a given time period in milliseconds. - * @param dt_msec Delay time in milliseconds. + * @brief Obtains the time difference between two given time points in + * milliseconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in milliseconds. *****************************************************************************/ -void Time_DelayMSec(uint32_t dt_msec); +inline uint32_t Time_DiffMSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToMSec(&t_diff); +} /*!*************************************************************************** - * @brief Time delay for a given time period in seconds. - * @param dt_sec Delay time in seconds. + * @brief Obtains the time difference between two given time points in + * seconds. + * @details Result is defined as t_diff = t_end - t_start. + * Refers to Time_Diff() and handles overflow. + * @param t_start Start time point. + * @param t_end End time point. + * @return Time difference in seconds. *****************************************************************************/ -void Time_DelaySec(uint32_t dt_sec); +inline uint32_t Time_DiffSec(ltc_t const *t_start, ltc_t const *t_end) +{ + ltc_t t_diff; + Time_Diff(&t_diff, t_start, t_end); + return Time_ToSec(&t_diff); +} + /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout Timeout period. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed time since a given time point. + * @details Calculates the currently elapsed time since a specified start time + * (/p t_start). + * + * Note that /p t_start must be in the past! Otherwise, the behavior is + * undefined! + * + * @param t_elapsed Returns the elapsed time since /p t_start. + * @param t_start Start time point. *****************************************************************************/ -bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout); +inline void Time_GetElapsed(ltc_t *t_elapsed, ltc_t const *t_start) +{ + assert(t_elapsed != 0); + assert(t_start != 0); + assert(t_elapsed != t_start); + ltc_t t_now = Time_Now(); + Time_Diff(t_elapsed, t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_usec Timeout period in microseconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed microseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed microseconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec); +inline uint32_t Time_GetElapsedUSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffUSec(t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_msec Timeout period in milliseconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed milliseconds since a given time point. + * @details Wrap around effect due to uint32_t result format!! + * @param t_start Start time point. + * @return Elapsed milliseconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec); +inline uint32_t Time_GetElapsedMSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffMSec(t_start, &t_now); +} /*!*************************************************************************** - * @brief Checks if timeout is reached from a given starting time. - * @details Handles overflow. - * @param t_start Start time. - * @param t_timeout_sec Timeout period in seconds. - * @return Timeout elapsed? True/False (boolean value) + * @brief Obtains the elapsed seconds since a given time point. + * @param t_start Start time point. + * @return Elapsed seconds since t_start as uint32_t. *****************************************************************************/ -bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec); +inline uint32_t Time_GetElapsedSec(ltc_t const *t_start) +{ + assert(t_start != 0); + ltc_t t_now = Time_Now(); + return Time_DiffSec(t_start, &t_now); +} + /*!*************************************************************************** - * @brief Adds two ltc_t values. - * @details Result is defined as t = t1 + t2. Results are wrapped around at - * maximum values just like integers. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2 2nd operand. + * @brief Adds two #ltc_t values. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t, t1 and t2 may point to the same instance(s). + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2 2nd operand. *****************************************************************************/ -void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2); +inline void Time_Add(ltc_t *t, ltc_t const *t1, ltc_t const *t2) +{ + assert(t != 0); + assert(t1 != 0); + assert(t2 != 0); + + t->sec = t1->sec + t2->sec; + t->usec = t1->usec + t2->usec; + + if (t->usec > 999999U) { + t->sec += 1U; + t->usec -= 1000000U; + } +} /*!*************************************************************************** - * @brief Adds a given time in microseconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_usec 2nd operand in microseconds. + * @brief Adds a given time in microseconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_usec 2nd operand in microseconds. *****************************************************************************/ -void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec); +inline void Time_AddUSec(ltc_t *t, ltc_t const *t1, uint32_t t2_usec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromUSec(&t2, t2_usec); + Time_Add(t, t1, &t2); +} /*!*************************************************************************** - * @brief Adds a given time in milliseconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_msec 2nd operand in milliseconds. + * @brief Adds a given time in milliseconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_msec 2nd operand in milliseconds. *****************************************************************************/ -void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec); +inline void Time_AddMSec(ltc_t *t, ltc_t const *t1, uint32_t t2_msec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromMSec(&t2, t2_msec); + Time_Add(t, t1, &t2); +} /*!*************************************************************************** - * @brief Adds a given time in seconds to an ltc_t value. - * @param t Return value: t = t1 + t2. - * @param t1 1st operand. - * @param t2_sec 2nd operand in seconds. + * @brief Adds a given time in seconds to an #ltc_t value. + * @details Result is defined as t = t1 + t2. + * The results are wrapped around at maximum values just like integers. + * The references for t and t1 may point to the same instance. + * + * @param t Return value: t = t1 + t2. + * @param t1 1st operand. + * @param t2_sec 2nd operand in seconds. *****************************************************************************/ -void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec); +inline void Time_AddSec(ltc_t *t, ltc_t const *t1, uint32_t t2_sec) +{ + assert(t != 0); + assert(t1 != 0); + ltc_t t2; + Time_FromSec(&t2, t2_sec); + Time_Add(t, t1, &t2); +} + /*!*************************************************************************** - * @brief Converts ltc_t to microseconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in microseconds. + * @brief Checks if /p t is within the time interval /p t_start and /p t_end. + * @details The interval is from /p t_start to /p t_end. + * The function returns true if /p t >= /p t_start AND /p t < /p t_end. + * If /p t_end is before /p t_start, /p t_end is consider to be wrapped + * around and the condition inverts (i.e. the function returns true if + * /p < /p t_end OR /p t >= t_start. + * @param t_start The start of the time interval. + * @param t_end The end of the time interval. + * @param t The time to be checked if it is with the interval. + * @return True if t is within t_start and t_stop. *****************************************************************************/ -uint32_t Time_ToUSec(ltc_t const *t); +inline bool Time_CheckWithin(ltc_t const *t_start, ltc_t const *t_end, ltc_t const *t) +{ + if (Time_GreaterEqual(t_end, t_start)) { + return Time_GreaterEqual(t, t_start) && !Time_GreaterEqual(t, t_end); + + } else { + return Time_GreaterEqual(t, t_start) || !Time_GreaterEqual(t, t_end); + } +} + /*!*************************************************************************** - * @brief Converts ltc_t to milliseconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in milliseconds. + * @brief Checks if timeout is reached from a given starting time. + * @details Checks if a specified time (/p t_timeout) has elapsed since a + * specified start time (/p t_start). + * Handles overflow/wraparound of time values at the maximum value. + * @param t_start Start time. + * @param t_timeout Timeout period. + * @return Timeout elapsed? True/False (boolean value) *****************************************************************************/ -uint32_t Time_ToMSec(ltc_t const *t); +inline bool Time_CheckTimeout(ltc_t const *t_start, ltc_t const *t_timeout) +{ + assert(t_start != 0); + assert(t_timeout != 0); + + ltc_t t_end; + ltc_t t_now = Time_Now(); + Time_Add(&t_end, t_start, t_timeout); + return !Time_CheckWithin(t_start, &t_end, &t_now); +} /*!*************************************************************************** - * @brief Converts ltc_t to seconds (uint32_t). - * @param t Input ltc_t struct. - * @return Time value in seconds. + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_usec Timeout period in microseconds. + * @return Timeout elapsed? True/False (boolean value) *****************************************************************************/ -uint32_t Time_ToSec(ltc_t const *t); +inline bool Time_CheckTimeoutUSec(ltc_t const *t_start, uint32_t const t_timeout_usec) +{ + ltc_t t_timeout; + Time_FromUSec(&t_timeout, t_timeout_usec); + return Time_CheckTimeout(t_start, &t_timeout); +} + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_msec Timeout period in milliseconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +inline bool Time_CheckTimeoutMSec(ltc_t const *t_start, uint32_t const t_timeout_msec) +{ + ltc_t t_timeout; + Time_FromMSec(&t_timeout, t_timeout_msec); + return Time_CheckTimeout(t_start, &t_timeout); +} + +/*!*************************************************************************** + * @brief Checks if timeout is reached from a given starting time. + * @details Handles overflow. + * @param t_start Start time. + * @param t_timeout_sec Timeout period in seconds. + * @return Timeout elapsed? True/False (boolean value) + *****************************************************************************/ +inline bool Time_CheckTimeoutSec(ltc_t const *t_start, uint32_t const t_timeout_sec) +{ + ltc_t t_timeout; + Time_FromSec(&t_timeout, t_timeout_sec); + return Time_CheckTimeout(t_start, &t_timeout); +} + + +/*!*************************************************************************** + * @brief Time delay for a given time period. + * @param dt Delay time. + *****************************************************************************/ +inline void Time_Delay(ltc_t const *dt) +{ + assert(dt != 0); + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeout(&t_start, dt)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in microseconds. + * @param dt_usec Delay time in microseconds. + *****************************************************************************/ +inline void Time_DelayUSec(uint32_t dt_usec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutUSec(&t_start, dt_usec)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in milliseconds. + * @param dt_msec Delay time in milliseconds. + *****************************************************************************/ +inline void Time_DelayMSec(uint32_t dt_msec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutMSec(&t_start, dt_msec)); +} + +/*!*************************************************************************** + * @brief Time delay for a given time period in seconds. + * @param dt_sec Delay time in seconds. + *****************************************************************************/ +inline void Time_DelaySec(uint32_t dt_sec) +{ + ltc_t t_start = Time_Now(); + + while (!Time_CheckTimeoutSec(&t_start, dt_sec)); +} + /*! @} */ +#ifdef __cplusplus +} // extern "C" +#endif #endif /* TIME_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a index c646ab3116..e4e6dc0c52 100644 Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a index 905b2fa66d..23bcd6fbea 100644 Binary files a/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a and b/src/drivers/distance_sensor/broadcom/afbrs50/Lib/libafbrs50_m4_fpu_os.a differ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c index 5f8fd4b1f1..2e0e8dff16 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.c @@ -1,10 +1,10 @@ /*************************************************************************//** - * @file argus_hal_test.c - * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * @file + * @brief Tests for the AFBR-S50 API hardware abstraction layer. * * @copyright * - * Copyright (c) 2021, Broadcom, Inc. + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -34,6 +34,10 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ +/*!*************************************************************************** + * @addtogroup argus_test + * @{ + *****************************************************************************/ /******************************************************************************* * Include Files @@ -46,44 +50,44 @@ #include "platform/argus_nvm.h" #include "platform/argus_irq.h" -#include -#include -#include - /******************************************************************************* * Definitions ******************************************************************************/ -/*! An error log message via print(); */ +/*! An error log message via #print function. */ #define error_log(fmt, ...) print("ERROR: " fmt "\n", ##__VA_ARGS__) /******************************************************************************* * Prototypes ******************************************************************************/ +static status_t VerifyHALImplementation(s2pi_slave_t spi_slave); + static status_t TimerPlausibilityTest(void); static status_t TimerWraparoundTest(void); static status_t SpiConnectionTest(s2pi_slave_t slave); -static status_t SpiInterruptTest(s2pi_slave_t slave); +static status_t SpiMaxLengthTest(s2pi_slave_t slave); +//static status_t SpiInterruptTest(s2pi_slave_t slave); +static status_t GpioInterruptTest(s2pi_slave_t slave); static status_t GpioModeTest(s2pi_slave_t slave); static status_t TimerTest(s2pi_slave_t slave); static status_t PITTest(void); +static status_t SpiTransferFromInterruptTest(s2pi_slave_t slave); static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct); -static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size); +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, size_t size); static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim); -static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples); -static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms); +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples, s2pi_callback_t callback, void *callbackData); static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom); static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *RcoTrim); static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples); static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n); static void PIT_Callback(void *param); -static void DataReadyCallback(void *param); +static void GPIO_Callback(void *param); /// @cond EXTERN extern uint32_t EEPROM_ReadChipId(uint8_t const *eeprom); -extern argus_module_version_t EEPROM_ReadModule(uint8_t const *eeprom); +extern uint8_t EEPROM_ReadModule(uint8_t const *eeprom); extern status_t EEPROM_Read(s2pi_slave_t slave, uint8_t address, uint8_t *data); extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); /// @endcond @@ -98,99 +102,129 @@ extern uint8_t hamming_decode(uint8_t const *code, uint8_t *data); status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave) { - status_t status = STATUS_OK; + print("########################################################\n"); + print("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); + print("########################################################\n"); + print("- SPI Slave: %d \n\n", spi_slave); - PX4_INFO_RAW("########################################################\n"); - PX4_INFO_RAW("# Running HAL Verification Test - " HAL_TEST_VERSION "\n"); - PX4_INFO_RAW("########################################################\n\n"); + const status_t status = VerifyHALImplementation(spi_slave); - PX4_INFO_RAW("1 > Timer Plausibility Test\n"); - status = TimerPlausibilityTest(); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("1 > PASS\n\n"); - - PX4_INFO_RAW("2 > Timer Wraparound Test\n"); - status = TimerWraparoundTest(); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("2 > PASS\n\n"); - - PX4_INFO_RAW("3 > SPI Connection Test\n"); - status = SpiConnectionTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("3 > PASS\n\n"); - - PX4_INFO_RAW("4 > SPI Interrupt Test\n"); - status = SpiInterruptTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("4 > PASS\n\n"); - - PX4_INFO_RAW("5 > GPIO Mode Test\n"); - status = GpioModeTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("5 > PASS\n\n"); - - PX4_INFO_RAW("6 > Lifetime Counter Timer (LTC) Test\n"); - status = TimerTest(spi_slave); - - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("6 > PASS\n\n"); - - PX4_INFO_RAW("7 > Periodic Interrupt Timer (PIT) Test\n"); - status = PITTest(); - - if (status == ERROR_NOT_IMPLEMENTED) { - PX4_INFO_RAW("7 > SKIPPED (PIT is not implemented)\n\n"); - - } else { - if (status != STATUS_OK) { goto summary; } - - PX4_INFO_RAW("7 > PASS\n\n"); - } - - -summary: - PX4_INFO_RAW("########################################################\n"); + print("########################################################\n"); if (status != STATUS_OK) { - PX4_INFO_RAW("# FAIL: HAL Verification Test finished with error %d!\n", (int)status); + print("# FAIL: HAL Verification Test finished with error %d!\n", status); } else { - PX4_INFO_RAW("# PASS: HAL Verification Test finished successfully!\n"); + print("# PASS: HAL Verification Test finished successfully!\n"); } - PX4_INFO_RAW("########################################################\n\n"); + print("########################################################\n\n"); + return status; } /*!*************************************************************************** - * @brief Checks the validity of timer counter values. + * @brief Executes a series of tests in order to verify the HAL implementation. * - * @details This verifies that the counter values returned from the - * #Timer_GetCounterValue function are valid. This means, the low - * counter value \p lct is within 0 and 999999 µs. + * @details See #Argus_VerifyHALImplementation for details. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @param spi_slave The SPI hardware slave. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + *****************************************************************************/ +static status_t VerifyHALImplementation(s2pi_slave_t spi_slave) +{ + status_t status = STATUS_OK; + + print("1 > Timer Plausibility Test\n"); + status = TimerPlausibilityTest(); + + if (status != STATUS_OK) { return status; } + + print("1 > PASS\n\n"); + + print("2 > Timer Wraparound Test\n"); + status = TimerWraparoundTest(); + + if (status != STATUS_OK) { return status; } + + print("2 > PASS\n\n"); + + print("3 > SPI Connection Test\n"); + status = SpiConnectionTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("3 > PASS\n\n"); + + print("4 > SPI Maximum Data Length Test\n"); + status = SpiMaxLengthTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("4 > PASS\n\n"); + + print("5 > GPIO Interrupt Test\n"); + status = GpioInterruptTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("5 > PASS\n\n"); + + print("6 > GPIO Mode Test\n"); + status = GpioModeTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("6 > PASS\n\n"); + + print("7 > Lifetime Counter Timer (LTC) Test\n"); + status = TimerTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("7 > PASS\n\n"); + + print("8 > Periodic Interrupt Timer (PIT) Test\n"); + status = PITTest(); + + if (status == ERROR_NOT_IMPLEMENTED) { + print("8 > SKIPPED (PIT is not implemented)\n\n"); + + } else { + if (status != STATUS_OK) { return status; } + + print("8 > PASS\n\n"); + } + + print("9 > SPI Interrupt Test\n"); + status = SpiTransferFromInterruptTest(spi_slave); + + if (status != STATUS_OK) { return status; } + + print("9 > PASS\n\n"); + + return status; +} + +/*!*************************************************************************** + * @brief Checks the validity of timer counter values. + * + * @details This verifies that the counter values returned from the + * #Timer_GetCounterValue function are valid. This means, the low + * counter value \p lct is within 0 and 999999 µs. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) { if (lct > 999999) { - PX4_INFO_RAW("Timer plausibility check:\n" - "The parameter \"lct\" of Timer_GetCounterValue() must always " - "be within 0 and 999999.\n" - "Current Values: hct = %d, lct = %d", (uint)hct, (uint)lct); + error_log("Timer plausibility check:\n" + "The parameter \"lct\" of Timer_GetCounterValue() must always " + "be within 0 and 999999.\n" + "Current Values: hct = %d, lct = %d", hct, lct); return ERROR_FAIL; } @@ -198,24 +232,24 @@ static status_t CheckTimerCounterValues(uint32_t hct, uint32_t lct) } /*!*************************************************************************** - * @brief Plausibility Test for Timer HAL Implementation. + * @brief Plausibility Test for Timer HAL Implementation. * - * @details Rudimentary tests the lifetime counter (LTC) implementation. - * This verifies that the LTC is running by checking if the returned - * values of two consecutive calls to the #Timer_GetCounterValue - * function are ascending. An artificial delay using the NOP operation - * is induced such that the timer is not read to fast. + * @details Rudimentary tests the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. * * @warning If using an ultra-fast processor with a rather low timer granularity, - * the test may fail! In this case, it could help to increase the delay - * by increasing the for-loop exit criteria. + * the test may fail! In this case, it could help to increase the delay + * by increasing the for-loop exit criteria. * - * @warning This test does not test yet verify if the timing is correct at all! - * This it done in later test... + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t TimerPlausibilityTest(void) { @@ -230,7 +264,7 @@ static status_t TimerPlausibilityTest(void) /* Check max value is not exceeded for LCT timer (us) */ status_t status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Adding a delay. Depending on MCU speed, this takes any time. * However, the Timer should be able to solve this on any MCU. */ @@ -242,18 +276,18 @@ static status_t TimerPlausibilityTest(void) /* Check max value is not exceeded for LCT timer (us) */ status = CheckTimerCounterValues(hct1, lct1); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Either the hct value must have been increased or the lct value if the hct * value is still the same. */ if (!((hct1 > hct0) || ((hct1 == hct0) && (lct1 > lct0)))) { - PX4_INFO_RAW("Timer plausibility check: the elapsed time could not be " - "measured with the Timer_GetCounterValue() function; no time " - "has elapsed!\n" - "The delay was induced by the following code:\n" - "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n" - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + error_log("Timer plausibility check: the elapsed time could not be " + "measured with the Timer_GetCounterValue() function; no time " + "has elapsed!\n" + "The delay was induced by the following code:\n" + "for (volatile uint32_t i = 0; i < 100000; ++i) __asm(\"nop\");\n", + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + hct0, lct0, hct1, lct1); return ERROR_FAIL; } @@ -261,29 +295,29 @@ static status_t TimerPlausibilityTest(void) } /*!*************************************************************************** - * @brief Wraparound Test for the Timer HAL Implementation. + * @brief Wraparound Test for the Timer HAL Implementation. * * @details The LTC values must wrap from 999999 µs to 0 µs and increase the * seconds counter accordingly. This test verifies the correct wrapping * by consecutively calling the #Timer_GetCounterValue function until * at least 2 wraparound events have been occurred. * - * @note This test requires the timer to basically run and return ascending - * values. Also, if the timer is too slow, this may take very long! - * Usually, the test takes 2 seconds, since 2 wraparound events are - * verified. + * @note This test requires the timer to basically run and return ascending + * values. Also, if the timer is too slow, this may take very long! + * Usually, the test takes 2 seconds, since 2 wraparound events are + * verified. * - * @warning This test does not test yet verify if the timing is correct at all! - * This it done in later test... + * @warning This test does not test yet verify if the timing is correct at all! + * This it done in later test... * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL on failure (check the error log for more information). + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL on failure (check the error log for more information). *****************************************************************************/ static status_t TimerWraparoundTest(void) { /* Test parameter configuration: *****************************************/ - const int8_t n = 2; // The number of wraparounds to test. + const uint8_t n = 2; // The number of wraparounds to test. /*************************************************************************/ uint32_t hct0 = 0; @@ -297,14 +331,12 @@ static status_t TimerWraparoundTest(void) /* Check max value is not exceeded for LCT timer (us) */ status_t status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Set end after 2 seconds, i.e. 2 wrap around events. */ uint32_t hct2 = hct0 + n; uint32_t lct2 = lct0; - px4_usleep(20000); - /* Periodically read timer values. From previous tests we * already know the timer value is increasing. */ while (hct0 < hct2 || lct0 < lct2) { @@ -315,69 +347,92 @@ static status_t TimerWraparoundTest(void) /* Check max value is not exceeded for LCT timer (us) */ status = CheckTimerCounterValues(hct0, lct0); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } /* Testing if calls to Timer_GetCounterValue are equal or increasing. * Also testing if wraparound is correctly handled. * Assumption here is that two sequential calls to the get functions are - * only a few µs appart! I.e. if hct wraps, the new lct must be smaller + * only a few µs apart! I.e. if hct wraps, the new lct must be smaller * than previous one. */ if (!(((hct1 == hct0 + 1) && (lct1 < lct0)) || ((hct1 == hct0) && (lct1 >= lct0)))) { - PX4_INFO_RAW("Timer plausibility check: the wraparound of \"lct\" or " - "\"hct\" parameters of the Timer_GetCounterValue() " - "function was not handled correctly!\n" - "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", - (uint)hct0, (uint)lct0, (uint)hct1, (uint)lct1); + error_log("Timer plausibility check: the wraparound of \"lct\" or " + "\"hct\" parameters of the Timer_GetCounterValue() " + "function was not handled correctly!\n" + "Current Values: hct0 = %d, lct0 = %d, hct1 = %d, lct1 = %d", + hct0, lct0, hct1, lct1); return ERROR_FAIL; } hct0 = hct1; lct0 = lct1; - - px4_usleep(20000); } return STATUS_OK; } /*!*************************************************************************** - * @brief Helper function for transfer data to SPI in blocking mode. + * @brief SPI interrupt callback function for the SPI transfer interrupt test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * finishing the SPI transfer. The callback is used by the + * #SPITransferSync helper function to retrieve the status of the + * SPI transfer. + * + * @param status The S2PI module status passed to the callback. + * @param param The abstract interrupt callback parameter. + * + * @return Returns #STATUS_OK. + *****************************************************************************/ +static status_t SpiTransferInterruptCallback(status_t status, void *param) +{ + *((status_t *)param) = status; + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief Helper function for transfer data to SPI in blocking mode. * * @details Calls the #S2PI_TransferFrame function and waits until the transfer - * has been finished by checking the #S2PI_GetStatus return code to - * become #STATUS_IDLE (or #STATUS_OK). + * has been finished by checking the #S2PI_GetStatus return code to + * become #STATUS_IDLE (or #STATUS_OK). * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param data The data array to be transfered. - * @param size The size of the data array to be transfered. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param data The data array to be transferred. + * @param size The size of the data array to be transferred. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) +static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, size_t size) { /* Test parameter configuration: *****************************************/ - const uint32_t timeout_ms = 100; // The transfer timeout in ms. + const uint32_t timeout_ms = 100; // The transfer timeout in ms. /*************************************************************************/ - status_t status = S2PI_TransferFrame(slave, data, data, size, 0, 0); + /* The status will be changed in the SPI callback. */ + volatile status_t callbackStatus = STATUS_BUSY; - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI transfer failed! The call to S2PI_TransferFrame " - "yielded error code: %d", (int)status); + status_t status = S2PI_TransferFrame(slave, data, data, size, + SpiTransferInterruptCallback, + (void *)&callbackStatus); + + if (status != STATUS_OK) { + error_log("SPI transfer failed! The call to S2PI_TransferFrame " + "yielded error code: %d", + status); return status; } @@ -388,53 +443,67 @@ static status_t SPITransferSync(s2pi_slave_t slave, uint8_t *data, uint8_t size) Time_GetNow(&start); do { - status = S2PI_GetStatus(); + status = S2PI_GetStatus(slave); if (status < STATUS_OK) { - PX4_INFO_RAW("SPI transfer failed! The call to S2PI_GetStatus " - "yielded error code: %d", (int)status); - S2PI_Abort(); + error_log("SPI transfer failed! The call to S2PI_GetStatus " + "yielded error code: %d", status); + S2PI_Abort(slave); return status; } if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI transfer failed! The operation did not finished " - "within %u ms. This may also be caused by an invalid " - "timer implementation!", (uint)timeout_ms); + error_log("SPI transfer failed! The operation did not finished " + "within %d ms. This may also be caused by an invalid " + "timer implementation!", timeout_ms); return ERROR_TIMEOUT; } } while (status == STATUS_BUSY); + if (callbackStatus != STATUS_OK) { + error_log("Invocation of the SPI callback failed! The SPI transfer " + "callback yielded error code: %d", callbackStatus); + return callbackStatus; + } + return status; } /*!*************************************************************************** - * @brief SPI Connection Test for S2PI HAL Implementation. + * @brief SPI Connection Test for S2PI HAL Implementation. * * @details This test verifies the basic functionality of the SPI interface. - * The test utilizes the devices laser pattern register, which can - * be freely programmed by any 128-bit pattern. Thus, it writes a byte - * sequence and reads back the written values on the consecutive SPI - * access. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * Note: The test verifies the SPI interface transfer functionality + * in blocking mode and also verifies the interrupt callback. + * In order to wait for the transfer to finish, it reads the S2PI + * status in a loop. If the status does not change to #STATUS_IDLE, + * the test will fail with an #ERROR_TIMEOUT. Finally, the test will + * verify the SPI transfer callback status. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - #ERROR_FAIL if the device access failed and the read data did not - * match the expected values. - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or the SPI callback yield any negative status. *****************************************************************************/ static status_t SpiConnectionTest(s2pi_slave_t slave) { @@ -448,8 +517,8 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI connection test failed!"); + if (status != STATUS_OK) { + error_log("SPI connection test failed!"); return status; } @@ -460,18 +529,18 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) status = SPITransferSync(slave, data, 17U); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI connection test failed!"); + if (status != STATUS_OK) { + error_log("SPI connection test failed!"); return status; } /* Verify the read pattern. */ for (uint8_t i = 1; i < 17U; ++i) { if (data[i] != i) { - PX4_INFO_RAW("SPI connection test failed!\n" - "Verification of read data is invalid!\n" - "read_data[%d] = %d, but expected was %d", - i, data[i], i); + error_log("SPI connection test failed!\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data[i], i); return ERROR_FAIL; } } @@ -479,142 +548,236 @@ static status_t SpiConnectionTest(s2pi_slave_t slave) return STATUS_OK; } - /*!*************************************************************************** - * @brief The data ready callback invoked by the API. + * @brief Maximum SPI Data Size Test for S2PI HAL Implementation. * - * @details The callback is invoked by the API when the device GPIO IRQ is - * pending after a measurement has been executed and data is ready to - * be read from the device. + * @details This test verifies the maximum data transfer length of the SPI + * interface. The test sends and receives up to 396 data bytes plus + * a single address byte over the SPI interface and verifies that no + * data get lost. * - * @param param The abstract pointer to the boolean value that determines if - * the callback is invoked. + * The test utilizes the channel select register which is 3 bytes plus + * address. This register can be repeatedly written with any pattern + * using the DMA mode. The register is written 100 times in a row + * to verify that long data frames with up to 400 bytes can be + * transmitted. + * + * Note that this test was motivated by an invalid implementation that + * used uint8_t type for the frame length in the #S2PI_TransferFrame + * function instead of an uint16_t value. This resulted in a maximum + * data length of 141 bytes (367 & 0xFF = 141) when reading the + * data value register. + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static void DataReadyCallback(void *param) +static status_t SpiMaxLengthTest(s2pi_slave_t slave) { - IRQ_LOCK(); - *((bool *) param) = true; - IRQ_UNLOCK(); + status_t status = STATUS_OK; + uint8_t data[400U] = { 0 }; + + /* Setup device (enable DMA mode). */ + data[0] = 0x10; data[1] = 0x12; + status = SPITransferSync(slave, data, 2); + + if (status != STATUS_OK) { + error_log("Device configuration failed!"); + return status; + } + + data[0] = 0x12; data[1] = 0x00; data[2] = 0x2B; + status = SPITransferSync(slave, data, 3); + + if (status != STATUS_OK) { + error_log("Device configuration failed!"); + return status; + } + + /* Transfer a pattern to the register */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + data[i + 0] = 0x1E; // Address + data[i + 1] = (uint8_t)i; // Random Data Byte 0 + data[i + 2] = (uint8_t)(i + 1); // Random Data Byte 1 + data[i + 3] = (uint8_t)(i * 2); // Random Data Byte 2 + } + + status = SPITransferSync(slave, data, sizeof(data)); + + if (status != STATUS_OK) { + error_log("SPI maximum data length test failed!"); + return status; + } + + /* Repeat ... */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + data[i + 0] = 0x1E; // Address + data[i + 1] = (uint8_t)i; // Random Data Byte 0 + data[i + 2] = (uint8_t)(i + 1); // Random Data Byte 1 + data[i + 3] = (uint8_t)(i * 2); // Random Data Byte 2 + } + + status = SPITransferSync(slave, data, sizeof(data)); + + if (status != STATUS_OK) { + error_log("SPI maximum data length test failed!"); + return status; + } + + /* Verify the read pattern; skip all address bytes. */ + for (uint32_t i = 0; i < sizeof(data); i += 4) { + uint32_t j = (i + 4) % sizeof(data); + + if (data[j + 1] != (uint8_t)i + || data[j + 2] != (uint8_t)(i + 1) + || data[j + 3] != (uint8_t)(i * 2)) { + error_log("SPI maximum data length test failed!\n" + "Verification of read data is invalid at byte %d!\n" + " - expected: 0x%02X%02X%02X\n" + " - actual: 0x%02X%02X%02X", + i, (uint8_t)i, (uint8_t)(i + 1), (uint8_t)(i * 2), + data[j + 1], data[j + 2], data[j + 3]); + return ERROR_FAIL; + } + } + + return STATUS_OK; } /*!*************************************************************************** - * @brief Configures the device with a bare minimum setup to run the tests. + * @brief Configures the device with a bare minimum setup to run the tests. * - * @details This function applies a number of configuration values to the - * device, such that a pseudo measurement w/o laser output can be - * performed. + * @details This function applies a number of configuration values to the + * device, such that a pseudo measurement w/o laser output can be + * performed. * - * A \p rcoTrim parameter can be passed to adjust the actual clock - * setup. + * A \p rcoTrim parameter can be passed to adjust the actual clock + * setup. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param rcoTrim The RCO Trimming value added to the nominal RCO register - * value. Pass 0 if no fine tuning is required. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcoTrim The RCO Trimming value added to the nominal RCO register + * value. Pass 0 if no fine tuning is required. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the SPI operation did not finished within a - * specified time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the SPI operation did not finished within a + * specified time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) { /* Setup Device and Trigger Measurement. */ - uint16_t v = 0x0010U | (((34 + rcoTrim) & 0x3F) << 6); - uint8_t d1[] = { 0x14, v >> 8, v & 0xFF, 0x21 }; + assert(rcoTrim >= -34 && rcoTrim < 0x3F - 34); + const uint16_t v = (uint16_t)(0x0010U | (((uint16_t)(34 + rcoTrim) & 0x3F) << 6U)); + uint8_t d1[] = { 0x14U, (uint8_t)(v >> 8U), v & 0xFFU, 0x21U }; status_t status = SPITransferSync(slave, d1, sizeof(d1)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d2[] = { 0x16, 0x7F, 0xFF, 0x7F, 0xE9 }; status = SPITransferSync(slave, d2, sizeof(d2)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d3[] = { 0x18, 0x00, 0x00, 0x03 }; status = SPITransferSync(slave, d3, sizeof(d3)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d4[] = { 0x10, 0x12 }; status = SPITransferSync(slave, d4, sizeof(d4)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d5[] = { 0x12, 0x00, 0x2B }; status = SPITransferSync(slave, d5, sizeof(d5)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d6[] = { 0x08, 0x04, 0x84, 0x10 }; status = SPITransferSync(slave, d6, sizeof(d6)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d7[] = { 0x0A, 0xFE, 0x51, 0x0F, 0x05 }; status = SPITransferSync(slave, d7, sizeof(d7)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d8[] = { 0x0C, 0x00, 0x00, 0x00 }; status = SPITransferSync(slave, d8, sizeof(d8)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d9[] = { 0x1E, 0x00, 0x00, 0x00 }; status = SPITransferSync(slave, d9, sizeof(d9)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d10[] = { 0x20, 0x01, 0xFF, 0xFF }; status = SPITransferSync(slave, d10, sizeof(d10)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } uint8_t d11[] = { 0x22, 0xFF, 0xFF, 0x04 }; status = SPITransferSync(slave, d11, sizeof(d11)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Device configuration failed!"); + if (status != STATUS_OK) { + error_log("Device configuration failed!"); return status; } @@ -622,40 +785,55 @@ static status_t ConfigureDevice(s2pi_slave_t slave, int8_t rcoTrim) } /*!*************************************************************************** - * @brief Triggers a measurement on the device with specified sample count. + * @brief Triggers a measurement on the device with specified sample count. * * @details The function triggers a measurement cycle on the device. A - * \p sample count can be specified to setup individual number of - * digital averaging. + * \p sample count can be specified to setup individual number of + * digital averaging. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * The measurement in triggered asynchronously without waiting + * for any event to finish. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param samples The specified number of averaging samples for the measurement. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if the operation did not finished within a specified - * time (check also timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus - * return any negative status. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. + * @param callback An optional SPI callback. + * @param callbackData The optional callback data parameter. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. *****************************************************************************/ -static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) +static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples, + s2pi_callback_t callback, void *callbackData) { // samples is zero based, i.e. writing 0 yields 1 sample samples = samples > 0 ? samples - 1 : samples; - uint16_t v = 0x8000U | ((samples & 0x03FFU) << 5U); - uint8_t d[] = { 0x1C, v >> 8, v & 0xFFU }; - status_t status = SPITransferSync(slave, d, sizeof(d)); + const uint16_t v = (uint16_t)(0x8000U | ((samples & 0x03FFU) << 5U)); - if (status < STATUS_OK) { - PX4_INFO_RAW("Trigger measurement failed!"); + // data is static as the transfer is asynchronous and the buffer must persist. + static uint8_t data[] = { 0x1CU, 0x00U, 0x00U }; + data[0] = 0x1CU; + data[1] = (uint8_t)(v >> 8U); + data[2] = v & 0xFFU; + + status_t status = S2PI_TransferFrame(slave, data, data, sizeof(data), + callback, callbackData); + + if (status != STATUS_OK) { + error_log("SPI transfer failed to trigger measurements! " + "The call to S2PI_TransferFrame yielded error code: %d", + status); return status; } @@ -663,162 +841,263 @@ static status_t TriggerMeasurement(s2pi_slave_t slave, uint16_t samples) } /*!*************************************************************************** - * @brief Waits for the data ready interrupt to be pending. + * @brief Data structure for the GPIO interrupt test. * - * @details The function polls the current interrupt pending state of the data - * ready interrupt from the device, i.e. reads the IRQ GPIO pin until - * it is pulled to low by the device. - * - * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. - * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param timeout_ms The timeout to cancel waiting for the IRQ. - * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @details Contains data that is required by the GPIO interrupt test. *****************************************************************************/ -static status_t AwaitDataReady(s2pi_slave_t slave, uint32_t timeout_ms) -{ - ltc_t start; - Time_GetNow(&start); +typedef struct gpio_data_t { + /* The S2PI slave parameter passed to the S2PI HAL functions. */ + s2pi_slave_t Slave; - while (S2PI_ReadIrqPin(slave)) { - if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did not " - "determine an pending interrupt within %u ms.", (uint)timeout_ms); - return ERROR_TIMEOUT; - } + /* The callback status. */ + volatile status_t Status; + + /* The GPIO timeout in milliseconds. */ + uint32_t Timeout_ms; + + /* A counter to determine how often the callback is invoked. */ + volatile uint32_t CallbackInvoked; + + /* The return value of the #S2PI_ReadIrqPin function. */ + volatile uint32_t ReadIrqPinValue; + +} gpio_data_t; + +/*!*************************************************************************** + * @brief The IRQ callback dedicated to the #GpioInterruptTest. + * + * @details The callback is invoked by the API when the device GPIO IRQ is + * pending after a measurement has been executed and data is ready to + * be read from the device. + * + * @param param The abstract pointer to the boolean value that determines if + * the callback is invoked. + *****************************************************************************/ +static void GPIO_Callback(void *param) +{ + if (param == NULL) { + error_log("GPIO interrupt test failed: callback parameter \"param\" was NULL!"); + return; } + gpio_data_t *data = (gpio_data_t *)param; + data->CallbackInvoked = 1; +} + +/*!*************************************************************************** + * @brief The SPI transfer callback dedicated to the #GpioInterruptTest. + * + * @details The callback is invoked by the S2PI layer when the SPI transfer + * finished IRQ is invoked. The callback is used to simulate a + * deferred GPIO interrupt by locking the interrupts until the + * #S2PI_ReadIrqPin detects an GPIO interrupt pending state and + * returns 0. + * + * @param status The status of the SPI transfer. + * @param param The abstract pointer to the boolean value that determines if + * the callback is invoked. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the \p param parameter is NULL. + * - #ERROR_TIMEOUT if the #S2PI_ReadIrqPin does not return 0 after + * a specified time (check also timer HAL implementation). + * - The S2PI layer error code that may be received from the S2PI + * module via the \p status parameter. + *****************************************************************************/ +static status_t GPIO_SPI_Callback(status_t status, void *param) +{ + IRQ_LOCK(); // prevents GPIO interrupt to preempt if set to higher priority. + + if (param == NULL) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed: callback parameter \"param\" was NULL!"); + return ERROR_FAIL; + } + + gpio_data_t *data = (gpio_data_t *)param; + + if (status != STATUS_OK) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed: callback parameter \"status\" was %d!", + status); + data->Status = status; + return status; + } + + /* The S2PI_ReadIrqPin must correctly return the GPIO IRQ state if the GPIO + * interrupt is pending but deferred due to any higher priority or critical + * sections. Therefore, the SPI callback with the #IRQ_LOCK/#IRQ_UNLOCK is + * used to delay the GPIO callback and test the #S2PI_ReadIrqPin function. + * + * The purpose is to simulate a delayed GPIO interrupt that can in the + * production code happen due to any higher priority interrupts (such as + * the SPI interrupt in this test). In those cases, the API relies on the + * #S2PI_ReadIrqPin method to obtain if the device has finished in time and + * the interrupt is already pending. Otherwise, it would fail with an + * timeout due to the deferred GPIO interrupt callback event. */ + + ltc_t start; + Time_GetNow(&start); + data->ReadIrqPinValue = S2PI_ReadIrqPin(data->Slave); + + while (data->ReadIrqPinValue) { + if (Time_CheckTimeoutMSec(&start, data->Timeout_ms)) { + IRQ_UNLOCK(); + error_log("GPIO interrupt test failed! The IRQ pin did not assert " + "to low state when reading from the IRQ callback. " + "Elapsed %d ms.", data->Timeout_ms); + data->Status = ERROR_TIMEOUT; + return ERROR_TIMEOUT; + } + + data->ReadIrqPinValue = S2PI_ReadIrqPin(data->Slave); + } + + IRQ_UNLOCK(); + data->Status = STATUS_OK; return STATUS_OK; } /*!*************************************************************************** - * @brief SPI Interrupt Test for S2PI HAL Implementation. + * @brief SPI Interrupt Test for S2PI HAL Implementation. * - * @details This test verifies the correct implementation of the device - * integration finished interrupt callback. Therefore it configures - * the device with a minimal setup to run a pseudo measurement that - * does not emit any laser light. + * @details This test verifies the correct implementation of the device + * integration finished interrupt callback, a.k.a. the GPIO interrupt. + * Therefore it configures the device with a minimal setup to run a + * pseudo measurement that does not emit any laser light but triggers + * an GPIO interrupt once finished. * - * Note that this test does verify the GPIO interrupt that occurs - * whenever the device has finished the integration/measurement and - * new data is waiting to be read from the device. This does not test - * the interrupt that is triggered when the SPI transfer has finished. + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs and + * the #S2PI_ReadIrqPin function to obtain the pending interrupt state. * - * The data ready interrupt implies two S2PI layer functions that - * are tested in this test: The #S2PI_SetIrqCallback function installs - * a callback function that is invoked whenever the IRQ occurs. - * The IRQ can be delayed due to higher priority task, e.g. from the - * user code. It is essential for the laser safety timeout algorithm - * to determine the device ready signal as fast as possible, another - * method is implemented to read if the IRQ is pending but the - * callback has not been reset yet. This is what the #S2PI_ReadIrqPin - * function is for. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible. Thus a + * method is required to obtain if the IRQ is currently pending but + * the callback has not been invoked yet. This is what the + * #S2PI_ReadIrqPin function is for. Note that the #S2PI_ReadIrqPin + * must return 0 if not interrupt is pending and 1 else. Just like + * the IRQ pin is active low. * + * The test simulate a delayed GPIO interrupt by locking the interrupts + * until the #S2PI_ReadIrqPin detects an GPIO interrupt pending state + * and returns 0. This is done by the #GPIO_SPI_Callback function. + * + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. * * @warning The test assumes the device is in a fresh power on state and no - * additional reset is required. If the test fail, one may want to - * power cycle the device and try again. + * additional reset is required. If the test fail, one may want to + * power cycle the device and try again. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test locks the interrupts for a quite long period of time in + * order to simulate a delayed GPIO interrupt. This is not a good + * practice in production code. However, it is required to test the + * #S2PI_ReadIrqPin function. Please be aware of that when you run + * this test. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - #ERROR_FAIL if the IRQ pin readout failed and the no or invalid - * interrupt was detected. - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - #ERROR_FAIL if the IRQ pin readout failed and the no or invalid + * interrupt was detected. + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. *****************************************************************************/ -static status_t SpiInterruptTest(s2pi_slave_t slave) +static status_t GpioInterruptTest(s2pi_slave_t slave) { /* Test parameter configuration: *****************************************/ const uint32_t timeout_ms = 300; // timeout for measurement, might be increased.. /*************************************************************************/ - /* Install IRQ callback. */ - volatile bool isDataReady = false; - status_t status = S2PI_SetIrqCallback(slave, DataReadyCallback, (void *)&isDataReady); + gpio_data_t data = { .Slave = slave, + .Status = ERROR_FAIL, + .Timeout_ms = timeout_ms, + .ReadIrqPinValue = 12345, + .CallbackInvoked = 0 + }; - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "yielded error code: %d", (int)status); + /* Install IRQ callback. */ + status_t status = S2PI_SetIrqCallback(slave, GPIO_Callback, &data); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed! The call to S2PI_SetIrqCallback " + "yielded error code: %d", status); + return status; + } + + /* Setup Device. */ + status = ConfigureDevice(slave, 0); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); return status; } /* Check if IRQ is not yet pending. */ if (S2PI_ReadIrqPin(slave) == 0) { - PX4_INFO_RAW("SPI interrupt test failed! The S2PI_ReadIrqPin did " - "return 0 but no interrupt is pending since no " - "measurements are executed yet!"); + error_log("GPIO interrupt test failed! The S2PI_ReadIrqPin did " + "return 0 but no interrupt is pending since no " + "measurements are executed yet!"); return ERROR_FAIL; }; - /* Setup Device. */ - status = ConfigureDevice(slave, 0); - - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); - return status; - } - /* Trigger Measurement. */ - status = TriggerMeasurement(slave, 0); + status = TriggerMeasurement(slave, 0, GPIO_SPI_Callback, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); - return status; - } - - ltc_t start; - Time_GetNow(&start); - - /* Wait for Interrupt using the S2PI_ReadIrqPin method. */ - status = AwaitDataReady(slave, timeout_ms); - - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed!"); + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); return status; } /* Wait for Interrupt using the callback method. */ - while (!isDataReady) { + ltc_t start; + Time_GetNow(&start); + + while (!data.CallbackInvoked) { if (Time_CheckTimeoutMSec(&start, timeout_ms)) { - PX4_INFO_RAW("SPI interrupt test failed! The IRQ callback was not " - "invoked within %u ms.", (uint)timeout_ms); + error_log("GPIO interrupt test failed! The IRQ callback was not " + "invoked within %d ms.", timeout_ms); return ERROR_TIMEOUT; } } + /* Verify ... */ + if (data.Status != STATUS_OK) { + error_log("GPIO interrupt test failed! The SPI IRQ callback yielded " + "an error status: %d (expected 0)", data.Status); + return ERROR_FAIL; + } + + if (data.ReadIrqPinValue != 0) { + error_log("GPIO interrupt test failed! The IRQ pin returned " + "the wrong value: %d (expected 0)", data.ReadIrqPinValue); + return ERROR_FAIL; + } + /* Remove callback. */ status = S2PI_SetIrqCallback(slave, 0, 0); - if (status < STATUS_OK) { - PX4_INFO_RAW("SPI interrupt test failed! The call to S2PI_SetIrqCallback " - "with null pointers yielded error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed! The call to S2PI_SetIrqCallback " + "with null pointers yielded error code: %d", status); return status; } @@ -826,28 +1105,28 @@ static status_t SpiInterruptTest(s2pi_slave_t slave) } /*!*************************************************************************** - * @brief Reads the EEPROM bytewise and applies Hamming weight. - * @details The EEPROM bytes are consecutevly read from the device via GPIO mode. - * The #EEPROM_Read function is an internal API function that enables - * the GPIO mode from the S2PI module and reads the data via a software - * bit-banging protocol. Finally it disables the GPIO mode and returns - * to SPI mode. + * @brief Reads the EEPROM byte-wise and applies Hamming weight. + * @details The EEPROM bytes are consecutively read from the device via GPIO mode. + * The EEPROM_Read function is an internal API function that enables + * the GPIO mode from the S2PI module and reads the data via a software + * bit-banging protocol. Finally it disables the GPIO mode and returns + * to SPI mode. * - * The calls to S2PI HAL module is as follows: - * 1. S2PI_CaptureGpioControl - * 2. multiple calls to S2PI_WriteGpioPin and S2PI_ReadGpioPin - * 3. S2PI_ReleaseGpioControl + * The calls to S2PI HAL module is as follows: + * 1. S2PI_CaptureGpioControl + * 2. multiple calls to S2PI_WriteGpioPin and S2PI_ReadGpioPin + * 3. S2PI_ReleaseGpioControl * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param eeprom The 16 byte array to be filled with EEPROM data. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param eeprom The 16 byte array to be filled with EEPROM data. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * interrupt was detected. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * interrupt was detected. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) { @@ -855,9 +1134,9 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t d1[] = { 0x12, 0x00, 0x4B }; status_t status = SPITransferSync(slave, d1, sizeof(d1)); - if (status < STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " - "error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("EEPROM readout failed (enable EEPROM), " + "error code: %d", status); return status; } @@ -868,8 +1147,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) status = EEPROM_Read(slave, address, &data[address]); if (status != STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed @ address 0x%02x, " - "error code: %d!", address, (int)status); + error_log("EEPROM readout failed @ address 0x%02x, " + "error code: %d!", address, status); return status; } } @@ -878,9 +1157,9 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t d2[] = { 0x12, 0x00, 0x2B }; status = SPITransferSync(slave, d2, sizeof(d2)); - if (status < STATUS_OK) { - PX4_INFO_RAW("EEPROM readout failed (enable EEPROM), " - "error code: %d", (int)status); + if (status != STATUS_OK) { + error_log("EEPROM readout failed (enable EEPROM), " + "error code: %d", status); return status; } @@ -888,8 +1167,8 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) uint8_t err = hamming_decode(data, eeprom); if (err != 0) { - PX4_INFO_RAW("EEPROM readout failed! Failed to decoding " - "Hamming weight (error: %d)!", err); + error_log("EEPROM readout failed! Failed to decoding " + "Hamming weight (Hamming parity error: %d)!", err); return STATUS_ARGUS_EEPROM_BIT_ERROR; } @@ -900,30 +1179,30 @@ static status_t ReadEEPROM(s2pi_slave_t slave, uint8_t *eeprom) } /*!*************************************************************************** - * @brief GPIO Mode Test for S2PI HAL Implementation. + * @brief GPIO Mode Test for S2PI HAL Implementation. * * @details This test verifies the GPIO mode of the S2PI HAL module. This is - * done by leveraging the EEPROM readout sequence that accesses the - * devices EEPROM via a software protocol that depends on the GPIO - * mode. + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. * - * This the requires several steps, most of them are already verified - * in previous tests: - * - Basic device configuration and enable EEPROM. - * - Read EERPOM via GPIO mode and apply Hamming weight - * - Repeat several times (to eliminate random readout issues). - * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) - * - Check if Module Number and Chip ID is not 0 + * This the requires several steps, most of them are already verified + * in previous tests: + * - Basic device configuration and enable EEPROM. + * - Read EEPROM via GPIO mode and apply Hamming weight + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c) + * - Check if Module Number and Chip ID is not 0 * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the GPIO test fails. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the GPIO test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t GpioModeTest(s2pi_slave_t slave) { @@ -934,66 +1213,66 @@ static status_t GpioModeTest(s2pi_slave_t slave) status_t status = ReadEEPROM(slave, eeprom1); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (1st attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (1st attempt)!"); return status; } status = ReadEEPROM(slave, eeprom2); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (2nd attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (2nd attempt)!"); return status; } status = ReadEEPROM(slave, eeprom3); - if (status < STATUS_OK) { - PX4_INFO_RAW("GPIO mode test failed (3rd attempt)!"); + if (status != STATUS_OK) { + error_log("GPIO mode test failed (3rd attempt)!"); return status; } /* Verify EEPROM data. */ if ((memcmp(eeprom1, eeprom2, 16) != 0) || (memcmp(eeprom1, eeprom3, 16) != 0)) { - PX4_INFO_RAW("GPIO Mode test failed (data comparison)!\n" - "The data from 3 distinct EEPROM readout does not match!"); + error_log("GPIO Mode test failed (data comparison)!\n" + "The data from 3 distinct EEPROM readout does not match!"); return ERROR_FAIL; } /* Check EEPROM data for reasonable chip and module number (i.e. not 0) */ uint32_t chipID = EEPROM_ReadChipId(eeprom1); - argus_module_version_t module = EEPROM_ReadModule(eeprom1); + uint8_t module = EEPROM_ReadModule(eeprom1); if (chipID == 0 || module == 0) { - PX4_INFO_RAW("GPIO Mode test failed (data verification)!\n" - "Invalid EEPROM data: Module = %d; Chip ID = %u!", module, (uint)chipID); + error_log("GPIO Mode test failed (data verification)!\n" + "Invalid EEPROM data: Module = %d; Chip ID = %d!", module, chipID); return ERROR_FAIL; } - PX4_INFO_RAW("EEPROM Readout succeeded!\n"); - PX4_INFO_RAW("- Module: %d\n", module); - PX4_INFO_RAW("- Device ID: %u\n", (uint)chipID); + print("EEPROM Readout succeeded!\n"); + print("- Module: %d\n", module); + print("- Device ID: %d\n", chipID); return STATUS_OK; } /*!*************************************************************************** - * @brief Reads the RCO_TRIM value from the devices EEPROM. + * @brief Reads the RCO_TRIM value from the devices EEPROM. * * @details The function reads the devices EEPROM via GPIO mode and extracts - * the RCO_TRIM value from the EEPROM map. + * the RCO_TRIM value from the EEPROM map. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param rcotrim The read RCO_TRIM value will be returned via this pointer. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param rcotrim The read RCO_TRIM value will be returned via this pointer. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. - * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. - * - The S2PI layer error code if #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or - * #S2PI_ReadGpioPin return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - The S2PI layer error code if #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or + * #S2PI_ReadGpioPin return any negative status. *****************************************************************************/ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) { @@ -1003,25 +1282,15 @@ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) if (status != STATUS_OK) { return status; } - argus_module_version_t module = EEPROM_ReadModule(eeprom); - - switch (module) { - case AFBR_S50MV85G_V1: - case AFBR_S50MV85G_V2: - case AFBR_S50MV85G_V3: - case AFBR_S50LV85D_V1: - case AFBR_S50MV68B_V1: - case AFBR_S50MV85I_V1: - case AFBR_S50SV85K_V1: + uint8_t module = EEPROM_ReadModule(eeprom); + if (module > 0 && module < 8) { /* Read RCO Trim Value from EEPROM Map 1/2/3: */ *rcotrim = ((int8_t) eeprom[0]) >> 3; - break; - case MODULE_NONE: /* Uncalibrated module; use all 0 data. */ - default: - - PX4_INFO_RAW("EEPROM Readout failed! Unknown module number: %d", module); + } else { + /* Uncalibrated module; use all 0 data. */ + error_log("EEPROM Readout failed! Unknown module number: %d", module); return ERROR_ARGUS_UNKNOWN_MODULE; } @@ -1029,112 +1298,151 @@ static status_t ReadRcoTrim(s2pi_slave_t slave, int8_t *rcotrim) } /*!*************************************************************************** - * @brief Triggers a measurement on the device and waits for the data ready - * interrupt. + * @brief Callback function for the data ready interrupt. + * + * @details The function is called by the S2PI layer when the data ready + * interrupt is pending. The function sets the \p param to + * #STATUS_IDLE. + * + * @param param The parameter passed to the #S2PI_SetIrqCallback function as + * an abstract pointer to an #status_t type. + *****************************************************************************/ +static void MeasurementCallback(void *param) +{ + *(status_t *) param = STATUS_IDLE; +} + +/*!*************************************************************************** + * @brief Triggers a measurement on the device and waits for the data ready + * interrupt. * * @details The function triggers a measurement cycle on the device and waits - * until the measurement has been finished. A \p sample count can be - * specified to setup individual number of digital averaging. + * until the measurement has been finished. A \p sample count can be + * specified to setup individual number of digital averaging. * - * @warning The test utilizes already the timer HAL in order to implement a - * rudimentary timeout. However, at this time, only some basic - * plausibility checks are performed on the timer HAL. I.e. if there - * is an issue in the time HAL, e.g. too fast or too slow time - * counting, the test may fail with an #ERROR_TIMEOUT. In this case, - * one also needs to verify the timer HAL, especially the - * #Timer_GetCounterValue function. + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. - * @param samples The specified number of averaging samples for the measurement. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param samples The specified number of averaging samples for the measurement. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus - * or #S2PI_SetIrqCallback return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus + * or #S2PI_SetIrqCallback return any negative status. *****************************************************************************/ static status_t RunMeasurement(s2pi_slave_t slave, uint16_t samples) { - status_t status = TriggerMeasurement(slave, samples); + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 300; // The transfer timeout in ms. + /*************************************************************************/ - if (status < STATUS_OK) { - PX4_INFO_RAW("Speed test failed!\n" - "Call to TransferFrame returned code: %d", - (int)status); + volatile status_t callbackStatus = STATUS_BUSY; + + status_t status = S2PI_SetIrqCallback(slave, MeasurementCallback, (void *)&callbackStatus); + + if (status != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "Call to SetIrqCallback returned code: %d", status); + return status; + } + + status = TriggerMeasurement(slave, samples, 0, 0); + + if (status != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "Call to TransferFrame returned code: %d", status); return status; } /* Wait until the transfer is finished using a timeout. */ - status = AwaitDataReady(slave, 300); - if (status < STATUS_OK) { - PX4_INFO_RAW("Speed test failed!\n" - "SPI Read IRQ pin didn't raised, timeout activated at 200ms, error code: %d", - (int)status); - return status; + ltc_t start; + Time_GetNow(&start); + + while (callbackStatus == STATUS_BUSY) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("Failed to run a measurement!\n" + "Timeout occurred while waiting for the SPI interrupt (%d ms).", + timeout_ms); + return ERROR_TIMEOUT; + } } - return status; + if (callbackStatus != STATUS_OK) { + error_log("Failed to run a measurement!\n" + "The SPI callback yielded returned code: %d", + callbackStatus); + return callbackStatus; + } + + return STATUS_OK; } /*!*************************************************************************** - * @brief Test for Timer HAL Implementation by comparing timings to the device. + * @brief Test for Timer HAL Implementation by comparing timings to the device. * - * @details The test verifies the timer HAL implementation by comparing the - * timings to the AFBR-S50 device as a reference. - * Therefore several measurement are executed on the device, each with - * a different averaging sample count. The elapsed time increases - * linearly with the number of averaging samples. In order to remove - * the time for software/setup, a linear regression fit is applied to - * the measurement results and only the slope is considered for the - * result. A delta of 102.4 microseconds per sample is expected. - * If the measured delta per sample is within an specified error range, - * the timer implementation is considered correct. + * @details The test verifies the timer HAL implementation by comparing the + * timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. * - * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the timer test fails. - * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the EEPROM Hamming weight fails. - * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. - * - #ERROR_TIMEOUT if either the SPI operation did not finished - * or the IRQ was not detected within a specified time (check also - * timer HAL implementation). - * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus, - * #S2PI_SetIrqCallback, #S2PI_CaptureGpioControl, - * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or #S2PI_ReadGpioPin - * return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the timer test fails. + * - #STATUS_ARGUS_EEPROM_BIT_ERROR if the EEPROM Hamming weight fails. + * - #ERROR_ARGUS_UNKNOWN_MODULE if the EEPROM module number is invalid. + * - #ERROR_TIMEOUT if either the SPI operation did not finished + * or the IRQ was not detected within a specified time (check also + * timer HAL implementation). + * - The S2PI layer error code if #S2PI_TransferFrame, #S2PI_GetStatus, + * #S2PI_SetIrqCallback, #S2PI_CaptureGpioControl, + * #S2PI_ReleaseGpioControl, #S2PI_WriteGpioPin or #S2PI_ReadGpioPin + * return any negative status. *****************************************************************************/ static status_t TimerTest(s2pi_slave_t slave) { /* Test parameter configuration: *****************************************/ - const int8_t n = 10; // The number of measurements. - const uint32_t ds = 100; // The step size in averaging samples. - const float exp_slope = 102.4; // Expected slope is 102.4 µs / phase / sample - const float rel_slope_error = 3e-2; // Relative slope tolerance is 3%. + const int8_t n = 10; // The number of measurements. + const uint32_t ds = 100; // The step size in averaging samples. + const float exp_slope = 102.4f; // Expected slope is 102.4 µs / phase / sample + const float rel_slope_error = 3e-2f; // Relative slope tolerance is 3%. /*************************************************************************/ /* Read RCOTrim value from EEPROM*/ int8_t RcoTrim = 0; status_t status = ReadRcoTrim(slave, &RcoTrim); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "EEPROM Read test returned code: %d", (int)status); + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "EEPROM Read test returned code: %d", status); return status; } - PX4_INFO_RAW("RCOTrim = %d\n", RcoTrim); + print("RCOTrim = %d\n", RcoTrim); /* Configure the device with calibrated RCO to 24MHz. */ status = ConfigureDevice(slave, RcoTrim); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "Configuration test returned code: %d", (int)status); + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "Configuration test returned code: %d", status); return status; } @@ -1146,21 +1454,23 @@ static status_t TimerTest(s2pi_slave_t slave) float x2sum = 0; float xysum = 0; - PX4_INFO_RAW("+-------+---------+------------+\n"); - PX4_INFO_RAW("| count | samples | elapsed us |\n"); - PX4_INFO_RAW("+-------+---------+------------+\n"); + print("+-------+---------+------------+\n"); + print("| count | samples | elapsed us |\n"); + print("+-------+---------+------------+\n"); for (uint8_t i = 1; i <= n; ++i) { ltc_t start; Time_GetNow(&start); - int samples = ds * i; - status = RunMeasurement(slave, samples); + uint32_t samples = ds * i; + assert(samples < UINT16_MAX); - if (status < STATUS_OK) { - PX4_INFO_RAW("Timer test failed!\n" - "Run measurement returned code: %d", - (int)status); + status = RunMeasurement(slave, (uint16_t)samples); + + if (status != STATUS_OK) { + error_log("Timer test failed!\n" + "Run measurement returned code: %d", + status); return status; } @@ -1168,30 +1478,30 @@ static status_t TimerTest(s2pi_slave_t slave) xsum += (float) samples; ysum += (float) elapsed_usec; - x2sum += (float) samples * samples; - xysum += (float) samples * elapsed_usec; + x2sum += (float) samples * (float) samples; + xysum += (float) samples * (float) elapsed_usec; - PX4_INFO_RAW("| %5d | %7d | %10d |\n", i, samples, (uint)elapsed_usec); + print("| %5d | %7d | %10d |\n", i, samples, elapsed_usec); } - PX4_INFO_RAW("+-------+---------+------------+\n"); + print("+-------+---------+------------+\n"); const float slope = (n * xysum - xsum * ysum) / (n * x2sum - xsum * xsum); const float intercept = (ysum * x2sum - xsum * xysum) / (n * x2sum - xsum * xsum); - PX4_INFO_RAW("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", - (int)(10 * slope), (int)(10 * intercept)); + print("Linear Regression: y(x) = %dE-7 sec * x + %dE-7 sec\n", + (int)(10 * slope), (int)(10 * intercept)); /* Check the error of the slope. */ const float max_slope = exp_slope * (1.f + rel_slope_error); const float min_slope = exp_slope * (1.f - rel_slope_error); if (slope > max_slope || slope < min_slope) { - PX4_INFO_RAW("Time test failed!\n" - "The measured time slope does not match the expected value! " - "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", - (int)(10 * slope), (int)(10 * exp_slope), - (int)(10 * min_slope), (int)(10 * max_slope)); + error_log("Time test failed!\n" + "The measured time slope does not match the expected value! " + "(actual: %dE-7, expected: %dE-7, min: %dE-7, max: %dE-7)\n", + (int)(10 * slope), (int)(10 * exp_slope), + (int)(10 * min_slope), (int)(10 * max_slope)); return ERROR_FAIL; } @@ -1200,11 +1510,11 @@ static status_t TimerTest(s2pi_slave_t slave) /*!*************************************************************************** - * @brief Data structure for the PIT test. + * @brief Data structure for the PIT test. * - * @details Contains data that is required by the PIT timer test. + * @details Contains data that is required by the PIT timer test. *****************************************************************************/ -typedef struct { +typedef struct pit_data_t { /*! The number of PIT callback events. */ volatile uint32_t n; @@ -1216,90 +1526,113 @@ typedef struct { } pit_data_t; - - /*!*************************************************************************** - * @brief Callback function invoked by the PIT. + * @brief Callback function invoked by the PIT. * - * @details The function that is invoked every time a specified interval elapses. - * An abstract parameter is passed to the function whenever it is called. + * @details The function that is invoked every time a specified interval elapses. + * An abstract parameter is passed to the function whenever it is called. * - * This implementation collects callback time stamps and counts the - * number of callback events using the abstract parameter. + * This implementation collects callback time stamps and counts the + * number of callback events using the abstract parameter. * - * @param param An abstract parameter to be passed to the callback. This is - * also the identifier of the given interval. + * @param param An abstract parameter to be passed to the callback. This is + * also the identifier of the given interval. *****************************************************************************/ static void PIT_Callback(void *param) { - pit_data_t *data = (pit_data_t *) param; - - if (data->n == 0) { - Time_GetNow(&data->t_first); - data->t_last = data->t_first; + if (param == NULL) { + error_log("PIT interrupt test failed: callback parameter \"param\" was NULL!"); } else { - Time_GetNow(&data->t_last); - } + pit_data_t *data = (pit_data_t *)param; - data->n++; + if (data->n == 0) { + Time_GetNow(&data->t_first); + data->t_last = data->t_first; + + } else { + Time_GetNow(&data->t_last); + } + + data->n++; + } } /*!*************************************************************************** - * @brief Executes a PIT measurement and verifies the callback interval. + * @brief Executes a PIT measurement and verifies the callback interval. * * @details The function configures the PIT with a given interval and waits - * several callback events to happen. In each callback event, the - * elapsed time is measured and the number of calls are counted. - * Finally, the average interrupt period is compared with the - * lifetime timer that has been already verified in a previous test - * (see #Timer_Test). + * several callback events to happen. In each callback event, the + * elapsed time is measured and the number of calls are counted. + * Finally, the average interrupt period is compared with the + * lifetime timer that has been already verified in a previous test + * (see #TimerTest). The time until the first interrupt event is also + * verified. * - * @param exp_dt_us The expected timer interval in microseconds. - * @param n The number of PIT events to await. + * @param exp_dt_us The expected timer interval in microseconds. + * @param n The number of PIT events to await. * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_FAIL if the measured interval does not match the - * expectations or the PIT was not disabled properly. - * - #ERROR_TIMEOUT if either the PIT events do not occur within the - * expected time. - * - The PIT layer error code if #Timer_SetInterval return any - * negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval return any + * negative status. *****************************************************************************/ static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) { /* Test parameter configuration: *****************************************/ - const float rel_dt_error = 1e-3; // Relative timer interval tolerance is 0.1%. - const float abs_dt_error = 1.0; // Absolute timer interval tolerance is 1us. + const float rel_dt_error = 5e-3f; // Relative timer interval tolerance: 0.5 %. + const float abs_dt_error = 5.0f; // Absolute timer interval tolerance: 5.0 us. /*************************************************************************/ - float dt = exp_dt_us * rel_dt_error; + float dt = (float) exp_dt_us * rel_dt_error; if (dt < abs_dt_error) { dt = abs_dt_error; } - const float max_dt = exp_dt_us + dt; - const float min_dt = exp_dt_us - dt; + const float max_dt = (float) exp_dt_us + dt; + const float min_dt = (float) exp_dt_us - dt; + + if (dt < abs_dt_error * 3) { dt = abs_dt_error * 3; } + + const float t_first_max = (float) exp_dt_us + dt * 5; // use 5x tolerance for + const float t_first_min = (float) exp_dt_us - dt * 5; // the first interval /*************************************************************************/ + print("Run PIT Test (w/ %d us interval):\n" + " - expected event count: %d\n" + " - expected interval: %d us, min: %d us, max: %d us\n" + " - expected first event: %d us, min: %d us, max: %d us\n", + exp_dt_us, n, exp_dt_us, (int)min_dt, (int)max_dt, + exp_dt_us, (int)t_first_min, (int)t_first_max); + /* Setup the PIT callback with specified interval. */ pit_data_t data = { 0 }; status_t status = Timer_SetInterval(exp_dt_us, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", status); return status; } /* Wait until n PIT callback have been happened. */ - uint32_t timeout_us = (n + 1) * exp_dt_us; + const uint32_t timeout_us = (n + 1) * exp_dt_us; + ltc_t start; Time_GetNow(&start); while (data.n < n) { if (Time_CheckTimeoutUSec(&start, timeout_us)) { - PX4_INFO_RAW("PIT test failed!\n" - "Waiting for the PIT interrupt events yielded a timeout."); + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + const uint32_t t_first_us = Time_DiffUSec(&start, &data.t_first); + const uint32_t t_last_us = Time_DiffUSec(&start, &data.t_last); + error_log("PIT test failed!\n" + "Waiting for the PIT interrupt events yielded a timeout.\n" + "Timeout: %d us; Elapsed: %d us (%d of %d events).\n" + "First event @ %d us, last event @ %d us", + timeout_us, elapsed_us, data.n, n, t_first_us, t_last_us); status = ERROR_TIMEOUT; break; } @@ -1309,59 +1642,71 @@ static status_t RunPITTest(uint32_t exp_dt_us, uint32_t n) /* Disable the PIT timer callback. */ status = Timer_SetInterval(0, &data); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetInterval returned status code: %d", status); } } if (status == STATUS_OK) { /* Check if PIT callback is not invoked any more. */ - timeout_us = 2 * exp_dt_us; - Time_GetNow(&start); - - while (!Time_CheckTimeoutUSec(&start, timeout_us)) { __asm("nop"); } + Time_DelayUSec(3 * exp_dt_us); if (data.n > n) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetInterval has been called after it was disabled."); + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + error_log("PIT test failed!\n" + "Timer_SetInterval has been called again after it was disabled\n" + "(within %d us; %d of %d events in total).", + elapsed_us, data.n, n); status = ERROR_FAIL; } } /* Verify the measured average timer interval. */ - const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (n - 1); + const float act_dt_us = Time_DiffUSec(&data.t_first, &data.t_last) / (float)(n - 1); + const uint32_t t_first_us = Time_DiffUSec(&start, &data.t_first); + const uint32_t t_last_us = Time_DiffUSec(&start, &data.t_last); - if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { - PX4_INFO_RAW("PIT test failed!\n" - "The measured timer interval does not match the expected value!\n"); + print(" - actual event count: %d\n" + " - actual interval: %d us\n" + " - actual first event: %d us\n" + " - actual last event: %d us\n\n", + data.n, (int)act_dt_us, t_first_us, t_last_us); + + if (status == STATUS_OK && (t_first_us > t_first_max || t_first_us < t_first_min)) { + error_log("PIT test failed!\n" + "The first timer event did not occur after the expected interval!"); status = ERROR_FAIL; } - PX4_INFO_RAW("PIT Test Results:\n" - " - event count: %u\n" - " - actual interval: %d us\n" - " - expected interval: %d us, min: %d us, max: %d us\n", - (uint)data.n, (int)act_dt_us, (uint)exp_dt_us, (int)min_dt, (int)max_dt); + if (status == STATUS_OK && (act_dt_us > max_dt || act_dt_us < min_dt)) { + error_log("PIT test failed!\n" + "The measured timer interval does not match the expected value!"); + status = ERROR_FAIL; + } + + print(" - test status: %d\n\n", status); return status; } /*!*************************************************************************** - * @brief Test for PIT HAL Implementation by comparing timings to the device. + * @brief Test for PIT HAL Implementation by comparing timings to the device. * - * @details The test verifies the timer HAL implementation by comparing the + * @details The test verifies the timer HAL implementation by comparing the + * period between the interrupts with the lifetime timer values + * that has been already verified in a previous test (see #TimerTest). * - * @return Returns the \link #status_t status\endlink: - * - #STATUS_OK on success. - * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not - * implemented and the test is skipped. - * - #ERROR_FAIL if the measured interval does not match the - * expectations or the PIT was not disabled properly. - * - #ERROR_TIMEOUT if either the PIT events do not occur within the - * expected time. - * - The PIT layer error code if #Timer_SetInterval or - * #Timer_SetCallback return any negative status. + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_FAIL if the measured interval does not match the + * expectations or the PIT was not disabled properly. + * - #ERROR_TIMEOUT if either the PIT events do not occur within the + * expected time. + * - The PIT layer error code if #Timer_SetInterval or + * #Timer_SetCallback return any negative status. *****************************************************************************/ static status_t PITTest(void) { @@ -1369,31 +1714,595 @@ static status_t PITTest(void) if (status == ERROR_NOT_IMPLEMENTED) { return status; } - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetCallback returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetCallback returned status code: %d", status); return status; } + status = RunPITTest(200000, 5); + + if (status != STATUS_OK) { return status; } + status = RunPITTest(10000, 10); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { return status; } - status = RunPITTest(333, 1000); + /* High Speed Test down to 1000 microseconds. If this fails, just print + * a message that very high frame rates might have issues. */ + status = RunPITTest(1000, 500); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { + print("WARNING: PIT test failed for 1000 us interval!\n" + " This is only critical if high frame rates (up to 1000 fps)\n" + " need to be achieved. Otherwise, the error can be safely ignored.\n"); + status = STATUS_IGNORE; // ignore + } - status = RunPITTest(100000, 5); + if (status == STATUS_OK) { // only run if previous test succeeded! + /* High Speed Test down to 333 microseconds. If this fails, just print + * a message that very high frame rates might have issues. */ + status = RunPITTest(333, 500); - if (status < STATUS_OK) { return status; } + if (status != STATUS_OK) { + print("WARNING: PIT test failed for 333 us interval!\n" + " This is only critical if very high frame rates (up to 3000 fps)\n" + " need to be achieved. Otherwise, the error can be safely ignored.\n"); + status = STATUS_IGNORE; // ignore + } + } status = Timer_SetCallback(0); - if (status < STATUS_OK) { - PX4_INFO_RAW("PIT test failed!\n" - "Timer_SetCallback to 0 returned status code: %d", (int)status); + if (status != STATUS_OK) { + error_log("PIT test failed!\n" + "Timer_SetCallback to 0 returned status code: %d", status); return status; } return STATUS_OK; } + +/*!*************************************************************************** + * @brief Data structure for the S2PI transfer from interrupt tests. + * + * @details Contains data that is required by the S2PI transfer from interrupt + * test. The data structure is passed to the corresponding interrupt + * callback functions. + *****************************************************************************/ +typedef struct spi_irq_data_t { + /*! The status of the interrupt callback function. */ + volatile status_t Status; + + /*! The S2PI slave parameter passed to the S2PI HAL functions. */ + s2pi_slave_t Slave; + + /*! The data buffer to be transferred from/to the device for testing purposes. */ + uint8_t Data[17U]; + + /*! Set to true when all SPI transfers are finished. */ + volatile bool Finished; + + /*! Set to true when the second SPI transfers is started. + The second transfer is used to read-back the previously set values. */ + volatile bool ReadBack; + +} spi_irq_data_t; + + +/*!*************************************************************************** + * @brief SPI interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * finishing the SPI transfer. The callback is used by the + * #SpiTransferFromSpiInterrupt test to trigger the second SPI transfer + * from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param status The S2PI module status passed to the callback. + * @param param The abstract interrupt callback parameter. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_INVALID_ARGUMENT if the \p param is NULL. + * - The S2PI layer error code if any is passed to the callback function. + * - The S2PI layer error code if #S2PI_TransferFrame return any. + *****************************************************************************/ +static status_t SpiTransferFromSpiInterruptCallback(status_t status, void *param) +{ + if (param == NULL) { + error_log("SPI transfer from SPI interrupt test failed\n" + "callback parameter \"param\" was NULL!"); + return ERROR_INVALID_ARGUMENT; + } + + spi_irq_data_t *data = (spi_irq_data_t *) param; + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "callback received error! Error code: %d", status); + data->Status = status; + return status; + } + + if (!data->ReadBack) { + print("Invoking SPI transfer from SPI interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = 0; } + + status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Calling S2PI_TransferFrame from SPI interrupt " + "returned error code: %d", status); + data->Status = status; + return status; + } + + data->ReadBack = true; + + } else { + data->Finished = true; + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief SPI transfer from SPI interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the SPI + * interrupt service routine context. + * + * The test basically repeats the #SpiConnectionTest but this time it + * invokes the second SPI transfer from the SPI callback function. + * A very common error is that the callback is invoked while the SPI + * module is still busy which does not allow to invoke another SPI + * transfer from the callback. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the SPI + * callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromSpiInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_us = 100000; // timeout for SPI transfers to finish + /*************************************************************************/ + + status_t status = STATUS_OK; + spi_irq_data_t data = { .Slave = slave }; + + print("Invoking SPI transfer from task level...\n"); + + /* Transfer a pattern to the register */ + data.Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data.Data[i] = i; } + + status = S2PI_TransferFrame(slave, data.Data, data.Data, 17U, + SpiTransferFromSpiInterruptCallback, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Failed to transfer a data frame! Error code: %d", status); + return status; + } + + /* Wait until transfers has finished. */ + ltc_t start; + Time_GetNow(&start); + + while (!data.Finished && (data.Status == STATUS_OK)) { + if (Time_CheckTimeoutUSec(&start, timeout_us)) { + const uint32_t elapsed_us = Time_GetElapsedUSec(&start); + error_log("SPI transfer from SPI interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a timeout.\n" + "Timeout: %d us; Elapsed: %d us (%d of %d events).", + timeout_us, elapsed_us); + status = ERROR_TIMEOUT; + break; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from SPI interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief GPIO interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the S2PI module upon + * receiving an GPIO interrupt from the devices IRQ pin. The callback + * is used by the #SpiTransferFromGpioInterrupt test to trigger the + * first SPI transfer from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param param The abstract interrupt callback parameter. + *****************************************************************************/ +static void SpiTransferFromGpioInterruptCallback(void *param) +{ + if (param == NULL) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "callback parameter \"param\" was NULL!"); + return; + } + + print("Invoking SPI transfer from GPIO interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + spi_irq_data_t *data = (spi_irq_data_t *) param; + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = i; } + + status_t status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Calling S2PI_TransferFrame from GPIO interrupt " + "returned error code: %d", status); + data->Status = status; + return; + } +} + +/*!*************************************************************************** + * @brief SPI transfer from GPIO interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * GPIO interrupt service routine context. + * + * The test basically repeats the #SpiTransferFromSpiInterrupt but + * this time it invokes the first SPI transfer from the GPIO callback + * function. In order to trigger a GPIO interrupt, the device is + * configured and a measurement is started (see #GpioInterruptTest). + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the GPIO or + * SPI callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromGpioInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 300; // timeout for measurement, might be increased.. + /*************************************************************************/ + + spi_irq_data_t data = { .Slave = slave }; + + /* Install IRQ callback. */ + status_t status = S2PI_SetIrqCallback(slave, SpiTransferFromGpioInterruptCallback, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "The call to S2PI_SetIrqCallback returned error code: %d", status); + return status; + } + + /* Setup Device for invoking GPIO interrupt. */ + status = ConfigureDevice(slave, 0); + + if (status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed."); + return status; + } + + /* Trigger Measurement and invoke GPIO interrupt. */ + status = TriggerMeasurement(slave, 0, 0, 0); + + if (status != STATUS_OK) { + error_log("GPIO interrupt test failed!"); + return status; + } + + ltc_t start; + Time_GetNow(&start); + + /* Wait for Interrupt using the callback method. */ + while (!data.Finished) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "The IRQ callback was not invoked within %d ms.", + timeout_ms); + return ERROR_TIMEOUT; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from GPIO interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief PIT interrupt callback function for the SPI transfer from IRQ test. + * + * @details The interrupt callback is invoked from the PIT module upon periodic + * timeout event. The callback is used by the + * #SpiTransferFromPitInterrupt test to trigger the first SPI transfer + * from the interrupt callback context. + * + * @note The callback also utilizes the #print functionality. This requires + * a correct implementation of the corresponding function such that it + * can be invoked from the given interrupt context. This usually + * requires the underlying send (e.g. UART or USB send functions) to + * have higher priority that this interrupt in order to finished the + * print statement asynchronously. + * + * @param param The abstract interrupt callback parameter. + *****************************************************************************/ +static void SpiTransferFromPitInterruptCallback(void *param) +{ + status_t status = Timer_SetInterval(0, param); // disable timer + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback to 0 returned status code: %d", + status); + + if (param != NULL) { ((spi_irq_data_t *)param)->Status = status; } + + return; + } + + + if (param == NULL) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "callback parameter \"param\" was NULL!"); + return; + } + + print("Invoking SPI transfer from PIT interrupt callback...\n"); + + /* Clear the laser pattern and read back previous values. */ + spi_irq_data_t *data = (spi_irq_data_t *) param; + data->Data[0] = 0x04; // Laser Pattern Register Address + + for (uint8_t i = 1; i < 17U; ++i) { data->Data[i] = i; } + + status = S2PI_TransferFrame(data->Slave, data->Data, data->Data, 17U, + SpiTransferFromSpiInterruptCallback, param); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Calling S2PI_TransferFrame from GPIO interrupt " + "returned error code: %d", status); + data->Status = status; + return; + } +} + +/*!*************************************************************************** + * @brief SPI transfer from PIT interrupt callback test. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * PIT interrupt service routine context. + * + * The test basically repeats the #SpiTransferFromSpiInterrupt but + * this time it invokes the first SPI transfer from the PIT callback + * function. In order to trigger a PIT interrupt, the timer is + * configured with a small interval and immediately disabled upon the + * first event. + * + * Note that this test is only executed if the PIT module is actually + * implemented. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_NOT_IMPLEMENTED if the PIT functionality is not + * implemented and the test is skipped. + * - #ERROR_TIMEOUT if the test did not finish within a specified time. + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or the SPI + * callback yield in any non-OK status. + * - The PIT layer error code if #Timer_SetCallback or the PIT + * callback yield in any non-OK status. + *****************************************************************************/ +static status_t SpiTransferFromPitInterrupt(s2pi_slave_t slave) +{ + /* Test parameter configuration: *****************************************/ + const uint32_t timeout_ms = 100; // timeout for test. + const uint32_t interval_us = 1000; // PIT interval for the first event. + /*************************************************************************/ + + spi_irq_data_t data = { .Slave = slave }; + + status_t status = Timer_SetCallback(SpiTransferFromPitInterruptCallback); + + if (status == ERROR_NOT_IMPLEMENTED) { return status; } + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback returned status code: %d", status); + return status; + } + + /* Setup the PIT callback with specified interval. */ + status = Timer_SetInterval(interval_us, &data); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetInterval returned status code: %d", status); + return status; + } + + ltc_t start; + Time_GetNow(&start); + + /* Wait for test to be finished. */ + while (!data.Finished) { + if (Time_CheckTimeoutMSec(&start, timeout_ms)) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "The IRQ callback was not invoked within %d ms.", + timeout_ms); + return ERROR_TIMEOUT; + } + } + + if (data.Status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Waiting for the transfers to be finished yielded a error code: %d", + data.Status); + return data.Status; + } + + status = Timer_SetCallback(0); + + if (status != STATUS_OK) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Timer_SetCallback to 0 returned status code: %d", status); + return status; + } + + print("Verify read data...\n"); + + /* Verify the read pattern. */ + for (uint8_t i = 1; i < 17U; ++i) { + if (data.Data[i] != i) { + error_log("SPI transfer from PIT interrupt test failed:\n" + "Verification of read data is invalid!\n" + "read_data[%d] = %d, but expected was %d", + i, data.Data[i], i); + return ERROR_FAIL; + } + } + + return STATUS_OK; +} + +/*!*************************************************************************** + * @brief SPI Transfer from Interrupt Test for S2PI HAL Implementation. + * + * @details This test verifies the interrupt functionality of the SPI interface. + * The test verifies that an SPI transfer can be triggered from the + * interrupt service routine context. I.e. the #S2PI_TransferFrame + * function is called from the following interrupts: + * - SPI interrupt + * - GPIO interrupt + * - PIT interrupt (optional, if PIT is implemented) + * + * @warning The test utilizes already the timer HAL in order to implement a + * rudimentary timeout. However, at this time, only some basic + * plausibility checks are performed on the timer HAL. I.e. if there + * is an issue in the time HAL, e.g. too fast or too slow time + * counting, the test may fail with an #ERROR_TIMEOUT. In this case, + * one also needs to verify the timer HAL, especially the + * #Timer_GetCounterValue function. + * + * @param slave The S2PI slave parameter passed to the S2PI HAL functions. + * + * @return Returns the \link #status_t status\endlink: + * - #STATUS_OK on success. + * - #ERROR_TIMEOUT if the operation did not finished within a specified + * time (check also timer HAL implementation). + * - #ERROR_FAIL if the device access failed and the read data did not + * match the expected values. + * - The S2PI layer error code if #S2PI_TransferFrame or #S2PI_GetStatus + * return any negative status. + *****************************************************************************/ +static status_t SpiTransferFromInterruptTest(s2pi_slave_t slave) +{ + status_t status = STATUS_OK; + + print(" .1 >> SPI Transfer from SPI Interrupt Test\n"); + status = SpiTransferFromSpiInterrupt(slave); + + if (status != STATUS_OK) { return status; } + + print(" .1 >> PASS\n\n"); + + print(" .2 >> SPI Transfer from GPIO Interrupt Test\n"); + status = SpiTransferFromGpioInterrupt(slave); + + if (status != STATUS_OK) { return status; } + + print(" .2 >> PASS\n\n"); + + print(" .3 >> SPI Transfer from PIT Interrupt Test\n"); + status = SpiTransferFromPitInterrupt(slave); + + if (status == ERROR_NOT_IMPLEMENTED) { + print(" .3 >> SKIPPED (PIT is not implemented)\n\n"); + + } else { + if (status != STATUS_OK) { return status; } + + print(" .3 >> PASS\n\n"); + } + + return STATUS_OK; +} + +/*! @} */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h index 5af6660dfa..be9c0071e4 100644 --- a/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h +++ b/src/drivers/distance_sensor/broadcom/afbrs50/argus_hal_test.h @@ -1,10 +1,10 @@ /*************************************************************************//** - * @file argus_hal_test.c - * @brief Tests for the AFBR-S50 API hardware abstraction layer. + * @file + * @brief Tests for the AFBR-S50 API hardware abstraction layer. * * @copyright * - * Copyright (c) 2021, Broadcom, Inc. + * Copyright (c) 2023, Broadcom Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -41,147 +41,192 @@ extern "C" { #endif - /*!*************************************************************************** - * @defgroup argustest HAL Self Test + * @defgroup argus_test HAL Self Test + * @ingroup argus * - * @brief A test module to verify implementation of the HAL. + * @brief A test module to verify implementation of the HAL. * - * @details A series of automated tests that can be executed on the target - * platform in order to verify the implementation of the HAL that - * are required by the API. + * @details A series of automated tests that can be executed on the target + * platform in order to verify the implementation of the HAL that + * are required by the API. * - * @addtogroup argustest + * See #Argus_VerifyHALImplementation for a detailed documentation. + * + * @addtogroup argus_test * @{ *****************************************************************************/ #include "argus.h" /*!*************************************************************************** - * @brief Version number of the HAL Self Test. + * @brief Version number of the HAL Self Test. * * @details Changes: - * * v1.0: - * - Initial release. - * * v1.1: - * - Added additional print output. - * - Increased tolerance for timer test to 3%. - * - Fixed callback issue by disabling it after IRQ test. - * * v1.1: - * - Added PIT test cases. + * * v1.0: + * - Initial release. + * * v1.1: + * - Added additional print output. + * - Increased tolerance for timer test to 3%. + * - Fixed callback issue by disabling it after IRQ test. + * * v1.2: + * - Added PIT test cases. + * * v1.3: + * - Added test case for SPI maximum data transfer size. + * - Added tests for SPI transfers invoked from all IRQ callbacks. + * - Added verification of first PIT event occurrence. + * - Relaxed PIT pass conditions (0.1% -> 0.5%) + * * v1.4: + * - Adopted to new multi-device HAL interface of API v1.4.4 release. + * - Added verification of SPI callback invocation. + * - Updated GPIO interrupt test to verify if delayed interrupt + * pending states can be detected via #S2PI_ReadIrqPin. + * *****************************************************************************/ -#define HAL_TEST_VERSION "v1.2" +#define HAL_TEST_VERSION "v1.4" /*!*************************************************************************** - * @brief Executes a series of tests in order to verify the HAL implementation. + * @brief Executes a series of tests in order to verify the HAL implementation. * * @details A series of automated tests are executed on the target platform in - * order to verify the implementation of the HAL that are required by - * the API. + * order to verify the implementation of the HAL that are required by + * the API. * - * Each test will write an error description via the print (i.e. UART) - * function that shows what went wrong. Also an corresponding status is - * returned in case no print functionality is available. + * Each test will write an error description via the print (i.e. UART) + * function that shows what went wrong. Also an corresponding status is + * returned in case no print functionality is available. * - * The following tests are executed: + * The following tests are executed: * - * **1) Timer Plausibility Test:** + * **1) Timer Plausibility Test:** * - * Rudimentary tests of the lifetime counter (LTC) implementation. - * This verifies that the LTC is running by checking if the returned - * values of two consecutive calls to the #Timer_GetCounterValue - * function are ascending. An artificial delay using the NOP operation - * is induced such that the timer is not read to fast. + * Rudimentary tests of the lifetime counter (LTC) implementation. + * This verifies that the LTC is running by checking if the returned + * values of two consecutive calls to the #Timer_GetCounterValue + * function are ascending. An artificial delay using the NOP operation + * is induced such that the timer is not read to fast. * - * **2) Timer Wraparound Test:** + * **2) Timer Wraparound Test:** * - * The LTC values must wrap from 999999 µs to 0 µs and increase the - * seconds counter accordingly. This test verifies the correct wrapping - * by consecutively calling the #Timer_GetCounterValue function until - * at least 2 wraparound events have been occurred. + * The LTC values must wrap from 999999 µs to 0 µs and increase the + * seconds counter accordingly. This test verifies the correct wrapping + * by consecutively calling the #Timer_GetCounterValue function until + * at least 2 wraparound events have been occurred. * - * **3) SPI Connection Test:** + * **3) SPI Connection Test:** * - * This test verifies the basic functionality of the SPI interface. - * The test utilizes the devices laser pattern register, which can - * be freely programmed by any 128-bit pattern. Thus, it writes a byte - * sequence and reads back the written values on the consecutive SPI - * access. + * This test verifies the basic functionality of the SPI interface. + * The test utilizes the devices laser pattern register, which can + * be freely programmed by any 128-bit pattern. Thus, it writes a byte + * sequence and reads back the written values on the consecutive SPI + * access. * - * **4) SPI Interrupt Test:** + * **4) SPI Maximum Data Length Test**: * - * This test verifies the correct implementation of the device - * integration finished interrupt callback. Therefore it configures - * the device with a minimal setup to run a pseudo measurement that - * does not emit any laser light. + * This test verifies the maximum data transfer length of the SPI + * interface. The test sends and receives up to 396 data bytes plus + * a single address byte over the SPI interface and verifies that no + * data get lost. * - * Note that this test does verify the GPIO interrupt that occurs - * whenever the device has finished the integration/measurement and - * new data is waiting to be read from the device. This does not test - * the interrupt that is triggered when the SPI transfer has finished. + * **5) SPI Interrupt Test:** * - * The data ready interrupt implies two S2PI layer functions that - * are tested in this test: The #S2PI_SetIrqCallback function installs - * a callback function that is invoked whenever the IRQ occurs. - * The IRQ can be delayed due to higher priority task, e.g. from the - * user code. It is essential for the laser safety timeout algorithm - * to determine the device ready signal as fast as possible, another - * method is implemented to read if the IRQ is pending but the - * callback has not been reset yet. This is what the #S2PI_ReadIrqPin - * function is for. + * This test verifies the correct implementation of the device + * integration finished interrupt callback. Therefore it configures + * the device with a minimal setup to run a pseudo measurement that + * does not emit any laser light. * - * **5) GPIO Mode Test:** + * Note that this test does verify the GPIO interrupt that occurs + * whenever the device has finished the integration/measurement and + * new data is waiting to be read from the device. This does not test + * the interrupt that is triggered when the SPI transfer has finished. * - * This test verifies the GPIO mode of the S2PI HAL module. This is - * done by leveraging the EEPROM readout sequence that accesses the - * devices EEPROM via a software protocol that depends on the GPIO - * mode. + * The data ready interrupt implies two S2PI layer functions that + * are tested in this test: The #S2PI_SetIrqCallback function installs + * a callback function that is invoked whenever the IRQ occurs. + * The IRQ can be delayed due to higher priority task, e.g. from the + * user code. It is essential for the laser safety timeout algorithm + * to determine the device ready signal as fast as possible, another + * method is implemented to read if the IRQ is pending but the + * callback has not been reset yet. This is what the #S2PI_ReadIrqPin + * function is for. * - * This the requires several steps, most of them are already verified - * in previous tests: + * **6) GPIO Mode Test:** * - * - Basic device configuration and enable EEPROM. - * - Read EERPOM via GPIO mode and apply Hamming weight. - * - Repeat several times (to eliminate random readout issues). - * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). - * - Check if Module Number and Chip ID is not 0. + * This test verifies the GPIO mode of the S2PI HAL module. This is + * done by leveraging the EEPROM readout sequence that accesses the + * devices EEPROM via a software protocol that depends on the GPIO + * mode. * - * **6) Timer Test for Lifetime Counter:** + * This the requires several steps, most of them are already verified + * in previous tests: * - * The test verifies the lifetime counter timer HAL implementation by - * comparing the timings to the AFBR-S50 device as a reference. - * Therefore several measurement are executed on the device, each with - * a different averaging sample count. The elapsed time increases - * linearly with the number of averaging samples. In order to remove - * the time for software/setup, a linear regression fit is applied to - * the measurement results and only the slope is considered for the - * result. A delta of 102.4 microseconds per sample is expected. - * If the measured delta per sample is within an specified error range, - * the timer implementation is considered correct. + * - Basic device configuration and enable EEPROM. + * - Read EERPOM via GPIO mode and apply Hamming weight. + * - Repeat several times (to eliminate random readout issues). + * - Decode the EEPROM (using EEPROM_Decode in argus_cal_eeprom.c). + * - Check if Module Number and Chip ID is not 0. * - * **7) Timer Test for Periodic Interrupt Timer:** + * **7) Timer Test for Lifetime Counter:** * - * The test verifies the correct implementation of the periodic - * interrupt timer (PIT). It sets different intervals and waits for - * a certain number of interrupts to happen. Each interrupt event - * is counted and the time between the first and the last interrupt - * is measured. Finally, the measured interval is compared to the - * expectations. + * The test verifies the lifetime counter timer HAL implementation by + * comparing the timings to the AFBR-S50 device as a reference. + * Therefore several measurement are executed on the device, each with + * a different averaging sample count. The elapsed time increases + * linearly with the number of averaging samples. In order to remove + * the time for software/setup, a linear regression fit is applied to + * the measurement results and only the slope is considered for the + * result. A delta of 102.4 microseconds per sample is expected. + * If the measured delta per sample is within an specified error range, + * the timer implementation is considered correct. * + * **8) Timer Test for Periodic Interrupt Timer (optional):** * - * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ - * lines. This is actually just a number that is passed - * to the SPI interface to distinct for multiple SPI slave - * devices. Note that the slave must be not equal to 0, - * since is reserved for error handling. + * The test verifies the correct implementation of the periodic + * interrupt timer (PIT). It sets different intervals and waits for + * a certain number of interrupts to happen. Each interrupt event + * is counted and the time between the first and the last interrupt + * is measured. Finally, the measured interval is compared to the + * expectations. * - * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). + * Note that this test is only executed if the PIT is actually + * implemented. Otherwise, the test is skipped. + * + * **9) SPI Transfer from Interrupt Callback Test:** + * + * The test verifies that the #S2PI_TransferFrame method of the + * S2PI layer can be invoked from a interrupt callback function too. + * Thus, it repeats the S2PI Connection Test but this time from + * different interrupt callback functions: + * + * - SPI Callback: The first transfer is invoked from thread level, + * the second transfer is invoke from the SPI interrupt callback + * function. + * + * - GPIO Callback: The device is setup to trigger an GPIO interrupt + * (see also the SPI Interrupt Test). The corresponding GPIO + * interrupt callback function will trigger the first transfer while + * the second one is triggered from the SPI callback function. + * + * - PIT Callback (optional): This test is only executed optional if + * the PIT interface is implemented. The test sequence is the same + * as for the GPIO callback, but the first transfer is triggered + * from the PIT callback function. + * + * @note See #HAL_TEST_VERSION for a version history and change log of + * the HAL self tests. + * + * @param spi_slave The SPI hardware slave, i.e. the specified CS and IRQ + * lines. This is actually just a number that is passed + * to the SPI interface to distinct for multiple SPI slave + * devices. Note that the slave must be not equal to 0, + * since is reserved for error handling. + * + * @return Returns the \link #status_t status\endlink (#STATUS_OK on success). *****************************************************************************/ status_t Argus_VerifyHALImplementation(s2pi_slave_t spi_slave); -#ifdef __cplusplus -} -#endif - /*! @} */ -#endif /* ARGUS_CAL_API_H */ +#ifdef __cplusplus +} // extern "C" +#endif +#endif /* ARGUS_HAL_TEST_H */ diff --git a/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c new file mode 100644 index 0000000000..28ef2a17c1 --- /dev/null +++ b/src/drivers/distance_sensor/broadcom/afbrs50/parameters.c @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * AFBR Rangefinder Mode + * + * This parameter defines the mode of the AFBR Rangefinder. + * + * @reboot_required true + * @min 0 + * @max 3 + * @group Sensors + * + * @value 0 Short Range Mode + * @value 1 Long Range Mode + * @value 2 High Speed Short Range Mode + * @value 3 High Speed Long Range Mode + */ +PARAM_DEFINE_INT32(SENS_AFBR_MODE, 1); + +/** + * AFBR Rangefinder Short Range Rate + * + * This parameter defines measurement rate of the AFBR Rangefinder in short range mode. + * + * @min 1 + * @max 100 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_S_RATE, 50); + +/** + * AFBR Rangefinder Long Range Rate + * + * This parameter defines measurement rate of the AFBR Rangefinder in long range mode. + * + * @min 1 + * @max 100 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_L_RATE, 25); + +/** + * AFBR Rangefinder Short/Long Range Threshold + * + * This parameter defines the threshold for switching between short and long range mode. + * The mode will switch from short to long range when the distance is greater than the threshold plus the hysteresis. + * The mode will switch from long to short range when the distance is less than the threshold minus the hysteresis. + * + * @unit m + * @min 1 + * @max 50 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_THRESH, 5); + + +/** + * AFBR Rangefinder Short/Long Range Threshold Hysteresis + * + * This parameter defines the hysteresis for switching between short and long range mode. + * + * @unit m + * @min 1 + * @max 10 + * @group Sensors + * + */ +PARAM_DEFINE_INT32(SENS_AFBR_HYSTER, 1); diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index baa2649d48..aafa605768 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -195,6 +195,13 @@ int LightwareLaser::init() _type = Type::LW20c; break; + case 7: + /* SF/LW30/d (200m 49-20'000Hz) */ + _px4_rangefinder.set_min_distance(0.2f); + _px4_rangefinder.set_max_distance(200.0f); + _conversion_interval = 20409; + break; + default: PX4_ERR("invalid HW model %" PRId32 ".", hw_model); return ret; diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c index c417efbe9e..b0564780be 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c +++ b/src/drivers/distance_sensor/lightware_laser_i2c/parameters.c @@ -45,5 +45,6 @@ * @value 4 SF11/c * @value 5 SF/LW20/b * @value 6 SF/LW20/c + * @value 7 SF/LW30/d */ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0); diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp index de418c69d7..f27636657f 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp +++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp @@ -308,7 +308,7 @@ void LightwareLaserSerial::Run() // LW20: Enable serial mode by sending some characters if (hw_model == 8) { const char *data = "www\r\n"; - (void)!::write(_fd, &data, strlen(data)); + (void)!::write(_fd, data, strlen(data)); } } diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp index 090152d9bc..fb95686381 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp +++ b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp @@ -35,10 +35,11 @@ ***********/ #include "vl53l1x.hpp" +#include -#define VL53L1X_DELAY 20 //ms +#define VL53L1X_DELAY 50000 // delay to reduce CPU usage (us) #define VL53L1X_SAMPLE_RATE 200 // ms, default -#define VL53L1X_INTER_MEAS_MS 200 // ms +#define VL53L1X_INTER_MEAS_MS 200 // ms #define VL53L1X_SHORT_RANGE 1 // sub-2 meter distance mode #define VL53L1X_LONG_RANGE 2 // sub-4 meter distance mode #define VL53L1X_RANGE_STATUS_OUT_OF_BOUNDS 13 // region of interest out of bounds error @@ -261,18 +262,19 @@ int VL53L1X::probe() void VL53L1X::RunImpl() { + uint8_t dataReady = 0; + VL53L1X_CheckForDataReady(&dataReady); if (dataReady == 1) { collect(); } + // Reduce CPU usage ScheduleDelayed(VL53L1X_DELAY); - // zone modulus increment _zone_index = (_zone_index + 1) % _zone_limit; - // Set the ROI center based on zone incrementation VL53L1X_SetROICenter(roi_center[_zone_index]); } @@ -292,6 +294,7 @@ int VL53L1X::init() uint8_t x = 4; uint8_t y = 4; + ret |= VL53L1X_SensorInit(); ret |= VL53L1X_ConfigBig(_distance_mode, VL53L1X_SAMPLE_RATE); ret |= VL53L1X_SetROI(x, y); @@ -488,6 +491,7 @@ int8_t VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady) uint8_t IntPol; int8_t status = 0; + status = VL53L1X_GetInterruptPolarity(&IntPol); status = VL53L1_RdByte(GPIO__TIO_HV_STATUS, &Temp); @@ -498,6 +502,7 @@ int8_t VL53L1X::VL53L1X_CheckForDataReady(uint8_t *isDataReady) } else { *isDataReady = 0; + } } @@ -520,6 +525,7 @@ int8_t VL53L1X::VL53L1X_ClearInterrupt() int8_t status = 0; status = VL53L1_WrByte(SYSTEM__INTERRUPT_CLEAR, 0x01); + return status; } @@ -528,6 +534,8 @@ int8_t VL53L1X::VL53L1X_StopRanging() int8_t status = 0; status = VL53L1_WrByte(SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */ + + ScheduleClear(); return status; } diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index 17563002c4..3e295abfc9 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -294,17 +294,17 @@ namespace time_literals // User-defined integer literals for different time units. // The base unit is hrt_abstime in microseconds -constexpr hrt_abstime operator "" _s(unsigned long long seconds) +constexpr hrt_abstime operator ""_s(unsigned long long seconds) { return hrt_abstime(seconds * 1000000ULL); } -constexpr hrt_abstime operator "" _ms(unsigned long long milliseconds) +constexpr hrt_abstime operator ""_ms(unsigned long long milliseconds) { return hrt_abstime(milliseconds * 1000ULL); } -constexpr hrt_abstime operator "" _us(unsigned long long microseconds) +constexpr hrt_abstime operator ""_us(unsigned long long microseconds) { return hrt_abstime(microseconds); } diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 13251bfebd..6aedf298a3 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -61,41 +61,11 @@ __BEGIN_DECLS */ #define PWM_LOWEST_MIN 90 -/** - * Default value for a shutdown motor - */ -#define PWM_MOTOR_OFF 900 - -/** - * Default minimum PWM in us - */ -#define PWM_DEFAULT_MIN 1000 - -/** - * Highest PWM allowed as the minimum PWM - */ -#define PWM_HIGHEST_MIN 1600 - /** * Highest maximum PWM in us */ #define PWM_HIGHEST_MAX 2500 -/** - * Default maximum PWM in us - */ -#define PWM_DEFAULT_MAX 2000 - -/** - * Default trim PWM in us - */ -#define PWM_DEFAULT_TRIM 0 - -/** - * Lowest PWM allowed as the maximum PWM - */ -#define PWM_LOWEST_MAX 200 - #endif // not PX4_PWM_ALTERNATE_RANGES /** diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index fa64e1d7d3..549b7313c1 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,6 +82,7 @@ #define DRV_IMU_DEVTYPE_IIM42652 0x2B #define DRV_IMU_DEVTYPE_IAM20680HP 0x2C #define DRV_IMU_DEVTYPE_ICM42686P 0x2D +#define DRV_IMU_DEVTYPE_IIM42653 0x2E #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 @@ -114,6 +115,7 @@ #define DRV_DIFF_PRESS_DEVTYPE_SDP32 0x4B #define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x4C + #define DRV_BARO_DEVTYPE_TCBP001TA 0x4D #define DRV_BARO_DEVTYPE_MS5837 0x4E #define DRV_BARO_DEVTYPE_SPL06 0x4F @@ -233,6 +235,7 @@ #define DRV_INS_DEVTYPE_VN100 0xE1 #define DRV_INS_DEVTYPE_VN200 0xE2 #define DRV_INS_DEVTYPE_VN300 0xE3 +#define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4 #define DRV_DEVTYPE_UNUSED 0xff diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index f0cb4458f4..4f314cebb7 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -143,7 +143,7 @@ private: void handle_vehicle_commands(); - MixingOutput _mixing_output {PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + MixingOutput _mixing_output{PARAM_PREFIX, DIRECT_PWM_OUTPUT_CHANNELS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; uint32_t _reversible_outputs{}; Telemetry *_telemetry{nullptr}; diff --git a/src/drivers/gpio/Kconfig b/src/drivers/gpio/Kconfig new file mode 100644 index 0000000000..902613ebbf --- /dev/null +++ b/src/drivers/gpio/Kconfig @@ -0,0 +1,9 @@ +menu "GPIO" + menuconfig COMMON_GPIO + bool "Common GPIOs" + default n + select DRIVERS_GPIO_MCP23009 + ---help--- + Enable default set of GPIO drivers + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/gpio/mcp23009/CMakeLists.txt b/src/drivers/gpio/mcp23009/CMakeLists.txt new file mode 100644 index 0000000000..c7897fea8e --- /dev/null +++ b/src/drivers/gpio/mcp23009/CMakeLists.txt @@ -0,0 +1,41 @@ +############################################################################ +# +# 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__mcp23009 + MAIN mcp23009 + COMPILE_FLAGS + SRCS + mcp23009_main.cpp + mcp23009.cpp + DEPENDS + ) diff --git a/src/drivers/gpio/mcp23009/Kconfig b/src/drivers/gpio/mcp23009/Kconfig new file mode 100644 index 0000000000..73078bf8eb --- /dev/null +++ b/src/drivers/gpio/mcp23009/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_GPIO_MCP23009 + bool "mcp23009" + default n + ---help--- + Enable support for mcp23009 diff --git a/src/drivers/gpio/mcp23009/mcp23009.cpp b/src/drivers/gpio/mcp23009/mcp23009.cpp new file mode 100644 index 0000000000..8d8eb92885 --- /dev/null +++ b/src/drivers/gpio/mcp23009/mcp23009.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * 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 "mcp23009.h" + +int MCP23009::read_reg(Register address, uint8_t &data) +{ + return transfer((uint8_t *)&address, 1, &data, 1); +} + +int MCP23009::write_reg(Register address, uint8_t value) +{ + uint8_t data[2] = {(uint8_t)address, value}; + return transfer(data, sizeof(data), nullptr, 0); +} + +int MCP23009::init(uint8_t direction, uint8_t state, uint8_t pull_up) +{ + // do I2C init (and probe) first + int ret = I2C::init(); + + if (ret != PX4_OK) { + PX4_ERR("I2C init failed"); + return ret; + } + + // buffer the new initial states + _iodir = direction; + _olat = state; + _gppu = pull_up; + + // Write the initial state to the device + ret = write_reg(Register::OLAT, _olat); + ret |= write_reg(Register::IODIR, _iodir); + ret |= write_reg(Register::GPPU, _gppu); + + if (ret != PX4_OK) { + PX4_ERR("Device init failed (%i)", ret); + return ret; + } + + return init_uorb(); +} + +int MCP23009::probe() +{ + // no whoami, try to read IOCON + uint8_t data; + return read_reg(Register::IOCON, data); +} + +int MCP23009::read(uint8_t *mask) +{ + return read_reg(Register::GPIO, *mask); +} + +int MCP23009::write(uint8_t mask_set, uint8_t mask_clear) +{ + // no need to read, we can use the buffered register value + _olat = (_olat & ~mask_clear) | mask_set; + return write_reg(Register::OLAT, _olat); +} + +int MCP23009::configure(uint8_t mask, PinType type) +{ + // no need to read, we can use the buffered register values + switch (type) { + case PinType::Input: + _iodir |= mask; + _gppu &= ~mask; + break; + + case PinType::InputPullUp: + _iodir |= mask; + _gppu |= mask; + break; + + case PinType::Output: + _iodir &= ~mask; + break; + + default: + return -EINVAL; + } + + int ret = write_reg(Register::GPPU, _gppu); + + if (ret != 0) { + return ret; + } + + return write_reg(Register::IODIR, _iodir); +} diff --git a/src/drivers/gpio/mcp23009/mcp23009.h b/src/drivers/gpio/mcp23009/mcp23009.h new file mode 100644 index 0000000000..fce68576c5 --- /dev/null +++ b/src/drivers/gpio/mcp23009/mcp23009.h @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class MCP23009 : public device::I2C, public I2CSPIDriver +{ +public: + MCP23009(const I2CSPIDriverConfig &config); + ~MCP23009() override; + + void RunImpl(); + + static void print_usage(); + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + +protected: + int init_uorb(); + int init(uint8_t direction, uint8_t state, uint8_t pull_up); + + int probe() override; + void print_status() override; + void exit_and_cleanup() override; + +private: + enum class + Register : uint8_t { + IODIR = 0x00, + IPOL = 0x01, + GPINTEN = 0x02, + DEFVAL = 0x03, + INTCON = 0x04, + IOCON = 0x05, + GPPU = 0x06, + INTF = 0x07, + INTCAP = 0x08, + GPIO = 0x09, + OLAT = 0x0a + }; + + enum class + PinType : uint8_t { + Output, + Input, + InputPullUp, + }; + + uORB::SubscriptionCallbackWorkItem _gpio_out_sub{this, ORB_ID(gpio_out)}; + uORB::SubscriptionCallbackWorkItem _gpio_request_sub{this, ORB_ID(gpio_request)}; + uORB::SubscriptionCallbackWorkItem _gpio_config_sub{this, ORB_ID(gpio_config)}; + + uORB::Publication _to_gpio_in{ORB_ID(gpio_in)}; + + perf_counter_t _cycle_perf; + + // buffer often used (write-only!) registers here + uint8_t _olat; + uint8_t _iodir; + uint8_t _gppu; + + int read(uint8_t *mask); + int write(uint8_t mask_set, uint8_t mask_clear); + int configure(uint8_t mask, PinType type); + + int read_reg(Register address, uint8_t &data); + int write_reg(Register address, uint8_t data); +}; diff --git a/src/drivers/gpio/mcp23009/mcp23009_main.cpp b/src/drivers/gpio/mcp23009/mcp23009_main.cpp new file mode 100644 index 0000000000..f5f9a2cfc4 --- /dev/null +++ b/src/drivers/gpio/mcp23009/mcp23009_main.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Driver for the MCP23009 connected via I2C. + */ + +#include "mcp23009.h" +#include + +MCP23009::MCP23009(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": single-sample")) +{ +} + +MCP23009::~MCP23009() +{ + ScheduleClear(); + perf_free(_cycle_perf); +} + +int MCP23009::init_uorb() +{ + if (!_gpio_config_sub.registerCallback() || + !_gpio_request_sub.registerCallback() || + !_gpio_out_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return -1; + } + + return PX4_OK; +} + +void MCP23009::exit_and_cleanup() +{ + _gpio_config_sub.unregisterCallback(); + _gpio_request_sub.unregisterCallback(); + _gpio_out_sub.unregisterCallback(); +} + +void MCP23009::RunImpl() +{ + perf_begin(_cycle_perf); + + gpio_config_s config; + + if (_gpio_config_sub.update(&config) && config.device_id == get_device_id()) { + PinType type = PinType::Input; + + switch (config.config) { + case config.INPUT_PULLUP: type = PinType::InputPullUp; break; + + case config.OUTPUT: type = PinType::Output; break; + } + + write(config.state, config.mask); + configure(config.mask, type); + } + + gpio_out_s output; + + if (_gpio_out_sub.update(&output) && output.device_id == get_device_id()) { + write(output.state, output.mask); + } + + // read every time we run, either when requested or when scheduled on interval + { + gpio_in_s _gpio_in; + _gpio_in.timestamp = hrt_absolute_time(); + _gpio_in.device_id = get_device_id(); + uint8_t input; + read(&input); + _gpio_in.state = input; + _to_gpio_in.publish(_gpio_in); + } + + perf_end(_cycle_perf); +} + +void MCP23009::print_usage() +{ + PRINT_MODULE_USAGE_NAME("MCP23009", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x25); + PRINT_MODULE_USAGE_PARAM_INT('D', 0, 0, 255, "Direction", true); + PRINT_MODULE_USAGE_PARAM_INT('O', 0, 0, 255, "Output", true); + PRINT_MODULE_USAGE_PARAM_INT('P', 0, 0, 255, "Pullups", true); + PRINT_MODULE_USAGE_PARAM_INT('U', 0, 0, 1000, "Update Interval [ms]", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +void MCP23009::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_cycle_perf); +} + +struct init_config_t { + uint16_t interval; + uint8_t direction; + uint8_t state; + uint8_t pullup; +}; + +I2CSPIDriverBase *MCP23009::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + auto *init = (const init_config_t *)config.custom_data; + auto *instance = new MCP23009(config); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init(init->direction, init->state, init->pullup)) { + delete instance; + return nullptr; + } + + if (init->interval) { + instance->ScheduleOnInterval(init->interval * 1000); + } + + return instance; +} + +extern "C" int mcp23009_main(int argc, char *argv[]) +{ + using ThisDriver = MCP23009; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = 400000; + cli.i2c_address = 0x25; + init_config_t config_data{}; + + int ch; + + while ((ch = cli.getOpt(argc, argv, "D:O:P:U:")) != EOF) { + switch (ch) { + case 'D': + config_data.direction = (int)strtol(cli.optArg(), nullptr, 0); + break; + + case 'O': + config_data.state = (int)strtol(cli.optArg(), nullptr, 0); + break; + + case 'P': + config_data.pullup = (int)strtol(cli.optArg(), nullptr, 0); + break; + + case 'U': + config_data.interval = atoi(cli.optArg()); + break; + } + } + + cli.custom_data = &config_data; + + const char *verb = cli.optArg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_GPIO_DEVTYPE_MCP23009); + + 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/gps/devices b/src/drivers/gps/devices index 99f5960eca..b443b79f57 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit 99f5960eca66150e33dc277e71a4c99187c27ddc +Subproject commit b443b79f57c5e89353430940af9e4511ea8eb0b8 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 008f4d3977..4ed29a8c6e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -203,7 +204,7 @@ private: const Instance _instance; - uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; uORB::Publication _gps_inject_data_pub{ORB_ID(gps_inject_data)}; uORB::Publication _dump_communication_pub{ORB_ID(gps_dump)}; gps_dump_s *_dump_to_device{nullptr}; @@ -530,13 +531,15 @@ void GPS::handleInjectDataTopic() // 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 (uint8_t i = 0; i < gps_inject_data_s::MAX_INSTANCES; i++) { - if (_orb_inject_data_sub.ChangeInstance(i)) { - if (_orb_inject_data_sub.copy(&msg)) { + for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { + const bool exists = _orb_inject_data_sub[instance].advertised(); + + if (exists) { + if (_orb_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 = i; + _selected_rtcm_instance = instance; break; } } @@ -544,9 +547,6 @@ void GPS::handleInjectDataTopic() } } - // Reset instance in case we didn't actually want to switch - _orb_inject_data_sub.ChangeInstance(_selected_rtcm_instance); - bool updated = already_copied; // Limit maximum number of GPS injections to 8 since usually @@ -574,7 +574,7 @@ void GPS::handleInjectDataTopic() } } - updated = _orb_inject_data_sub.update(&msg); + updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); } while (updated && num_injections < max_num_injections); } diff --git a/src/drivers/heater/heater.cpp b/src/drivers/heater/heater.cpp index 170bd859a4..aba6f7066e 100644 --- a/src/drivers/heater/heater.cpp +++ b/src/drivers/heater/heater.cpp @@ -231,7 +231,7 @@ void Heater::Run() _controller_time_on_usec = math::constrain(_controller_time_on_usec, 0, _controller_period_usec); - if (abs(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { + if (fabsf(temperature_delta) < TEMPERATURE_TARGET_THRESHOLD) { _temperature_target_met = true; } else { diff --git a/src/drivers/imu/adis16497/ADIS16497.cpp b/src/drivers/imu/adis16497/ADIS16497.cpp index 9b39f4a071..3d94c4c2c5 100644 --- a/src/drivers/imu/adis16497/ADIS16497.cpp +++ b/src/drivers/imu/adis16497/ADIS16497.cpp @@ -342,9 +342,9 @@ ADIS16497::read_reg16(uint8_t reg) cmd[0] = ((reg | DIR_READ) << 8) & 0xff00; transferhword(cmd, nullptr, 1); - up_udelay(T_STALL); + px4_udelay(T_STALL); transferhword(nullptr, cmd, 1); - up_udelay(T_STALL); + px4_udelay(T_STALL); return cmd[0]; } @@ -367,9 +367,9 @@ ADIS16497::write_reg16(uint8_t reg, uint16_t value) cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8); transferhword(cmd, nullptr, 1); - up_udelay(T_STALL); + px4_udelay(T_STALL); transferhword(cmd + 1, nullptr, 1); - up_udelay(T_STALL); + px4_udelay(T_STALL); } void diff --git a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp index 0aa216c940..c12e8d356d 100644 --- a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp +++ b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp @@ -283,6 +283,7 @@ void ADIS16507::RunImpl() if (buffer.checksum != checksum) { //PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", buffer.checksum, checksum); perf_count(_bad_transfer_perf); + perf_count(_perf_crc_bad); } if (buffer.DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) { diff --git a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp index d2ea514530..cf8a2495ca 100644 --- a/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi055/BMI055_Gyroscope.cpp @@ -33,8 +33,6 @@ #include "BMI055_Gyroscope.hpp" -#include - using namespace time_literals; namespace Bosch::BMI055::Gyroscope diff --git a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp index 5c8714712f..d945a815f9 100644 --- a/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp +++ b/src/drivers/imu/bosch/bmi088/BMI088_Gyroscope.cpp @@ -33,8 +33,6 @@ #include "BMI088_Gyroscope.hpp" -#include - using namespace time_literals; namespace Bosch::BMI088::Gyroscope diff --git a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt b/src/drivers/imu/invensense/iim42653/CMakeLists.txt similarity index 82% rename from src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt rename to src/drivers/imu/invensense/iim42653/CMakeLists.txt index e9f9c54232..4f0e7a7986 100644 --- a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt +++ b/src/drivers/imu/invensense/iim42653/CMakeLists.txt @@ -30,15 +30,19 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ - - -px4_add_library(ArmStateMachine - ArmStateMachine.cpp -) -target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks) - -px4_add_functional_gtest(SRC ArmStateMachineTest.cpp - LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util +px4_add_module( + MODULE drivers__imu__invensense__iim42653 + MAIN iim42653 + COMPILE_FLAGS + ${MAX_CUSTOM_OPT_LEVEL} + #-DDEBUG_BUILD + SRCS + iim42653_main.cpp + IIM42653.cpp + IIM42653.hpp + InvenSense_IIM42653_registers.hpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope ) - diff --git a/src/drivers/imu/invensense/iim42653/IIM42653.cpp b/src/drivers/imu/invensense/iim42653/IIM42653.cpp new file mode 100644 index 0000000000..d5f1f57c0c --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/IIM42653.cpp @@ -0,0 +1,861 @@ +/**************************************************************************** + * + * 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 "IIM42653.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +static constexpr uint16_t combine_uint(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +IIM42653::IIM42653(const I2CSPIDriverConfig &config) : + SPI(config), + I2CSPIDriver(config), + _drdy_gpio(config.drdy_gpio), + _px4_accel(get_device_id(), config.rotation), + _px4_gyro(get_device_id(), config.rotation) +{ + if (config.drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + if (config.custom1 != 0) { + _enable_clock_input = true; + _input_clock_freq = config.custom1; + ConfigureCLKIN(); + + } else { + _enable_clock_input = false; + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +IIM42653::~IIM42653() +{ + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_missed_perf); +} + +int IIM42653::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool IIM42653::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void IIM42653::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void IIM42653::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + PX4_INFO("Clock input: %s", _enable_clock_input ? "enabled" : "disabled"); + + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_missed_perf); +} + +int IIM42653::probe() +{ + for (int i = 0; i < 3; i++) { + uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami == WHOAMI) { + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + + uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL); + int bank = reg_bank_sel >> 4; + + if (bank >= 1 && bank <= 3) { + DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank); + // force bank selection and retry + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0, true); + } + } + } + + return PX4_ERROR; +} + +void IIM42653::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // DEVICE_CONFIG: Software reset configuration + RegisterWrite(Register::BANK_0::DEVICE_CONFIG, DEVICE_CONFIG_BIT::SOFT_RESET_CONFIG); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(1_ms); // wait 1 ms for soft reset to be effective + break; + + case STATE::WAIT_FOR_RESET: + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::BANK_0::DEVICE_CONFIG) == 0x00) + && (RegisterRead(Register::BANK_0::INT_STATUS) & INT_STATUS_BIT::RESET_DONE_INT)) { + + // Wakeup accel and gyro and schedule remaining configuration + RegisterWrite(Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE); + _state = STATE::CONFIGURE; + ScheduleDelayed(30_ms); // 30 ms gyro startup time, 10 ms accel from sleep to valid data + + } else { + // RESET not complete + 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::CONFIGURE: + if (Configure()) { + // if configure succeeded then reset the FIFO + _state = STATE::FIFO_RESET; + ScheduleDelayed(1_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::FIFO_RESET: + + _state = STATE::FIFO_READ; + FIFOReset(); + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + break; + + case STATE::FIFO_READ: { + hrt_abstime timestamp_sample = now; + uint8_t 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); + + if ((now - drdy_timestamp_sample) < _fifo_empty_interval_us) { + timestamp_sample = drdy_timestamp_sample; + samples = _fifo_gyro_samples; + + } else { + perf_count(_drdy_missed_perf); + } + + // 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) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_count == 0) { + perf_count(_fifo_empty_perf); + + } else { + // FIFO count (size in bytes) + samples = (fifo_count / sizeof(FIFO::DATA)); + + // 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 (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; + } + } + } + + bool success = false; + + if (samples >= 1) { + if (FIFORead(timestamp_sample, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0]) + && RegisterCheck(_register_bank1_cfg[_checked_register_bank1]) + && RegisterCheck(_register_bank2_cfg[_checked_register_bank2]) + ) { + _last_config_check_timestamp = now; + _checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg; + _checked_register_bank1 = (_checked_register_bank1 + 1) % size_register_bank1_cfg; + _checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +void IIM42653::ConfigureSampleRate(int sample_rate) +{ + // round down to nearest FIFO sample dt + const float min_interval = FIFO_SAMPLE_DT; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); + + ConfigureFIFOWatermark(_fifo_gyro_samples); +} + +void IIM42653::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; + + } else if (r.reg == Register::BANK_0::FIFO_CONFIG3) { + // FIFO_WM[11:8] FIFO_CONFIG3 + r.set_bits = (fifo_watermark_threshold >> 8) & 0x0F; + } + } +} + +void IIM42653::ConfigureCLKIN() +{ + for (auto &r0 : _register_bank0_cfg) { + if (r0.reg == Register::BANK_0::INTF_CONFIG1) { + r0.set_bits = INTF_CONFIG1_BIT::RTC_MODE; + r0.set_bits = INTF_CONFIG1_BIT::CLKSEL; + r0.clear_bits = INTF_CONFIG1_BIT::CLKSEL_CLEAR; + } + } + + for (auto &r1 : _register_bank1_cfg) { + if (r1.reg == Register::BANK_1::INTF_CONFIG5) { + r1.set_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_SET; + r1.clear_bits = INTF_CONFIG5_BIT::PIN9_FUNCTION_CLKIN_CLEAR; + } + } +} + +void IIM42653::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force) +{ + if (bank != _last_register_bank || force) { + // select BANK_0 + uint8_t cmd_bank_sel[2] {}; + cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); + cmd_bank_sel[1] = bank; + transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel)); + + _last_register_bank = bank; + } +} + +bool IIM42653::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_bank0_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_bank1_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_bank2_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + for (const auto ®_cfg : _register_bank1_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + for (const auto ®_cfg : _register_bank2_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // 20-bits data format used + // the only FSR settings that are operational are ±2000dps for gyroscope and ±16g for accelerometer + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + _px4_gyro.set_range(math::radians(2000.f)); + + return success; +} + +int IIM42653::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void IIM42653::DataReady() +{ + _drdy_timestamp_sample.store(hrt_absolute_time()); + ScheduleNow(); +} + +bool IIM42653::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool IIM42653::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +template +bool IIM42653::RegisterCheck(const T ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +template +uint8_t IIM42653::RegisterRead(T reg) +{ + uint8_t cmd[2] {}; + cmd[0] = static_cast(reg) | DIR_READ; + SelectRegisterBank(reg); + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +template +void IIM42653::RegisterWrite(T reg, uint8_t value) +{ + uint8_t cmd[2] { (uint8_t)reg, value }; + SelectRegisterBank(reg); + transfer(cmd, cmd, sizeof(cmd)); +} + +template +void IIM42653::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t IIM42653::FIFOReadCount() +{ + // read FIFO count + uint8_t fifo_count_buf[3] {}; + fifo_count_buf[0] = static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ; + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); + + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool IIM42653::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 4, FIFO::SIZE); + SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + if (buffer.INT_STATUS & INT_STATUS_BIT::FIFO_FULL_INT) { + perf_count(_fifo_overflow_perf); + FIFOReset(); + 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++) { + bool valid = true; + + // With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx + const uint8_t FIFO_HEADER = buffer.f[i].FIFO_Header; + + if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_MSG) { + // FIFO sample empty if HEADER_MSG set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ACCEL)) { + // accel bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_GYRO)) { + // gyro bit not set + valid = false; + + } else if (!(FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_20)) { + // Packet does not contain a new and valid extended 20-bit data + valid = false; + + } else if ((FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_TIMESTAMP_FSYNC) != Bit3) { + // Packet does not contain ODR timestamp + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_ACCEL) { + // accel ODR changed + valid = false; + + } else if (FIFO_HEADER & FIFO::FIFO_HEADER_BIT::HEADER_ODR_GYRO) { + // gyro ODR changed + valid = false; + } + + if (valid) { + valid_samples++; + + } else { + perf_count(_bad_transfer_perf); + break; + } + } + + if (valid_samples > 0) { + if (ProcessTemperature(buffer.f, valid_samples)) { + ProcessGyro(timestamp_sample, buffer.f, valid_samples); + ProcessAccel(timestamp_sample, buffer.f, valid_samples); + return true; + } + } + + return false; +} + +void IIM42653::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // SIGNAL_PATH_RESET: FIFO flush + RegisterSetBits(Register::BANK_0::SIGNAL_PATH_RESET, SIGNAL_PATH_RESET_BIT::FIFO_FLUSH); + + // reset while FIFO is disabled + _drdy_timestamp_sample.store(0); +} + +static constexpr int32_t reassemble_20bit(const uint32_t a, const uint32_t b, const uint32_t c) +{ + // 0xXXXAABBC + uint32_t high = ((a << 12) & 0x000FF000); + uint32_t low = ((b << 4) & 0x00000FF0); + uint32_t lowest = (c & 0x0000000F); + + uint32_t x = high | low | lowest; + + if (a & Bit7) { + // sign extend + x |= 0xFFF00000u; + } + + return static_cast(x); +} + +void IIM42653::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + + // 18-bits of accelerometer data + bool scale_20bit = false; + + // first pass + for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + accel.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + accel.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + + // 20 bit hires mode + // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) + // Accel data is 18 bit () + int32_t accel_x = reassemble_20bit(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0, + fifo[i].Ext_Accel_X_Gyro_X & 0xF0 >> 4); + int32_t accel_y = reassemble_20bit(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0, + fifo[i].Ext_Accel_Y_Gyro_Y & 0xF0 >> 4); + int32_t accel_z = reassemble_20bit(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0, + fifo[i].Ext_Accel_Z_Gyro_Z & 0xF0 >> 4); + + // sample invalid if -524288 + if (accel_x != -524288 && accel_y != -524288 && accel_z != -524288) { + // check if any values are going to exceed int16 limits + static constexpr int16_t max_accel = INT16_MAX; + static constexpr int16_t min_accel = INT16_MIN; + + if (accel_x >= max_accel || accel_x <= min_accel) { + scale_20bit = true; + } + + if (accel_y >= max_accel || accel_y <= min_accel) { + scale_20bit = true; + } + + if (accel_z >= max_accel || accel_z <= min_accel) { + scale_20bit = true; + } + + // shift by 2 (2 least significant bits are always 0) + accel.x[accel.samples] = accel_x / 4; + accel.y[accel.samples] = accel_y / 4; + accel.z[accel.samples] = accel_z / 4; + accel.samples++; + } + } + + if (!scale_20bit) { + // if highres enabled accel data is always 4096 LSB/g + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); + + } else { + // 20 bit data scaled to 16 bit (2^4) + for (int i = 0; i < samples; i++) { + // 20 bit hires mode + // Sign extension + Accel [19:12] + Accel [11:4] + Accel [3:2] (20 bit extension byte) + // Accel data is 18 bit () + int16_t accel_x = combine(fifo[i].ACCEL_DATA_X1, fifo[i].ACCEL_DATA_X0); + int16_t accel_y = combine(fifo[i].ACCEL_DATA_Y1, fifo[i].ACCEL_DATA_Y0); + int16_t accel_z = combine(fifo[i].ACCEL_DATA_Z1, fifo[i].ACCEL_DATA_Z0); + + accel.x[i] = accel_x; + accel.y[i] = accel_y; + accel.z[i] = accel_z; + } + + _px4_accel.set_scale(CONSTANTS_ONE_G / 1024.f); + } + + // correct frame for publication + for (int i = 0; i < accel.samples; i++) { + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[i] = accel.x[i]; + accel.y[i] = (accel.y[i] == INT16_MIN) ? INT16_MAX : -accel.y[i]; + accel.z[i] = (accel.z[i] == INT16_MIN) ? INT16_MAX : -accel.z[i]; + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + } +} + +void IIM42653::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = 0; + + // 20-bits of gyroscope data + bool scale_20bit = false; + + // first pass + for (int i = 0; i < samples; i++) { + + uint16_t timestamp_fifo = combine_uint(fifo[i].TimeStamp_h, fifo[i].TimeStamp_l); + + if (_enable_clock_input) { + gyro.dt = (float)timestamp_fifo * ((1.f / _input_clock_freq) * 1e6f); + + } else { + gyro.dt = (float)timestamp_fifo * FIFO_TIMESTAMP_SCALING; + } + + // 20 bit hires mode + // Gyro [19:12] + Gyro [11:4] + Gyro [3:0] (bottom 4 bits of 20 bit extension byte) + int32_t gyro_x = reassemble_20bit(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0, fifo[i].Ext_Accel_X_Gyro_X & 0x0F); + int32_t gyro_y = reassemble_20bit(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0, fifo[i].Ext_Accel_Y_Gyro_Y & 0x0F); + int32_t gyro_z = reassemble_20bit(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0, fifo[i].Ext_Accel_Z_Gyro_Z & 0x0F); + + // check if any values are going to exceed int16 limits + static constexpr int16_t max_gyro = INT16_MAX; + static constexpr int16_t min_gyro = INT16_MIN; + + if (gyro_x >= max_gyro || gyro_x <= min_gyro) { + scale_20bit = true; + } + + if (gyro_y >= max_gyro || gyro_y <= min_gyro) { + scale_20bit = true; + } + + if (gyro_z >= max_gyro || gyro_z <= min_gyro) { + scale_20bit = true; + } + + gyro.x[gyro.samples] = gyro_x / 2; + gyro.y[gyro.samples] = gyro_y / 2; + gyro.z[gyro.samples] = gyro_z / 2; + gyro.samples++; + } + + if (!scale_20bit) { + // if highres enabled gyro data is always 65.5 LSB/dps + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); + + } else { + // 20 bit data scaled to 16 bit (2^4) + for (int i = 0; i < samples; i++) { + gyro.x[i] = combine(fifo[i].GYRO_DATA_X1, fifo[i].GYRO_DATA_X0); + gyro.y[i] = combine(fifo[i].GYRO_DATA_Y1, fifo[i].GYRO_DATA_Y0); + gyro.z[i] = combine(fifo[i].GYRO_DATA_Z1, fifo[i].GYRO_DATA_Z0); + } + + _px4_gyro.set_scale(math::radians(4000.f / 32768.f)); + } + + // correct frame for publication + for (int i = 0; i < gyro.samples; i++) { + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + gyro.x[i] = gyro.x[i]; + gyro.y[i] = (gyro.y[i] == INT16_MIN) ? INT16_MAX : -gyro.y[i]; + gyro.z[i] = (gyro.z[i] == INT16_MIN) ? INT16_MAX : -gyro.z[i]; + } + + _px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (gyro.samples > 0) { + _px4_gyro.updateFIFO(gyro); + } +} + +bool IIM42653::ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples) +{ + int16_t temperature[FIFO_MAX_SAMPLES]; + float temperature_sum{0}; + + int valid_samples = 0; + + for (int i = 0; i < samples; i++) { + const int16_t t = combine(fifo[i].TEMP_DATA1, fifo[i].TEMP_DATA0); + + // sample invalid if -32768 + if (t != -32768) { + temperature_sum += t; + temperature[valid_samples] = t; + valid_samples++; + } + } + + if (valid_samples > 0) { + const float temperature_avg = temperature_sum / valid_samples; + + for (int i = 0; i < valid_samples; i++) { + // temperature changing wildly is an indication of a transfer error + if (fabsf(temperature[i] - temperature_avg) > 1000) { + perf_count(_bad_transfer_perf); + return false; + } + } + + // use average temperature reading + const float TEMP_degC = (temperature_avg / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + return true; + + } else { + perf_count(_bad_transfer_perf); + } + } + + return false; +} diff --git a/src/drivers/imu/invensense/iim42653/IIM42653.hpp b/src/drivers/imu/invensense/iim42653/IIM42653.hpp new file mode 100644 index 0000000000..ab9c551a7a --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/IIM42653.hpp @@ -0,0 +1,227 @@ +/**************************************************************************** + * + * 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 IIM42653.hpp + * + * Driver for the Invensense IIM42653 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_IIM42653_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_IIM42653; + +class IIM42653 : public device::SPI, public I2CSPIDriver +{ +public: + IIM42653(const I2CSPIDriverConfig &config); + ~IIM42653() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // Sensor Configuration + static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f}; // 8000 Hz accel & gyro ODR configured + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; + static constexpr float ACCEL_RATE{1e6f / FIFO_SAMPLE_DT}; + + static constexpr float FIFO_TIMESTAMP_SCALING{16.f *(32.f / 30.f)}; // Used when not using clock input + + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo + static constexpr int32_t FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0]), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::BANK_0::INT_STATUS) | DIR_READ}; + uint8_t INT_STATUS{0}; + uint8_t FIFO_COUNTH{0}; + uint8_t FIFO_COUNTL{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (4 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank1_config_t { + Register::BANK_1 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank2_config_t { + Register::BANK_2 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + void ConfigureSampleRate(int sample_rate); + void ConfigureFIFOWatermark(uint8_t samples); + void ConfigureCLKIN(); + + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false); + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_0); } + void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_1); } + void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::BANK_SEL_2); } + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + template bool RegisterCheck(const T ®_cfg); + template uint8_t RegisterRead(T reg); + template void RegisterWrite(T reg, uint8_t value); + template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); + template void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + template void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + void ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + bool ProcessTemperature(const FIFO::DATA fifo[], const uint8_t samples); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")}; + perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")}; + perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")}; + perf_counter_t _drdy_missed_perf{nullptr}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + bool _enable_clock_input{false}; + float _input_clock_freq{0.f}; + + enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::BANK_SEL_0}; + + px4::atomic _drdy_timestamp_sample{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_RESET, + FIFO_READ, + } _state{STATE::RESET}; + + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval + int32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; + + uint8_t _checked_register_bank0{0}; + static constexpr uint8_t size_register_bank0_cfg{16}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::INT_CONFIG, INT_CONFIG_BIT::INT1_MODE | INT_CONFIG_BIT::INT1_DRIVE_CIRCUIT, INT_CONFIG_BIT::INT1_POLARITY }, + { Register::BANK_0::FIFO_CONFIG, FIFO_CONFIG_BIT::FIFO_MODE_STOP_ON_FULL, 0 }, + { Register::BANK_0::INTF_CONFIG1, 0, 0}, // RTC_MODE[2] set at runtime + { Register::BANK_0::PWR_MGMT0, PWR_MGMT0_BIT::GYRO_MODE_LOW_NOISE | PWR_MGMT0_BIT::ACCEL_MODE_LOW_NOISE, 0 }, + { Register::BANK_0::GYRO_CONFIG0, GYRO_CONFIG0_BIT::GYRO_FS_SEL_4000_DPS | GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_SET, GYRO_CONFIG0_BIT::GYRO_ODR_8KHZ_CLEAR }, + { Register::BANK_0::ACCEL_CONFIG0, ACCEL_CONFIG0_BIT::ACCEL_FS_SEL_32G | ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_SET, ACCEL_CONFIG0_BIT::ACCEL_ODR_8KHZ_CLEAR }, + { Register::BANK_0::GYRO_CONFIG1, 0, GYRO_CONFIG1_BIT::GYRO_UI_FILT_ORD }, + { Register::BANK_0::GYRO_ACCEL_CONFIG0, 0, GYRO_ACCEL_CONFIG0_BIT::ACCEL_UI_FILT_BW | GYRO_ACCEL_CONFIG0_BIT::GYRO_UI_FILT_BW }, + { Register::BANK_0::ACCEL_CONFIG1, 0, ACCEL_CONFIG1_BIT::ACCEL_UI_FILT_ORD }, + { Register::BANK_0::TMST_CONFIG, TMST_CONFIG_BIT::TMST_EN | TMST_CONFIG_BIT::TMST_DELTA_EN | TMST_CONFIG_BIT::TMST_TO_REGS_EN | TMST_CONFIG_BIT::TMST_RES, TMST_CONFIG_BIT::TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG1, FIFO_CONFIG1_BIT::FIFO_WM_GT_TH | FIFO_CONFIG1_BIT::FIFO_HIRES_EN | FIFO_CONFIG1_BIT::FIFO_TEMP_EN | FIFO_CONFIG1_BIT::FIFO_GYRO_EN | FIFO_CONFIG1_BIT::FIFO_ACCEL_EN, FIFO_CONFIG1_BIT::FIFO_TMST_FSYNC_EN }, + { Register::BANK_0::FIFO_CONFIG2, 0, 0 }, // FIFO_WM[7:0] set at runtime + { Register::BANK_0::FIFO_CONFIG3, 0, 0 }, // FIFO_WM[11:8] set at runtime + { Register::BANK_0::INT_CONFIG0, INT_CONFIG0_BIT::CLEAR_ON_FIFO_READ, 0 }, + { Register::BANK_0::INT_CONFIG1, 0, INT_CONFIG1_BIT::INT_ASYNC_RESET }, + { Register::BANK_0::INT_SOURCE0, INT_SOURCE0_BIT::FIFO_THS_INT1_EN, 0 }, + }; + + uint8_t _checked_register_bank1{0}; + static constexpr uint8_t size_register_bank1_cfg{5}; + register_bank1_config_t _register_bank1_cfg[size_register_bank1_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_1::GYRO_CONFIG_STATIC2, 0, GYRO_CONFIG_STATIC2_BIT::GYRO_NF_DIS | GYRO_CONFIG_STATIC2_BIT::GYRO_AAF_DIS }, + { Register::BANK_1::GYRO_CONFIG_STATIC3, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_SET, GYRO_CONFIG_STATIC3_BIT::GYRO_AAF_DELT_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC4, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_SET, GYRO_CONFIG_STATIC4_BIT::GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR}, + { Register::BANK_1::GYRO_CONFIG_STATIC5, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_SET | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_SET, GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_BITSHIFT_585HZ_CLEAR | GYRO_CONFIG_STATIC5_BIT::GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR}, + { Register::BANK_1::INTF_CONFIG5, 0, 0 }, + }; + + uint8_t _checked_register_bank2{0}; + static constexpr uint8_t size_register_bank2_cfg{8}; + register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_2::ACCEL_CONFIG_STATIC2, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_SET, ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DELT_585HZ_CLEAR | ACCEL_CONFIG_STATIC2_BIT::ACCEL_AAF_DIS }, + { Register::BANK_2::ACCEL_CONFIG_STATIC3, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_SET, ACCEL_CONFIG_STATIC3_BIT::ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR }, + { Register::BANK_2::ACCEL_CONFIG_STATIC4, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_SET | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_SET, ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_BITSHIFT_585HZ_CLEAR | ACCEL_CONFIG_STATIC4_BIT::ACCEL_AAF_DELTSQR_MSB_CLEAR }, + { Register::BANK_2::AUX1_CONFIG1, 0, AUX1_CONFIG1_BIT::AUX1_ACCEL_LP_CLK_SEL | AUX1_CONFIG1_BIT::GYRO_AUX1_EN | AUX1_CONFIG1_BIT::ACCEL_AUX1_EN}, + { Register::BANK_2::AUX1_CONFIG2, AUX1_CONFIG2_BIT::GYRO_AUX1_HPF_DIS, 0}, + { Register::BANK_2::AUX1_SPI_REG1, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_SET, AUX1_SPI_REG1_BIT::AUX1_SPI_REG1_CLEAR }, + { Register::BANK_2::AUX1_SPI_REG2, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_SET, AUX1_SPI_REG2_BIT::AUX1_SPI_REG2_CLEAR }, + { Register::BANK_2::AUX1_SPI_REG3, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_SET, AUX1_SPI_REG3_BIT::AUX1_SPI_REG3_CLEAR }, + }; +}; diff --git a/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp b/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp new file mode 100644 index 0000000000..7bd183787f --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/InvenSense_IIM42653_registers.hpp @@ -0,0 +1,453 @@ +/**************************************************************************** + * + * 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 InvenSense_IIM42653_registers.hpp + * + * Invensense IIM-42653 registers. + * + */ + +#pragma once + +#include +#include + +namespace InvenSense_IIM42653 +{ +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 24 * 1000 * 1000; // 24 MHz SPI +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0x56; + +static constexpr float TEMPERATURE_SENSITIVITY = 132.48f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 25.f; // C + +namespace Register +{ + +enum class BANK_0 : uint8_t { + DEVICE_CONFIG = 0x11, + + INT_CONFIG = 0x14, + + FIFO_CONFIG = 0x16, + + TEMP_DATA1 = 0x1D, + TEMP_DATA0 = 0x1E, + + INT_STATUS = 0x2D, + FIFO_COUNTH = 0x2E, + FIFO_COUNTL = 0x2F, + FIFO_DATA = 0x30, + + SIGNAL_PATH_RESET = 0x4B, + INTF_CONFIG0 = 0x4C, + INTF_CONFIG1 = 0x4D, + PWR_MGMT0 = 0x4E, + GYRO_CONFIG0 = 0x4F, + ACCEL_CONFIG0 = 0x50, + GYRO_CONFIG1 = 0x51, + GYRO_ACCEL_CONFIG0 = 0x52, + ACCEL_CONFIG1 = 0x53, + TMST_CONFIG = 0x54, + + FIFO_CONFIG1 = 0x5F, + FIFO_CONFIG2 = 0x60, + FIFO_CONFIG3 = 0x61, + + INT_CONFIG0 = 0x63, + INT_CONFIG1 = 0x64, + + INT_SOURCE0 = 0x65, + + SELF_TEST_CONFIG = 0x70, + + WHO_AM_I = 0x75, + REG_BANK_SEL = 0x76, +}; + +enum class BANK_1 : uint8_t { + GYRO_CONFIG_STATIC2 = 0x0B, + GYRO_CONFIG_STATIC3 = 0x0C, + GYRO_CONFIG_STATIC4 = 0x0D, + GYRO_CONFIG_STATIC5 = 0x0E, + + INTF_CONFIG5 = 0x7B, +}; + +enum class BANK_2 : uint8_t { + ACCEL_CONFIG_STATIC2 = 0x03, + ACCEL_CONFIG_STATIC3 = 0x04, + ACCEL_CONFIG_STATIC4 = 0x05, + AUX1_CONFIG1 = 0x44, + AUX1_CONFIG2 = 0x45, + AUX1_CONFIG3 = 0x46, + AUX1_SPI_REG1 = 0x71, + AUX1_SPI_REG2 = 0x72, + AUX1_SPI_REG3 = 0x73, +}; + +}; + +//---------------- BANK0 Register bits + +// DEVICE_CONFIG +enum DEVICE_CONFIG_BIT : uint8_t { + SOFT_RESET_CONFIG = Bit0, // +}; + +// INT_CONFIG +enum INT_CONFIG_BIT : uint8_t { + INT1_MODE = Bit2, + INT1_DRIVE_CIRCUIT = Bit1, + INT1_POLARITY = Bit0, +}; + +// FIFO_CONFIG +enum FIFO_CONFIG_BIT : uint8_t { + // 7:6 FIFO_MODE + FIFO_MODE_STOP_ON_FULL = Bit7 | Bit6, // 11: STOP-on-FULL Mode +}; + +// INT_STATUS +enum INT_STATUS_BIT : uint8_t { + RESET_DONE_INT = Bit4, + DATA_RDY_INT = Bit3, + FIFO_THS_INT = Bit2, + FIFO_FULL_INT = Bit1, +}; + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + ABORT_AND_RESET = Bit3, + FIFO_FLUSH = Bit1, +}; + +enum INTF_CONFIG1_BIT : uint8_t { + RTC_MODE = Bit2, // 0: No input RTC clock is required, 1: RTC clock input is required + CLKSEL = Bit0, + CLKSEL_CLEAR = Bit1, +}; + +// PWR_MGMT0 +enum PWR_MGMT0_BIT : uint8_t { + GYRO_MODE_LOW_NOISE = Bit3 | Bit2, // 11: Places gyroscope in Low Noise (LN) Mode + ACCEL_MODE_LOW_NOISE = Bit1 | Bit0, // 11: Places accelerometer in Low Noise (LN) Mode +}; + +// GYRO_CONFIG0 +enum GYRO_CONFIG0_BIT : uint8_t { + // 7:5 GYRO_FS_SEL + GYRO_FS_SEL_4000_DPS = 0, // 0b000 = ±4000dps (default) + GYRO_FS_SEL_2000_DPS = Bit5, + GYRO_FS_SEL_1000_DPS = Bit6, + GYRO_FS_SEL_500_DPS = Bit6 | Bit5, + GYRO_FS_SEL_250_DPS = Bit7, + + + // 3:0 GYRO_ODR + // 0001: 32kHz + GYRO_ODR_32KHZ_SET = Bit0, + GYRO_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + GYRO_ODR_16KHZ_SET = Bit1, + GYRO_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + GYRO_ODR_8KHZ_SET = Bit1 | Bit0, + GYRO_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + GYRO_ODR_1KHZ_SET = Bit2 | Bit1, + GYRO_ODR_1KHZ_CLEAR = Bit3 | Bit0, +}; + +// ACCEL_CONFIG0 +enum ACCEL_CONFIG0_BIT : uint8_t { + // 7:5 ACCEL_FS_SEL + ACCEL_FS_SEL_32G = 0, // 000: ±32g (default) + ACCEL_FS_SEL_16G = Bit5, + ACCEL_FS_SEL_8G = Bit6, + ACCEL_FS_SEL_4G = Bit6 | Bit5, + + + // 3:0 ACCEL_ODR + // 0001: 32kHz + ACCEL_ODR_32KHZ_SET = Bit0, + ACCEL_ODR_32KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0010: 16kHz + ACCEL_ODR_16KHZ_SET = Bit1, + ACCEL_ODR_16KHZ_CLEAR = Bit3 | Bit2 | Bit0, + // 0011: 8kHz + ACCEL_ODR_8KHZ_SET = Bit1 | Bit0, + ACCEL_ODR_8KHZ_CLEAR = Bit3 | Bit2, + // 0110: 1kHz (default) + ACCEL_ODR_1KHZ_SET = Bit2 | Bit1, + ACCEL_ODR_1KHZ_CLEAR = Bit3 | Bit0, +}; + +// GYRO_CONFIG1 +enum GYRO_CONFIG1_BIT : uint8_t { + GYRO_UI_FILT_ORD = Bit3 | Bit2, // 00: 1st Order +}; + +// GYRO_ACCEL_CONFIG0 +enum GYRO_ACCEL_CONFIG0_BIT : uint8_t { + // 7:4 ACCEL_UI_FILT_BW + ACCEL_UI_FILT_BW = Bit7 | Bit6 | Bit5 | Bit4, // 0: BW=ODR/2 + + // 3:0 GYRO_UI_FILT_BW + GYRO_UI_FILT_BW = Bit3 | Bit2 | Bit1 | Bit0, // 0: BW=ODR/2 +}; + +// ACCEL_CONFIG1 +enum ACCEL_CONFIG1_BIT : uint8_t { + ACCEL_UI_FILT_ORD = Bit4 | Bit3, // 00: 1st Order +}; + +// TMST_CONFIG +enum TMST_CONFIG_BIT : uint8_t { + TMST_TO_REGS_EN = Bit4, // 1: TMST_VALUE[19:0] read returns timestamp value + TMST_RES = Bit3, // 0: 1us resolution, 1: 16us resolution + TMST_DELTA_EN = Bit2, // 1: Time Stamp delta enable + TMST_FSYNC_EN = Bit1, // 1: The contents of the Timestamp feature of FSYNC is enabled + TMST_EN = Bit0, // 1: Time Stamp register enable (default) +}; + +// FIFO_CONFIG1 +enum FIFO_CONFIG1_BIT : uint8_t { + FIFO_RESUME_PARTIAL_RD = Bit6, + FIFO_WM_GT_TH = Bit5, + FIFO_HIRES_EN = Bit4, + FIFO_TMST_FSYNC_EN = Bit3, + FIFO_TEMP_EN = Bit2, + FIFO_GYRO_EN = Bit1, + FIFO_ACCEL_EN = Bit0, +}; + +// INT_CONFIG0 +enum INT_CONFIG0_BIT : uint8_t { + // 3:2 FIFO_THS_INT_CLEAR + CLEAR_ON_FIFO_READ = Bit3, +}; + +// INT_CONFIG1 +enum INT_CONFIG1_BIT : uint8_t { + INT_ASYNC_RESET = Bit4, +}; + +// INT_SOURCE0 +enum INT_SOURCE0_BIT : uint8_t { + UI_FSYNC_INT1_EN = Bit6, + PLL_RDY_INT1_EN = Bit5, + RESET_DONE_INT1_EN = Bit4, + UI_DRDY_INT1_EN = Bit3, + FIFO_THS_INT1_EN = Bit2, // FIFO threshold interrupt routed to INT1 + FIFO_FULL_INT1_EN = Bit1, + UI_AGC_RDY_INT1_EN = Bit0, +}; + +// REG_BANK_SEL +enum REG_BANK_SEL_BIT : uint8_t { + // 2:0 BANK_SEL + BANK_SEL_0 = 0, // 000: Bank 0 (default) + BANK_SEL_1 = Bit0, // 001: Bank 1 + BANK_SEL_2 = Bit1, // 010: Bank 2 + BANK_SEL_3 = Bit1 | Bit0, // 011: Bank 3 + BANK_SEL_4 = Bit2, // 100: Bank 4 +}; + + +//---------------- BANK1 Register bits + +// GYRO_CONFIG_STATIC2 +enum GYRO_CONFIG_STATIC2_BIT : uint8_t { + GYRO_AAF_DIS = Bit1, // 1: Disable gyroscope anti-aliasing filter + GYRO_NF_DIS = Bit0, // 1: Disable Notch Filter +}; + +// GYRO_CONFIG_STATIC3 +enum GYRO_CONFIG_STATIC3_BIT : uint8_t { + // 5:0 GYRO_AAF_DELT + // 585 Hz = 13 (0b00'1101) + GYRO_AAF_DELT_585HZ_SET = Bit3 | Bit2 | Bit0, + GYRO_AAF_DELT_585HZ_CLEAR = Bit5 | Bit4 | Bit1, +}; + +// GYRO_CONFIG_STATIC4 +enum GYRO_CONFIG_STATIC4_BIT : uint8_t { + // 7:0 GYRO_AAF_DELTSQR + // 585 Hz = 170 (0b1010'1010) + GYRO_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + GYRO_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// GYRO_CONFIG_STATIC5 +enum GYRO_CONFIG_STATIC5_BIT : uint8_t { + // 7:4 GYRO_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + GYRO_AAF_BITSHIFT_585HZ_SET = Bit7, + GYRO_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 GYRO_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + GYRO_AAF_DELTSQR_MSB_585HZ_SET = 0, + GYRO_AAF_DELTSQR_MSB_585HZ_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// INTF_CONFIG5 +enum INTF_CONFIG5_BIT : uint8_t { + // 2:1 PIN9_FUNCTION + PIN9_FUNCTION_CLKIN_SET = Bit2, // 0b10: CLKIN + PIN9_FUNCTION_CLKIN_CLEAR = Bit1, + + PIN9_FUNCTION_RESET_SET = 0, + PIN9_FUNCTION_RESET_CLEAR = Bit2 | Bit1, +}; + +//---------------- BANK2 Register bits + +// ACCEL_CONFIG_STATIC2 +enum ACCEL_CONFIG_STATIC2_BIT : uint8_t { + // 6:1 ACCEL_AAF_DELT + // 585 Hz = 13 (0b00'1101) + ACCEL_AAF_DELT_585HZ_SET = Bit4 | Bit3 | Bit1, + ACCEL_AAF_DELT_585HZ_CLEAR = Bit6 | Bit5 | Bit2, + + // 0 ACCEL_AAF_DIS + ACCEL_AAF_DIS = Bit0, // 0: Enable accelerometer anti-aliasing filter (default) +}; + +// ACCEL_CONFIG_STATIC3 +enum ACCEL_CONFIG_STATIC3_BIT : uint8_t { + // 7:0 ACCEL_AAF_DELTSQR[7:0] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_LSB_585HZ_SET = Bit7 | Bit5 | Bit3 | Bit1, + ACCEL_AAF_DELTSQR_LSB_585HZ_CLEAR = Bit6 | Bit4 | Bit2 | Bit0, +}; + +// ACCEL_CONFIG_STATIC4 +enum ACCEL_CONFIG_STATIC4_BIT : uint8_t { + // 7:4 ACCEL_AAF_BITSHIFT + // 585 Hz = 8 (0b1000) + ACCEL_AAF_BITSHIFT_585HZ_SET = Bit7, + ACCEL_AAF_BITSHIFT_585HZ_CLEAR = Bit6 | Bit5 | Bit4, + + // 3:0 ACCEL_AAF_DELTSQR[11:8] + // 585 Hz = 170 (0b0000'1010'1010) + ACCEL_AAF_DELTSQR_MSB_SET = 0, + ACCEL_AAF_DELTSQR_MSB_CLEAR = Bit3 | Bit2 | Bit1 | Bit0, +}; + +// AUX1_CONFIG1 +enum AUX1_CONFIG1_BIT : uint8_t { + AUX1_ACCEL_LP_CLK_SEL = Bit5, + GYRO_AUX1_EN = Bit1, + ACCEL_AUX1_EN = Bit0, +}; + +// AUX1_CONFIG2 +enum AUX1_CONFIG2_BIT : uint8_t { + GYRO_AUX1_HPF_DIS = Bit6, +}; + +// AUX1_SPI_REG1 +enum AUX1_SPI_REG1_BIT : uint8_t { + AUX1_SPI_REG1_CLEAR = Bit1, + AUX1_SPI_REG1_SET = Bit0, +}; + +// AUX1_SPI_REG2 +enum AUX1_SPI_REG2_BIT : uint8_t { + AUX1_SPI_REG2_CLEAR = Bit1, + AUX1_SPI_REG2_SET = Bit0, +}; + +// AUX1_SPI_REG3 +enum AUX1_SPI_REG3_BIT : uint8_t { + AUX1_SPI_REG3_CLEAR = Bit1, + AUX1_SPI_REG3_SET = Bit0, +}; + +namespace FIFO +{ +static constexpr size_t SIZE = 2048; + +// FIFO_DATA layout when FIFO_CONFIG1 has FIFO_GYRO_EN and FIFO_ACCEL_EN set + +// Packet 4 +struct DATA { + uint8_t FIFO_Header; + uint8_t ACCEL_DATA_X1; // Accel X [19:12] + uint8_t ACCEL_DATA_X0; // Accel X [11:4] + uint8_t ACCEL_DATA_Y1; // Accel Y [19:12] + uint8_t ACCEL_DATA_Y0; // Accel Y [11:4] + uint8_t ACCEL_DATA_Z1; // Accel Z [19:12] + uint8_t ACCEL_DATA_Z0; // Accel Z [11:4] + uint8_t GYRO_DATA_X1; // Gyro X [19:12] + uint8_t GYRO_DATA_X0; // Gyro X [11:4] + uint8_t GYRO_DATA_Y1; // Gyro Y [19:12] + uint8_t GYRO_DATA_Y0; // Gyro Y [11:4] + uint8_t GYRO_DATA_Z1; // Gyro Z [19:12] + uint8_t GYRO_DATA_Z0; // Gyro Z [11:4] + uint8_t TEMP_DATA1; // Temperature[15:8] + uint8_t TEMP_DATA0; // Temperature[7:0] + uint8_t TimeStamp_h; // TimeStamp[15:8] + uint8_t TimeStamp_l; // TimeStamp[7:0] + uint8_t Ext_Accel_X_Gyro_X; // Accel X [3:0] Gyro X [3:0] + uint8_t Ext_Accel_Y_Gyro_Y; // Accel Y [3:0] Gyro Y [3:0] + uint8_t Ext_Accel_Z_Gyro_Z; // Accel Z [3:0] Gyro Z [3:0] +}; + +// With FIFO_ACCEL_EN and FIFO_GYRO_EN header should be 8’b_0110_10xx +enum FIFO_HEADER_BIT : uint8_t { + HEADER_MSG = Bit7, // 1: FIFO is empty + HEADER_ACCEL = Bit6, // 1: Packet is sized so that accel data have location in the packet, FIFO_ACCEL_EN must be 1 + HEADER_GYRO = Bit5, // 1: Packet is sized so that gyro data have location in the packet, FIFO_GYRO_EN must be1 + HEADER_20 = Bit4, // 1: Packet has a new and valid sample of extended 20-bit data for gyro and/or accel + HEADER_TIMESTAMP_FSYNC = Bit3 | Bit2, // 10: Packet contains ODR Timestamp + HEADER_ODR_ACCEL = Bit1, // 1: The ODR for accel is different for this accel data packet compared to the previous accel packet + HEADER_ODR_GYRO = Bit0, // 1: The ODR for gyro is different for this gyro data packet compared to the previous gyro packet +}; + +} +} // namespace InvenSense_IIM42653 diff --git a/src/drivers/imu/invensense/iim42653/Kconfig b/src/drivers/imu/invensense/iim42653/Kconfig new file mode 100644 index 0000000000..903f3d3cb9 --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_IMU_INVENSENSE_IIM42653 + bool "iim42653" + default n + ---help--- + Enable support for iim42653 diff --git a/src/drivers/imu/invensense/iim42653/iim42653_main.cpp b/src/drivers/imu/invensense/iim42653/iim42653_main.cpp new file mode 100644 index 0000000000..655fdd2e19 --- /dev/null +++ b/src/drivers/imu/invensense/iim42653/iim42653_main.cpp @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * 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 "IIM42653.hpp" + +#include +#include + +void IIM42653::print_usage() +{ + PRINT_MODULE_USAGE_NAME("iim42653", "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_PARAM_INT('C', 0, 0, 35000, "Input clock frequency (Hz)", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int iim42653_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = IIM42653; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getOpt(argc, argv, "C:R:")) != EOF) { + switch (ch) { + case 'C': + cli.custom1 = atoi(cli.optArg()); + break; + + 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_IIM42653); + + 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/ins/vectornav/VectorNav.cpp b/src/drivers/ins/vectornav/VectorNav.cpp index 8b3bc95145..1608b91b23 100644 --- a/src/drivers/ins/vectornav/VectorNav.cpp +++ b/src/drivers/ins/vectornav/VectorNav.cpp @@ -38,9 +38,15 @@ #include +using matrix::Vector2f; + VectorNav::VectorNav(const char *port) : ModuleParams(nullptr), - ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)) + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), + _attitude_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_attitude) : ORB_ID(vehicle_attitude)), + _local_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_local_position) : ORB_ID(vehicle_local_position)), + _global_position_pub((_param_vn_mode.get() == 0) ? ORB_ID(external_ins_global_position) : ORB_ID( + vehicle_global_position)) { // store port name strncpy(_port, port, sizeof(_port) - 1); @@ -48,6 +54,27 @@ VectorNav::VectorNav(const char *port) : // enforce null termination _port[sizeof(_port) - 1] = '\0'; + // VN_MODE 1 full INS + if (_param_vn_mode.get() == 1) { + int32_t v = 0; + + // EKF2_EN 0 (disabled) + 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); + + // SENS_MAG_MODE (VN handles sensor selection) + v = 0; + param_set(param_find("SENS_MAG_MODE"), &v); + } + device::Device::DeviceId device_id{}; device_id.devid_s.devtype = DRV_INS_DEVTYPE_VN300; device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; @@ -75,6 +102,16 @@ VectorNav::~VectorNav() perf_free(_sample_perf); perf_free(_comms_errors); + + perf_free(_accel_pub_interval_perf); + perf_free(_gyro_pub_interval_perf); + perf_free(_mag_pub_interval_perf); + perf_free(_gnss_pub_interval_perf); + perf_free(_baro_pub_interval_perf); + + perf_free(_attitude_pub_interval_perf); + perf_free(_local_position_pub_interval_perf); + perf_free(_global_position_pub_interval_perf); } void VectorNav::binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex) @@ -108,7 +145,7 @@ void VectorNav::sensorCallback(VnUartPacket *packet) if (VnUartPacket_isCompatible(packet, COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -118,17 +155,21 @@ void VectorNav::sensorCallback(VnUartPacket *packet) uint64_t time_startup = VnUartPacket_extractUint64(packet); (void)time_startup; - // IMUGROUP_UNCOMPACCEL + // IMUGROUP_ACCEL vec3f accel = VnUartPacket_extractVec3f(packet); - // IMUGROUP_UNCOMPGYRO + // IMUGROUP_ANGULARRATE vec3f angular_rate = VnUartPacket_extractVec3f(packet); // publish sensor_accel _px4_accel.update(time_now_us, accel.c[0], accel.c[1], accel.c[2]); + perf_count(_accel_pub_interval_perf); // publish sensor_gyro _px4_gyro.update(time_now_us, angular_rate.c[0], angular_rate.c[1], angular_rate.c[2]); + perf_count(_gyro_pub_interval_perf); + + _time_last_valid_imu_us.store(hrt_absolute_time()); } // binary output 2 @@ -188,9 +229,11 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_baro.temperature = temperature; sensor_baro.timestamp = hrt_absolute_time(); _sensor_baro_pub.publish(sensor_baro); + perf_count(_baro_pub_interval_perf); // publish sensor_mag _px4_mag.update(time_now_us, mag.c[0], mag.c[1], mag.c[2]); + perf_count(_mag_pub_interval_perf); // publish attitude vehicle_attitude_s attitude{}; @@ -201,13 +244,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet) attitude.q[3] = quaternion.c[2]; attitude.timestamp = hrt_absolute_time(); _attitude_pub.publish(attitude); + perf_count(_attitude_pub_interval_perf); // mode const uint16_t mode = (insStatus & 0b11); //const bool mode_initializing = (mode == 0b00); const bool mode_aligning = (mode == 0b01); const bool mode_tracking = (mode == 0b10); - const bool mode_loss_gnss = (mode == 0b11); + //const bool mode_loss_gnss = (mode == 0b11); // mode_initializing @@ -228,41 +272,88 @@ void VectorNav::sensorCallback(VnUartPacket *packet) // - attitude is good // - treat as mode 0 - - if ((mode_aligning || mode_tracking) && !mode_loss_gnss) { + if (mode_aligning || mode_tracking) { // publish local_position - // TODO: projection + const double lat = positionEstimatedLla.c[0]; + const double lon = positionEstimatedLla.c[1]; + const float alt = positionEstimatedLla.c[2]; + + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(lat, lon, time_now_us); + _gps_alt_ref = alt; + } + + const Vector2f pos_ned = _pos_ref.project(lat, lon); + vehicle_local_position_s local_position{}; local_position.timestamp_sample = time_now_us; - local_position.ax = accelerationLinearNed.c[0]; - local_position.ay = accelerationLinearNed.c[1]; - local_position.az = accelerationLinearNed.c[2]; - local_position.x = positionEstimatedLla.c[0]; // TODO - local_position.y = positionEstimatedLla.c[1]; // TODO - local_position.z = positionEstimatedLla.c[2]; // TODO + + local_position.xy_valid = true; + local_position.z_valid = true; + local_position.v_xy_valid = true; + local_position.v_z_valid = true; + + local_position.x = pos_ned(0); + local_position.y = pos_ned(1); + local_position.z = -(alt - _gps_alt_ref); + local_position.vx = velocityEstimatedNed.c[0]; local_position.vy = velocityEstimatedNed.c[1]; local_position.vz = velocityEstimatedNed.c[2]; + local_position.z_deriv = velocityEstimatedNed.c[2]; // TODO + + local_position.ax = accelerationLinearNed.c[0]; + local_position.ay = accelerationLinearNed.c[1]; + local_position.az = accelerationLinearNed.c[2]; + + matrix::Quatf q{attitude.q}; + local_position.heading = matrix::Eulerf{q}.psi(); + local_position.heading_good_for_control = mode_tracking; + + if (_pos_ref.isInitialized()) { + local_position.xy_global = true; + local_position.z_global = true; + local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position.ref_lat = _pos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _pos_ref.getProjectionReferenceLon(); + local_position.ref_alt = _gps_alt_ref; + } + + local_position.dist_bottom_valid = false; + local_position.eph = positionUncertaintyEstimated; local_position.epv = positionUncertaintyEstimated; local_position.evh = velocityUncertaintyEstimated; local_position.evv = velocityUncertaintyEstimated; - local_position.xy_valid = true; - local_position.heading_good_for_control = mode_tracking; + + local_position.dead_reckoning = false; + + local_position.vxy_max = INFINITY; + local_position.vz_max = INFINITY; + local_position.hagl_min = INFINITY; + local_position.hagl_max = INFINITY; + + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_pub.publish(local_position); - + perf_count(_local_position_pub_interval_perf); // publish global_position vehicle_global_position_s global_position{}; global_position.timestamp_sample = time_now_us; - global_position.lat = positionEstimatedLla.c[0]; - global_position.lon = positionEstimatedLla.c[1]; - global_position.alt = positionEstimatedLla.c[2]; + global_position.lat = lat; + global_position.lon = lon; + global_position.alt = alt; + global_position.alt = alt; + + global_position.eph = positionUncertaintyEstimated; + global_position.epv = positionUncertaintyEstimated; + global_position.timestamp = hrt_absolute_time(); _global_position_pub.publish(global_position); + perf_count(_global_position_pub_interval_perf); } } @@ -320,10 +411,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.fix_type = gpsFix; - sensor_gps.lat = positionGpsLla.c[0] * 1e7; - sensor_gps.lon = positionGpsLla.c[1] * 1e7; - sensor_gps.alt = positionGpsLla.c[2] * 1e3; - sensor_gps.alt_ellipsoid = sensor_gps.alt; + sensor_gps.latitude_deg = positionGpsLla.c[0]; + sensor_gps.longitude_deg = positionGpsLla.c[1]; + 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_n_m_s = velocityGpsNed.c[0]; @@ -333,13 +424,14 @@ void VectorNav::sensorCallback(VnUartPacket *packet) sensor_gps.hdop = dop.hDOP; sensor_gps.vdop = dop.vDOP; - sensor_gps.eph = positionUncertaintyGpsNed.c[0]; + sensor_gps.eph = sqrtf(sq(positionUncertaintyGpsNed.c[0]) + sq(positionUncertaintyGpsNed.c[1])); sensor_gps.epv = positionUncertaintyGpsNed.c[2]; sensor_gps.s_variance_m_s = velocityUncertaintyGps; sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(sensor_gps); + perf_count(_gnss_pub_interval_perf); } } } @@ -493,7 +585,7 @@ bool VectorNav::configure() 1, // divider COMMONGROUP_NONE, TIMEGROUP_TIMESTARTUP, - (ImuGroup)(IMUGROUP_UNCOMPACCEL | IMUGROUP_UNCOMPGYRO), + (ImuGroup)(IMUGROUP_ACCEL | IMUGROUP_ANGULARRATE), GPSGROUP_NONE, ATTITUDEGROUP_NONE, INSGROUP_NONE, @@ -551,6 +643,8 @@ bool VectorNav::configure() VnSensor_registerAsyncPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); VnSensor_registerErrorPacketReceivedHandler(&_vs, VectorNav::binaryAsyncMessageReceived, this); + _time_configured_us.store(hrt_absolute_time()); + return true; } @@ -582,11 +676,42 @@ void VectorNav::Run() _initialized = true; } else { - ScheduleDelayed(3_s); + ScheduleDelayed(1_s); return; } } + if (_connected && _configured && _initialized) { + + // check for timeout + const hrt_abstime time_configured_us = _time_configured_us.load(); + const hrt_abstime time_last_valid_imu_us = _time_last_valid_imu_us.load(); + + if (_param_vn_mode.get() == 1) { + if ((time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) < 3_s)) + + // update sensor_selection if configured in INS mode + if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) { + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _px4_accel.get_device_id(); + sensor_selection.gyro_device_id = _px4_gyro.get_device_id(); + sensor_selection.timestamp = hrt_absolute_time(); + _sensor_selection_pub.publish(sensor_selection); + } + } + + if ((time_configured_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 5_s) + && (time_last_valid_imu_us != 0) && (hrt_elapsed_time(&time_last_valid_imu_us) > 1_s) + ) { + PX4_ERR("timeout, reinitializing"); + VnSensor_unregisterAsyncPacketReceivedHandler(&_vs); + VnSensor_disconnect(&_vs); + _connected = false; + _configured = false; + _initialized = false; + } + } + ScheduleDelayed(100_ms); } diff --git a/src/drivers/ins/vectornav/VectorNav.hpp b/src/drivers/ins/vectornav/VectorNav.hpp index bc1244a009..175a8c5194 100644 --- a/src/drivers/ins/vectornav/VectorNav.hpp +++ b/src/drivers/ins/vectornav/VectorNav.hpp @@ -56,6 +56,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -65,6 +66,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -99,6 +101,9 @@ private: static void binaryAsyncMessageReceived(void *userData, VnUartPacket *packet, size_t runningIndex); + // return the square of two floating point numbers + static constexpr float sq(float var) { return var * var; } + void sensorCallback(VnUartPacket *packet); char _port[20] {}; @@ -108,7 +113,8 @@ private: bool _connected{false}; bool _configured{false}; - hrt_abstime _last_read{0}; + px4::atomic _time_configured_us{false}; + px4::atomic _time_last_valid_imu_us{false}; VnSensor _vs{}; @@ -122,16 +128,31 @@ private: PX4Gyroscope _px4_gyro{0}; PX4Magnetometer _px4_mag{0}; + MapProjection _pos_ref{}; + float _gps_alt_ref{NAN}; ///< WGS-84 height (m) + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - uORB::PublicationMulti _attitude_pub{ORB_ID(external_ins_attitude)}; - uORB::PublicationMulti _local_position_pub{ORB_ID(external_ins_local_position)}; - uORB::PublicationMulti _global_position_pub{ORB_ID(external_ins_global_position)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + + uORB::PublicationMulti _attitude_pub; + uORB::PublicationMulti _local_position_pub; + uORB::PublicationMulti _global_position_pub; perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")}; perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel publish interval")}; + perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro publish interval")}; + perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": mag publish interval")}; + perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": GNSS publish interval")}; + perf_counter_t _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": baro publish interval")}; + + perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": attitude publish interval")}; + perf_counter_t _local_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": local position publish interval")}; + perf_counter_t _global_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": global position publish interval")}; + // TODO: params for GNSS antenna offsets // A @@ -154,4 +175,8 @@ private: // VN_GNSS_ANTB_POS_X // Uncertainty in the X-axis position measurement. + + DEFINE_PARAMETERS( + (ParamInt) _param_vn_mode + ) }; diff --git a/src/drivers/ins/vectornav/module.yaml b/src/drivers/ins/vectornav/module.yaml index c95bfeb608..349fb9869b 100644 --- a/src/drivers/ins/vectornav/module.yaml +++ b/src/drivers/ins/vectornav/module.yaml @@ -1,7 +1,22 @@ module_name: VectorNav (VN-100, VN-200, VN-300) + serial_config: - command: vectornav start -d ${SERIAL_DEV} port_config_param: name: SENS_VN_CFG group: Sensors +parameters: + - group: Sensors + definitions: + + VN_MODE: + description: + short: VectorNav driver mode + long: INS or sensors + category: System + type: enum + values: + 0: Sensors Only (default) + 1: INS + default: 0 diff --git a/src/drivers/linux_pwm_out/module.yaml b/src/drivers/linux_pwm_out/module.yaml index 133e04e155..abc29c704c 100644 --- a/src/drivers/linux_pwm_out/module.yaml +++ b/src/drivers/linux_pwm_out/module.yaml @@ -4,7 +4,7 @@ actuator_output: - param_prefix: PWM_MAIN channel_label: 'Channel' standard_params: - disarmed: { min: 800, max: 2200, default: 900 } + disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp index 3dcc472e60..f5f8987d07 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp index b017ad4afe..0f65bd9582 100644 --- a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp +++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index a0957cff3c..44e609cdfd 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index 9646259b63..91ea43cd9b 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index fe13bebfd3..3aeba5d57e 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp index 0ae20b0206..3d8f84498a 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -40,7 +40,6 @@ #include #include -#include #include #include #include diff --git a/src/drivers/optical_flow/paa3905/PAA3905.cpp b/src/drivers/optical_flow/paa3905/PAA3905.cpp index be4ae70655..f5e5facd1e 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.cpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-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 @@ -194,13 +194,13 @@ void PAA3905::RunImpl() _state = STATE::READ; if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; + _motion_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(1_s); } else { - _data_ready_interrupt_enabled = false; + _motion_interrupt_enabled = false; ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } @@ -222,7 +222,7 @@ void PAA3905::RunImpl() case STATE::READ: { hrt_abstime timestamp_sample = now; - if (_data_ready_interrupt_enabled) { + if (_motion_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); @@ -258,12 +258,6 @@ void PAA3905::RunImpl() PX4_ERR("invalid RawData_Sum > 0x98"); } - // Number of Features = SQUAL * 4 - // RawData_Sum maximum register value is 0x98 - bool data_valid = (buffer.data.SQUAL > 0) - && (buffer.data.RawData_Sum <= 0x98) - && (_discard_reading == 0); - // Bit [5:0] check if chip is working correctly // 0x3F: chip is working correctly if ((buffer.data.Observation & 0x3F) != 0x3F) { @@ -313,7 +307,7 @@ void PAA3905::RunImpl() if (prev_mode != _mode) { // update scheduling on mode change - if (!_data_ready_interrupt_enabled) { + if (!_motion_interrupt_enabled) { ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } } @@ -348,6 +342,14 @@ void PAA3905::RunImpl() const uint32_t shutter = (shutter_upper << 16) | (shutter_middle << 8) | shutter_lower; + // Number of Features = SQUAL * 4 + // RawData_Sum maximum register value is 0x98 + bool data_valid = (buffer.data.SQUAL > 0) + && (buffer.data.RawData_Sum > 0) + && (buffer.data.RawData_Sum <= 0x98) + && (shutter > 0) + && (_discard_reading == 0); + switch (_mode) { case Mode::Bright: sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0; @@ -392,12 +394,17 @@ void PAA3905::RunImpl() // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MotionOccurred); - if (data_valid) { - if (motion_reported) { - // only populate flow if data valid (motion and quality > 0) - const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); - const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); + const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + if (data_valid) { + + const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0); + const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1); + + bool publish = false; + + if (motion_reported) { // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; @@ -412,15 +419,53 @@ void PAA3905::RunImpl() sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + + _last_motion = timestamp_sample; + + } else if (zero_flow && (timestamp_sample > _last_motion)) { + // no motion, but burst read looks valid and we should have seen new data by now if there was any motion + const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev) + || (shutter != _shutter_prev) + || (buffer.data.RawData_Sum != _raw_data_sum_prev) + || (buffer.data.SQUAL != _quality_prev); + + if (burst_read_changed) { + + sensor_optical_flow.pixel_flow[0] = 0; + sensor_optical_flow.pixel_flow[1] = 0; + + sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + } } - // only publish when there's motion or at least every second - if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { + // only publish when there's valid data or on timeout + if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { sensor_optical_flow.timestamp = hrt_absolute_time(); _sensor_optical_flow_pub.publish(sensor_optical_flow); - _last_publish = sensor_optical_flow.timestamp; + _last_publish = sensor_optical_flow.timestamp_sample; + } + + // backup schedule if we're reliant on the motion interrupt and there's very little flow + if (_motion_interrupt_enabled && little_to_no_flow) { + switch (_mode) { + case Mode::Bright: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_2); + break; + } } success = true; @@ -430,6 +475,12 @@ void PAA3905::RunImpl() } } + _delta_x_raw_prev = delta_x_raw; + _delta_y_raw_prev = delta_y_raw; + _shutter_prev = shutter; + _raw_data_sum_prev = buffer.data.RawData_Sum; + _quality_prev = buffer.data.SQUAL; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paa3905/PAA3905.hpp b/src/drivers/optical_flow/paa3905/PAA3905.hpp index e27bdf0fb6..c850b0405a 100644 --- a/src/drivers/optical_flow/paa3905/PAA3905.hpp +++ b/src/drivers/optical_flow/paa3905/PAA3905.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2022-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 @@ -114,14 +114,22 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; + hrt_abstime _last_motion{0}; + + int16_t _delta_x_raw_prev{0}; + int16_t _delta_y_raw_prev{0}; + uint32_t _shutter_prev{0}; + uint8_t _quality_prev{0}; + uint8_t _raw_data_sum_prev{0}; + int _failure_count{0}; int _discard_reading{0}; px4::atomic _drdy_timestamp_sample{0}; - bool _data_ready_interrupt_enabled{false}; + bool _motion_interrupt_enabled{false}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2}; - static constexpr uint32_t kBackupScheduleIntervalUs{200_ms}; + static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval Mode _mode{Mode::LowLight}; diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp index 9b30e7efcf..05a8092a3d 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.cpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -199,13 +199,13 @@ void PAW3902::RunImpl() _state = STATE::READ; if (DataReadyInterruptConfigure()) { - _data_ready_interrupt_enabled = true; + _motion_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(1_s); } else { - _data_ready_interrupt_enabled = false; + _motion_interrupt_enabled = false; ScheduleOnInterval(_scheduled_interval_us, _scheduled_interval_us); } @@ -227,7 +227,7 @@ void PAW3902::RunImpl() case STATE::READ: { hrt_abstime timestamp_sample = now; - if (_data_ready_interrupt_enabled) { + if (_motion_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); @@ -263,12 +263,6 @@ void PAW3902::RunImpl() PX4_ERR("invalid RawData_Sum > 0x98"); } - // Number of Features = SQUAL * 4 - // RawData_Sum maximum register value is 0x98 - bool data_valid = (buffer.data.SQUAL > 0) - && (buffer.data.RawData_Sum <= 0x98) - && (_discard_reading == 0); - // publish sensor_optical_flow sensor_optical_flow_s sensor_optical_flow{}; sensor_optical_flow.timestamp_sample = timestamp_sample; @@ -293,6 +287,14 @@ void PAW3902::RunImpl() const uint16_t shutter = (shutter_upper << 8) | shutter_lower; + // Number of Features = SQUAL * 4 + // RawData_Sum maximum register value is 0x98 + bool data_valid = (buffer.data.SQUAL > 0) + && (buffer.data.RawData_Sum > 0) + && (buffer.data.RawData_Sum <= 0x98) + && (shutter > 0) + && (_discard_reading == 0); + switch (_mode) { case Mode::Bright: sensor_optical_flow.integration_timespan_us = SAMPLE_INTERVAL_MODE_0; @@ -395,12 +397,17 @@ void PAW3902::RunImpl() // motion in burst transfer const bool motion_reported = (buffer.data.Motion & Motion_Bit::MOT); - if (data_valid) { - if (motion_reported) { - // only populate flow if data valid (motion and quality > 0) - const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); - const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + const int16_t delta_x_raw = combine(buffer.data.Delta_X_H, buffer.data.Delta_X_L); + const int16_t delta_y_raw = combine(buffer.data.Delta_Y_H, buffer.data.Delta_Y_L); + if (data_valid) { + + const bool zero_flow = (delta_x_raw == 0) && (delta_y_raw == 0); + const bool little_to_no_flow = (abs(delta_x_raw) <= 1) && (abs(delta_y_raw) <= 1); + + bool publish = false; + + if (motion_reported) { // rotate measurements in yaw from sensor frame to body frame const matrix::Vector3f pixel_flow_rotated = _rotation * matrix::Vector3f{(float)delta_x_raw, (float)delta_y_raw, 0.f}; @@ -415,15 +422,53 @@ void PAW3902::RunImpl() sensor_optical_flow.pixel_flow[1] = pixel_flow_rotated(1) * SCALE; sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + + _last_motion = timestamp_sample; + + } else if (zero_flow && (timestamp_sample > _last_motion)) { + // no motion, but burst read looks valid and we should have seen new data by now if there was any motion + const bool burst_read_changed = (delta_x_raw != _delta_x_raw_prev) || (delta_y_raw != _delta_y_raw_prev) + || (shutter != _shutter_prev) + || (buffer.data.RawData_Sum != _raw_data_sum_prev) + || (buffer.data.SQUAL != _quality_prev); + + if (burst_read_changed) { + + sensor_optical_flow.pixel_flow[0] = 0; + sensor_optical_flow.pixel_flow[1] = 0; + + sensor_optical_flow.quality = buffer.data.SQUAL; + + publish = true; + } } - // only publish when there's motion or at least every second - if (motion_reported || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { + // only publish when there's valid data or on timeout + if (publish || (hrt_elapsed_time(&_last_publish) >= kBackupScheduleIntervalUs)) { sensor_optical_flow.timestamp = hrt_absolute_time(); _sensor_optical_flow_pub.publish(sensor_optical_flow); - _last_publish = sensor_optical_flow.timestamp; + _last_publish = sensor_optical_flow.timestamp_sample; + } + + // backup schedule if we're reliant on the motion interrupt and there's very little flow + if (_motion_interrupt_enabled && little_to_no_flow) { + switch (_mode) { + case Mode::Bright: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_0); + break; + + case Mode::LowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_1); + break; + + case Mode::SuperLowLight: + ScheduleDelayed(SAMPLE_INTERVAL_MODE_2); + break; + } } success = true; @@ -433,6 +478,12 @@ void PAW3902::RunImpl() } } + _delta_x_raw_prev = delta_x_raw; + _delta_y_raw_prev = delta_y_raw; + _shutter_prev = shutter; + _raw_data_sum_prev = buffer.data.RawData_Sum; + _quality_prev = buffer.data.SQUAL; + } else { perf_count(_bad_transfer_perf); } diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp index f5c624e856..b2e568d619 100644 --- a/src/drivers/optical_flow/paw3902/PAW3902.hpp +++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -114,14 +114,22 @@ private: hrt_abstime _reset_timestamp{0}; hrt_abstime _last_publish{0}; + hrt_abstime _last_motion{0}; + + int16_t _delta_x_raw_prev{0}; + int16_t _delta_y_raw_prev{0}; + uint16_t _shutter_prev{0}; + uint8_t _quality_prev{0}; + uint8_t _raw_data_sum_prev{0}; + int _failure_count{0}; int _discard_reading{0}; px4::atomic _drdy_timestamp_sample{0}; - bool _data_ready_interrupt_enabled{false}; + bool _motion_interrupt_enabled{false}; uint32_t _scheduled_interval_us{SAMPLE_INTERVAL_MODE_0 / 2}; - static constexpr uint32_t kBackupScheduleIntervalUs{200_ms}; + static constexpr uint32_t kBackupScheduleIntervalUs{SAMPLE_INTERVAL_MODE_2}; // longest expected interval Mode _mode{Mode::LowLight}; diff --git a/src/drivers/osd/msp_osd/msp_defines.h b/src/drivers/osd/msp_osd/msp_defines.h index fd1c3e5aec..eab7bebe52 100644 --- a/src/drivers/osd/msp_osd/msp_defines.h +++ b/src/drivers/osd/msp_osd/msp_defines.h @@ -386,9 +386,9 @@ struct msp_raw_gps_t { uint8_t numSat; int32_t lat; // 1 / 10000000 deg int32_t lon; // 1 / 10000000 deg - int16_t alt; // meters + int16_t alt; // centimeters since MSP API 1.39, meters before int16_t groundSpeed; // cm/s - int16_t groundCourse; // unit: degree x 10 + int16_t groundCourse; // unit: degree x 100, centidegrees uint16_t hdop; } __attribute__((packed)); diff --git a/src/drivers/osd/msp_osd/uorb_to_msp.cpp b/src/drivers/osd/msp_osd/uorb_to_msp.cpp index f30960ddc5..1a78179855 100644 --- a/src/drivers/osd/msp_osd/uorb_to_msp.cpp +++ b/src/drivers/osd/msp_osd/uorb_to_msp.cpp @@ -314,9 +314,9 @@ msp_raw_gps_t construct_RAW_GPS(const sensor_gps_s &vehicle_gps_position, msp_raw_gps_t raw_gps {0}; if (vehicle_gps_position.fix_type >= 2) { - raw_gps.lat = vehicle_gps_position.lat; - raw_gps.lon = vehicle_gps_position.lon; - raw_gps.alt = vehicle_gps_position.alt / 10; + raw_gps.lat = static_cast(vehicle_gps_position.latitude_deg * 1e7); + raw_gps.lon = static_cast(vehicle_gps_position.longitude_deg * 1e7); + raw_gps.alt = static_cast(vehicle_gps_position.altitude_msl_m * 100.0); float course = math::degrees(vehicle_gps_position.cog_rad); @@ -432,14 +432,14 @@ msp_altitude_t construct_ALTITUDE(const sensor_gps_s &vehicle_gps_position, msp_altitude_t altitude {0}; if (vehicle_gps_position.fix_type >= 2) { - altitude.estimatedActualPosition = vehicle_gps_position.alt / 10; + altitude.estimatedActualPosition = static_cast(vehicle_gps_position.altitude_msl_m * 100.0); // cm } else { altitude.estimatedActualPosition = 0; } if (estimator_status.solution_status_flags & (1 << 5)) { - altitude.estimatedActualVelocity = -vehicle_local_position.vz * 10; //m/s to cm/s + altitude.estimatedActualVelocity = -vehicle_local_position.vz * 100; //m/s to cm/s } else { altitude.estimatedActualVelocity = 0; diff --git a/src/drivers/pca9685_pwm_out/main.cpp b/src/drivers/pca9685_pwm_out/main.cpp index 1c141d5cfa..4c3ee60c54 100644 --- a/src/drivers/pca9685_pwm_out/main.cpp +++ b/src/drivers/pca9685_pwm_out/main.cpp @@ -133,6 +133,15 @@ int PCA9685Wrapper::init() return ret; } + param_t param_h = param_find("PCA9685_RATE"); + + if (param_h != PARAM_INVALID) { + param_get(param_h, &_targetFreq); + + } else { + PX4_DEBUG("PARAM_INVALID: PCA9685_RATE"); + } + this->ChangeWorkQueue(px4::device_bus_to_wq(pca9685->get_device_id())); PX4_INFO("running on I2C bus %d address 0x%.2x", pca9685->get_device_bus(), pca9685->get_device_address()); diff --git a/src/drivers/pca9685_pwm_out/module.yaml b/src/drivers/pca9685_pwm_out/module.yaml index 8833091e3c..78801fbdb6 100644 --- a/src/drivers/pca9685_pwm_out/module.yaml +++ b/src/drivers/pca9685_pwm_out/module.yaml @@ -4,8 +4,24 @@ actuator_output: - param_prefix: PCA9685 channel_label: 'Channel' standard_params: - disarmed: { min: 800, max: 2200, default: 900 } + disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } num_channels: 16 +parameters: + - group: PCA9685 + definitions: + PCA9685_RATE: + description: + short: PWM frequency for PCA9685 + long: | + Update rate at which the PWM signal is sent to the ESC. + type: float + decimal: 1 + increment: 0.1 + default: 50 + min: 50 + max: 450 + unit: Hz + reboot_required: true diff --git a/src/drivers/power_monitor/ina228/ina228.h b/src/drivers/power_monitor/ina228/ina228.h index d010d5e4b9..9b57e5011a 100644 --- a/src/drivers/power_monitor/ina228/ina228.h +++ b/src/drivers/power_monitor/ina228/ina228.h @@ -214,7 +214,7 @@ using namespace time_literals; #define INA228_ENERGY_SHIFTS (0) #define INA228_ENERGY_MASK (UINT64_C(0xffffffffff) << INA228_ENERGY_SHIFTS) -/* INA228 Energy Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ +/* INA228 Charge Result (CHARGE) 40-bit Register (Address = Ah) [reset = 0h] */ #define INA228_CHARGE_SHIFTS (0) #define INA228_CHARGE_MASK (UINT64_C(0xffffffffff) << INA228_CHARGE_SHIFTS) diff --git a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt new file mode 100644 index 0000000000..b715c5cec5 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE drivers__power_monitor_pm_selector_auterion + MAIN pm_selector_auterion + SRCS + PowerMonitorSelectorAuterion.cpp + DEPENDS + px4_work_queue + ) diff --git a/src/drivers/power_monitor/pm_selector_auterion/Kconfig b/src/drivers/power_monitor/pm_selector_auterion/Kconfig new file mode 100644 index 0000000000..e9d1c3f16f --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION + bool "pm_selector_auterion" + default n + ---help--- + Enable support for pm_selector_auterion diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp new file mode 100644 index 0000000000..97c89bea61 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp @@ -0,0 +1,223 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Automatic handling power monitors + */ + +#include "PowerMonitorSelectorAuterion.h" +#include "../ina226/ina226.h" + +#include +#include + +PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default; + +bool PowerMonitorSelectorAuterion::init() +{ + int32_t sens_en = 0; + param_get(param_find("SENS_EN_INA226"), &sens_en); + + if (sens_en == 1) { + + sens_en = 0; + param_set(param_find("SENS_EN_INA226"), &sens_en); + const char *stop_argv[] {"ina226", "stop", NULL}; + exec_builtin("ina226", (char **)stop_argv, NULL, 0); + } + + ScheduleNow(); + return true; +} + +void PowerMonitorSelectorAuterion::Run() +{ + if (should_exit()) { + exit_and_cleanup(); + return; + } + + actuator_armed_s actuator_armed{}; + _actuator_armed_sub.copy(&actuator_armed); + + if (actuator_armed.armed) { + exit_and_cleanup(); + return; + } + + for (uint32_t i = 0U; i < SENSORS_NUMBER; ++i) { + + if (!_sensors[i].started) { + + 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[] { + _sensors[i].name, + "-X", "-b", bus_number, "-a", _sensors[i].i2c_addr, + "-t", _sensors[i].id, "-q", "start", NULL + }; + + int status = PX4_ERROR; + int pid = exec_builtin(_sensors[i].name, (char **)start_argv, NULL, 0); + + if (pid != -1) { + waitpid(pid, &status, WUNTRACED); + } + + if (status == PX4_OK) { + _sensors[i].started = true; + } + } + } + } + + ScheduleDelayed(RUN_INTERVAL); +} + +int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance) +{ + struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number); + int ret = PX4_ERROR; + + if (i2c != nullptr) { + + struct i2c_msg_s msgv[2]; + + uint8_t txdata[1] = {0}; + uint8_t rxdata[2] = {0}; + + msgv[0].frequency = I2C_SPEED_STANDARD; + msgv[0].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[0].flags = 0; + msgv[0].buffer = txdata; + msgv[0].length = sizeof(txdata); + + msgv[1].frequency = I2C_SPEED_STANDARD; + msgv[1].addr = static_cast(strtol(_sensors[instance].i2c_addr, NULL, 0)); + msgv[1].flags = I2C_M_READ; + msgv[1].buffer = rxdata; + msgv[1].length = sizeof(rxdata); + + txdata[0] = {INA226_MFG_ID}; + ret = I2C_TRANSFER(i2c, msgv, 2); + uint16_t value = static_cast(rxdata[1] | rxdata[0] << 8); + + if (ret != PX4_OK || value != INA226_MFG_ID_TI) { + + ret = PX4_ERROR; + + } else { + + txdata[0] = {INA226_MFG_DIEID}; + ret = I2C_TRANSFER(i2c, msgv, 2); + value = static_cast(rxdata[1] | rxdata[0] << 8); + + if (ret != PX4_OK || value != INA226_MFG_DIE) { + ret = PX4_ERROR; + } + } + + px4_i2cbus_uninitialize(i2c); + } + + return ret; +} + +int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[]) +{ + PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion(); + + 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 PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int PowerMonitorSelectorAuterion::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description +Driver for starting and auto-detecting different power monitors. + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int pm_selector_auterion_main(int argc, char *argv[]) +{ + return PowerMonitorSelectorAuterion::main(argc, argv); +} diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h new file mode 100644 index 0000000000..2bc9bffbe3 --- /dev/null +++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * 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 + +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class PowerMonitorSelectorAuterion : public ModuleBase, public px4::ScheduledWorkItem +{ + +public: + PowerMonitorSelectorAuterion(); + virtual ~PowerMonitorSelectorAuterion(); + + /** @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); + +private: + + void Run() override; + + bool init(); + + int ina226_probe(uint32_t instance); + + uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)}; ///< system armed control topic + + struct Sensor { + const char *name; + const char *i2c_addr; + const uint8_t bus_number; + float shunt_value; + bool started; + const char *id; + }; + + static constexpr int RUN_INTERVAL{500_ms}; + static constexpr int SENSORS_NUMBER{4}; + + Sensor _sensors[SENSORS_NUMBER] = { + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 1, + .shunt_value = 0.0008f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 1, + .shunt_value = 0.0005f, + .started = false, + .id = "1" + }, + { + .name = "ina226", + .i2c_addr = "0x41", + .bus_number = 2, + .shunt_value = 0.0008f, + .started = false, + .id = "2" + }, + { + .name = "ina226", + .i2c_addr = "0x40", + .bus_number = 2, + .shunt_value = 0.0005f, + .started = false, + .id = "2" + } + }; +}; diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp index 3a93e0e9d6..6b482a36f7 100644 --- a/src/drivers/pwm_out/PWMOut.cpp +++ b/src/drivers/pwm_out/PWMOut.cpp @@ -218,42 +218,55 @@ void PWMOut::update_params() updateParams(); - // Automatically set the PWM rate and disarmed value when a channel is first set to a servo + // Automatically set PWM configuration when a channel is first assigned if (!_first_update_cycle) { for (size_t i = 0; i < _num_outputs; i++) { if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) { int32_t output_function; - if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0 - && output_function >= (int)OutputFunction::Servo1 - && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo - int32_t val = 1500; - PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i); - param_set(_mixing_output.disarmedParamHandle(i), &val); + if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) { + // Servos need PWM rate 50Hz and disramed value 1500us + if (output_function >= (int)OutputFunction::Servo1 + && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo + int32_t val = 1500; + PX4_INFO("Setting channel %i disarmed to %i", (int) val, i); + param_set(_mixing_output.disarmedParamHandle(i), &val); - // If the whole timer group was not set previously, then set the pwm rate to 50 Hz - for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { + // If the whole timer group was not set previously, then set the pwm rate to 50 Hz + for (int timer = 0; timer < MAX_IO_TIMERS; ++timer) { - uint32_t channels = io_timer_get_group(timer); + uint32_t channels = io_timer_get_group(timer); - if ((channels & (1u << i)) == 0) { - continue; - } + if ((channels & (1u << i)) == 0) { + continue; + } - if ((channels & previously_set_functions) == 0) { // None of the channels was set - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + if ((channels & previously_set_functions) == 0) { // None of the channels was set + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - int32_t tim_config = 0; - param_t handle = param_find(param_name); + int32_t tim_config = 0; + param_t handle = param_find(param_name); - if (param_get(handle, &tim_config) == 0 && tim_config == 400) { - tim_config = 50; - PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); - param_set(handle, &tim_config); + if (param_get(handle, &tim_config) == 0 && tim_config == 400) { + tim_config = 50; + PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); + param_set(handle, &tim_config); + } } } } + + // Motors need a minimum value that idles the motor and have a deadzone at the top of the range + if (output_function >= (int)OutputFunction::Motor1 + && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor + int32_t val = 1100; + PX4_INFO("Setting channel %i minimum to %i", (int) val, i); + param_set(_mixing_output.minParamHandle(i), &val); + val = 1900; + PX4_INFO("Setting channel %i maximum to %i", (int) val, i); + param_set(_mixing_output.maxParamHandle(i), &val); + } } } } diff --git a/src/drivers/pwm_out/module.yaml b/src/drivers/pwm_out/module.yaml index 733276c678..a55af5fae2 100644 --- a/src/drivers/pwm_out/module.yaml +++ b/src/drivers/pwm_out/module.yaml @@ -5,7 +5,7 @@ actuator_output: param_prefix: '${PWM_MAIN_OR_AUX}' channel_labels: ['${PWM_MAIN_OR_AUX}', '${PWM_MAIN_OR_AUX_CAP}'] standard_params: - disarmed: { min: 800, max: 2200, default: 900 } + disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } diff --git a/src/drivers/px4io/module.yaml b/src/drivers/px4io/module.yaml index 5b5a197579..01d21a4afe 100644 --- a/src/drivers/px4io/module.yaml +++ b/src/drivers/px4io/module.yaml @@ -7,7 +7,7 @@ actuator_output: channel_label_module_name_prefix: false timer_config_file: "boards/px4/io-v2/src/timer_config.cpp" standard_params: - disarmed: { min: 800, max: 2200, default: 900 } + disarmed: { min: 800, max: 2200, default: 1000 } min: { min: 800, max: 1400, default: 1000 } max: { min: 1600, max: 2200, default: 2000 } failsafe: { min: 800, max: 2200 } diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3dba560206..ca5470a89b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -702,36 +702,48 @@ void PX4IO::update_params() if ((previously_set_functions & (1u << i)) == 0 && _mixing_output.functionParamHandle(i) != PARAM_INVALID) { int32_t output_function; - if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0 - && output_function >= (int)OutputFunction::Servo1 - && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo - int32_t val = 1500; - PX4_INFO("Setting disarmed to %i for channel %i", (int) val, i); - param_set(_mixing_output.disarmedParamHandle(i), &val); + if (param_get(_mixing_output.functionParamHandle(i), &output_function) == 0) { + if (output_function >= (int)OutputFunction::Servo1 + && output_function <= (int)OutputFunction::ServoMax) { // Function got set to a servo + int32_t val = 1500; + PX4_INFO("Setting channel %i disarmed to %i", (int) val, i); + param_set(_mixing_output.disarmedParamHandle(i), &val); - // If the whole timer group was not set previously, then set the pwm rate to 50 Hz - for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) { + // If the whole timer group was not set previously, then set the pwm rate to 50 Hz + for (int timer = 0; timer < (int)(sizeof(_group_channels) / sizeof(_group_channels[0])); ++timer) { - uint32_t channels = _group_channels[timer]; + uint32_t channels = _group_channels[timer]; - if ((channels & (1u << i)) == 0) { - continue; - } + if ((channels & (1u << i)) == 0) { + continue; + } - if ((channels & previously_set_functions) == 0) { // None of the channels was set - char param_name[17]; - snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); + if ((channels & previously_set_functions) == 0) { // None of the channels was set + char param_name[17]; + snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer); - int32_t tim_config = 0; - param_t handle = param_find(param_name); + int32_t tim_config = 0; + param_t handle = param_find(param_name); - if (param_get(handle, &tim_config) == 0 && tim_config == 400) { - tim_config = 50; - PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); - param_set(handle, &tim_config); + if (param_get(handle, &tim_config) == 0 && tim_config == 400) { + tim_config = 50; + PX4_INFO("setting timer %i to %i Hz", timer, (int) tim_config); + param_set(handle, &tim_config); + } } } } + + // Motors need a minimum value that idles the motor + if (output_function >= (int)OutputFunction::Motor1 + && output_function <= (int)OutputFunction::MotorMax) { // Function got set to a motor + int32_t val = 1100; + PX4_INFO("Setting channel %i minimum to %i", (int) val, i); + param_set(_mixing_output.minParamHandle(i), &val); + val = 1900; + PX4_INFO("Setting channel %i maximum to %i", (int) val, i); + param_set(_mixing_output.maxParamHandle(i), &val); + } } } } diff --git a/src/drivers/rc/crsf_rc/CrsfRc.cpp b/src/drivers/rc/crsf_rc/CrsfRc.cpp index 9ba3bc939e..dc30620e98 100644 --- a/src/drivers/rc/crsf_rc/CrsfRc.cpp +++ b/src/drivers/rc/crsf_rc/CrsfRc.cpp @@ -241,11 +241,11 @@ void CrsfRc::Run() sensor_gps_s sensor_gps; if (_vehicle_gps_position_sub.update(&sensor_gps)) { - int32_t latitude = sensor_gps.lat; - int32_t longitude = sensor_gps.lon; + int32_t latitude = static_cast(round(sensor_gps.latitude_deg * 1e7)); + 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 = sensor_gps.alt + 1000; + uint16_t altitude = static_cast(sensor_gps.altitude_msl_m * 1e3) + 1000; uint8_t num_satellites = sensor_gps.satellites_used; this->SendTelemetryGps(latitude, longitude, groundspeed, gps_heading, altitude, num_satellites); } diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index 5b1993eaac..5caab7e973 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -124,6 +124,7 @@ RCInput::task_spawn(int argc, char *argv[]) int ch; const char *myoptarg = nullptr; const char *device_name = nullptr; + bool silent = false; #if defined(RC_SERIAL_PORT) device_name = RC_SERIAL_PORT; #endif // RC_SERIAL_PORT @@ -133,6 +134,7 @@ RCInput::task_spawn(int argc, char *argv[]) // if RC_SERIAL_PORT == PX4IO_SERIAL_DEVICE then don't use it by default if the px4io is running if ((strcmp(RC_SERIAL_PORT, PX4IO_SERIAL_DEVICE) == 0) && (access("/dev/px4io", R_OK) == 0)) { device_name = nullptr; + silent = true; } #endif // RC_SERIAL_PORT && PX4IO_SERIAL_DEVICE @@ -141,6 +143,7 @@ RCInput::task_spawn(int argc, char *argv[]) switch (ch) { case 'd': device_name = myoptarg; + silent = false; break; case '?': @@ -173,6 +176,9 @@ RCInput::task_spawn(int argc, char *argv[]) return PX4_OK; + } else if (silent) { + return PX4_OK; + } else { if (device_name) { PX4_ERR("invalid device (-d) %s", device_name); diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index e11687b5d8..720437c823 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -96,11 +96,11 @@ bool CRSFTelemetry::send_gps() return false; } - int32_t latitude = vehicle_gps_position.lat; - int32_t longitude = vehicle_gps_position.lon; + int32_t latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); + int32_t longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); uint16_t groundspeed = vehicle_gps_position.vel_d_m_s / 3.6f * 10.f; uint16_t gps_heading = math::degrees(vehicle_gps_position.cog_rad) * 100.f; - uint16_t altitude = vehicle_gps_position.alt + 1000; + uint16_t altitude = static_cast(round(vehicle_gps_position.altitude_msl_m + 1.0)); uint8_t num_satellites = vehicle_gps_position.satellites_used; return crsf_send_telemetry_gps(_uart_fd, latitude, longitude, groundspeed, diff --git a/src/drivers/rc_input/ghst_telemetry.cpp b/src/drivers/rc_input/ghst_telemetry.cpp index 2b253bbc3f..6edd0de598 100644 --- a/src/drivers/rc_input/ghst_telemetry.cpp +++ b/src/drivers/rc_input/ghst_telemetry.cpp @@ -110,9 +110,9 @@ bool GHSTTelemetry::send_gps1_status() return false; } - int32_t latitude = vehicle_gps_position.lat; // 1e-7 degrees - int32_t longitude = vehicle_gps_position.lon; // 1e-7 degrees - uint16_t altitude = vehicle_gps_position.alt / 1000; // mm -> m + 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); } diff --git a/src/drivers/tap_esc/TAP_ESC.cpp b/src/drivers/tap_esc/TAP_ESC.cpp index 2d6983d2a2..0d37dda2c0 100644 --- a/src/drivers/tap_esc/TAP_ESC.cpp +++ b/src/drivers/tap_esc/TAP_ESC.cpp @@ -65,6 +65,8 @@ int TAP_ESC::init() return ret; } + _esc_feedback_pub.advertise(); + /* Respect boot time required by the ESC FW */ hrt_abstime uptime_us = hrt_absolute_time(); diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 5eaa83d011..7d7013651b 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -268,9 +268,9 @@ void BST::RunImpl() if (gps.fix_type >= 3 && gps.eph < 50.0f) { BSTPacket bst_gps = {}; bst_gps.type = 0x02; - bst_gps.payload.lat = swap_int32(gps.lat); - bst_gps.payload.lon = swap_int32(gps.lon); - bst_gps.payload.alt = swap_int16(gps.alt / 1000 + 1000); + bst_gps.payload.lat = swap_int32(static_cast(round(gps.latitude_deg * 1e7))); + bst_gps.payload.lon = swap_int32(static_cast(round(gps.longitude_deg * 1e7))); + bst_gps.payload.alt = swap_int16(static_cast(round(gps.altitude_msl_m)) + 1000); bst_gps.payload.gs = swap_int16(gps.vel_m_s * 360.0f); bst_gps.payload.heading = swap_int16(gps.cog_rad * 18000.0f / M_PI_F); bst_gps.payload.sats = gps.satellites_used; diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp index 238f8949c0..cac0b1c719 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp @@ -233,11 +233,11 @@ void sPort_send_GPS_LON(int uart) /* send longitude */ /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 1 and bit 30=sign */ /* precision is approximately 0.1m */ - uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lon); + uint32_t iLon = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg * 1e7); iLon |= (1 << 31); - if (s_port_subscription_data->vehicle_gps_position_sub.get().lon < 0) { iLon |= (1 << 30); } + if (s_port_subscription_data->vehicle_gps_position_sub.get().longitude_deg < 0) { iLon |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLon); } @@ -246,9 +246,9 @@ void sPort_send_GPS_LAT(int uart) { /* send latitude */ /* convert to 30 bit signed magnitude degrees*6E5 with MSb = 0 and bit 30=sign */ - uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().lat); + uint32_t iLat = 6E-2 * fabs(s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg * 1e7); - if (s_port_subscription_data->vehicle_gps_position_sub.get().lat < 0) { iLat |= (1 << 30); } + if (s_port_subscription_data->vehicle_gps_position_sub.get().latitude_deg < 0) { iLat |= (1 << 30); } sPort_send_data(uart, SMARTPORT_ID_GPS_LON_LAT, iLat); } @@ -256,7 +256,7 @@ void sPort_send_GPS_LAT(int uart) void sPort_send_GPS_ALT(int uart) { /* send altitude */ - uint32_t iAlt = s_port_subscription_data->vehicle_gps_position_sub.get().alt / 10; + uint32_t iAlt = static_cast(s_port_subscription_data->vehicle_gps_position_sub.get().altitude_msl_m * 1e2); sPort_send_data(uart, SMARTPORT_ID_GPS_ALT, iAlt); } diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 96c2dd3386..f800a00566 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -242,14 +242,14 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat)) * 1e-7d; + double lat = gps.latitude_deg; /* Set the N or S specifier */ msg.latitude_ns = 0; if (lat < 0) { msg.latitude_ns = 1; - lat = abs(lat); + lat = fabs(lat); } int deg; @@ -265,7 +265,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon)) * 1e-7d; + double lon = gps.longitude_deg; /* Set the E or W specifier */ msg.longitude_ew = 0; @@ -285,7 +285,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt * 1e-3f + 500.0f); + uint16_t alt = (uint16_t)(round(gps.altitude_msl_m) + 500.0); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp index cd0f5b11e5..379f651be9 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp @@ -455,7 +455,7 @@ void SagetechMXS::determine_furthest_aircraft() continue; } - const float distance = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE, + const float distance = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg, vehicle_list[index].lat, vehicle_list[index].lon); @@ -492,8 +492,8 @@ void SagetechMXS::handle_vehicle(const transponder_report_s &vehicle) // needs to handle updating the vehicle list, keeping track of which vehicles to drop // and which to keep, allocating new vehicles, and publishing to the transponder_report topic uint16_t index = list_size_allocated + 1; // Make invalid to start with. - const bool my_loc_is_zero = (_gps.lat == 0) && (_gps.lon == 0); - const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.lat * GPS_SCALE, _gps.lon * GPS_SCALE, + const bool my_loc_is_zero = (fabs(_gps.latitude_deg) < DBL_EPSILON) && (fabs(_gps.longitude_deg) < DBL_EPSILON); + const float my_loc_distance_to_vehicle = get_distance_to_next_waypoint(_gps.latitude_deg, _gps.longitude_deg, vehicle.lat, vehicle.lon); const bool is_tracked_in_list = find_index(vehicle, &index); // const bool is_special = is_special_vehicle(vehicle.icao_address); @@ -745,7 +745,8 @@ void SagetechMXS::send_operating_msg() mxs_state.op.altRes25 = !mxs_state.inst.altRes100; // Host Altitude Resolution from install - mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + mxs_state.op.altitude = static_cast(_gps.altitude_msl_m * + SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet mxs_state.op.identOn = _adsb_ident.get(); @@ -806,8 +807,8 @@ void SagetechMXS::send_gps_msg() } // Get Vehicle Longitude and Latitude and Convert to string - const int32_t longitude = _gps.lon; - const int32_t latitude = _gps.lat; + const int32_t longitude = static_cast(_gps.longitude_deg * 1e7); + const int32_t latitude = static_cast(_gps.latitude_deg * 1e7); const double lon_deg = longitude * 1.0E-7 * (longitude < 0 ? -1 : 1); const double lon_minutes = (lon_deg - int(lon_deg)) * 60; snprintf((char *)&gps.longitude, 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, @@ -836,7 +837,7 @@ void SagetechMXS::send_gps_msg() snprintf((char *)&gps.timeOfFix, 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (_gps.time_utc_usec % 1000000) * 1.0e-6); - gps.height = _gps.alt_ellipsoid * 1E-3; + gps.height = (float)_gps.altitude_ellipsoid_m; // checkGPSInputs(&gps); last.msg.type = SG_MSG_TYPE_HOST_GPS; @@ -1284,7 +1285,8 @@ void SagetechMXS::auto_config_operating() mxs_state.op.altHostAvlbl = false; mxs_state.op.altRes25 = true; // Host Altitude Resolution from install - mxs_state.op.altitude = _gps.alt * SAGETECH_SCALE_MM_TO_FT; // Height above sealevel in feet + mxs_state.op.altitude = static_cast(_gps.altitude_msl_m * + SAGETECH_SCALE_M_TO_FT); // Height above sealevel in feet mxs_state.op.identOn = false; diff --git a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp index f63661b629..ad4314d130 100644 --- a/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp +++ b/src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp @@ -142,7 +142,7 @@ private: static constexpr float SAGETECH_SCALE_KNOTS_TO_M_PER_SEC{0.514444F}; static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_KNOTS{1.94384F}; static constexpr float SAGETECH_SCALE_FT_PER_MIN_TO_M_PER_SEC{0.00508F}; - static constexpr float SAGETECH_SCALE_MM_TO_FT{0.00328084F}; + static constexpr double SAGETECH_SCALE_M_TO_FT{3.28084}; static constexpr float SAGETECH_SCALE_M_PER_SEC_TO_FT_PER_MIN{196.85F}; static constexpr uint8_t ADSB_ALTITUDE_TYPE_PRESSURE_QNH{0}; static constexpr uint8_t ADSB_ALTITUDE_TYPE_GEOMETRIC{1}; @@ -156,7 +156,6 @@ private: static constexpr uint16_t INVALID_SQUAWK{7777}; static constexpr unsigned BAUD_460800{0010004}; // B460800 not defined in MacOS termios static constexpr unsigned BAUD_921600{0010007}; // B921600 not defined in MacOS termios - static constexpr double GPS_SCALE{1.0E-7}; // Stored variables uint64_t _loop_count; diff --git a/src/drivers/uavcan/Kconfig b/src/drivers/uavcan/Kconfig index c5f3d1e76a..d2f064e8ba 100644 --- a/src/drivers/uavcan/Kconfig +++ b/src/drivers/uavcan/Kconfig @@ -4,6 +4,81 @@ menuconfig DRIVERS_UAVCAN ---help--- Enable support for uavcan +if DRIVERS_UAVCAN + config UAVCAN_ARMING_CONTROLLER + bool "Include arming status controller" + default y + + config UAVCAN_BEEP_CONTROLLER + bool "Include beep controller" + default y + + config UAVCAN_OUTPUTS_CONTROLLER + bool "Include servo & ESC controller" + default y + + config UAVCAN_HARDPOINT_CONTROLLER + bool "Include hardpoint controller" + default y + + config UAVCAN_SAFETY_STATE_CONTROLLER + bool "Include safety state controller" + default y + + config UAVCAN_RGB_CONTROLLER + bool "Include rgb controller" + default y + + config UAVCAN_SENSOR_ACCEL + bool "Subscribe to IMU: uavcan::equipment::ahrs::RawIMU" + default y + + config UAVCAN_SENSOR_AIRSPEED + bool "Subscribe to Airspeed: uavcan::equipment::air_data::IndicatedAirspeed | uavcan::equipment::air_data::TrueAirspeed | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BARO + bool "Subscribe to Barometer: uavcan::equipment::air_data::StaticPressure | uavcan::equipment::air_data::StaticTemperature" + default y + + config UAVCAN_SENSOR_BATTERY + bool "Subscribe to Battery: uavcan::equipment::power::BatteryInfo | ardupilot::equipment::power::BatteryInfoAux" + default y + + config UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE + bool "Subscribe to Differential Pressure: uavcan::equipment::air_data::RawAirData" + default y + + config UAVCAN_SENSOR_FLOW + bool "Subscribe to Flow: com::hex::equipment::flow::Measurement" + default y + + config UAVCAN_SENSOR_GNSS + bool "Subscribe to GPS: uavcan::equipment::gnss::Auxiliary | uavcan::equipment::gnss::Fix | uavcan::equipment::gnss::Fix2" + default y + + config UAVCAN_SENSOR_HYGROMETER + bool "Subscribe to Hygrometer: dronecan::sensors::hygrometer::Hygrometer" + default y + + config UAVCAN_SENSOR_ICE_STATUS + bool "Subscribe to Internal Combustion Engine: uavcan::equipment::ice::reciprocating::Status" + default y + + config UAVCAN_SENSOR_MAG + bool "Subscribe to Magnetometer: uavcan::equipment::ahrs::MagneticFieldStrength | uavcan::equipment::ahrs::MagneticFieldStrength2" + default y + + config UAVCAN_SENSOR_RANGEFINDER + bool "Subscribe to Rangefinder: uavcan::equipment::range_sensor::Measurement" + default y + + config UAVCAN_SENSOR_SAFETY_BUTTON + bool "Subscribe to Safety Button: ardupilot::indication::Button" + default y + +endif #DRIVERS_UAVCAN + menuconfig BOARD_UAVCAN_INTERFACES depends on DRIVERS_UAVCAN || DRIVERS_UAVCANNODE diff --git a/src/drivers/uavcan/actuators/esc.cpp b/src/drivers/uavcan/actuators/esc.cpp index 2031b31877..12f5d8600b 100644 --- a/src/drivers/uavcan/actuators/esc.cpp +++ b/src/drivers/uavcan/actuators/esc.cpp @@ -64,6 +64,8 @@ UavcanEscController::init() return res; } + _esc_status_pub.advertise(); + return res; } diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index 2eb59d3b26..ddbc7907e6 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -337,10 +337,10 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure */ report.timestamp = hrt_absolute_time(); - report.lat = msg.latitude_deg_1e8 / 10; - report.lon = msg.longitude_deg_1e8 / 10; - report.alt = msg.height_msl_mm; - report.alt_ellipsoid = msg.height_ellipsoid_mm; + report.latitude_deg = msg.latitude_deg_1e8 / 1e8; + report.longitude_deg = msg.longitude_deg_1e8 / 1e8; + report.altitude_msl_m = msg.height_msl_mm / 1e3; + report.altitude_ellipsoid_m = msg.height_ellipsoid_mm / 1e3; if (valid_pos_cov) { // Horizontal position uncertainty @@ -467,29 +467,61 @@ void UavcanGnssBridge::update() // to work. void UavcanGnssBridge::handleInjectDataTopic() { - // Limit maximum number of GPS injections to 6 since usually - // GPS injections should consist of 1-4 packets (GPS, Glonass, BeiDou, Galileo). - // Looking at 6 packets thus guarantees, that at least a full injection - // data set is evaluated. - static constexpr size_t MAX_NUM_INJECTIONS = 6; + // 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; - size_t num_injections = 0; - gps_inject_data_s gps_inject_data; + // 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) { - while ((num_injections <= MAX_NUM_INJECTIONS) && _gps_inject_data_sub.update(&gps_inject_data)) { - // 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. - if (_publish_rtcm_stream) { - PublishRTCMStream(gps_inject_data.data, gps_inject_data.len); + for (int instance = 0; instance < _orb_inject_data_sub.size(); instance++) { + const bool exists = _orb_inject_data_sub[instance].advertised(); + + if (exists) { + if (_orb_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; + } + } + } } - - if (_publish_moving_baseline_data) { - PublishMovingBaselineData(gps_inject_data.data, gps_inject_data.len); - } - - num_injections++; } + + 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++; + + // 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. + if (_publish_rtcm_stream) { + PublishRTCMStream(msg.data, msg.len); + } + + if (_publish_moving_baseline_data) { + PublishMovingBaselineData(msg.data, msg.len); + } + + _last_rtcm_injection_time = hrt_absolute_time(); + } + + updated = _orb_inject_data_sub[_selected_rtcm_instance].update(&msg); + + } while (updated && num_injections < max_num_injections); } bool UavcanGnssBridge::PublishRTCMStream(const uint8_t *const data, const size_t data_len) diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index b60c238f2f..5b65f57bb6 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -45,6 +45,7 @@ #pragma once #include +#include #include #include #include @@ -123,7 +124,9 @@ private: float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data)}; + uORB::SubscriptionMultiArray _orb_inject_data_sub{ORB_ID::gps_inject_data}; + 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 bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index cff084292c..5e15b8d51f 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -108,14 +108,13 @@ void UavcanRangefinderBridge::range_sub_cb(const _inited = true; } - /* - * FIXME HACK - * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. - * It stopped working when the time sync feature has been introduced, because it caused libuavcan - * to use an independent time source (based on hardware TIM5) instead of HRT. - * The proper solution is to be developed. - */ - rangefinder->update(hrt_absolute_time(), msg.range); + int8_t quality = -1; + + if (msg.reading_type == uavcan::equipment::range_sensor::Measurement::READING_TYPE_VALID_RANGE) { + quality = 100; + } + + rangefinder->update(hrt_absolute_time(), msg.range, quality); } int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index 023a16cf1d..3f1d424bd1 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -38,19 +38,43 @@ #include "sensor_bridge.hpp" #include +#if defined(CONFIG_UAVCAN_SENSOR_ACCEL) #include "accel.hpp" -#include "airspeed.hpp" -#include "baro.hpp" -#include "battery.hpp" -#include "differential_pressure.hpp" -#include "flow.hpp" -#include "gnss.hpp" #include "gyro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) +#include "airspeed.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BARO) +#include "baro.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_BATTERY) +#include "battery.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_DIFFERENTIAL_PRESSURE) +#include "differential_pressure.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_FLOW) +#include "flow.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_GNSS) +#include "gnss.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_HYGROMETER) #include "hygrometer.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_ICE_STATUS) #include "ice_status.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_MAG) #include "mag.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_RANGEFINDER) #include "rangefinder.hpp" +#endif +#if defined(CONFIG_UAVCAN_SENSOR_SAFETY_BUTTON) #include "safety_button.hpp" +#endif /* * IUavcanSensorBridge @@ -58,6 +82,7 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, List &list) { // airspeed +#if defined(CONFIG_UAVCAN_SENSOR_AIRSPEED) int32_t uavcan_sub_aspd = 1; param_get(param_find("UAVCAN_SUB_ASPD"), &uavcan_sub_aspd); @@ -65,6 +90,9 @@ void IUavcanSensorBridge::make_all(uavcan::INode &node, ListhandleTxInterrupt(utc_usec); + + } else if ((fdcan::Can[iface_index]->IR & FDCAN_IR_BO) != 0) { + + fdcan::Can[iface_index]->IR = FDCAN_IR_BO; + ifaces[iface_index]->handleBusOff(); + } } @@ -184,6 +190,11 @@ inline void handleRxInterrupt(uavcan::uint8_t iface_index) fdcan::Can[iface_index]->IR = (FDCAN_IR_RF1N | FDCAN_IR_RF1F); ifaces[iface_index]->handleRxInterrupt(1); + } else if ((IR & FDCAN_IR_BO) != 0) { + + fdcan::Can[iface_index]->IR = FDCAN_IR_BO; + ifaces[iface_index]->handleBusOff(); + } else { UAVCAN_ASSERT(0); } @@ -500,6 +511,37 @@ uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::Monotonic uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs, uavcan::uint16_t num_configs) { + + /* + * Software initialization is started by setting INIT bit in FDCAN_CCCR register, either by + * software or by a hardware reset, or by going Bus_Off. While INIT bit in FDCAN_CCCR + * register is set, message transfer from and to the CAN bus is stopped, the status of the CAN + * bus output FDCAN_TX is recessive (high). The counters of the error management logic + * (EML) are unchanged. Setting INIT bit in FDCAN_CCCR does not change any configuration + * register. Clearing INIT bit in FDCAN_CCCR finishes the software initialization. Afterwards + * the bit stream processor (BSP) synchronizes itself to the data transfer on the CAN bus by + * waiting for the occurrence of a sequence of 11 consecutive recessive bits (Bus_Idle) before + * it can take part in bus activities and start the message transfer. + */ + + /* + * Access to the FDCAN configuration registers is only enabled when both INIT bit in + * FDCAN_CCCR register and CCE bit in FDCAN_CCCR register are set + */ + + /* + * CCE bit in FDCAN_CCCR register can only be set/cleared while INIT bit in FDCAN_CCCR + * is set. CCE bit in FDCAN_CCCR register is automatically cleared when INIT bit in + * FDCAN_CCCR is cleared + */ + + /* + * Up to 128 filter elements can be configured for 11-bit standard IDs. When accessing a + * standard message ID filter element, its address is the filter list standard start address + * FDCAN_SIDFC.FLSSA plus the index of the filter element (0 ... 127 + */ + + /* * The FDCAN controller handles standard ID and extended ID filters separately. * We must scan through the requested filter configurations, and group them by @@ -509,21 +551,95 @@ uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter // Filter config registers are protected; write access is only available // when bit CCE and bit INIT of FDCAN_CCCR register are set to 1. - // CriticalSectionLocker lock; + uint32_t num_of_sid_filter = 0; + uint32_t num_of_xid_filter = 0; - // // Request Init mode, then wait for completion - // can_->CCCR |= FDCAN_CCCR_INIT; - // while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {} + if (num_configs <= NumFilters) { - // // Configuration Chane Enable - // can_->CCCR |= FDCAN_CCCR_CCE; + CriticalSectionLocker lock; - // /// TODO ------------------------- + // // Request Init mode, then wait for completion + can_->CCCR |= FDCAN_CCCR_INIT; - // // Leave Init mode - // can_->CCCR &= ~FDCAN_CCCR_INIT; + while ((can_->CCCR & FDCAN_CCCR_INIT) == 0) {}; - return 0; + // // Configuration Chane Enable + can_->CCCR |= FDCAN_CCCR_CCE; + + for (uint8_t i = 0; i < NumFilters; i++) { + + if (i < num_configs) { + + // determine what type of filter is this: + // and add to the number of filter + const uavcan::CanFilterConfig *const cfg = filter_configs + i; + + // extended message + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) { + + volatile uint32_t *xid_filter_address = (uint32_t *)((can_->XIDFC | FDCAN_XIDFC_FLESA_Msk) + 2 * num_of_xid_filter); + num_of_xid_filter += 1; + + uint32_t f0 = 0; + uint32_t f1 = 0; + + // bit 31:29 EFEC[2:0], extended filter element configuration -> set Priority + f0 |= 4 << 29; + + // bit 28:0 EFID[28:0], Extended Filter ID + f0 |= cfg->id; + + // bit 31:30 EFEC[2:0], extended filter type -> classic filter + f1 |= 2 << 30; + + // bit 28:0 EFID2[28:18], Extended Filter ID2 + f1 |= cfg->mask; + + *xid_filter_address = f0; + xid_filter_address += 1; + *(xid_filter_address) = f1; + } + + // standard message + else { + volatile uint32_t *sid_filter_address = (uint32_t *)((can_->SIDFC | FDCAN_SIDFC_FLSSA_Msk) + num_of_sid_filter); + + num_of_sid_filter += 1; + + uint32_t filter = 0; + + // bit 31:30 SFT[1:0], standard filter type, -> classic + filter |= 2 << 30; + + // bit 29:27 SFEC[2:0], standard filter element configuration, -> Set priority + filter |= (4 << 27); + + // bit 26:16 SFID1[10:0], Standard Filter ID1 + filter |= (cfg->id << 16); + + // bit 15:0 SFID2[15:10], Standard Filter ID2 + filter |= (cfg->mask << 10); + + *sid_filter_address = filter; + + + } + } + + } + + // set the number of SID filters + can_->SIDFC |= (num_of_sid_filter << FDCAN_SIDFC_LSS_Pos); + // set the number of XID filters + can_->XIDFC |= (num_of_xid_filter << FDCAN_XIDFC_LSE_Pos); + + // // Leave Init mode + can_->CCCR &= ~FDCAN_CCCR_INIT; + return 0; + } + + + return -ErrFilterNumConfigs; } bool CanIface::waitCCCRBitStateChange(uint32_t mask, bool target_state) @@ -646,7 +762,8 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) | FDCAN_IE_RF0NE // Rx FIFO 0 new message | FDCAN_IE_RF0FE // Rx FIFO 0 FIFO full | FDCAN_IE_RF1NE // Rx FIFO 1 new message - | FDCAN_IE_RF1FE; // Rx FIFO 1 FIFO full + | FDCAN_IE_RF1FE // Rx FIFO 1 FIFO full + | FDCAN_IE_BOE; // bus off state // Keep Rx interrupts on Line 0; move Tx to Line 1 can_->ILS = FDCAN_ILS_TCL; // TC interrupt on line 1 @@ -681,8 +798,9 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) * factor of 4 necessary in the address relative to the SA register values. */ - // Location of this interface's message RAM - address in CPU memory address - // and relative address (in words) used for configuration + +// Location of this interface's message RAM - address in CPU memory address +// and relative address (in words) used for configuration const uint32_t iface_ram_base = (2560 / 2) * self_index_; const uint32_t gl_ram_base = SRAMCAN_BASE; uint32_t ram_offset = iface_ram_base; @@ -692,14 +810,16 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) message_ram_.StdIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH; can_->SIDFC = ((n_stdid << FDCAN_SIDFC_LSS_Pos) | ram_offset << FDCAN_SIDFC_FLSSA_Pos); + memset((void *)message_ram_.StdIdFilterSA, 0, WORD_LENGTH * n_stdid); // make sure filters are disabled ram_offset += n_stdid; - // Extended ID Filters: Allow space for 128 filters (128 words) - const uint8_t n_extid = 128; + // Extended ID Filters: Allow space for 64 filters (128 words) + const uint8_t n_extid = 64; message_ram_.ExtIdFilterSA = gl_ram_base + ram_offset * WORD_LENGTH; can_->XIDFC = ((n_extid << FDCAN_XIDFC_LSE_Pos) | ram_offset << FDCAN_XIDFC_FLESA_Pos); - ram_offset += n_extid; + memset((void *)message_ram_.ExtIdFilterSA, 0, (2 * WORD_LENGTH) * n_extid); // make sure filters are disabled + ram_offset += 2 * n_extid; // Set size of each element in the Rx/Tx buffers and FIFOs can_->RXESC = 0; // 8 byte space for every element (Rx buffer, FIFO1, FIFO0) @@ -754,6 +874,24 @@ void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) pollErrorFlagsFromISR(); } + +void CanIface::handleBusOff() +{ + + /* + * The bus off recovery sequence consists of 128 occurrences of 11 consecutive recessive bits. MCAN controllers + * start sensing the bus looking for the recovery sequence when the INIT bit of control register (CCCR) is reset by + * the user. The bus off recovery sequence cannot be shortened by setting or resetting CCCR[INIT]. + * Summarizing, if the device raises a bus off condition, CCCR[INIT] is set stopping all bus activities. Once + * CCCR[INIT] has been cleared again by the software, the device will then wait for 129 occurrences of bus idle + * (129 x 11 consecutive recessive bits) before resuming on normal operation. At the end of the bus off recovery + * sequence, the error management counters will be reset, and so PSR.BO, ECR.TEC, and ECR.REC. + */ + + can_->CCCR &= ~FDCAN_CCCR_INIT; + +} + void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index) { UAVCAN_ASSERT(fifo_index < 2); diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index c054304d76..277b2526dd 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -80,14 +80,26 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), ModuleParams(nullptr), _node(can_driver, system_clock, _pool_allocator), +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) _arming_status_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) _beep_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _esc_controller(_node), _servo_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) _hardpoint_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) _safety_state_controller(_node), - _log_message_controller(_node), +#endif +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) _rgbled_controller(_node), +#endif + _log_message_controller(_node), _time_sync_master(_node), _time_sync_slave(_node), _node_status_monitor(_node), @@ -103,8 +115,10 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys std::abort(); } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanEscController::MAX_RATE_HZ); _mixing_interface_servo.mixingOutput().setMaxTopicUpdateRate(1000000 / UavcanServoController::MAX_RATE_HZ); +#endif } UavcanNode::~UavcanNode() @@ -395,8 +409,10 @@ UavcanNode::get_param(int remote_node_id, const char *name) void UavcanNode::update_params() { +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.updateParams(); _mixing_interface_servo.updateParams(); +#endif } int @@ -429,8 +445,11 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) } _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _instance->_mixing_interface_esc.ScheduleNow(); _instance->_mixing_interface_servo.ScheduleNow(); +#endif return OK; } @@ -482,6 +501,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) int ret; // UAVCAN_PUB_ARM +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) int32_t uavcan_pub_arm = 0; param_get(param_find("UAVCAN_PUB_ARM"), &uavcan_pub_arm); @@ -493,43 +513,60 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } } +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) ret = _beep_controller.init(); if (ret < 0) { return ret; } +#endif + // Actuators +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) ret = _esc_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) ret = _hardpoint_controller.init(); if (ret < 0) { return ret; } +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_CONTROLLER) ret = _safety_state_controller.init(); if (ret < 0) { return ret; } +#endif + ret = _log_message_controller.init(); if (ret < 0) { return ret; } +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) ret = _rgbled_controller.init(); if (ret < 0) { return ret; } +#endif + /* Start node info retriever to fetch node info from new nodes */ ret = _node_info_retriever.start(); @@ -552,6 +589,8 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) PX4_DEBUG("sensor bridge '%s' init ok", br->get_name()); } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) + // Ensure we don't exceed maximum limits and assumptions. FIXME: these should be static assertions if (UavcanEscController::max_output_value() >= UavcanEscController::DISARMED_OUTPUT_VALUE || UavcanEscController::max_output_value() > (int)UINT16_MAX) { @@ -560,6 +599,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events) } _mixing_interface_esc.mixingOutput().setAllDisarmedValues(UavcanEscController::DISARMED_OUTPUT_VALUE); +#endif /* Set up shared service clients */ _param_getset_client.setCallback(GetSetCallback(this, &UavcanNode::cb_getset)); @@ -921,15 +961,20 @@ UavcanNode::Run() pthread_mutex_unlock(&_node_mutex); if (_task_should_exit.load()) { + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) _mixing_interface_esc.mixingOutput().unregister(); _mixing_interface_esc.ScheduleClear(); + _mixing_interface_servo.mixingOutput().unregister(); _mixing_interface_servo.ScheduleClear(); +#endif ScheduleClear(); _instance = nullptr; } } +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) bool UavcanMixingInterfaceESC::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { @@ -974,6 +1019,7 @@ void UavcanMixingInterfaceServo::Run() _mixing_output.updateSubscriptions(false); pthread_mutex_unlock(&_node_mutex); } +#endif void UavcanNode::print_info() @@ -1016,10 +1062,13 @@ UavcanNode::print_info() printf("\n"); +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) printf("ESC outputs:\n"); _mixing_interface_esc.mixingOutput().printStatus(); + printf("Servo outputs:\n"); _mixing_interface_servo.mixingOutput().printStatus(); +#endif printf("\n"); diff --git a/src/drivers/uavcan/uavcan_main.hpp b/src/drivers/uavcan/uavcan_main.hpp index 7fcc86e968..54119259ef 100644 --- a/src/drivers/uavcan/uavcan_main.hpp +++ b/src/drivers/uavcan/uavcan_main.hpp @@ -46,15 +46,36 @@ #include #include +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) #include "actuators/esc.hpp" -#include "actuators/hardpoint.hpp" #include "actuators/servo.hpp" +#endif + +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) +#include "actuators/hardpoint.hpp" +#endif + + #include "allocator.hpp" + +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) #include "arming_status.hpp" +#endif + +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) #include "beep.hpp" +#endif + #include "logmessage.hpp" + +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) #include "rgbled.hpp" +#endif + +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) #include "safety_state.hpp" +#endif + #include "sensors/sensor_bridge.hpp" #include "uavcan_driver.hpp" #include "uavcan_servers.hpp" @@ -91,6 +112,8 @@ class UavcanNode; * a fixed rate or upon bus updates). * All work items are expected to run on the same work queue. */ + +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) class UavcanMixingInterfaceESC : public OutputModuleInterface { public: @@ -143,6 +166,7 @@ private: UavcanServoController &_servo_controller; MixingOutput _mixing_output{"UAVCAN_SV", UavcanServoController::MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; }; +#endif /** * A UAVCAN node. @@ -225,16 +249,30 @@ private: uavcan::Node<> _node; ///< library instance pthread_mutex_t _node_mutex; +#if defined(CONFIG_UAVCAN_ARMING_CONTROLLER) UavcanArmingStatus _arming_status_controller; +#endif +#if defined(CONFIG_UAVCAN_BEEP_CONTROLLER) UavcanBeepController _beep_controller; +#endif +#if defined(CONFIG_UAVCAN_OUTPUTS_CONTROLLER) UavcanEscController _esc_controller; - UavcanServoController _servo_controller; UavcanMixingInterfaceESC _mixing_interface_esc{_node_mutex, _esc_controller}; + + UavcanServoController _servo_controller; UavcanMixingInterfaceServo _mixing_interface_servo{_node_mutex, _servo_controller}; +#endif +#if defined(CONFIG_UAVCAN_HARDPOINT_CONTROLLER) UavcanHardpointController _hardpoint_controller; +#endif +#if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER) UavcanSafetyState _safety_state_controller; - UavcanLogMessage _log_message_controller; +#endif +#if defined(CONFIG_UAVCAN_RGB_CONTROLLER) UavcanRGBController _rgbled_controller; +#endif + + UavcanLogMessage _log_message_controller; uavcan::GlobalTimeSyncMaster _time_sync_master; uavcan::GlobalTimeSyncSlave _time_sync_slave; diff --git a/src/drivers/uavcannode/Publishers/GnssFix2.hpp b/src/drivers/uavcannode/Publishers/GnssFix2.hpp index e7453e04f9..5cd977a1bf 100644 --- a/src/drivers/uavcannode/Publishers/GnssFix2.hpp +++ b/src/drivers/uavcannode/Publishers/GnssFix2.hpp @@ -81,10 +81,10 @@ public: fix2.gnss_time_standard = fix2.GNSS_TIME_STANDARD_UTC; fix2.gnss_timestamp.usec = gps.time_utc_usec; - fix2.latitude_deg_1e8 = (int64_t)gps.lat * 10; - fix2.longitude_deg_1e8 = (int64_t)gps.lon * 10; - fix2.height_msl_mm = gps.alt; - fix2.height_ellipsoid_mm = gps.alt_ellipsoid; + fix2.latitude_deg_1e8 = (int64_t)(gps.latitude_deg * 1e8); + fix2.longitude_deg_1e8 = (int64_t)(gps.longitude_deg * 1e8); + fix2.height_msl_mm = (int32_t)(gps.altitude_msl_m * 1e3); + fix2.height_ellipsoid_mm = (int32_t)(gps.altitude_ellipsoid_m * 1e3); fix2.status = gps.fix_type; fix2.ned_velocity[0] = gps.vel_n_m_s; fix2.ned_velocity[1] = gps.vel_e_m_s; diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index 810a4fd0ca..2940906c25 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -158,6 +158,7 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr _time_sync_slave(_node), _fw_update_listner(_node), _param_server(_node), + _dyn_node_id_client(_node), _reset_timer(_node) { int res = pthread_mutex_init(&_node_mutex, nullptr); @@ -168,6 +169,10 @@ UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDr if (res < 0) { std::abort(); } + + // Ensure this param is marked as used + int32_t bitrate_temp = 0; + (void)param_get(param_find("CANNODE_BITRATE"), &bitrate_temp); } UavcanNode::~UavcanNode() @@ -451,6 +456,31 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); + const int can_init_res = _can->init((uint32_t)_bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + int rv = _node.start(); + + if (rv < 0) { + PX4_ERR("Failed to start the node"); + } + + // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation + + if (_node.getNodeID() == 0) { + + int client_start_res = _dyn_node_id_client.start( + _node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE + _node.getNodeID()); + + if (client_start_res < 0) { + PX4_ERR("Failed to start the dynamic node ID client"); + } + } + return 1; } @@ -477,67 +507,36 @@ void UavcanNode::Run() if (!_initialized) { + /* + * Waiting for the client to obtain a node ID. + * This may take a few seconds. + */ - const int can_init_res = _can->init((uint32_t)_bitrate); + if (_dyn_node_id_client.isAllocationComplete()) { + PX4_INFO("Got node ID %d", _dyn_node_id_client.getAllocatedNodeID().get()); - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); + _node.setNodeID(_dyn_node_id_client.getAllocatedNodeID()); } - int rv = _node.start(); + if (_node.getNodeID() != 0) { + up_time = hrt_absolute_time(); + get_node().setRestartRequestHandler(&restart_request_handler); + _param_server.start(&_param_manager); - if (rv < 0) { - PX4_ERR("Failed to start the node"); - } + // Set up the time synchronization + const int slave_init_res = _time_sync_slave.start(); - // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation - - if (_node.getNodeID() == 0) { - - uavcan::DynamicNodeIDClient client(_node); - - int client_start_res = client.start(_node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE - _node.getNodeID()); - - if (client_start_res < 0) { - PX4_ERR("Failed to start the dynamic node ID client"); + if (slave_init_res < 0) { + PX4_ERR("Failed to start time_sync_slave"); + _task_should_exit.store(true); } - watchdog_pet(); // If allocation takes too long reboot + _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - /* - * Waiting for the client to obtain a node ID. - * This may take a few seconds. - */ + _node.setModeOperational(); - while (!client.isAllocationComplete()) { - const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter - - if (res < 0) { - PX4_ERR("Transient failure: %d", res); - } - } - - _node.setNodeID(client.getAllocatedNodeID()); + _initialized = true; } - - up_time = hrt_absolute_time(); - get_node().setRestartRequestHandler(&restart_request_handler); - _param_server.start(&_param_manager); - - // Set up the time synchronization - const int slave_init_res = _time_sync_slave.start(); - - if (slave_init_res < 0) { - PX4_ERR("Failed to start time_sync_slave"); - _task_should_exit.store(true); - } - - _node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); - - _node.setModeOperational(); - - _initialized = true; } perf_begin(_cycle_perf); @@ -636,6 +635,19 @@ void UavcanNode::PrintInfo() { pthread_mutex_lock(&_node_mutex); + // Firmware version + printf("Hardware and software status:\n"); + printf("\tNode ID: %d\n", int(_node.getNodeID().get())); + printf("\tHardware version: %d.%d\n", + int(_node.getHardwareVersion().major), + int(_node.getHardwareVersion().minor)); + printf("\tSoftware version: %d.%d.%08x\n", + int(_node.getSoftwareVersion().major), + int(_node.getSoftwareVersion().minor), + int(_node.getSoftwareVersion().vcs_commit)); + + printf("\n"); + // Memory status printf("Pool allocator status:\n"); printf("\tCapacity hard/soft: %u/%u blocks\n", @@ -752,7 +764,6 @@ extern "C" int uavcannode_start(int argc, char *argv[]) } else #endif { - (void)param_get(param_find("CANNODE_NODE_ID"), &node_id); (void)param_get(param_find("CANNODE_BITRATE"), &bitrate); } } @@ -761,7 +772,7 @@ extern "C" int uavcannode_start(int argc, char *argv[]) #if defined(SUPPORT_ALT_CAN_BOOTLOADER) board_booted_by_px4() && #endif - (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast())) { + (node_id < 0 || node_id > uavcan::NodeID::Max)) { PX4_ERR("Invalid Node ID %" PRId32, node_id); return 1; } diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index f7a3a25374..bbe7158ec1 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -181,6 +181,8 @@ private: UavcanNodeParamManager _param_manager; uavcan::ParamServer _param_server; + uavcan::DynamicNodeIDClient _dyn_node_id_client; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; diff --git a/src/drivers/uavcannode/uavcannode_params.c b/src/drivers/uavcannode/uavcannode_params.c index 8a9e1ba0ee..5b6a27e44e 100644 --- a/src/drivers/uavcannode/uavcannode_params.c +++ b/src/drivers/uavcannode/uavcannode_params.c @@ -31,17 +31,6 @@ * ****************************************************************************/ -/** - * UAVCAN Node ID. - * - * Read the specs at http://uavcan.org to learn more about Node ID. - * - * @min 1 - * @max 125 - * @group UAVCAN - */ -PARAM_DEFINE_INT32(CANNODE_NODE_ID, 120); - /** * UAVCAN CAN bus bitrate. * diff --git a/src/drivers/uwb/uwb_sr150/module.yaml b/src/drivers/uwb/uwb_sr150/module.yaml index 68085187bf..7a62d0cc75 100644 --- a/src/drivers/uwb/uwb_sr150/module.yaml +++ b/src/drivers/uwb/uwb_sr150/module.yaml @@ -4,12 +4,10 @@ serial_config: port_config_param: name: UWB_PORT_CFG group: UWB - default: "" parameters: - group: UWB definitions: - UWB_INIT_OFF_X: description: short: UWB sensor X offset in body frame @@ -32,7 +30,7 @@ parameters: UWB_INIT_OFF_Z: description: - short: UWB sensor Y offset in body frame + short: UWB sensor Z offset in body frame long: UWB sensor positioning in relation to Drone in NED. Z offset. type: float unit: m @@ -40,14 +38,52 @@ parameters: increment: 0.01 default: 0.00 - UWB_INIT_OFF_YAW: + UWB_SENS_ROT: description: - short: UWB sensor YAW offset in body frame - long: UWB sensor positioning in relation to Drone in NED. Yaw rotation in relation to direction of FMU. - type: float - unit: deg - decimal: 1 - increment: 0.1 - default: 0.00 - - + short: UWB sensor orientation + long: The orientation of the sensor relative to the forward direction of the body frame. Look up table in src/lib/conversion/rotation.h + Default position is the antannaes downward facing, UWB board parallel with body frame. + type: enum + values: + 0: ROTATION_NONE + 1: ROTATION_YAW_45 + 2: ROTATION_YAW_90 + 3: ROTATION_YAW_135 + 4: ROTATION_YAW_180 + 5: ROTATION_YAW_225 + 6: ROTATION_YAW_270 + 7: ROTATION_YAW_315 + 8: ROTATION_ROLL_180 + 9: ROTATION_ROLL_180_YAW_45 + 10: ROTATION_ROLL_180_YAW_90 + 11: ROTATION_ROLL_180_YAW_135 + 12: ROTATION_PITCH_180 + 13: ROTATION_ROLL_180_YAW_225 + 14: ROTATION_ROLL_180_YAW_270 + 15: ROTATION_ROLL_180_YAW_315 + 16: ROTATION_ROLL_90 + 17: ROTATION_ROLL_90_YAW_45 + 18: ROTATION_ROLL_90_YAW_90 + 19: ROTATION_ROLL_90_YAW_135 + 20: ROTATION_ROLL_270 + 21: ROTATION_ROLL_270_YAW_45 + 22: ROTATION_ROLL_270_YAW_90 + 23: ROTATION_ROLL_270_YAW_135 + 24: ROTATION_PITCH_90 + 25: ROTATION_PITCH_270 + 26: ROTATION_PITCH_180_YAW_90 + 27: ROTATION_PITCH_180_YAW_270 + 28: ROTATION_ROLL_90_PITCH_90 + 29: ROTATION_ROLL_180_PITCH_90 + 30: ROTATION_ROLL_270_PITCH_90 + 31: ROTATION_ROLL_90_PITCH_180 + 32: ROTATION_ROLL_270_PITCH_180 + 33: ROTATION_ROLL_90_PITCH_270 + 34: ROTATION_ROLL_180_PITCH_270 + 35: ROTATION_ROLL_270_PITCH_270 + 36: ROTATION_ROLL_90_PITCH_180_YAW_90 + 37: ROTATION_ROLL_90_YAW_270 + 38: ROTATION_ROLL_90_PITCH_68_YAW_293 + 39: ROTATION_PITCH_315 + 40: ROTATION_ROLL_90_PITCH_315 + default: 0 diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp index 7f917adbc0..ec7b8f69ae 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -63,128 +63,130 @@ // The default baudrate of the uwb_sr150 module before configuration #define DEFAULT_BAUD B115200 -const uint8_t CMD_RANGING_STOP[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_STOP}; -const uint8_t CMD_RANGING_START[UWB_CMD_LEN ] = {UWB_CMD, 0x00, 0x02, UWB_DRONE_CTL, UWB_CMD_START}; -const uint8_t CMD_APP_START[UWB_CMD_LEN ] = {0x01, 0x00, 0x02, UWB_APP_START, UWB_PRECNAV_APP}; -const uint8_t CMD_APP_STOP[0x04 ] = {0x01, 0x00, 0x01, UWB_APP_STOP}; - extern "C" __EXPORT int uwb_sr150_main(int argc, char *argv[]); -UWB_SR150::UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug): +UWB_SR150::UWB_SR150(const char *port): ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)), _read_count_perf(perf_alloc(PC_COUNT, "uwb_sr150_count")), _read_err_perf(perf_alloc(PC_COUNT, "uwb_sr150_err")) { - _uwb_pos_debug = uwb_pos_debug; - // start serial port - _uart = open(device_name, O_RDWR | O_NOCTTY); - - if (_uart < 0) { err(1, "could not open %s", device_name); } - - int ret = 0; - struct termios uart_config {}; - ret = tcgetattr(_uart, &uart_config); - - if (ret < 0) { err(1, "failed to get attr"); } - - uart_config.c_oflag &= ~ONLCR; // no CR for every LF - ret = cfsetispeed(&uart_config, baudrate); - - if (ret < 0) { err(1, "failed to set input speed"); } - - ret = cfsetospeed(&uart_config, baudrate); - - if (ret < 0) { err(1, "failed to set output speed"); } - - ret = tcsetattr(_uart, TCSANOW, &uart_config); - - if (ret < 0) { err(1, "failed to set attr"); } + /* store port name */ + strncpy(_port, port, sizeof(_port) - 1); + /* enforce null termination */ + _port[sizeof(_port) - 1] = '\0'; } UWB_SR150::~UWB_SR150() { printf("UWB: Ranging Stopped\t\n"); - int written = write(_uart, &CMD_APP_STOP, sizeof(CMD_APP_STOP)); - - if (written < (int) sizeof(CMD_APP_STOP)) { - PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_APP_STOP)); - } + // stop{}; will be implemented when this is changed to a scheduled work task perf_free(_read_err_perf); perf_free(_read_count_perf); close(_uart); } -void UWB_SR150::run() +bool UWB_SR150::init() { - // Subscribe to parameter_update message - parameters_update(); - - //TODO replace with BLE grid configuration - grid_info_read(&_uwb_grid_info.target_pos); - _uwb_grid_info.num_anchors = 4; - _uwb_grid_info.initator_time = hrt_absolute_time(); - _uwb_grid_info.mac_mode = 0x00; - - /* Grid Info Message*/ - _uwb_grid.timestamp = hrt_absolute_time(); - _uwb_grid.initator_time = _uwb_grid_info.initator_time; - _uwb_grid.num_anchors = _uwb_grid_info.num_anchors; - - memcpy(&_uwb_grid.target_pos, &_uwb_grid_info.target_pos, sizeof(position_t) * 5); //write Grid positions - - _uwb_grid_pub.publish(_uwb_grid); - - /* Ranging Command */ - int written = write(_uart, CMD_RANGING_START, UWB_CMD_LEN); - - if (written < UWB_CMD_LEN) { - PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(uint8_t) * UWB_CMD_LEN); + // execute Run() on every sensor_accel publication + if (!_sensor_uwb_sub.registerCallback()) { + PX4_ERR("callback registration failed"); + return false; } - while (!should_exit()) { - written = UWB_SR150::distance(); //evaluate Ranging Messages until Stop - } + // alternatively, Run on fixed interval + // ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate - if (!written) { printf("ERROR: Distance Failed"); } - - // Automatic Stop. This should not be reachable - written = write(_uart, &CMD_RANGING_STOP, UWB_CMD_LEN); - - if (written < (int) sizeof(CMD_RANGING_STOP)) { - PX4_ERR("Only wrote %d bytes out of %d.", written, (int) sizeof(CMD_RANGING_STOP)); - } + return true; } -void UWB_SR150::grid_info_read(position_t *grid) +void UWB_SR150::start() { - //place holder, until UWB initiator can respond with Grid info - /* This Reads the position of each Anchor in the Grid. - Right now the Grid configuration is saved on the SD card. - In the future, we would like, that the Initiator responds with the Grid Information (Including Position). */ - PX4_INFO("Reading UWB GRID from SD... \t\n"); - FILE *file; - file = fopen(UWB_GRID_CONFIG, "r"); + /* schedule a cycle to start things */ + ScheduleNow(); +} - int bread = 0; +void UWB_SR150::stop() +{ + ScheduleClear(); +} - for (int i = 0; i < 5; i++) { - bread += fscanf(file, "%hd,%hd,%hd\n", &grid[i].x, &grid[i].y, &grid[i].z); +void UWB_SR150::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; } - if (bread == 5 * 3) { - PX4_INFO("GRID INFO READ! bytes read: %d \t\n", bread); + if (_uart < 0) { + /* open fd */ + _uart = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK); - } else { //use UUID from Grid survey - PX4_INFO("GRID INFO Missing! bytes read: %d \t\n Using standrd Grid \t\n", bread); - position_t grid_setup[5] = {{0x0, 0x0, 0x0}, {(int16_t)0xff68, 0x0, 0x0a}, {0x99, 0x0, 0x0a}, {0x0, 0x96, 0x64}, {0x0, (int16_t)0xff6a, 0x63}}; - memcpy(grid, &grid_setup, sizeof(grid_setup)); - PX4_INFO("Insert \"uwb_grid_config.csv\" containing gridinfo in cm at \"/fs/microsd/etc/\".\t\n"); - PX4_INFO("n + 1 Anchor Positions starting with Landing Target. Int16 Format: \"x,y,z\" \t\n"); + if (_uart < 0) { + PX4_ERR("open failed (%i)", errno); + return; + } + + struct termios uart_config; + + int termios_state; + + /* fill the struct for the new configuration */ + tcgetattr(_uart, &uart_config); + + /* clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + //TODO: should I keep this? + /* no parity, one stop bit */ + uart_config.c_cflag &= ~(CSTOPB | PARENB); + + unsigned speed = DEFAULT_BAUD; + + /* set baud rate */ + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d ISPD", termios_state); + } + + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + PX4_ERR("CFG: %d OSPD", termios_state); + } + + if ((termios_state = tcsetattr(_uart, TCSANOW, &uart_config)) < 0) { + PX4_ERR("baud %d ATTR", termios_state); + } } - fclose(file); + /* perform collection */ + int collect_ret = collectData(); + + if (collect_ret == -EAGAIN) { + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ + ScheduleDelayed(1042 * 8); + + return; + } + + if (OK != collect_ret) { + + /* we know the sensor needs about four seconds to initialize */ + if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) { + PX4_ERR("collection error #%u", _consecutive_fail_count); + } + + _consecutive_fail_count++; + + /* restart the measurement state machine */ + start(); + return; + + } else { + /* apparently success */ + _consecutive_fail_count = 0; + } } int UWB_SR150::custom_command(int argc, char *argv[]) @@ -214,43 +216,20 @@ $ uwb start -d /dev/ttyS2 PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, "", "Name of device for serial communication with UWB", false); PRINT_MODULE_USAGE_PARAM_STRING('b', nullptr, "", "Baudrate for serial communication", false); - PRINT_MODULE_USAGE_PARAM_STRING('p', nullptr, "", "Position Debug: displays errors in Multilateration", false); PRINT_MODULE_USAGE_COMMAND("stop"); PRINT_MODULE_USAGE_COMMAND("status"); return 0; } int UWB_SR150::task_spawn(int argc, char *argv[]) -{ - int task_id = px4_task_spawn_cmd( - "uwb_driver", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 2048, - &run_trampoline, - argv - ); - - if (task_id < 0) { - return -errno; - - } else { - _task_id = task_id; - return 0; - } -} - -UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[]) { int ch; int option_index = 1; const char *option_arg; - const char *device_name = nullptr; - bool error_flag = false; + const char *device_name = UWB_DEFAULT_PORT; int baudrate = 0; - bool uwb_pos_debug = false; // Display UWB position calculation debug Messages - while ((ch = px4_getopt(argc, argv, "d:b:p", &option_index, &option_arg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "d:b", &option_index, &option_arg)) != EOF) { switch (ch) { case 'd': device_name = option_arg; @@ -260,47 +239,54 @@ UWB_SR150 *UWB_SR150::instantiate(int argc, char *argv[]) px4_get_parameter_value(option_arg, baudrate); break; - case 'p': - - uwb_pos_debug = true; - break; - default: PX4_WARN("Unrecognized flag: %c", ch); - error_flag = true; break; } } - if (!error_flag && device_name == nullptr) { - print_usage("Device name not provided. Using default Device: TEL1:/dev/ttyS4 \n"); - device_name = "TEL2"; - error_flag = true; - } + UWB_SR150 *instance = new UWB_SR150(device_name); - if (!error_flag && baudrate == 0) { - printf("Baudrate not provided. Using default Baud: 115200 \n"); - baudrate = B115200; - } + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; - if (!error_flag && uwb_pos_debug == true) { - printf("UWB Position algorithm Debugging \n"); - } + instance->ScheduleOnInterval(5000_us); - if (error_flag) { - PX4_WARN("Failed to start UWB driver. \n"); - return nullptr; + if (instance->init()) { + return PX4_OK; + } } else { - PX4_INFO("Constructing UWB_SR150. Device: %s", device_name); - return new UWB_SR150(device_name, baudrate, uwb_pos_debug); + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int uwb_sr150_main(int argc, char *argv[]) +{ + return UWB_SR150::main(argc, argv); +} + +void UWB_SR150::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); } } -int UWB_SR150::distance() +int UWB_SR150::collectData() { - _uwb_init_offset_v3f = matrix::Vector3f(_uwb_init_off_x.get(), _uwb_init_off_y.get(), - _uwb_init_off_z.get()); //set offset at the start uint8_t *buffer = (uint8_t *) &_distance_result_msg; FD_ZERO(&_uart_set); @@ -350,366 +336,54 @@ int UWB_SR150::distance() perf_count(_read_count_perf); // All of the following criteria must be met for the message to be acceptable: - // - Size of message == sizeof(distance_msg_t) (51 bytes) + // - Size of message == sizeof(distance_msg_t) (36 bytes) // - status == 0x00 - // - Values of all 3 position measurements are reasonable - // (If one or more anchors is missed, then position might be an unreasonably large number.) - bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); //|| - //(buffer_location == sizeof(grid_msg_t) && _distance_result_msg.stop == 0x1b) - //); + bool ok = (buffer_location == sizeof(distance_msg_t) && _distance_result_msg.stop == 0x1b); if (ok) { /* Ranging Message*/ - _uwb_distance.timestamp = hrt_absolute_time(); - _uwb_distance.time_uwb_ms = _distance_result_msg.time_uwb_ms; - _uwb_distance.counter = _distance_result_msg.seq_ctr; - _uwb_distance.sessionid = _distance_result_msg.sessionId; - _uwb_distance.time_offset = _distance_result_msg.range_interval; + _sensor_uwb.timestamp = hrt_absolute_time(); - for (int i = 0; i < 4; i++) { - _uwb_distance.anchor_distance[i] = _distance_result_msg.measurements[i].distance; - _uwb_distance.nlos[i] = _distance_result_msg.measurements[i].nLos; - _uwb_distance.aoafirst[i] = float(_distance_result_msg.measurements[i].aoaFirst) / - 128; // Angle of Arrival has Format Q9.7; dividing by 2^7 results in the correct value - } + _sensor_uwb.sessionid = _distance_result_msg.sessionId; + _sensor_uwb.time_offset = _distance_result_msg.range_interval; + _sensor_uwb.counter = _distance_result_msg.seq_ctr; + _sensor_uwb.mac = _distance_result_msg.MAC; - // Algorithm goes here - UWB_POS_ERROR_CODES UWB_POS_ERROR = UWB_SR150::localization(); + _sensor_uwb.mac_dest = _distance_result_msg.measurements.MAC; + _sensor_uwb.status = _distance_result_msg.measurements.status; + _sensor_uwb.nlos = _distance_result_msg.measurements.nLos; + _sensor_uwb.distance = double(_distance_result_msg.measurements.distance) / 100; - _uwb_distance.status = UWB_POS_ERROR; - if (UWB_OK == UWB_POS_ERROR) { + /*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/ + _sensor_uwb.aoa_azimuth_dev = - double(_distance_result_msg.measurements.aoa_azimuth) / 128; + _sensor_uwb.aoa_elevation_dev = - double(_distance_result_msg.measurements.aoa_elevation) / 128; + _sensor_uwb.aoa_azimuth_resp = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128; + _sensor_uwb.aoa_elevation_resp = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128; - _uwb_distance.position[0] = _rel_pos(0); - _uwb_distance.position[1] = _rel_pos(1); - _uwb_distance.position[2] = _rel_pos(2); - } else { - //only print the error if debug is enabled - if (_uwb_pos_debug) { - switch (UWB_POS_ERROR) { //UWB POSITION ALGORItHM Errors - case UWB_ANC_BELOW_THREE: - PX4_INFO("UWB not enough anchors for doing localization"); - break; + /*Angle of Arrival has Format Q9.7; dividing by 2^7 and negating results in the correct value*/ + _sensor_uwb.aoa_azimuth_fom = - double(_distance_result_msg.measurements.aoa_azimuth) / 128; + _sensor_uwb.aoa_elevation_fom = - double(_distance_result_msg.measurements.aoa_elevation) / 128; + _sensor_uwb.aoa_dest_azimuth_fom = - double(_distance_result_msg.measurements.aoa_dest_azimuth) / 128; + _sensor_uwb.aoa_dest_elevation_fom = - double(_distance_result_msg.measurements.aoa_dest_elevation) / 128; - case UWB_LIN_DEP_FOR_THREE: - PX4_INFO("UWB localization: linear dependent with 3 Anchors"); - break; + /* Sensor physical offset*/ //for now we propagate the physical configuration via Uorb + _sensor_uwb.orientation = _sensor_rot.get(); + _sensor_uwb.offset_x = _offset_x.get(); + _sensor_uwb.offset_y = _offset_y.get(); + _sensor_uwb.offset_z = _offset_z.get(); - case UWB_ANC_ON_ONE_LEVEL: - PX4_INFO("UWB localization: Anchors are on a X,Y Plane and there are not enought Anchors"); - break; - - case UWB_LIN_DEP_FOR_FOUR: - PX4_INFO("UWB localization: linear dependent with four or more Anchors"); - break; - - case UWB_RANK_ZERO: - PX4_INFO("UWB localization: rank is zero"); - break; - - default: - PX4_INFO("UWB localization: Unknown failure in Position Algorithm"); - break; - } - } - } - - _uwb_distance_pub.publish(_uwb_distance); + _sensor_uwb_pub.publish(_sensor_uwb); } else { - //PX4_ERR("Read %d bytes instead of %d.", (int) buffer_location, (int) sizeof(distance_msg_t)); perf_count(_read_err_perf); if (buffer_location == 0) { PX4_WARN("UWB module is not responding."); - - //TODO add retry Ranging Start Message. Sometimes the UWB devices Crashes. (Check Power) } } return 1; } - -UWB_POS_ERROR_CODES UWB_SR150::localization() -{ -// WIP - /****************************************************** - ****************** 3D Localization ******************* - *****************************************************/ - - /*!@brief: This function calculates the 3D position of the initiator from the anchor distances and positions using least squared errors. - * The function expects more than 4 anchors. The used equation system looks like follows:\n - \verbatim - - - - | M_11 M_12 M_13 | x b[0] - | M_12 M_22 M_23 | * y = b[1] - | M_23 M_13 M_33 | z b[2] - - - - \endverbatim - * @param distances_cm_in_pt: Pointer to array that contains the distances to the anchors in cm (including invalid results) - * @param no_distances: Number of valid distances in distance array (it's not the size of the array) - * @param anchor_pos: Pointer to array that contains anchor positions in cm (including positions related to invalid results) - * @param no_anc_positions: Number of valid anchor positions in the position array (it's not the size of the array) - * @param position_result_pt: Pointer toposition. position_t variable that holds the result of this calculation - * @return: The function returns a status code. */ - - /* Algorithm used: - * Linear Least Sqaures to solve Multilateration - * with a Special case if there are only 3 Anchors. - * Output is the Coordinates of the Initiator in relation to Anchor 0 in NEU (North-East-Up) Framing - * In cm - */ - - /* Resulting Position Vector*/ - int64_t x_pos = 0; - int64_t y_pos = 0; - int64_t z_pos = 0; - /* Matrix components (3*3 Matrix resulting from least square error method) [cm^2] */ - int64_t M_11 = 0; - int64_t M_12 = 0; // = M_21 - int64_t M_13 = 0; // = M_31 - int64_t M_22 = 0; - int64_t M_23 = 0; // = M_23 - int64_t M_33 = 0; - - /* Vector components (3*1 Vector resulting from least square error method) [cm^3] */ - int64_t b[3] = {0}; - - /* Miscellaneous variables */ - int64_t temp = 0; - int64_t temp2 = 0; - int64_t nominator = 0; - int64_t denominator = 0; - bool anchors_on_x_y_plane = true; // Is true, if all anchors are on the same height => x-y-plane - bool lin_dep = true; // All vectors are linear dependent, if this variable is true - uint8_t ind_y_indi = - 0; //numberr of independet vectors // First anchor index, for which the second row entry of the matrix [(x_1 - x_0) (x_2 - x_0) ... ; (y_1 - x_0) (y_2 - x_0) ...] is non-zero => linear independent - - /* Arrays for used distances and anchor positions (without rejected ones) */ - uint8_t no_distances = _uwb_grid_info.num_anchors; - uint32_t distances_cm[no_distances]; - position_t anchor_pos[no_distances]; //position in CM - uint8_t no_valid_distances = 0; - - /* Reject invalid distances (including related anchor position) */ - for (int i = 0; i < no_distances; i++) { - if (_distance_result_msg.measurements[i].distance != 0xFFFFu) { - //excludes any distance that is 0xFFFFU (int16 Maximum Value) - distances_cm[no_valid_distances] = _distance_result_msg.measurements[i].distance; - anchor_pos[no_valid_distances] = _uwb_grid_info.anchor_pos[i]; - no_valid_distances++; - - } - } - - /* Check, if there are enough valid results for doing the localization at all */ - if (no_valid_distances < 3) { - return UWB_ANC_BELOW_THREE; - } - - /* Check, if anchors are on the same x-y plane */ - for (int i = 1; i < no_valid_distances; i++) { - if (anchor_pos[i].z != anchor_pos[0].z) { - anchors_on_x_y_plane = false; - break; - } - } - - /**** Check, if there are enough linear independent anchor positions ****/ - - /* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) ... | has rank 2 - * |(y_1 - y_0) (y_2 - y_0) ... | */ - - for (ind_y_indi = 2; ((ind_y_indi < no_valid_distances) && (lin_dep == true)); ind_y_indi++) { - temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].x - - (int64_t)anchor_pos[0].x); - temp2 = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].x - - (int64_t)anchor_pos[0].x); - - if ((temp - temp2) != 0) { - lin_dep = false; - break; - } - } - - /* Leave function, if rank is below 2 */ - if (lin_dep == true) { - return UWB_LIN_DEP_FOR_THREE; - } - - /* If the anchors are not on the same plane, three vectors must be independent => check */ - if (!anchors_on_x_y_plane) { - /* Check, if there are enough valid results for doing the localization */ - if (no_valid_distances < 4) { - return UWB_ANC_ON_ONE_LEVEL; - } - - /* Check, if the matrix |(x_1 - x_0) (x_2 - x_0) (x_3 - x_0) ... | has rank 3 (Rank y, y already checked) - * |(y_1 - y_0) (y_2 - y_0) (y_3 - y_0) ... | - * |(z_1 - z_0) (z_2 - z_0) (z_3 - z_0) ... | */ - lin_dep = true; - - for (int i = 2; ((i < no_valid_distances) && (lin_dep == true)); i++) { - if (i != ind_y_indi) { - /* (x_1 - x_0)*[(y_2 - y_0)(z_n - z_0) - (y_n - y_0)(z_2 - z_0)] */ - temp = ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - - (int64_t)anchor_pos[0].z); - temp -= ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z - - (int64_t)anchor_pos[0].z); - temp2 = ((int64_t)anchor_pos[1].x - (int64_t)anchor_pos[0].x) * temp; - - /* Add (x_2 - x_0)*[(y_n - y_0)(z_1 - z_0) - (y_1 - y_0)(z_n - z_0)] */ - temp = ((int64_t)anchor_pos[i].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - (int64_t)anchor_pos[0].z); - temp -= ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[i].z - (int64_t)anchor_pos[0].z); - temp2 += ((int64_t)anchor_pos[ind_y_indi].x - (int64_t)anchor_pos[0].x) * temp; - - /* Add (x_n - x_0)*[(y_1 - y_0)(z_2 - z_0) - (y_2 - y_0)(z_1 - z_0)] */ - temp = ((int64_t)anchor_pos[1].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[ind_y_indi].z - - (int64_t)anchor_pos[0].z); - temp -= ((int64_t)anchor_pos[ind_y_indi].y - (int64_t)anchor_pos[0].y) * ((int64_t)anchor_pos[1].z - - (int64_t)anchor_pos[0].z); - temp2 += ((int64_t)anchor_pos[i].x - (int64_t)anchor_pos[0].x) * temp; - - if (temp2 != 0) { lin_dep = false; } - } - } - - /* Leave function, if rank is below 3 */ - if (lin_dep == true) { - return UWB_LIN_DEP_FOR_FOUR; - } - } - - /************************************************** Algorithm ***********************************************************************/ - - /* Writing values resulting from least square error method (A_trans*A*x = A_trans*r; row 0 was used to remove x^2,y^2,z^2 entries => index starts at 1) */ - for (int i = 1; i < no_valid_distances; i++) { - /* Matrix (needed to be multiplied with 2, afterwards) */ - M_11 += sq((int64_t)(anchor_pos[i].x - anchor_pos[0].x)); - M_12 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].y - anchor_pos[0].y)); - M_13 += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - M_22 += sq((int64_t)(anchor_pos[i].y - anchor_pos[0].y)); - M_23 += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * (int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - M_33 += sq((int64_t)(anchor_pos[i].z - anchor_pos[0].z)); - - /* Vector */ - temp = sq(distances_cm[0]) - sq(distances_cm[i]) - + sq(anchor_pos[i].x) + sq(anchor_pos[i].y) - + sq(anchor_pos[i].z) - sq(anchor_pos[0].x) - - sq(anchor_pos[0].y) - sq(anchor_pos[0].z); - - b[0] += (int64_t)((int64_t)(anchor_pos[i].x - anchor_pos[0].x) * temp); - b[1] += (int64_t)((int64_t)(anchor_pos[i].y - anchor_pos[0].y) * temp); - b[2] += (int64_t)((int64_t)(anchor_pos[i].z - anchor_pos[0].z) * temp); - } - - M_11 = 2 * M_11; - M_12 = 2 * M_12; - M_13 = 2 * M_13; - M_22 = 2 * M_22; - M_23 = 2 * M_23; - M_33 = 2 * M_33; - - /* Calculating the z-position, if calculation is possible (at least one anchor at z != 0) */ - if (anchors_on_x_y_plane == false) { - nominator = b[0] * (M_12 * M_23 - M_13 * M_22) + b[1] * (M_12 * M_13 - M_11 * M_23) + b[2] * - (M_11 * M_22 - M_12 * M_12); // [cm^7] - denominator = M_11 * (M_33 * M_22 - M_23 * M_23) + 2 * M_12 * M_13 * M_23 - M_33 * M_12 * M_12 - M_22 * M_13 * - M_13; // [cm^6] - - /* Check, if denominator is zero (Rank of matrix not high enough) */ - if (denominator == 0) { - return UWB_RANK_ZERO; - } - - z_pos = ((nominator * 10) / denominator + 5) / 10; // [cm] - } - - /* Else prepare for different calculation approach (after x and y were calculated) */ - else { - z_pos = 0; - } - - /* Calculating the y-position */ - nominator = b[1] * M_11 - b[0] * M_12 - (z_pos * (M_11 * M_23 - M_12 * M_13)); // [cm^5] - denominator = M_11 * M_22 - M_12 * M_12;// [cm^4] - - /* Check, if denominator is zero (Rank of matrix not high enough) */ - if (denominator == 0) { - return UWB_RANK_ZERO; - } - - y_pos = ((nominator * 10) / denominator + 5) / 10; // [cm] - - /* Calculating the x-position */ - nominator = b[0] - z_pos * M_13 - y_pos * M_12; // [cm^3] - denominator = M_11; // [cm^2] - - x_pos = ((nominator * 10) / denominator + 5) / 10;// [cm] - - /* Calculate z-position form x and y coordinates, if z can't be determined by previous steps (All anchors at z_n = 0) */ - if (anchors_on_x_y_plane == true) { - /* Calculate z-positon relative to the anchor grid's height */ - for (int i = 0; i < no_distances; i++) { - /* z² = dis_meas_n² - (x - x_anc_n)² - (y - y_anc_n)² */ - temp = (int64_t)((int64_t)pow(distances_cm[i], 2) - - (int64_t)pow((x_pos - (int64_t)anchor_pos[i].x), 2) - - (int64_t)pow((y_pos - (int64_t)anchor_pos[i].y), 2)); - - /* z² must be positive, else x and y must be wrong => calculate positive sqrt and sum up all calculated heights, if positive */ - if (temp >= 0) { - z_pos += (int64_t)sqrt(temp); - - } else { - z_pos = 0; - } - } - - z_pos = z_pos / no_distances; // Divide sum by number of distances to get the average - - /* Add height of the anchor grid's height */ - z_pos += anchor_pos[0].z; - } - - //Output is the Coordinates of the Initiator in relation to 0,0,0 in NEU (North-East-Up) Framing - // The end goal of this math is to get the position relative to the landing point in the NED frame. - _current_position_uwb_init = matrix::Vector3f(x_pos, y_pos, z_pos); - - // Construct the rotation from the UWB_R4frame to the NWU frame. - // The UWB_SR150 frame is just NWU, rotated by some amount about the Z (up) axis. (Parameter Yaw offset) - // To get back to NWU, just rotate by negative this amount about Z. - _uwb_init_to_nwu = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, - -(_uwb_init_off_yaw.get() * M_PI_F / 180.0f))); // - // The actual conversion: - // - Subtract _landing_point to get the position relative to the landing point, in UWB_R4 frame - // - Rotate by _rddrone_to_nwu to get into the NWU frame - // - Rotate by _nwu_to_ned to get into the NED frame - _current_position_ned = _nwu_to_ned * _uwb_init_to_nwu * _current_position_uwb_init; - - // Now the position is the landing point relative to the vehicle. - // so the only thing left is to convert cm to Meters and to add the Initiator offset - _rel_pos = _current_position_ned / 100 + _uwb_init_offset_v3f; - - // The UWB report contains the position of the vehicle relative to the landing point. - - return UWB_OK; -} - -int uwb_sr150_main(int argc, char *argv[]) -{ - return UWB_SR150::main(argc, argv); -} - -void UWB_SR150::parameters_update() -{ - if (_parameter_update_sub.updated()) { - parameter_update_s param_update; - _parameter_update_sub.copy(¶m_update); - - // If any parameter updated, call updateParams() to check if - // this class attributes need updating (and do so). - updateParams(); - } -} diff --git a/src/drivers/uwb/uwb_sr150/uwb_sr150.h b/src/drivers/uwb/uwb_sr150/uwb_sr150.h index 5b42b50e16..a405c361ea 100644 --- a/src/drivers/uwb/uwb_sr150/uwb_sr150.h +++ b/src/drivers/uwb/uwb_sr150/uwb_sr150.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -38,101 +38,63 @@ #include #include #include +#include +#include #include #include -#include +#include + #include -#include -#include -#include -#include #include +#include #include -#include +#include +#include + #include -#include + +#define UWB_DEFAULT_PORT "/dev/ttyS1" using namespace time_literals; -#define UWB_CMD 0x8e -#define UWB_CMD_START 0x01 -#define UWB_CMD_STOP 0x00 -#define UWB_CMD_RANGING 0x0A -#define STOP_B 0x0A - -#define UWB_PRECNAV_APP 0x04 -#define UWB_APP_START 0x10 -#define UWB_APP_STOP 0x11 -#define UWB_SESSION_START 0x22 -#define UWB_SESSION_STOP 0x23 -#define UWB_RANGING_START 0x01 -#define UWB_RANGING_STOP 0x00 -#define UWB_DRONE_CTL 0x0A - -#define UWB_CMD_LEN 0x05 -#define UWB_CMD_DISTANCE_LEN 0x21 -#define UWB_MAC_MODE 2 -#define MAX_ANCHORS 12 -#define UWB_GRID_CONFIG "/fs/microsd/etc/uwb_grid_config.csv" - -typedef struct { //needs higher accuracy? - float lat, lon, alt, yaw; //offset to true North -} gps_pos_t; - typedef struct { - int16_t x, y, z; //axis in cm -} position_t; // Position of a device or target in 3D space - -enum UWB_POS_ERROR_CODES { - UWB_OK, - UWB_ANC_BELOW_THREE, - UWB_LIN_DEP_FOR_THREE, - UWB_ANC_ON_ONE_LEVEL, - UWB_LIN_DEP_FOR_FOUR, - UWB_RANK_ZERO -}; - -typedef struct { - uint8_t MAC[2]; // MAC Adress of UWB device - uint8_t status; // Status of Measurement - uint16_t distance; // Distance in cm - uint8_t nLos; // line of sight y/n - uint16_t aoaFirst; // Angle of Arrival of incoming msg + uint16_t MAC; // MAC address of UWB device + uint8_t status; // Status of Measurement + uint16_t distance; // Distance in cm + uint8_t nLos; // line of sight y/n + int16_t aoa_azimuth; // AOA of incoming msg for Azimuth antenna pairing + int16_t aoa_elevation; // AOA of incoming msg for Altitude antenna pairing + int16_t aoa_dest_azimuth; // AOA destination Azimuth + int16_t aoa_dest_elevation; // AOA destination elevation + uint8_t aoa_azimuth_FOM; // AOA Azimuth FOM + uint8_t aoa_elevation_FOM; // AOA Elevation FOM + uint8_t aoa_dest_azimuth_FOM; // AOA Azimuth FOM + uint8_t aoa_dest_elevation_FOM; // AOA Elevation FOM } __attribute__((packed)) UWB_range_meas_t; typedef struct { - uint32_t initator_time; //timestamp of init - uint32_t sessionId; // Session ID of UWB session - uint8_t num_anchors; //number of anchors - gps_pos_t target_gps; //GPS position of Landing Point - uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte - uint8_t MAC[UWB_MAC_MODE][MAX_ANCHORS]; - position_t target_pos; //target position - position_t anchor_pos[MAX_ANCHORS]; // Position of each anchor - uint8_t stop; // Should be 27 -} grid_msg_t; - -typedef struct { - uint8_t cmd; // Should be 0x8E for distance result message - uint16_t len; // Should be 0x30 for distance result message - uint32_t time_uwb_ms; // Timestamp of UWB device in ms - uint32_t seq_ctr; // Number of Ranges since last Start of Ranging - uint32_t sessionId; // Session ID of UWB session - uint32_t range_interval; // Time between ranging rounds - uint8_t mac_mode; // MAC adress mode, either 2 Byte or 8 Byte - uint8_t no_measurements; // MAC adress mode, either 2 Byte or 8 Byte - UWB_range_meas_t measurements[4]; //Raw anchor_distance distances in CM 2*9 - uint8_t stop; // Should be 0x1B + uint8_t cmd; // Should be 0x8E for distance result message + uint16_t len; // Should be 0x30 for distance result message + uint32_t seq_ctr; // Number of Ranges since last Start of Ranging + uint32_t sessionId; // Session ID of UWB session + uint32_t range_interval; // Time between ranging rounds + uint16_t MAC; // MAC address of UWB device + UWB_range_meas_t measurements; //Raw anchor_distance distances in CM 2*9 + uint8_t stop; // Should be 0x1B } __attribute__((packed)) distance_msg_t; -class UWB_SR150 : public ModuleBase, public ModuleParams +class UWB_SR150 : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - UWB_SR150(const char *device_name, speed_t baudrate, bool uwb_pos_debug); - + UWB_SR150(const char *port); ~UWB_SR150(); + /** + * @see ModuleBase::task_spawn + */ + static int task_spawn(int argc, char *argv[]); + /** * @see ModuleBase::custom_command */ @@ -143,67 +105,51 @@ public: */ static int print_usage(const char *reason = nullptr); - /** - * @see ModuleBase::Multilateration - */ - UWB_POS_ERROR_CODES localization(); + bool init(); - /** - * @see ModuleBase::Distance Result - */ - int distance(); + void start(); - /** - * @see ModuleBase::task_spawn - */ - static int task_spawn(int argc, char *argv[]); + void stop(); - static UWB_SR150 *instantiate(int argc, char *argv[]); - - void run() override; + int collectData(); private: - static constexpr int64_t sq(int64_t x) { return x * x; } void parameters_update(); - void grid_info_read(position_t *grid); + void Run() override; - DEFINE_PARAMETERS( - (ParamFloat) _uwb_init_off_x, - (ParamFloat) _uwb_init_off_y, - (ParamFloat) _uwb_init_off_z, - (ParamFloat) _uwb_init_off_yaw - ) + // Publications + uORB::Publication _sensor_uwb_pub{ORB_ID(sensor_uwb)}; + // Subscriptions + uORB::SubscriptionCallbackWorkItem _sensor_uwb_sub{this, ORB_ID(sensor_uwb)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - int _uart; - fd_set _uart_set; - struct timeval _uart_timeout {}; - bool _uwb_pos_debug; - - uORB::Publication _uwb_grid_pub{ORB_ID(uwb_grid)}; - uwb_grid_s _uwb_grid{}; - - uORB::Publication _uwb_distance_pub{ORB_ID(uwb_distance)}; - uwb_distance_s _uwb_distance{}; - - uORB::Publication _landing_target_pub{ORB_ID(landing_target_pose)}; - landing_target_pose_s _landing_target{}; - - grid_msg_t _uwb_grid_info{}; - distance_msg_t _distance_result_msg{}; - matrix::Vector3f _rel_pos; - - matrix::Dcmf _uwb_init_to_nwu; - matrix::Dcmf _nwu_to_ned{matrix::Eulerf(M_PI_F, 0.0f, 0.0f)}; - matrix::Vector3f _current_position_uwb_init; - matrix::Vector3f _current_position_ned; - matrix::Vector3f _uwb_init_offset_v3f; - + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _uwb_port_cfg, + (ParamFloat) _offset_x, + (ParamFloat) _offset_y, + (ParamFloat) _offset_z, + (ParamInt) _sensor_rot + ) + // Performance (perf) counters perf_counter_t _read_count_perf; perf_counter_t _read_err_perf; -}; + sensor_uwb_s _sensor_uwb{}; + + char _port[20] {}; + hrt_abstime param_timestamp{0}; + + int _uart{-1}; + fd_set _uart_set; + struct timeval _uart_timeout {}; + + unsigned _consecutive_fail_count; + int _interval{100000}; + + distance_msg_t _distance_result_msg{}; +}; #endif //PX4_RDDRONE_H diff --git a/src/examples/fake_gps/FakeGps.cpp b/src/examples/fake_gps/FakeGps.cpp index 6b08b58666..e2f966e023 100644 --- a/src/examples/fake_gps/FakeGps.cpp +++ b/src/examples/fake_gps/FakeGps.cpp @@ -35,12 +35,12 @@ using namespace time_literals; -FakeGps::FakeGps(double latitude_deg, double longitude_deg, float altitude_m) : +FakeGps::FakeGps(double latitude_deg, double longitude_deg, double altitude_m) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default), - _latitude(latitude_deg * 10e6), - _longitude(longitude_deg * 10e6), - _altitude(altitude_m * 10e2f) + _latitude(latitude_deg), + _longitude(longitude_deg), + _altitude(altitude_m) { } @@ -60,10 +60,10 @@ void FakeGps::Run() sensor_gps_s sensor_gps{}; sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954; - sensor_gps.lat = _latitude; - sensor_gps.lon = _longitude; - sensor_gps.alt = _altitude; - sensor_gps.alt_ellipsoid = _altitude; + sensor_gps.latitude_deg = _latitude; + sensor_gps.longitude_deg = _longitude; + sensor_gps.altitude_msl_m = _altitude; + sensor_gps.altitude_ellipsoid_m = _altitude; sensor_gps.s_variance_m_s = 0.3740f; sensor_gps.c_variance_rad = 0.6737f; sensor_gps.eph = 2.1060f; diff --git a/src/examples/fake_gps/FakeGps.hpp b/src/examples/fake_gps/FakeGps.hpp index ef5a9197eb..eae3b47512 100644 --- a/src/examples/fake_gps/FakeGps.hpp +++ b/src/examples/fake_gps/FakeGps.hpp @@ -45,7 +45,7 @@ class FakeGps : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, float altitude_m = 30.1f); + FakeGps(double latitude_deg = 29.6603018, double longitude_deg = -82.3160500, double altitude_m = 30.1); ~FakeGps() override = default; @@ -67,7 +67,7 @@ private: uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; - int32_t _latitude{296603018}; // Latitude in 1e-7 degrees - int32_t _longitude{-823160500}; // Longitude in 1e-7 degrees - int32_t _altitude{30100}; // Altitude in 1e-3 meters above MSL, (millimetres) + double _latitude{29.6603018}; // Latitude in degrees + double _longitude{-82.3160500}; // Longitude in degrees + double _altitude{30.1}; // Altitude in meters above MSL, (millimetres) }; diff --git a/src/examples/fake_magnetometer/CMakeLists.txt b/src/examples/fake_magnetometer/CMakeLists.txt index c99b4267f3..e959f0ebee 100644 --- a/src/examples/fake_magnetometer/CMakeLists.txt +++ b/src/examples/fake_magnetometer/CMakeLists.txt @@ -42,4 +42,5 @@ px4_add_module( drivers_magnetometer geo px4_work_queue + world_magnetic_model ) diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 488ed0d407..425c09f3a7 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -66,8 +66,8 @@ void FakeMagnetometer::Run() if (_vehicle_gps_position_sub.copy(&gps)) { if (gps.eph < 1000) { - const double lat = gps.lat / 1.e7; - const double lon = gps.lon / 1.e7; + 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); diff --git a/src/include/containers/BlockingQueue.hpp b/src/include/containers/BlockingQueue.hpp index 3b1ec77a86..7a88b7bdb6 100644 --- a/src/include/containers/BlockingQueue.hpp +++ b/src/include/containers/BlockingQueue.hpp @@ -56,7 +56,7 @@ public: void push(T newItem) { - px4_sem_wait(&_sem_head); + do {} while (px4_sem_wait(&_sem_head) != 0); _data[_tail] = newItem; _tail = (_tail + 1) % N; @@ -66,7 +66,7 @@ public: T pop() { - px4_sem_wait(&_sem_tail); + do {} while (px4_sem_wait(&_sem_tail) != 0); T ret = _data[_head]; _head = (_head + 1) % N; diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index f6bcc9260b..479b0fa141 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -45,6 +45,7 @@ add_subdirectory(controllib EXCLUDE_FROM_ALL) add_subdirectory(conversion EXCLUDE_FROM_ALL) add_subdirectory(crc EXCLUDE_FROM_ALL) add_subdirectory(crypto EXCLUDE_FROM_ALL) +add_subdirectory(dataman_client EXCLUDE_FROM_ALL) add_subdirectory(drivers EXCLUDE_FROM_ALL) add_subdirectory(field_sensor_bias_estimator EXCLUDE_FROM_ALL) add_subdirectory(geo EXCLUDE_FROM_ALL) diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp index 593bbabbd7..008a8758a9 100644 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ b/src/lib/avoidance/ObstacleAvoidanceTest.cpp @@ -198,7 +198,6 @@ TEST_F(ObstacleAvoidanceTest, oa_desired) // WHEN: we subscribe to the uORB message out of the interface uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; - _sub_traj_wp_avoidance_desired.update(); // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 EXPECT_FLOAT_EQ(pos_sp(0), diff --git a/src/lib/battery/module.yaml b/src/lib/battery/module.yaml index 4cbf1da675..c7750801b6 100644 --- a/src/lib/battery/module.yaml +++ b/src/lib/battery/module.yaml @@ -86,6 +86,7 @@ parameters: type: enum values: + 0: Unknown 1: 1S Battery 2: 2S Battery 3: 3S Battery diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 80a9910d8b..b33e4a8383 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -91,8 +91,10 @@ enum Rotation : uint8_t { ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, ROTATION_PITCH_315 = 39, ROTATION_ROLL_90_PITCH_315 = 40, + ROTATION_MAX, - ROTATION_MAX + // Rotation Enum reserved for custom rotation using Euler Angles + ROTATION_CUSTOM = 100 }; struct rot_lookup_t { diff --git a/src/lib/dataman_client/CMakeLists.txt b/src/lib/dataman_client/CMakeLists.txt new file mode 100644 index 0000000000..8d60c9935d --- /dev/null +++ b/src/lib/dataman_client/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2023 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(dataman_client DatamanClient.cpp) diff --git a/src/lib/dataman_client/DatamanClient.cpp b/src/lib/dataman_client/DatamanClient.cpp new file mode 100644 index 0000000000..a6b37defa9 --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.cpp @@ -0,0 +1,670 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file DatamanClient.cpp + */ + +#include + +DatamanClient::DatamanClient() +{ + _dataman_request_pub.advertise(); + _dataman_response_sub = orb_subscribe(ORB_ID(dataman_response)); + + if (_dataman_response_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + + } else { + // make sure we don't get any stale response by doing an orb_copy + dataman_response_s response{}; + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + _fds.fd = _dataman_response_sub; + _fds.events = POLLIN; + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.request_type = DM_GET_ID; + request.client_id = CLIENT_ID_NOT_SET; + + bool success = syncHandler(request, response, timestamp, 1000_ms); + + if (success && (response.client_id > CLIENT_ID_NOT_SET)) { + + _client_id = response.client_id; + + } else { + PX4_ERR("Failed to get client ID!"); + } + } +} + +DatamanClient::~DatamanClient() +{ + if (_dataman_response_sub >= 0) { + orb_unsubscribe(_dataman_response_sub); + } +} + +bool DatamanClient::syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout) +{ + bool response_received = false; + int32_t ret = 0; + hrt_abstime time_elapsed = hrt_elapsed_time(&start_time); + _dataman_request_pub.publish(request); + + while (!response_received && (time_elapsed < timeout)) { + + uint32_t timeout_ms = 100; + ret = px4_poll(&_fds, 1, timeout_ms); + + if (ret < 0) { + PX4_ERR("px4_poll returned error: %" PRIu32, ret); + break; + + } else if (ret == 0) { + + // No response received, send new request + _dataman_request_pub.publish(request); + + } else { + + bool updated = false; + orb_check(_dataman_response_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + if (response.client_id == request.client_id) { + + if ((response.request_type == request.request_type) && + (response.item == request.item) && + (response.index == request.index)) { + response_received = true; + break; + } + + } else if (request.client_id == CLIENT_ID_NOT_SET) { + + // validate timestamp from response.data + if (0 == memcmp(&(request.timestamp), &(response.data), sizeof(hrt_abstime))) { + response_received = true; + break; + } + } + } + } + + time_elapsed = hrt_elapsed_time(&start_time); + } + + if (!response_received && ret >= 0) { + PX4_ERR("timeout after %" PRIu32 " ms!", static_cast(timeout / 1000)); + } + + return response_received; +} + +bool DatamanClient::readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_READ; + request.item = static_cast(item); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("readSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + + } else { + memcpy(buffer, response.data, length); + } + } + + return success; +} + +bool DatamanClient::writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_WRITE; + request.item = static_cast(item); + + memcpy(request.data, buffer, length); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("writeSync failed! status=%" PRIu8 ", item=%" PRIu8 ", index=%" PRIu32 ", length=%" PRIu32, + response.status, static_cast(item), index, length); + } + } + + return success; +} + +bool DatamanClient::clearSync(dm_item_t item, hrt_abstime timeout) +{ + bool success = false; + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.client_id = _client_id; + request.request_type = DM_CLEAR; + request.item = static_cast(item); + + dataman_response_s response{}; + success = syncHandler(request, response, timestamp, timeout); + + if (success) { + + if (response.status != dataman_response_s::STATUS_SUCCESS) { + + success = false; + PX4_ERR("clearSync failed! status=%" PRIu8 ", item=%" PRIu8, + response.status, static_cast(item)); + } + } + + return success; +} + +bool DatamanClient::readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_READ; + request.item = static_cast(item); + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_READ; + _active_request.item = item; + _active_request.index = index; + _active_request.buffer = buffer; + _active_request.length = length; + + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +bool DatamanClient::writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.index = index; + request.data_length = length; + request.client_id = _client_id; + request.request_type = DM_WRITE; + request.item = static_cast(item); + + memcpy(request.data, buffer, length); + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_WRITE; + _active_request.item = item; + _active_request.index = index; + _active_request.buffer = buffer; + _active_request.length = length; + + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +bool DatamanClient::clearAsync(dm_item_t item) +{ + bool success = false; + + if (_state == State::Idle) { + + hrt_abstime timestamp = hrt_absolute_time(); + + dataman_request_s request; + request.timestamp = timestamp; + request.client_id = _client_id; + request.request_type = DM_CLEAR; + request.item = static_cast(item); + request.index = 0; + + _active_request.timestamp = timestamp; + _active_request.request_type = DM_CLEAR; + _active_request.item = item; + _active_request.index = request.index; + _state = State::RequestSent; + + _dataman_request_pub.publish(request); + + success = true; + } + + return success; +} + +void DatamanClient::update() +{ + if (_state == State::RequestSent) { + + bool updated = false; + orb_check(_dataman_response_sub, &updated); + + dataman_response_s response; + + if (updated) { + orb_copy(ORB_ID(dataman_response), _dataman_response_sub, &response); + + if ((response.client_id == _client_id) && + (response.request_type == _active_request.request_type) && + (response.item == _active_request.item) && + (response.index == _active_request.index)) { + + if (response.request_type == DM_READ) { + memcpy(_active_request.buffer, response.data, _active_request.length); + } + + _response_status = response.status; + + if (_response_status != dataman_response_s::STATUS_SUCCESS) { + + PX4_ERR("Async request type %" PRIu8 " failed! status=%" PRIu8 " item=%" PRIu8 " index=%" PRIu32, + response.request_type, response.status, static_cast(_active_request.item), _active_request.index); + } + + _state = State::ResponseReceived; + } + } + + if (_state == State::RequestSent) { + + /* Retry the request if there is no answer */ + if (((_active_request.request_type != DM_CLEAR) && (hrt_elapsed_time(&_active_request.timestamp) > 100_ms)) || + (hrt_elapsed_time(&_active_request.timestamp) > 1000_ms) + ) { + + hrt_abstime timestamp = hrt_absolute_time(); + + _active_request.timestamp = timestamp; + + dataman_request_s request; + request.timestamp = timestamp; + request.index = _active_request.index; + request.data_length = _active_request.length; + request.client_id = _client_id; + request.request_type = static_cast(_active_request.request_type); + request.item = static_cast(_active_request.item); + + if (_active_request.request_type == DM_WRITE) { + memcpy(request.data, _active_request.buffer, _active_request.length); + } + + _dataman_request_pub.publish(request); + + _state = State::RequestSent; + } + } + } +} + +bool DatamanClient::lastOperationCompleted(bool &success) +{ + bool completed = false; + success = false; + + if (_state == State::ResponseReceived) { + + if (_response_status == dataman_response_s::STATUS_SUCCESS) { + success = true; + } + + _state = State::Idle; + completed = true; + } + + return completed; +} + +void DatamanClient::abortCurrentOperation() +{ + _state = State::Idle; +} + +DatamanCache::DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items) + : _cache_miss_perf(perf_alloc(PC_COUNT, cache_miss_perf_counter_name)) +{ + _items = new Item[num_items] {}; + + if (_items != nullptr) { + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +DatamanCache::~DatamanCache() +{ + delete[] _items; + perf_free(_cache_miss_perf); +} + +void DatamanCache::resize(uint32_t num_items) +{ + Item *new_items = new Item[num_items] {}; + + if (new_items != nullptr) { + uint32_t num_min = num_items < _num_items ? num_items : _num_items; + + for (uint32_t i = 0; i < num_min; ++i) { + new_items[i] = _items[i]; + } + + delete[] _items; + _items = new_items; + _num_items = num_items; + + } else { + PX4_ERR("alloc failed"); + } +} + +bool DatamanCache::load(dm_item_t item, uint32_t index) +{ + if (!_items) { + return false; + } + + bool success = false; + bool duplicate = false; + + //Prevent duplicates + for (uint32_t i = 0; i < _num_items; ++i) { + + if (_items[i].cache_state != State::Idle && + _items[i].cache_state != State::Error && + _items[i].response.item == item && + _items[i].response.index == index) { + duplicate = true; + break; + } + } + + if (!duplicate && (_item_counter < _num_items)) { + + _items[_load_index].cache_state = State::RequestPrepared; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; + + success = true; + } + + return success; +} + +bool DatamanCache::loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + if (!_items) { + return false; + } + + bool success = false; + bool item_found = false; + + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index)) { + item_found = true; + + if (_items[i].cache_state == State::ResponseReceived) { + memcpy(buffer, _items[i].response.data, length); + success = true; + break; + } + } + } + + if (!success && (timeout > 0)) { + perf_count(_cache_miss_perf); + success = _client.readSync(item, index, buffer, length, timeout); + + // Cache the item if not found already (it could be in the process of being loaded) + if (success && !item_found && _item_counter < _num_items) { + _items[_load_index].cache_state = State::ResponseReceived; + _items[_load_index].response.item = item; + _items[_load_index].response.index = index; + memcpy(_items[_load_index].response.data, buffer, length); + + _load_index = (_load_index + 1) % _num_items; + + ++_item_counter; // Still increase the counter here + } + } + + return success; +} + +bool DatamanCache::writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout) +{ + if (length > g_per_item_size[item]) { + PX4_ERR("Length %" PRIu32 " can't fit in data size for item %" PRIi8, length, static_cast(item)); + return false; + } + + bool success = _client.writeSync(item, index, buffer, length, timeout); + + if (success && _items) { + for (uint32_t i = 0; i < _num_items; ++i) { + if ((_items[i].response.item == item) && + (_items[i].response.index == index) && + ((_items[i].cache_state == State::ResponseReceived) || + (_items[i].cache_state == State::RequestPrepared))) { + + memcpy(_items[i].response.data, buffer, length); + _items[i].cache_state = State::ResponseReceived; + break; + } + } + } + + return success; +} + +void DatamanCache::update() +{ + if (_item_counter > 0) { + + _client.update(); + + bool success = false; + bool response_success = false; + + switch (_items[_update_index].cache_state) { + case State::Idle: + break; + + case State::ResponseReceived: + // Skip it + changeUpdateIndex(); + break; + + case State::RequestPrepared: + + success = _client.readAsync(static_cast(_items[_update_index].response.item), + _items[_update_index].response.index, + _items[_update_index].response.data, + g_per_item_size[_items[_update_index].response.item]); + + if (success) { + _items[_update_index].cache_state = State::RequestSent; + + } else { + _items[_update_index].cache_state = State::Error; + } + + break; + + case State::RequestSent: + + if (_client.lastOperationCompleted(response_success)) { + + if (response_success) { + + _items[_update_index].cache_state = State::ResponseReceived; + changeUpdateIndex(); + + } else { + _items[_update_index].cache_state = State::Error; + } + } + + break; + + case State::Error: + // Handled below + break; + } + + if (_items[_update_index].cache_state == State::Error) { + PX4_ERR("Caching: item %" PRIu8 ", index %" PRIu32", status %" PRIu8, + _items[_update_index].response.item, _items[_update_index].response.index, + _items[_update_index].response.status); + + changeUpdateIndex(); + } + + } +} + +void DatamanCache::invalidate() +{ + for (uint32_t i = 0; i < _num_items; ++i) { + _items[i].cache_state = State::Idle; + } + + _update_index = 0; + _item_counter = 0; + _load_index = 0; + _client.abortCurrentOperation(); +} + +inline void DatamanCache::changeUpdateIndex() +{ + _update_index = (_update_index + 1) % _num_items; + + if (_item_counter > 0) { + --_item_counter; + } +} diff --git a/src/lib/dataman_client/DatamanClient.hpp b/src/lib/dataman_client/DatamanClient.hpp new file mode 100644 index 0000000000..636b41d142 --- /dev/null +++ b/src/lib/dataman_client/DatamanClient.hpp @@ -0,0 +1,304 @@ +/**************************************************************************** + * + * 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 DatamanClient +{ +public: + DatamanClient(); + ~DatamanClient(); + + DatamanClient(const DatamanClient &) = delete; + DatamanClient &operator=(const DatamanClient &) = delete; + + /** + * @brief Reads data synchronously from the dataman for the specified item and index. + * + * @param[in] item The item to read data from. + * @param[in] index The index of the item to read data from. + * @param[out] buffer Pointer to the buffer to store the read data. + * @param[in] length The length of the data to read. + * @param[in] timeout The timeout in microseconds for waiting for the response. + * + * @return true if data was read successfully within the timeout, false otherwise. + */ + bool readSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + + /** + * @brief Write data to the dataman synchronously. + * + * @param[in] item The data item type to write. + * @param[in] index The index of the data item. + * @param[in] buffer The buffer that contains the data to write. + * @param[in] length The length of the data to write. + * @param[in] timeout The maximum time in microseconds to wait for the response. + * + * @return True if the write operation succeeded, false otherwise. + */ + bool writeSync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + + /** + * @brief Clears the data in the specified dataman item. + * + * @param[in] item The dataman item to clear. + * @param[in] timeout The timeout for the operation. + * + * @return True if the operation was successful, false otherwise. + */ + bool clearSync(dm_item_t item, hrt_abstime timeout = 1000_ms); + + /** + * @brief Initiates an asynchronous request to read the data from dataman for a specific item and index. + * + * @param[in] item The item to read from. + * @param[in] index The index within the item to read from. + * @param[out] buffer The buffer to store the read data in. + * @param[in] length The length of the data to read. + * + * @return True if the read request was successfully queued, false otherwise. + * + * @note The buffer must be kept alive as long as the request did not finish. + * The completion status of the request can be obtained with the + * lastOperationCompleted() function. + */ + bool readAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length); + + /** + * @brief Initiates an asynchronous request to write the data to dataman for a specific item and index. + * + * @param[in] item The item to write data to. + * @param[in] index The index of the item to write data to. + * @param[in] buffer The buffer containing the data to write. + * @param[in] length The length of the data to write. + * + * @return True if the write request was successfully sent, false otherwise. + * + * @note The buffer must be kept alive as long as the request did not finish. + * The completion status of the request can be obtained with the + * lastOperationCompleted() function. + */ + bool writeAsync(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length); + + /** + * @brief Initiates an asynchronous request to clear an item in dataman. + * + * The request is only initiated if the DatamanClient is in the Idle state. + * + * @param[in] item The item to clear. + * @return True if the request was successfully initiated, false otherwise. + */ + bool clearAsync(dm_item_t item); + + /** + * @brief Updates the state of the dataman client for asynchronous functions. + * + * This function shall be called regularly. It checks if there is any response from the dataman, + * and updates the state accordingly. If there is no response for a request, it retries the + * request after a timeout. + * + * @see readAsync(), writeAsync(), clearAsync(), lastOperationCompleted() + */ + void update(); + + /** + * @brief Check if the last dataman operation has completed and whether it was successful. + * + * @param[out] success Output parameter indicating whether the last operation was successful. + * @return true if the last operation has completed, false otherwise. + */ + bool lastOperationCompleted(bool &success); + + /** + * Abort any async operation currently in progress + */ + void abortCurrentOperation(); + +private: + + enum class State { + Idle, + RequestSent, + ResponseReceived + }; + + struct Request { + hrt_abstime timestamp; + dm_function_t request_type; + dm_item_t item; + uint32_t index; + uint8_t *buffer; + uint32_t length; + }; + + /* Synchronous response/request handler */ + bool syncHandler(const dataman_request_s &request, dataman_response_s &response, + const hrt_abstime &start_time, hrt_abstime timeout); + + State _state{State::Idle}; + Request _active_request{}; + uint8_t _response_status{}; + + int32_t _dataman_response_sub{}; + uORB::Publication _dataman_request_pub{ORB_ID(dataman_request)}; + + px4_pollfd_struct_t _fds; + + uint8_t _client_id{0}; + + static constexpr uint8_t CLIENT_ID_NOT_SET{0}; +}; + + +class DatamanCache +{ +public: + DatamanCache(const char *cache_miss_perf_counter_name, uint32_t num_items); + ~DatamanCache(); + + /** + * @brief Resizes the cache to hold the specified number of items. + * + * @param[in] num_items The number of items the cache should hold. + */ + void resize(uint32_t num_items); + + /** + * @brief Invalidates all cached items in the cache by resetting their cache_state to Idle. + */ + void invalidate(); + + /** + * @brief Adds an index for items to be cached. + * + * Calling this function will exit immediately. Data shall be acquired with 'update()' function and + * it will be cached at full size. Later it can be retrieved with 'loadWait()' function. + * + * @param[in] item The item to load. + * @param[in] index The index of the item to load. + * + * @return true if the item was added to be cached, false otherwise if the size of the cache is reached. + */ + bool load(dm_item_t item, uint32_t index); + + /** + * @brief Loads for a specific item from the cache or acquires and wait for it if not found in the cache. + * + * @param[in] item Dataman item type + * @param[in] index Item index + * @param[out] buffer Buffer for the data to be stored + * @param[in] length Length of the buffer in bytes to be stored + * @param[in] timeout Maximum time to wait for the item to be available in microseconds, 0 to return immediately + * + * @return true if item was successfully loaded from cache or acquired through the client, false otherwise. + * + * @note This function will block if timeout is set differently than 0 and data doesn't exist in cache. + */ + bool loadWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 0); + + /** + * @brief Write data back and update it in the cache if stored. + * + * @param[in] item The data item type to write. + * @param[in] index The index of the data item. + * @param[in] buffer The buffer that contains the data to write. + * @param[in] length The length of the data to write. + * @param[in] timeout The maximum time in microseconds to wait for the response. + * + * @return True if the write operation succeeded, false otherwise. + */ + bool writeWait(dm_item_t item, uint32_t index, uint8_t *buffer, uint32_t length, hrt_abstime timeout = 1000_ms); + + /** + * @brief Updates the dataman cache by checking for responses from the DatamanClient and processing them. + * + * If there are items in the cache, this function will call the DatamanClient's 'update()' function to check for responses. + * Depending on the state of each item, it will either send a request, wait for a response, or report an error. + * If a response is received for an item, it will be marked as "response received" and the update index will be changed + * to the next item in the cache. This function does not block and returns immediately. + * The data can be acquired with the 'loadWait()' function after it has been cached. + */ + void update(); + + /** + * @brief Function providing info if there items to be loaded to dataman cache. + * + * @return true if there are items to be processed. + */ + bool isLoading() { return (_item_counter > 0); } + + /** + * @brief Returns a reference to the DatamanClient instance used by the DatamanCache. + * + * @return A reference to the DatamanClient instance used by the DatamanCache. + */ + DatamanClient &client() { return _client; } + + int size() const { return _num_items; } + +private: + + enum class State { + Idle, + RequestPrepared, + RequestSent, + ResponseReceived, + Error + }; + + struct Item { + dataman_response_s response; + State cache_state; + }; + + inline void changeUpdateIndex(); + + Item *_items{nullptr}; + uint32_t _load_index{0}; ///< index for tracking last index used by load function + uint32_t _update_index{0}; ///< index for tracking last index used by update function + uint32_t _item_counter{0}; ///< number of items to process with update function + uint32_t _num_items{0}; ///< number of items that cache can store + + DatamanClient _client{}; + + perf_counter_t _cache_miss_perf; +}; diff --git a/src/lib/events/CMakeLists.txt b/src/lib/events/CMakeLists.txt index b3540ffb3d..eea3e9b2ec 100644 --- a/src/lib/events/CMakeLists.txt +++ b/src/lib/events/CMakeLists.txt @@ -110,3 +110,13 @@ add_custom_command(OUTPUT ${generated_events_header} ) add_custom_target(events_header DEPENDS ${generated_events_header}) add_dependencies(prebuild_targets events_header) + +# Build the interface(s) +if (${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT) + list(APPEND EXTRA_SRCS events_ioctl.cpp) + add_library(usr_events usr_events.cpp) + add_dependencies(usr_events prebuild_targets) +endif() + +add_library(events events.cpp ${EXTRA_SRCS}) +add_dependencies(events prebuild_targets) diff --git a/platforms/common/events.cpp b/src/lib/events/events.cpp similarity index 100% rename from platforms/common/events.cpp rename to src/lib/events/events.cpp diff --git a/src/lib/events/events.h b/src/lib/events/events.h new file mode 100644 index 0000000000..929dbc4982 --- /dev/null +++ b/src/lib/events/events.h @@ -0,0 +1,36 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +void events_ioctl_init(void); diff --git a/src/lib/events/events_ioctl.cpp b/src/lib/events/events_ioctl.cpp new file mode 100644 index 0000000000..a3c8260ddd --- /dev/null +++ b/src/lib/events/events_ioctl.cpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file events_ioctl.cpp + * + * Interface to send events from user space. + */ + +#include + +#include + +#include "events_ioctl.h" + +static int events_ioctl(unsigned int cmd, unsigned long arg) +{ + int ret = OK; + + switch (cmd) { + case EVENTSIOCSEND: { + eventiocsend_t *data = (eventiocsend_t *)arg; + events::send(data->event); + } + break; + + default: + ret = -ENOTTY; + break; + } + + return ret; +} + +void events_ioctl_init(void) +{ + px4_register_boardct_ioctl(_EVENTSIOCBASE, events_ioctl); +} diff --git a/src/lib/events/events_ioctl.h b/src/lib/events/events_ioctl.h new file mode 100644 index 0000000000..7b97c66aa4 --- /dev/null +++ b/src/lib/events/events_ioctl.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file events_ioctl.h + * + * User space - kernel space interface for dispatching events. + */ + +#pragma once + +#include +#include +#include + +#define _EVENTSIOC(_n) (_PX4_IOC(_EVENTSIOCBASE, _n)) + +#define EVENTSIOCSEND _EVENTSIOC(1) +typedef struct eventiocsend { + events::EventType &event; +} eventiocsend_t; diff --git a/src/lib/events/usr_events.cpp b/src/lib/events/usr_events.cpp new file mode 100644 index 0000000000..a2f076d065 --- /dev/null +++ b/src/lib/events/usr_events.cpp @@ -0,0 +1,53 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Technology Innovation Institute. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file usr_events.cpp + * + * User space interface for dispatching events. + */ + +#include + +#include "events_ioctl.h" + +namespace events +{ + +void send(EventType &event) +{ + eventiocsend_t data = {event}; + boardctl(EVENTSIOCSEND, reinterpret_cast(&data)); +} + +} /* namespace events */ diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 0635ae438c..4e651cfee9 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -116,9 +116,26 @@ public: float getCutoffFreq() const { return _cutoff_freq; } protected: - T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; } + T updateCalculation(const T &sample); + float _cutoff_freq{0.f}; float _alpha{0.f}; T _filter_state{}; }; + +template +T AlphaFilter::updateCalculation(const T &sample) { return _filter_state + _alpha * (sample - _filter_state); } + +/* Specialization for 3D rotations + * The filter is computed on the 3-sphere of unit quaternions instead of the cartesian space + * Additions and subtractions are done using the quaternion multiplication and + * the error is scaled on the tangent space. + */ +template <> inline +matrix::Quatf AlphaFilter::updateCalculation(const matrix::Quatf &sample) +{ + matrix::Quatf q_error(_filter_state.inversed() * sample); + q_error.canonicalize(); // prevent unwrapping + return _filter_state * matrix::Quatf(matrix::AxisAnglef(_alpha * matrix::AxisAnglef(q_error))); +} diff --git a/src/lib/mathlib/math/test/AlphaFilterTest.cpp b/src/lib/mathlib/math/test/AlphaFilterTest.cpp index 30661ea82b..63868f9aee 100644 --- a/src/lib/mathlib/math/test/AlphaFilterTest.cpp +++ b/src/lib/mathlib/math/test/AlphaFilterTest.cpp @@ -44,6 +44,8 @@ #include using matrix::Vector3f; +using matrix::Quatf; +using matrix::Eulerf; TEST(AlphaFilterTest, initializeToZero) { @@ -73,7 +75,7 @@ TEST(AlphaFilterTest, runZero) TEST(AlphaFilterTest, runPositive) { - // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9) + // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1) AlphaFilter filter_float{}; const float input = 1.f; filter_float.setAlpha(.1f); @@ -89,7 +91,7 @@ TEST(AlphaFilterTest, runPositive) TEST(AlphaFilterTest, runNegative) { - // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9) + // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1) AlphaFilter filter_float{}; const float input = -1.f; filter_float.setAlpha(.1f); @@ -105,7 +107,7 @@ TEST(AlphaFilterTest, runNegative) TEST(AlphaFilterTest, riseTime) { - // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9) + // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1) AlphaFilter filter_float{}; const float input = 1.f; filter_float.setAlpha(.1f); @@ -121,7 +123,7 @@ TEST(AlphaFilterTest, riseTime) TEST(AlphaFilterTest, convergence) { - // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.9) + // GIVEN an input of 1 in a filter with a default time constant of 9 (alpha = 0.1) AlphaFilter filter_float{}; const float input = 1.f; filter_float.setAlpha(.1f); @@ -137,7 +139,7 @@ TEST(AlphaFilterTest, convergence) TEST(AlphaFilterTest, convergenceVector3f) { - // GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.9) + // GIVEN an Vector3f input in a filter with a default time constant of 9 (alpha = 0.1) AlphaFilter filter_v3{}; const Vector3f input = {3.f, 7.f, -11.f}; filter_v3.setAlpha(.1f); @@ -280,3 +282,51 @@ TEST(AlphaFilterTest, ConvergenceTest) EXPECT_NEAR(last_value, -100.f, 1e-4f); } + +TEST(AlphaFilterTest, runPositiveRot3) +{ + // GIVEN a filter with a default time constant of 9 (alpha = 0.1) + const Quatf q_init(Eulerf(0.f, 0.f, -0.01f)); + const Quatf input(Eulerf(0.f, 0.f, M_PI_F / 2.f)); + + AlphaFilter filter{}; + filter.setAlpha(.1f); + filter.reset(q_init); + + // WHEN we run the filter 9 times + for (int i = 0; i < 9; i++) { + filter.update(input); + } + + // THEN the state of the filter should have reached 63% + EXPECT_NEAR(Eulerf(filter.getState()).psi(), 0.63f * (M_PI_F / 2.f - 0.01f), 0.03); + + EXPECT_FLOAT_EQ(Eulerf(filter.getState()).phi(), 0.f); + EXPECT_FLOAT_EQ(Eulerf(filter.getState()).theta(), 0.f); +} + +TEST(AlphaFilterTest, runNecgativeRot3) +{ + // GIVEN a filter with a default time constant of 9 (alpha = 0.1) + const float roll = -M_PI_F; + const float pitch = 0.f; + const float yaw_init = 0.01f; + const float yaw_input = -M_PI_F / 2.f; + const Quatf q_init(Eulerf(roll, pitch, yaw_init)); + const Quatf input(Eulerf(roll, pitch, yaw_input)); + + AlphaFilter filter{}; + filter.setAlpha(.1f); + filter.reset(q_init); + + // WHEN we run the filter 9 times + for (int i = 0; i < 9; i++) { + filter.update(input); + } + + // THEN the state of the filter should have reached 63% + EXPECT_NEAR(Eulerf(filter.getState()).psi(), 0.63f * (yaw_input + yaw_init), 0.03); + + EXPECT_TRUE(matrix::wrap_pi(Eulerf(filter.getState()).phi() - roll) < FLT_EPSILON); + EXPECT_TRUE(matrix::wrap_pi(Eulerf(filter.getState()).theta() - pitch) < FLT_EPSILON); +} diff --git a/src/lib/matrix/matrix/SquareMatrix.hpp b/src/lib/matrix/matrix/SquareMatrix.hpp index 20ecd67f2c..ce3b374992 100644 --- a/src/lib/matrix/matrix/SquareMatrix.hpp +++ b/src/lib/matrix/matrix/SquareMatrix.hpp @@ -126,18 +126,43 @@ public: return res; } - Type trace() const + template + Type trace(size_t first) const { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + Type res = 0; const SquareMatrix &self = *this; - for (size_t i = 0; i < M; i++) { + for (size_t i = first; i < (first + Width); i++) { res += self(i, i); } return res; } + Type trace() const + { + const SquareMatrix &self = *this; + return self.trace(0); + } + + // keep the sub covariance matrix and zero all covariance elements related + // to the rest of the matrix + template + void uncorrelateCovarianceBlock(size_t first) + { + static_assert(Width <= M, "Width bigger than matrix"); + assert(first + Width <= M); + + SquareMatrix &self = *this; + SquareMatrix cov = self.slice(first, first); + self.slice(0, first) = 0.f; + self.slice(first, 0) = 0.f; + self.slice(first, first) = cov; + } + // zero all offdiagonal elements and keep corresponding diagonal elements template void uncorrelateCovariance(size_t first) @@ -280,6 +305,27 @@ public: return self.isBlockSymmetric(first, eps); } + void copyLowerToUpperTriangle() + { + SquareMatrix &self = *this; + + for (size_t row_idx = 1; row_idx < M; row_idx++) { + for (size_t col_idx = 0 ; col_idx < row_idx; col_idx++) { + self(col_idx, row_idx) = self(row_idx, col_idx); + } + } + } + + void copyUpperToLowerTriangle() + { + SquareMatrix &self = *this; + + for (size_t row_idx = 1; row_idx < M; row_idx++) { + for (size_t col_idx = 0 ; col_idx < row_idx; col_idx++) { + self(row_idx, col_idx) = self(col_idx, row_idx); + } + } + } }; using SquareMatrix3f = SquareMatrix; @@ -322,6 +368,19 @@ SquareMatrix expm(const Matrix &A, size_t order = 5) return res; } +/** + * Deal with the special case where the square matrix is 1 + */ +template +bool inv(const SquareMatrix &A, SquareMatrix &inv, size_t rank = 1) +{ + if (std::fabs(A(0, 0)) < Type(FLT_EPSILON)) { + return false; + } + + inv(0, 0) = Type(1) / A(0, 0); + return true; +} /** * inverse based on LU factorization with partial pivotting diff --git a/src/lib/matrix/test/MatrixSquareTest.cpp b/src/lib/matrix/test/MatrixSquareTest.cpp index b52e9dd665..eafca26d74 100644 --- a/src/lib/matrix/test/MatrixSquareTest.cpp +++ b/src/lib/matrix/test/MatrixSquareTest.cpp @@ -47,6 +47,7 @@ TEST(MatrixSquareTest, Square) EXPECT_EQ(A.diag(), diag_check); EXPECT_FLOAT_EQ(A.trace(), 16); + EXPECT_FLOAT_EQ(A.trace<2>(1), 15); float data_check[9] = { 1.01158503f, 0.02190432f, 0.03238144f, @@ -117,6 +118,16 @@ TEST(MatrixSquareTest, Square) SquareMatrix E_check(data_E_check); EXPECT_EQ(E, E_check); + SquareMatrix A_block(data_4x4); + A_block.uncorrelateCovarianceBlock<2>(1); + float data_A_block_check[16] = {1, 0, 0, 4, + 0, 6, 7, 0, + 0, 10, 11, 0, + 13, 0, 0, 16 + }; + SquareMatrix A_block_check(data_A_block_check); + EXPECT_EQ(A_block, A_block_check); + // test symmetric functions SquareMatrix F(data_4x4); F.makeBlockSymmetric<2>(1); @@ -174,4 +185,28 @@ TEST(MatrixSquareTest, Square) }; SquareMatrix K(data_K); EXPECT_FALSE(K.isRowColSymmetric<1>(2)); + + float data_L[16] = {1, 0, 0, 0, + 2, 3, 0, 0, + 3, 4, 11, 0, + 4, 11, 15, 16 + }; + float data_L_check[16] = {1, 2, 3, 4, + 2, 3, 4, 11, + 3, 4, 11, 15, + 4, 11, 15, 16 + }; + SquareMatrix L(data_L); + L.copyLowerToUpperTriangle(); + SquareMatrix L_check(data_L_check); + EXPECT_EQ(L, L_check); + + float data_M[16] = {1, 2, 3, 4, + 0, 3, 4, 11, + 0, 0, 11, 15, + 0, 0, 0, 16 + }; + SquareMatrix M(data_M); + M.copyUpperToLowerTriangle(); + EXPECT_EQ(M, L_check); } diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp index 3ff8d4a7c3..5347be6b11 100644 --- a/src/lib/mixer_module/actuator_test.cpp +++ b/src/lib/mixer_module/actuator_test.cpp @@ -75,7 +75,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) float value = actuator_test.value; // handle motors - if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) { + if ((int)OutputFunction::Motor1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::MotorMax) { actuator_motors_s motors; motors.reversible_flags = 0; _actuator_motors_sub.copy(&motors); @@ -84,7 +84,7 @@ void ActuatorTest::update(int num_outputs, float thrust_curve) } // handle servos: add trim - if (actuator_test.function >= (int)OutputFunction::Servo1 && actuator_test.function <= (int)OutputFunction::ServoMax) { + if ((int)OutputFunction::Servo1 <= actuator_test.function && actuator_test.function <= (int)OutputFunction::ServoMax) { actuator_servos_trim_s trim{}; _actuator_servos_trim_sub.copy(&trim); int idx = actuator_test.function - (int)OutputFunction::Servo1; diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index 6873a3919f..fa42c8b63d 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -35,7 +35,7 @@ #include "FunctionProviderBase.hpp" -#include +#include /** * Functions: Motor1 ... MotorMax diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp index 29a07ba690..69c9acfb60 100644 --- a/src/lib/mixer_module/mixer_module.cpp +++ b/src/lib/mixer_module/mixer_module.cpp @@ -490,6 +490,24 @@ MixingOutput::limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updat output_limit_calc(_throttle_armed || _actuator_test.inTestMode(), _max_num_outputs, outputs); } + // We must calibrate the PWM and Oneshot ESCs to a consistent range of 1000-2000us (gets mapped to 125-250us for Oneshot) + // Doing so makes calibrations consistent among different configurations and hence PWM minimum and maximum have a consistent effect + // hence the defaults for these parameters also make most setups work out of the box + if (_armed.in_esc_calibration_mode) { + static constexpr uint16_t PWM_CALIBRATION_LOW = 1000; + static constexpr uint16_t PWM_CALIBRATION_HIGH = 2000; + + for (int i = 0; i < _max_num_outputs; i++) { + if (_current_output_value[i] == _min_value[i]) { + _current_output_value[i] = PWM_CALIBRATION_LOW; + } + + if (_current_output_value[i] == _max_value[i]) { + _current_output_value[i] = PWM_CALIBRATION_HIGH; + } + } + } + /* now return the outputs to the driver */ if (_interface.updateOutputs(stop_motors, _current_output_value, _max_num_outputs, has_updates)) { actuator_outputs_s actuator_outputs{}; @@ -526,21 +544,6 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const /* first evaluate state changes */ switch (_output_state) { - case OutputLimitState::INIT: - if (armed) { - // set arming time for the first call - if (_output_time_armed == 0) { - _output_time_armed = hrt_absolute_time(); - } - - // time for the ESCs to initialize (this is not actually needed if the signal is sent right after boot) - if (hrt_elapsed_time(&_output_time_armed) >= 50_ms) { - _output_state = OutputLimitState::OFF; - } - } - - break; - case OutputLimitState::OFF: if (armed) { if (_output_ramp_up) { @@ -589,7 +592,6 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const // then set _current_output_value based on state switch (local_limit_state) { case OutputLimitState::OFF: - case OutputLimitState::INIT: for (int i = 0; i < num_channels; i++) { _current_output_value[i] = _disarmed_value[i]; } @@ -598,53 +600,16 @@ MixingOutput::output_limit_calc(const bool armed, const int num_channels, const case OutputLimitState::RAMP: { hrt_abstime diff = hrt_elapsed_time(&_output_time_armed); + float progress = static_cast(diff) / RAMP_TIME_US; - static constexpr int PROGRESS_INT_SCALING = 10000; - int progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US; - - if (progress > PROGRESS_INT_SCALING) { - progress = PROGRESS_INT_SCALING; + if (progress > 1.f) { + progress = 1.f; } for (int i = 0; i < num_channels; i++) { - - float control_value = output[i]; - - /* check for invalid / disabled channels */ - if (!PX4_ISFINITE(control_value)) { - _current_output_value[i] = _disarmed_value[i]; - continue; - } - - uint16_t ramp_min_output; - - /* if a disarmed output value was set, blend between disarmed and min */ - if (_disarmed_value[i] > 0) { - - /* safeguard against overflows */ - auto disarmed = _disarmed_value[i]; - - if (disarmed > _min_value[i]) { - disarmed = _min_value[i]; - } - - int disarmed_min_diff = _min_value[i] - disarmed; - ramp_min_output = disarmed + (disarmed_min_diff * progress) / PROGRESS_INT_SCALING; - - } else { - /* no disarmed output value set, choose min output */ - ramp_min_output = _min_value[i]; - } - - if (_reverse_output_mask & (1 << i)) { - control_value = -1.f * control_value; - } - - _current_output_value[i] = control_value * (_max_value[i] - ramp_min_output) / 2 + (_max_value[i] + ramp_min_output) / - 2; - - /* last line of defense against invalid inputs */ - _current_output_value[i] = math::constrain(_current_output_value[i], ramp_min_output, _max_value[i]); + // Ramp from disarmed value to currently desired output that would apply without ramp + uint16_t desired_output = output_limit_calc_single(i, output[i]); + _current_output_value[i] = _disarmed_value[i] + progress * (desired_output - _disarmed_value[i]); } } break; diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp index d3dda97e83..0d8a495271 100644 --- a/src/lib/mixer_module/mixer_module.hpp +++ b/src/lib/mixer_module/mixer_module.hpp @@ -179,6 +179,8 @@ public: param_t functionParamHandle(int index) const { return _param_handles[index].function; } param_t disarmedParamHandle(int index) const { return _param_handles[index].disarmed; } + param_t minParamHandle(int index) const { return _param_handles[index].min; } + param_t maxParamHandle(int index) const { return _param_handles[index].max; } /** * Returns the actual failsafe value taking into account the assigned function @@ -248,10 +250,9 @@ private: enum class OutputLimitState { OFF = 0, - INIT, RAMP, ON - } _output_state{OutputLimitState::INIT}; + } _output_state{OutputLimitState::OFF}; hrt_abstime _output_time_armed{0}; const bool _output_ramp_up; ///< if true, motors will ramp up from disarmed to min_output after arming diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp index a4308fb150..196e0b2699 100644 --- a/src/lib/npfg/npfg.cpp +++ b/src/lib/npfg/npfg.cpp @@ -59,7 +59,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g const float wind_speed = wind_vel.norm(); const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; - const float signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle); + signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); // on-track wind triangle projections const float wind_cross_upt = wind_vel.cross(unit_path_tangent); @@ -68,7 +68,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g // calculate the bearing feasibility on the track at the current closest point feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); - const float track_error = fabsf(signed_track_error); + const float track_error = fabsf(signed_track_error_); // update control parameters considering upper and lower stability bounds (if enabled) // must be called before trackErrorBound() as it updates time_const_ @@ -86,7 +86,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g track_proximity_ = trackProximity(look_ahead_ang); - bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error); + bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); // wind triangle projections const float wind_cross_bearing = wind_vel.cross(bearing_vec_); @@ -112,7 +112,7 @@ void NPFG::guideToPath(const matrix::Vector2f &curr_pos_local, const Vector2f &g // lateral acceleration needed to stay on curved track (assuming no heading error) lateral_accel_ff_ = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, - wind_cross_upt, airspeed, wind_speed, signed_track_error, path_curvature); + wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature); // total lateral acceleration to drive aircaft towards track as well as account // for path curvature. The full effect of the feed-forward acceleration is smoothly diff --git a/src/lib/parameters/autosave.cpp b/src/lib/parameters/autosave.cpp index 7a1be1ae7c..a539ee484b 100644 --- a/src/lib/parameters/autosave.cpp +++ b/src/lib/parameters/autosave.cpp @@ -110,10 +110,22 @@ void ParamAutosave::Run() } PX4_DEBUG("Autosaving params"); - int ret = param_save_default(); + int ret = param_save_default(false); - if (ret != 0) { - PX4_ERR("param auto save failed (%i)", ret); + if (ret != PX4_OK) { + // re-request to be saved in the future, try 3 times at most + if (_retry_count < 3) { + _retry_count++; + PX4_INFO("param auto save unavailable (%i), retrying..", ret); + request(); + + } else { + PX4_ERR("param auto save failed (%i)", ret); + _retry_count = 0; + } + + } else { + _retry_count = 0; } } diff --git a/src/lib/parameters/autosave.h b/src/lib/parameters/autosave.h index 7a8857888e..0636a44def 100644 --- a/src/lib/parameters/autosave.h +++ b/src/lib/parameters/autosave.h @@ -59,5 +59,6 @@ public: private: hrt_abstime _last_timestamp{0}; px4::atomic_bool _scheduled{false}; + int _retry_count{0}; bool _disabled{false}; }; diff --git a/src/lib/parameters/param.h b/src/lib/parameters/param.h index 5e7802230e..dca29d4f21 100644 --- a/src/lib/parameters/param.h +++ b/src/lib/parameters/param.h @@ -400,13 +400,17 @@ __EXPORT const char *param_get_backup_file(void); /** * Save parameters to the default file. + * * Note: this method requires a large amount of stack size! * * This function saves all parameters with non-default values. * - * @return Zero on success. + * @param blocking If true, in case the default file is busy, the function blocks + * until the file is available for writing. + * + * @return Zero on success, -EWOULDBLOCK if the file is busy and blocking is false. */ -__EXPORT int param_save_default(void); +__EXPORT int param_save_default(bool blocking); /** * Load parameters from the default parameter file. diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 5a5603d4cf..b438de23a2 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -42,194 +42,7 @@ bool param_modify_on_import(bson_node_t node) { - // 2022-04-11: translate VT_PTCH_MIN to VT_PITCH_MIN - { - if (strcmp("VT_PTCH_MIN", node->name) == 0) { - strcpy(node->name, "VT_PITCH_MIN"); - PX4_INFO("copying %s -> %s", "VT_PTCH_MIN", "VT_PITCH_MIN"); - return true; - } - } - // 2022-04-11: translate VT_LND_PTCH_MIN to VT_LND_PITCH_MIN - { - if (strcmp("VT_LND_PTCH_MIN", node->name) == 0) { - strcpy(node->name, "VT_LND_PITCH_MIN"); - PX4_INFO("copying %s -> %s", "VT_LND_PTCH_MIN", "VT_LND_PITCH_MIN"); - return true; - } - } - - - // 2021-10-21: translate NAV_GPSF_LT to FW_GPSF_LT and NAV_GPSF_R to FW_GPSF_R - { - if (strcmp("NAV_GPSF_LT", node->name) == 0) { - strcpy(node->name, "FW_GPSF_LT"); - node->i32 = static_cast(node->d); - node->type = BSON_INT32; - PX4_INFO("copying %s -> %s", "NAV_GPSF_LT", "FW_GPSF_LT"); - return true; - } - - if (strcmp("NAV_GPSF_R", node->name) == 0) { - strcpy(node->name, "FW_GPSF_R"); - PX4_INFO("copying %s -> %s", "NAV_GPSF_R", "FW_GPSF_R"); - return true; - } - } - - // 2022-03-15: translate notch filter IMU_GYRO_NF_FREQ to IMU_GYRO_NF0_FRQ and IMU_GYRO_NF_BW -> IMU_GYRO_NF0_BW - { - if (strcmp("IMU_GYRO_NF_FREQ", node->name) == 0) { - strcpy(node->name, "IMU_GYRO_NF0_FRQ"); - PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_FREQ", "IMU_GYRO_NF0_FRQ"); - return true; - } - - if (strcmp("IMU_GYRO_NF_BW", node->name) == 0) { - strcpy(node->name, "IMU_GYRO_NF0_BW"); - PX4_INFO("copying %s -> %s", "IMU_GYRO_NF_BW", "IMU_GYRO_NF0_BW"); - return true; - } - } - - // 2022-04-25 (v1.13 alpha): translate MS4525->MS4525DO and MS5525->MS5525DSO - { - if (strcmp("SENS_EN_MS4525", node->name) == 0) { - strcpy(node->name, "SENS_EN_MS4525DO"); - PX4_INFO("copying %s -> %s", "SENS_EN_MS4525", "SENS_EN_MS4525DO"); - return true; - } - - if (strcmp("SENS_EN_MS5525", node->name) == 0) { - strcpy(node->name, "SENS_EN_MS5525DS"); - PX4_INFO("copying %s -> %s", "SENS_EN_MS5525", "SENS_EN_MS5525DS"); - return true; - } - } - - // 2022-06-09: migrate EKF2_WIND_NOISE->EKF2_WIND_NSD - { - if (strcmp("EKF2_WIND_NOISE", node->name) == 0) { - node->d /= 10.0; // at 100Hz (EKF2 rate), NSD is sqrt(100) times smaller than std_dev - strcpy(node->name, "EKF2_WIND_NSD"); - PX4_INFO("param migrating EKF2_WIND_NOISE (removed) -> EKF2_WIND_NSD: value=%.3f", node->d); - return true; - } - } - - // 2022-06-09: translate ASPD_SC_P_NOISE->ASPD_SCALE_NSD and ASPD_W_P_NOISE->ASPD_WIND_NSD - { - if (strcmp("ASPD_SC_P_NOISE", node->name) == 0) { - strcpy(node->name, "ASPD_SCALE_NSD"); - PX4_INFO("copying %s -> %s", "ASPD_SC_P_NOISE", "ASPD_SCALE_NSD"); - return true; - } - - if (strcmp("ASPD_W_P_NOISE", node->name) == 0) { - strcpy(node->name, "ASPD_WIND_NSD"); - PX4_INFO("copying %s -> %s", "ASPD_W_P_NOISE", "ASPD_WIND_NSD"); - return true; - } - } - - // 2022-07-07: translate FW_THR_CRUISE->FW_THR_TRIM - { - if (strcmp("FW_THR_CRUISE", node->name) == 0) { - strcpy(node->name, "FW_THR_TRIM"); - PX4_INFO("copying %s -> %s", "FW_THR_CRUISE", "FW_THR_TRIM"); - return true; - } - } - - // 2022-08-04: migrate EKF2_RNG_AID->EKF2_RNG_CTRL and EKF2_HGT_MODE->EKF2_HGT_REF - { - if (strcmp("EKF2_RNG_AID", node->name) == 0) { - strcpy(node->name, "EKF2_RNG_CTRL"); - PX4_INFO("param migrating EKF2_RNG_AID (removed) -> EKF2_RNG_CTRL: value=%" PRId32, node->i32); - return true; - } - - if (strcmp("EKF2_HGT_MODE", node->name) == 0) { - strcpy(node->name, "EKF2_HGT_REF"); - - // If was in range height mode, set range aiding to "always" - if (node->i32 == 2) { - int32_t rng_mode = 2; - param_set_no_notification(param_find("EKF2_RNG_CTRL"), &rng_mode); - } - - PX4_INFO("param migrating EKF2_HGT_MODE (removed) -> EKF2_HGT_REF: value=%" PRId32, node->i32); - return true; - } - } - - // 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y - { - if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) { - strcpy(node->name, "VT_FW_DIFTHR_S_Y"); - PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y"); - return true; - } - } - - // 2022-11-11: translate VT_F_TRANS_THR/VT_PSHER_RMP_DT -> VT_PSHER_SLEW - { - if (strcmp("VT_PSHER_RMP_DT", node->name) == 0) { - strcpy(node->name, "VT_PSHER_SLEW"); - double _param_vt_f_trans_thr = param_find("VT_F_TRANS_THR"); - node->d = _param_vt_f_trans_thr / node->d; - PX4_INFO("copying %s -> %s", "VT_PSHER_RMP_DT", "VT_PSHER_SLEW"); - } - } - - // 2022-11-09: translate several fixed-wing launch parameters - { - if (strcmp("LAUN_ALL_ON", node->name) == 0) { - strcpy(node->name, "FW_LAUN_DETCN_ON"); - PX4_INFO("copying %s -> %s", "LAUN_ALL_ON", "FW_LAUN_DETCN_ON"); - return true; - } - - if (strcmp("LAUN_CAT_A", node->name) == 0) { - strcpy(node->name, "FW_LAUN_AC_THLD"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_A", "FW_LAUN_AC_THLD"); - return true; - } - - if (strcmp("LAUN_CAT_T", node->name) == 0) { - strcpy(node->name, "FW_LAUN_AC_T"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_T", "FW_LAUN_AC_T"); - return true; - } - - if (strcmp("LAUN_CAT_MDEL", node->name) == 0) { - strcpy(node->name, "FW_LAUN_MOT_DEL"); - PX4_INFO("copying %s -> %s", "LAUN_CAT_MDEL", "FW_LAUN_MOT_DEL"); - return true; - } - } return false; - - //2023-02-08: translate L1 parameters after removing l1 control - { - if (strcmp("RWTO_L1_PERIOD", node->name) == 0) { - strcpy(node->name, "RWTO_NPFG_PERIOD"); - PX4_INFO("copying %s -> %s", "RWTO_L1_PERIOD", "RWTO_NPFG_PERIOD"); - return true; - } - - if (strcmp("FW_L1_R_SLEW_MAX", node->name) == 0) { - strcpy(node->name, "FW_PN_R_SLEW_MAX"); - PX4_INFO("copying %s -> %s", "FW_L1_R_SLEW_MAX", "FW_PN_R_SLEW_MAX"); - return true; - } - - if (strcmp("FW_L1_PERIOD", node->name) == 0) { - strcpy(node->name, "NPFG_PERIOD"); - PX4_INFO("copying %s -> %s", "FW_L1_PERIOD", "NPFG_PERIOD"); - return true; - } - } } diff --git a/src/lib/parameters/parameters.cpp b/src/lib/parameters/parameters.cpp index 94d4f125d7..ad35dca782 100644 --- a/src/lib/parameters/parameters.cpp +++ b/src/lib/parameters/parameters.cpp @@ -698,14 +698,19 @@ const char *param_get_backup_file() static int param_export_internal(int fd, param_filter_func filter); static int param_verify(int fd); -int param_save_default() +int param_save_default(bool blocking) { PX4_DEBUG("param_save_default"); // take the file lock - if (pthread_mutex_trylock(&file_mutex) != 0) { - PX4_ERR("param_save_default: file lock failed (already locked)"); - return PX4_ERROR; + if (blocking) { + pthread_mutex_lock(&file_mutex); + + } else { + if (pthread_mutex_trylock(&file_mutex) != 0) { + PX4_DEBUG("param_save_default: file lock failed (already locked)"); + return -EWOULDBLOCK; + } } int shutdown_lock_ret = px4_shutdown_lock(); diff --git a/src/lib/parameters/parameters_ioctl.cpp b/src/lib/parameters/parameters_ioctl.cpp index 47a567ec93..fe0350863e 100644 --- a/src/lib/parameters/parameters_ioctl.cpp +++ b/src/lib/parameters/parameters_ioctl.cpp @@ -168,7 +168,7 @@ int param_ioctl(unsigned int cmd, unsigned long arg) case PARAMIOCSAVEDEFAULT: { paramiocsavedefault_t *data = (paramiocsavedefault_t *)arg; - data->ret = param_save_default(); + data->ret = param_save_default(data->blocking); } break; diff --git a/src/lib/parameters/parameters_ioctl.h b/src/lib/parameters/parameters_ioctl.h index 4e046d213a..e33ba9224a 100644 --- a/src/lib/parameters/parameters_ioctl.h +++ b/src/lib/parameters/parameters_ioctl.h @@ -140,6 +140,7 @@ typedef struct paramiocresetgroup { #define PARAMIOCSAVEDEFAULT _PARAMIOC(15) typedef struct paramiocsavedefault { + bool blocking; int ret; } paramiocsavedefault_t; diff --git a/src/lib/parameters/px4params/jsonout.py b/src/lib/parameters/px4params/jsonout.py index a3ed2e9f09..45f6cf291d 100644 --- a/src/lib/parameters/px4params/jsonout.py +++ b/src/lib/parameters/px4params/jsonout.py @@ -140,7 +140,7 @@ class JsonOutput(): #Json string output. - self.output = json.dumps(all_json, indent=2, sort_keys=True) + self.output = json.dumps(all_json, sort_keys=True) def Save(self, filename): diff --git a/src/lib/parameters/usr_parameters_if.cpp b/src/lib/parameters/usr_parameters_if.cpp index 04be2219ff..ea4a7bea70 100644 --- a/src/lib/parameters/usr_parameters_if.cpp +++ b/src/lib/parameters/usr_parameters_if.cpp @@ -187,9 +187,9 @@ param_reset_specific(const char *resets[], int num_resets) boardctl(PARAMIOCRESETGROUP, reinterpret_cast(&data)); } -int param_save_default() +int param_save_default(bool blocking) { - paramiocsavedefault_t data = {PX4_ERROR}; + paramiocsavedefault_t data = {blocking, PX4_ERROR}; boardctl(PARAMIOCSAVEDEFAULT, reinterpret_cast(&data)); return data.ret; } diff --git a/src/lib/rate_control/rate_control.cpp b/src/lib/rate_control/rate_control.cpp index 277009c92d..baa4c8e7b5 100644 --- a/src/lib/rate_control/rate_control.cpp +++ b/src/lib/rate_control/rate_control.cpp @@ -47,13 +47,27 @@ void RateControl::setPidGains(const Vector3f &P, const Vector3f &I, const Vector _gain_d = D; } -void RateControl::setSaturationStatus(const Vector &saturation_positive, - const Vector &saturation_negative) +void RateControl::setSaturationStatus(const Vector3 &saturation_positive, + const Vector3 &saturation_negative) { _control_allocator_saturation_positive = saturation_positive; _control_allocator_saturation_negative = saturation_negative; } +void RateControl::setPositiveSaturationFlag(size_t axis, bool is_saturated) +{ + if (axis < 3) { + _control_allocator_saturation_positive(axis) = is_saturated; + } +} + +void RateControl::setNegativeSaturationFlag(size_t axis, bool is_saturated) +{ + if (axis < 3) { + _control_allocator_saturation_negative(axis) = is_saturated; + } +} + Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel, const float dt, const bool landed) { diff --git a/src/lib/rate_control/rate_control.hpp b/src/lib/rate_control/rate_control.hpp index f43493c55d..0ee8bd1516 100644 --- a/src/lib/rate_control/rate_control.hpp +++ b/src/lib/rate_control/rate_control.hpp @@ -75,8 +75,16 @@ public: * Set saturation status * @param control saturation vector from control allocator */ - void setSaturationStatus(const matrix::Vector &saturation_positive, - const matrix::Vector &saturation_negative); + void setSaturationStatus(const matrix::Vector3 &saturation_positive, + const matrix::Vector3 &saturation_negative); + + /** + * Set individual saturation flags + * @param axis 0 roll, 1 pitch, 2 yaw + * @param is_saturated value to update the flag with + */ + void setPositiveSaturationFlag(size_t axis, bool is_saturated); + void setNegativeSaturationFlag(size_t axis, bool is_saturated); /** * Run one control loop cycle calculation diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp index c7306283b3..95079371ec 100644 --- a/src/lib/sensor_calibration/Magnetometer.cpp +++ b/src/lib/sensor_calibration/Magnetometer.cpp @@ -110,7 +110,7 @@ void Magnetometer::SensorCorrectionsUpdate(bool force) bool Magnetometer::set_offset(const Vector3f &offset) { - if (Vector3f(_offset - offset).longerThan(0.01f)) { + if (Vector3f(_offset - offset).longerThan(0.005f)) { if (offset.isAllFinite()) { _offset = offset; _calibration_count++; @@ -159,12 +159,39 @@ bool Magnetometer::set_offdiagonal(const Vector3f &offdiagonal) return false; } -void Magnetometer::set_rotation(Rotation rotation) +void Magnetometer::set_rotation(const Rotation rotation) { - _rotation_enum = rotation; + if (rotation < ROTATION_MAX) { + _rotation_enum = rotation; + + } else { + // invalid rotation, resetting + _rotation_enum = ROTATION_NONE; + } + + // always apply level adjustments + _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(_rotation_enum); + + // clear any custom rotation + _rotation_custom_euler.zero(); +} + +void Magnetometer::set_custom_rotation(const Eulerf &rotation) +{ + _rotation_enum = ROTATION_CUSTOM; + + // store custom rotation + _rotation_custom_euler = rotation; // always apply board level adjustments - _rotation = Dcmf(GetSensorLevelAdjustment()) * get_rot_matrix(rotation); + _rotation = Dcmf(GetSensorLevelAdjustment()) * Dcmf(_rotation_custom_euler); + + // TODO: Note that ideally this shouldn't be necessary for an external sensors, as the definition of *rotation + // between sensor frame & vehicle's body frame isn't affected by the rotation of the Autopilot. + // however, since while doing the 'level-calibration', users don't put the vehicle truly *horizontal, the + // measured board roll/pitch offset isn't true. So this affects external sensors as well (which is why we apply + // internal SensorLevelAdjustment to all the sensors). We need to figure out how to set the sensor board offset + // values properly (i.e. finding Vehicle's true Forward-Right-Down frame in a user's perspective) } bool Magnetometer::set_calibration_index(int calibration_index) @@ -200,13 +227,39 @@ bool Magnetometer::ParametersLoad() // CAL_MAGx_ROT int32_t rotation_value = GetCalibrationParamInt32(SensorString(), "ROT", _calibration_index); + const float euler_roll_deg = GetCalibrationParamFloat(SensorString(), "ROLL", _calibration_index); + const float euler_pitch_deg = GetCalibrationParamFloat(SensorString(), "PITCH", _calibration_index); + const float euler_yaw_deg = GetCalibrationParamFloat(SensorString(), "YAW", _calibration_index); + if (_external) { - if ((rotation_value >= ROTATION_MAX) || (rotation_value < 0)) { + if (((rotation_value >= ROTATION_MAX) && (rotation_value != ROTATION_CUSTOM)) || (rotation_value < 0)) { // invalid rotation, resetting rotation_value = ROTATION_NONE; } - set_rotation(static_cast(rotation_value)); + // if CAL_MAGx_{ROLL,PITCH,YAW} manually set then CAL_MAGx_ROT needs to be ROTATION_CUSTOM + if ((rotation_value != ROTATION_CUSTOM) + && ((fabsf(euler_roll_deg) > FLT_EPSILON) + || (fabsf(euler_pitch_deg) > FLT_EPSILON) + || (fabsf(euler_yaw_deg) > FLT_EPSILON))) { + + rotation_value = ROTATION_CUSTOM; + SetCalibrationParam(SensorString(), "ROT", _calibration_index, rotation_value); + } + + // Handle custom specified euler angle + if (rotation_value == ROTATION_CUSTOM) { + + const matrix::Eulerf rotation_custom_euler{ + math::radians(euler_roll_deg), + math::radians(euler_pitch_deg), + math::radians(euler_yaw_deg)}; + + set_custom_rotation(rotation_custom_euler); + + } else { + set_rotation(static_cast(rotation_value)); + } } else { // internal sensors follow board rotation @@ -311,6 +364,10 @@ bool Magnetometer::ParametersSave(int desired_calibration_index, bool force) success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1); // internal } + success &= SetCalibrationParam(SensorString(), "ROLL", _calibration_index, math::degrees(_rotation_custom_euler(0))); + success &= SetCalibrationParam(SensorString(), "PITCH", _calibration_index, math::degrees(_rotation_custom_euler(1))); + success &= SetCalibrationParam(SensorString(), "YAW", _calibration_index, math::degrees(_rotation_custom_euler(2))); + return success; } diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp index 047cd3398b..8d328e7a7d 100644 --- a/src/lib/sensor_calibration/Magnetometer.hpp +++ b/src/lib/sensor_calibration/Magnetometer.hpp @@ -65,8 +65,21 @@ public: bool set_offset(const matrix::Vector3f &offset); bool set_scale(const matrix::Vector3f &scale); bool set_offdiagonal(const matrix::Vector3f &offdiagonal); + + /** + * @brief Set the rotation enum & corresponding rotation matrix for Magnetometer + * + * @param rotation Rotation enum + */ void set_rotation(Rotation rotation); + /** + * @brief Set the custom rotation & rotation enum to ROTATION_CUSTOM for Magnetometer + * + * @param rotation Rotation euler angles + */ + void set_custom_rotation(const matrix::Eulerf &rotation); + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } uint8_t calibration_count() const { return _calibration_count; } int8_t calibration_index() const { return _calibration_index; } @@ -108,7 +121,13 @@ private: Rotation _rotation_enum{ROTATION_NONE}; + /** + * @brief 3 x 3 Rotation matrix that translates from sensor frame (XYZ) to vehicle body frame (FRD) + */ matrix::Dcmf _rotation; + + matrix::Eulerf _rotation_custom_euler{0.f, 0.f, 0.f}; // custom rotation euler angles (optional) + matrix::Vector3f _offset; matrix::Matrix3f _scale; matrix::Vector3f _thermal_offset; diff --git a/src/lib/sensor_calibration/Utilities.cpp b/src/lib/sensor_calibration/Utilities.cpp index ca3a238838..f1c864bf3f 100644 --- a/src/lib/sensor_calibration/Utilities.cpp +++ b/src/lib/sensor_calibration/Utilities.cpp @@ -222,7 +222,7 @@ enum Rotation GetBoardRotation() int32_t board_rot = -1; param_get(param_find("SENS_BOARD_ROT"), &board_rot); - if (board_rot >= 0 && board_rot <= Rotation::ROTATION_MAX) { + if (board_rot >= 0 && board_rot < Rotation::ROTATION_MAX) { return static_cast(board_rot); } else { diff --git a/src/lib/slew_rate/SlewRate.hpp b/src/lib/slew_rate/SlewRate.hpp index 945276fedc..363f2d2cc7 100644 --- a/src/lib/slew_rate/SlewRate.hpp +++ b/src/lib/slew_rate/SlewRate.hpp @@ -49,6 +49,7 @@ class SlewRate { public: SlewRate() = default; + SlewRate(Type initial_value) { setForcedValue(initial_value); } ~SlewRate() = default; /** diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index f019b6f2d0..bf7a680990 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -217,13 +217,11 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1); PARAM_DEFINE_INT32(SYS_HAS_BARO, 1); /** - * Control the number of distance sensors on the vehicle + * Number of distance sensors to check being available * - * If set to the number of distance sensors, the preflight check will check - * for their presence and valid data publication. Disable with 0 if no distance - * sensor present or to disable the preflight check. + * The preflight check will fail if fewer than this number of distance sensors with valid data is present. * - * @reboot_required true + * Disable the check with 0. * * @group System * @min 0 @@ -239,7 +237,9 @@ PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0); * Note: this is only supported on boards with a separate calibration storage * /fs/mtd_caldata. * - * @boolean + * @value 0 Disabled + * @value 1 All sensors + * @value 2 All sensors except mag * @group System */ PARAM_DEFINE_INT32(SYS_FAC_CAL_MODE, 0); diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 5b6f5f2ffc..38ed565a05 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -156,6 +156,10 @@ void TECSAltitudeReferenceModel::update(const float dt, const AltitudeReferenceS _alt_control_traj_generator.setMaxAccel(param.vert_accel_limit); _alt_control_traj_generator.setMaxVel(fmax(param.max_climb_rate, param.max_sink_rate)); + // XXX: this is a bit risky.. .alt_rate here could be NAN (by interface design) - and is only ok to input to the + // setVelSpFeedback() method because it calls the reset in the logic below when it is NAN. + // TODO: stop it with the NAN interfaces, make sure to take care of this when refactoring and separating altitude + // and height rate control loops. _velocity_control_traj_generator.setVelSpFeedback(setpoint.alt_rate); bool control_altitude = true; diff --git a/src/lib/wind_estimator/python/derivation.py b/src/lib/wind_estimator/python/derivation.py index f3d844e266..fdc14ecee5 100755 --- a/src/lib/wind_estimator/python/derivation.py +++ b/src/lib/wind_estimator/python/derivation.py @@ -34,42 +34,43 @@ Description: Derivation of a wind and airspeed scale (EKF) estimator using SymForce """ -from symforce import symbolic as sm -from symforce import geo -from symforce import typing as T +import symforce +symforce.set_epsilon_to_symbol() + +import symforce.symbolic as sf from derivation_utils import * def fuse_airspeed( - v_local: geo.V3, - state: geo.V3, - P: geo.M33, - airspeed: T.Scalar, - R: T.Scalar, - epsilon: T.Scalar -) -> (geo.V3, geo.V3, T.Scalar, T.Scalar): + v_local: sf.V3, + state: sf.V3, + P: sf.M33, + airspeed: sf.Scalar, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.M13, sf.M31, sf.Scalar, sf.Scalar): - vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) + vel_rel = sf.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) airspeed_pred = vel_rel.norm(epsilon=epsilon) * state[2] innov = airspeed - airspeed_pred - H = geo.V1(airspeed_pred).jacobian(state) + H = sf.V1(airspeed_pred).jacobian(state) innov_var = (H * P * H.T + R)[0,0] K = P * H.T / sm.Max(innov_var, epsilon) - return (geo.V3(H), K, innov_var, innov) + return (H, K, innov_var, innov) def fuse_beta( - v_local: geo.V3, - state: geo.V3, - P: geo.M33, - q_att: geo.V4, - R: T.Scalar, - epsilon: T.Scalar -) -> (geo.V3, geo.V3, T.Scalar, T.Scalar): + v_local: sf.V3, + state: sf.V3, + P: sf.M33, + q_att: sf.V4, + R: sf.Scalar, + epsilon: sf.Scalar +) -> (sf.M13, sf.M31, sf.Scalar, sf.Scalar): - vel_rel = geo.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) + vel_rel = sf.V3(v_local[0] - state[0], v_local[1] - state[1], v_local[2]) relative_wind_body = quat_to_rot(q_att).T * vel_rel # Small angle approximation of side slip model @@ -78,29 +79,29 @@ def fuse_beta( innov = 0.0 - beta_pred - H = geo.V1(beta_pred).jacobian(state) + H = sf.V1(beta_pred).jacobian(state) innov_var = (H * P * H.T + R)[0,0] K = P * H.T / sm.Max(innov_var, epsilon) - return (geo.V3(H), K, innov_var, innov) + return (H, K, innov_var, innov) def init_wind_using_airspeed( - v_local: geo.V3, - heading: T.Scalar, - airspeed: T.Scalar, - v_var: T.Scalar, - heading_var: T.Scalar, - sideslip_var: T.Scalar, - airspeed_var: T.Scalar, -) -> (geo.V2, geo.M22): + v_local: sf.V3, + heading: sf.Scalar, + airspeed: sf.Scalar, + v_var: sf.Scalar, + heading_var: sf.Scalar, + sideslip_var: sf.Scalar, + airspeed_var: sf.Scalar, +) -> (sf.V2, sf.M22): # Initialise wind states assuming zero side slip and horizontal flight - wind = geo.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading)) + wind = sf.V2(v_local[0] - airspeed * sm.cos(heading), v_local[1] - airspeed * sm.sin(heading)) # Zero sideslip, propagate the sideslip variance using partial derivatives w.r.t heading J = wind.jacobian([v_local[0], v_local[1], heading, heading, airspeed]) - R = geo.M55() + R = sf.M55() R[0,0] = v_var R[1,1] = v_var R[2,2] = heading_var diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.h b/src/lib/wind_estimator/python/generated/fuse_airspeed.h index a3d677736d..5d1e293516 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.h +++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/lib/wind_estimator/python/generated/fuse_airspeed.py b/src/lib/wind_estimator/python/generated/fuse_airspeed.py index 3b6180320f..cdbdc597b1 100644 --- a/src/lib/wind_estimator/python/generated/fuse_airspeed.py +++ b/src/lib/wind_estimator/python/generated/fuse_airspeed.py @@ -1,21 +1,21 @@ # ----------------------------------------------------------------------------- # This file was autogenerated by symforce from template: -# backends/python/templates/function/FUNCTION.py.jinja +# function/FUNCTION.py.jinja # Do NOT modify by hand. # ----------------------------------------------------------------------------- -import math # pylint: disable=unused-import -import numpy # pylint: disable=unused-import -import typing as T # pylint: disable=unused-import +# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument,unused-import -import sym # pylint: disable=unused-import +import math +import typing as T +import numpy -# pylint: disable=too-many-locals,too-many-lines,too-many-statements,unused-argument +import sym def fuse_airspeed(v_local, state, P, airspeed, R, epsilon): - # type: (T.Sequence[float], T.Sequence[float], numpy.ndarray, float, float, float) -> T.Tuple[numpy.ndarray, T.List[float], float, float] + # type: (numpy.ndarray, numpy.ndarray, numpy.ndarray, float, float, float) -> T.Tuple[numpy.ndarray, numpy.ndarray, float, float] """ This function was autogenerated from a symbolic function. Do not modify by hand. @@ -39,34 +39,51 @@ def fuse_airspeed(v_local, state, P, airspeed, R, epsilon): # Total ops: 56 # Input arrays + if v_local.shape == (3,): + v_local = v_local.reshape((3, 1)) + elif v_local.shape != (3, 1): + raise IndexError( + "v_local is expected to have shape (3, 1) or (3,); instead had shape {}".format( + v_local.shape + ) + ) + + if state.shape == (3,): + state = state.reshape((3, 1)) + elif state.shape != (3, 1): + raise IndexError( + "state is expected to have shape (3, 1) or (3,); instead had shape {}".format( + state.shape + ) + ) # Intermediate terms (11) - _tmp0 = -state[0] + v_local[0] - _tmp1 = -state[1] + v_local[1] - _tmp2 = math.sqrt(_tmp0 ** 2 + _tmp1 ** 2 + epsilon + v_local[2] ** 2) - _tmp3 = state[2] / _tmp2 + _tmp0 = -state[0, 0] + v_local[0, 0] + _tmp1 = -state[1, 0] + v_local[1, 0] + _tmp2 = math.sqrt(_tmp0**2 + _tmp1**2 + epsilon + v_local[2, 0] ** 2) + _tmp3 = state[2, 0] / _tmp2 _tmp4 = _tmp0 * _tmp3 _tmp5 = _tmp1 * _tmp3 - _tmp6 = -P[0] * _tmp4 - _tmp7 = -P[4] * _tmp5 - _tmp8 = P[8] * _tmp2 + _tmp6 = -P[0, 0] * _tmp4 + _tmp7 = -P[1, 1] * _tmp5 + _tmp8 = P[2, 2] * _tmp2 _tmp9 = ( R - + _tmp2 * (-P[6] * _tmp4 - P[7] * _tmp5 + _tmp8) - - _tmp4 * (-P[1] * _tmp5 + P[2] * _tmp2 + _tmp6) - - _tmp5 * (-P[3] * _tmp4 + P[5] * _tmp2 + _tmp7) + + _tmp2 * (-P[0, 2] * _tmp4 - P[1, 2] * _tmp5 + _tmp8) + - _tmp4 * (-P[1, 0] * _tmp5 + P[2, 0] * _tmp2 + _tmp6) + - _tmp5 * (-P[0, 1] * _tmp4 + P[2, 1] * _tmp2 + _tmp7) ) - _tmp10 = max(_tmp9, epsilon) ** (-1) + _tmp10 = 1 / max(_tmp9, epsilon) # Output terms - _H = numpy.zeros((1, 3)) - _H[0, 0] = -_tmp4 - _H[0, 1] = -_tmp5 - _H[0, 2] = _tmp2 - _K = [0.0] * 3 - _K[0] = _tmp10 * (-P[3] * _tmp5 + P[6] * _tmp2 + _tmp6) - _K[1] = _tmp10 * (-P[1] * _tmp4 + P[7] * _tmp2 + _tmp7) - _K[2] = _tmp10 * (-P[2] * _tmp4 - P[5] * _tmp5 + _tmp8) + _H = numpy.zeros(3) + _H[0] = -_tmp4 + _H[1] = -_tmp5 + _H[2] = _tmp2 + _K = numpy.zeros(3) + _K[0] = _tmp10 * (-P[0, 1] * _tmp5 + P[0, 2] * _tmp2 + _tmp6) + _K[1] = _tmp10 * (-P[1, 0] * _tmp4 + P[1, 2] * _tmp2 + _tmp7) + _K[2] = _tmp10 * (-P[2, 0] * _tmp4 - P[2, 1] * _tmp5 + _tmp8) _innov_var = _tmp9 - _innov = -_tmp2 * state[2] + airspeed + _innov = -_tmp2 * state[2, 0] + airspeed return _H, _K, _innov_var, _innov diff --git a/src/lib/wind_estimator/python/generated/fuse_beta.h b/src/lib/wind_estimator/python/generated/fuse_beta.h index 39469ce892..cbfde79770 100644 --- a/src/lib/wind_estimator/python/generated/fuse_beta.h +++ b/src/lib/wind_estimator/python/generated/fuse_beta.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h index f79f656009..cbfa6fa152 100644 --- a/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h +++ b/src/lib/wind_estimator/python/generated/init_wind_using_airspeed.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -36,6 +36,9 @@ void InitWindUsingAirspeed(const matrix::Matrix& v_local, const Sc matrix::Matrix* const P = nullptr) { // Total ops: 22 + // Unused inputs + (void)heading_var; + // Input arrays // Intermediate terms (7) diff --git a/src/lib/world_magnetic_model/fetch_noaa_table.py b/src/lib/world_magnetic_model/fetch_noaa_table.py index 85863ffb84..8914506f91 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-2022 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-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 @@ -34,6 +34,7 @@ import math import json +import statistics import sys import urllib.request @@ -48,7 +49,7 @@ def constrain(n, nmin, nmax): header = """/**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -112,19 +113,57 @@ 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 + + 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(math.radians(p['declination'] * 10000))), 32767, -32768) + declination_int = constrain(int(round(declination_rad * 10000)), 32767, -32768) print('{0:6d},'.format(declination_int), end='') - print(' },') -print("};\n") + 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'}) @@ -139,19 +178,56 @@ 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 = () + 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()) + latitude_blackout_zone = False + print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') for p in data['result']: + inclination_rad = math.radians(p['inclination']) + + warning = False + + try: + if p['warning']: + warning = True + latitude_blackout_zone = True + #print("WARNING black out zone!", p) + + except: + pass + # inclination in radians * 10^-4 - inclination_int = constrain(int(round(math.radians(p['inclination'] * 10000))), 32767, -32768) + inclination_int = constrain(int(round(inclination_rad * 10000)), 32767, -32768) print('{0:6d},'.format(inclination_int), end='') - print(' },') -print("};\n") + if (inclination_rad > inclination_max): + inclination_max = inclination_rad + inclination_max_lat_lon = (p['latitude'], p['longitude']) + + if (inclination_rad < inclination_min): + inclination_min = inclination_rad + inclination_min_lat_lon = (p['latitude'], p['longitude']) + + if latitude_blackout_zone: + print(' }, // WARNING! black out zone') + else: + print(' },') + +print("};") + +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") # total intensity params = urllib.parse.urlencode({'lat1': 0, 'lat2': 0, 'lon1': 0, 'lon2': 0, 'latStepSize': 1, 'lonStepSize': 1, 'magneticComponent': 'i', 'resultFormat': 'json'}) @@ -166,6 +242,16 @@ print('\t// LONGITUDE: ', end='') for l in range(SAMPLING_MIN_LON, SAMPLING_MAX_LON+1, SAMPLING_RES): print('{0:5d},'.format(l), end='') print('') + +strength_min = float('inf') +strength_max = 0 +strength_min_lat_lon = () +strength_max_lat_lon = () +strength_sum = 0 +strength_sum_count = 0 + +strength_list = [] + 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)) @@ -173,8 +259,26 @@ for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): print('\t/* LAT: {0:3d} */'.format(latitude) + ' { ', end='') for p in data['result']: + + strength_gauss = p['totalintensity'] * 1e-3 + strength_list.append(strength_gauss) + totalintensity_int = int(round(p['totalintensity']/10)) print('{0:5d},'.format(totalintensity_int), end='') + if (strength_gauss > strength_max): + strength_max = strength_gauss + strength_max_lat_lon = (p['latitude'], p['longitude']) + + if (strength_gauss < strength_min): + strength_min = strength_gauss + strength_min_lat_lon = (p['latitude'], p['longitude']) + 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") diff --git a/src/lib/world_magnetic_model/generate_gtest.py b/src/lib/world_magnetic_model/generate_gtest.py index 5e7f5bc3fb..bcf204a49c 100755 --- a/src/lib/world_magnetic_model/generate_gtest.py +++ b/src/lib/world_magnetic_model/generate_gtest.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 ############################################################################ # -# Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-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 @@ -44,7 +44,7 @@ SAMPLING_MAX_LON = 180 header = """/**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -100,13 +100,13 @@ for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): data = json.loads(f.read()) for p in data['result']: - error = 1 + error_deg = 1.0 - # thing start getting worse here - if latitude <= -44: - error = 2 + # why is this area worse? + if (-45 <= p['latitude'] <= -44) and (100 <= p['longitude'] <= 120): + error_deg = 1.8 - print('\tEXPECT_NEAR(get_mag_declination_degrees({}, {}), {}, {} + {});'.format(p['latitude'], p['longitude'], p['declination'], p['declination_uncertainty'], error)) + print('\tEXPECT_NEAR(get_mag_declination_degrees({}, {}), {:.1f}, {:.2f} + {});'.format(p['latitude'], p['longitude'], p['declination'], p['declination_uncertainty'], error_deg)) print('}') print('') @@ -120,11 +120,11 @@ for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): for p in data['result']: error = 1.2 - # thing start getting worse here - if latitude <= -44: - error = 2 + # why is this area worse? + if (-45 <= p['latitude'] <= -44) and (100 <= p['longitude'] <= 120): + error_deg = 1.5 - print('\tEXPECT_NEAR(get_mag_inclination_degrees({}, {}), {}, {} + {});'.format(p['latitude'], p['longitude'], p['inclination'], p['inclination_uncertainty'], error)) + print('\tEXPECT_NEAR(get_mag_inclination_degrees({}, {}), {:.1f}, {:.2f} + {});'.format(p['latitude'], p['longitude'], p['inclination'], p['inclination_uncertainty'], error)) print('}') print('') @@ -136,7 +136,11 @@ for latitude in range(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+1, SAMPLING_RES): data = json.loads(f.read()) for p in data['result']: - error = 500 + error = 0.01 - print('\tEXPECT_NEAR(get_mag_strength_tesla({}, {}) * 1e9, {}, {} + {});'.format(p['latitude'], p['longitude'], p['totalintensity'], p['totalintensity_uncertainty'], error)) + # why is this area worse? + if (-45 <= p['latitude'] <= -35): + error = 0.017 + + print('\tEXPECT_NEAR(get_mag_strength_tesla({}, {}) * 1e9, {:.0f}, {:.0f} + {:.0f});'.format(p['latitude'], p['longitude'], p['totalintensity'], p['totalintensity_uncertainty'], p['totalintensity'] * error)) print('}') diff --git a/src/lib/world_magnetic_model/geo_magnetic_tables.hpp b/src/lib/world_magnetic_model/geo_magnetic_tables.hpp index 3366c1d4bf..a1210cb8be 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-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -47,80 +47,92 @@ static constexpr int LON_DIM = 37; // Magnetic declination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.3287, +// Date: 2023.6083, 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 */ { 25967, 24222, 22477, 20731, 18986, 17240, 15495, 13750, 12005, 10259, 8514, 6769, 5023, 3278, 1533, -212, -1958, -3703, -5448, -7194, -8939,-10684,-12429,-14175,-15920,-17665,-19411,-21156,-22902,-24647,-26392,-28138,-29883, 31203, 29458, 27713, 25967, }, - /* LAT: -80 */ { 22532, 20403, 18464, 16692, 15052, 13514, 12051, 10640, 9265, 7916, 6584, 5266, 3957, 2652, 1344, 24, -1318, -2691, -4102, -5555, -7051, -8589,-10168,-11788,-13454,-15173,-16960,-18833,-20817,-22937,-25212,-27647,-30215, 29979, 27363, 24859, 22532, }, - /* LAT: -70 */ { 14982, 13583, 12455, 11491, 10620, 9787, 8944, 8056, 7102, 6083, 5013, 3920, 2836, 1782, 761, -249, -1287, -2394, -3598, -4901, -6285, -7718, -9168,-10611,-12038,-13455,-14885,-16373,-18000,-19920,-22451,-26265, 30648, 24116, 19620, 16853, 14982, }, - /* LAT: -60 */ { 8438, 8189, 7905, 7627, 7371, 7115, 6804, 6369, 5752, 4931, 3929, 2818, 1699, 675, -202, -957, -1685, -2510, -3520, -4724, -6060, -7434, -8757, -9968,-11032,-11930,-12644,-13139,-13310,-12862,-10754, -3444, 4979, 7699, 8458, 8577, 8438, }, - /* LAT: -50 */ { 5499, 5534, 5475, 5383, 5307, 5268, 5231, 5102, 4756, 4090, 3077, 1804, 465, -710, -1572, -2128, -2520, -2962, -3656, -4683, -5938, -7221, -8364, -9263, -9857,-10090, -9889, -9125, -7611, -5244, -2337, 412, 2522, 3948, 4822, 5296, 5499, }, - /* LAT: -40 */ { 3965, 4057, 4063, 4015, 3953, 3917, 3921, 3908, 3733, 3195, 2167, 721, -841, -2148, -3000, -3440, -3614, -3670, -3846, -4436, -5432, -6517, -7400, -7932, -8030, -7641, -6742, -5354, -3645, -1948, -492, 728, 1773, 2642, 3301, 3733, 3965, }, - /* LAT: -30 */ { 2992, 3078, 3106, 3088, 3027, 2947, 2885, 2851, 2723, 2241, 1198, -327, -1933, -3178, -3906, -4240, -4309, -4088, -3651, -3450, -3841, -4601, -5295, -5635, -5498, -4900, -3934, -2730, -1529, -592, 81, 680, 1306, 1913, 2426, 2791, 2992, }, - /* LAT: -20 */ { 2349, 2395, 2409, 2408, 2363, 2266, 2156, 2079, 1933, 1434, 369, -1126, -2602, -3653, -4173, -4277, -4063, -3493, -2628, -1843, -1591, -1961, -2612, -3073, -3100, -2733, -2094, -1280, -512, -31, 227, 527, 971, 1455, 1882, 2194, 2349, }, - /* LAT: -10 */ { 1956, 1949, 1925, 1918, 1887, 1799, 1686, 1593, 1408, 855, -214, -1595, -2864, -3682, -3934, -3693, -3110, -2325, -1480, -731, -278, -324, -801, -1304, -1511, -1415, -1100, -603, -112, 122, 165, 322, 697, 1141, 1541, 1833, 1956, }, - /* LAT: 0 */ { 1742, 1708, 1649, 1637, 1623, 1551, 1442, 1325, 1070, 448, -596, -1824, -2872, -3446, -3431, -2920, -2149, -1369, -720, -187, 222, 325, 39, -378, -629, -683, -584, -325, -39, 44, -23, 62, 407, 858, 1284, 1610, 1742, }, - /* LAT: 10 */ { 1604, 1612, 1567, 1580, 1603, 1551, 1425, 1235, 858, 141, -881, -1955, -2774, -3103, -2887, -2274, -1497, -788, -277, 101, 425, 569, 402, 79, -153, -261, -287, -210, -109, -155, -300, -276, 29, 491, 978, 1391, 1604, }, - /* LAT: 20 */ { 1416, 1564, 1623, 1713, 1798, 1775, 1616, 1311, 766, -89, -1130, -2076, -2665, -2766, -2431, -1825, -1114, -467, -10, 297, 554, 697, 602, 359, 161, 42, -54, -124, -208, -402, -642, -702, -464, -11, 535, 1057, 1416, }, - /* LAT: 30 */ { 1108, 1474, 1733, 1957, 2116, 2125, 1935, 1511, 784, -243, -1355, -2225, -2633, -2564, -2162, -1581, -926, -309, 150, 454, 684, 828, 804, 654, 506, 380, 218, -1, -289, -666, -1036, -1196, -1033, -603, -24, 586, 1108, }, - /* LAT: 40 */ { 742, 1328, 1823, 2216, 2466, 2510, 2294, 1761, 851, -371, -1602, -2460, -2775, -2619, -2172, -1579, -925, -297, 211, 573, 842, 1038, 1128, 1116, 1040, 890, 615, 195, -350, -959, -1480, -1721, -1602, -1183, -585, 85, 742, }, - /* LAT: 50 */ { 443, 1188, 1870, 2430, 2800, 2909, 2682, 2030, 894, -593, -2005, -2911, -3202, -3009, -2519, -1873, -1163, -466, 148, 652, 1068, 1421, 1698, 1866, 1882, 1691, 1238, 519, -383, -1287, -1962, -2243, -2116, -1675, -1044, -317, 443, }, - /* LAT: 60 */ { 233, 1083, 1890, 2589, 3098, 3317, 3110, 2308, 810, -1130, -2833, -3808, -4061, -3803, -3233, -2489, -1664, -825, -19, 731, 1421, 2050, 2590, 2982, 3137, 2942, 2291, 1160, -263, -1582, -2443, -2746, -2579, -2089, -1403, -610, 233, }, - /* LAT: 70 */ { -27, 905, 1801, 2603, 3222, 3525, 3289, 2187, 4, -2659, -4601, -5437, -5459, -4985, -4223, -3294, -2274, -1211, -138, 920, 1943, 2908, 3773, 4477, 4919, 4942, 4316, 2820, 607, -1504, -2794, -3233, -3075, -2549, -1808, -946, -27, }, - /* LAT: 80 */ { -816, 104, 954, 1645, 2036, 1878, 760, -1675, -4727, -6835, -7634, -7551, -6956, -6063, -4988, -3801, -2543, -1244, 77, 1401, 2713, 3995, 5222, 6361, 7359, 8121, 8464, 8007, 6022, 2118, -1550, -3257, -3603, -3265, -2585, -1736, -816, }, - /* LAT: 90 */ { -29541,-27795,-26050,-24304,-22559,-20814,-19068,-17323,-15578,-13833,-12087,-10342, -8597, -6852, -5107, -3362, -1616, 129, 1874, 3619, 5364, 7110, 8855, 10600, 12346, 14091, 15836, 17582, 19327, 21073, 22818, 24564, 26309, 28055, 29800,-31286,-29541, }, + /* LAT: -90 */ { 25960, 24215, 22470, 20724, 18979, 17233, 15488, 13743, 11998, 10252, 8507, 6762, 5016, 3271, 1526, -219, -1965, -3710, -5455, -7201, -8946,-10691,-12436,-14182,-15927,-17672,-19418,-21163,-22909,-24654,-26399,-28145,-29890, 31196, 29451, 27706, 25960, }, + /* LAT: -80 */ { 22524, 20396, 18458, 16686, 15047, 13510, 12046, 10636, 9261, 7912, 6580, 5262, 3953, 2648, 1340, 20, -1322, -2696, -4107, -5561, -7058, -8596,-10176,-11796,-13462,-15182,-16969,-18843,-20828,-22948,-25223,-27658,-30226, 29968, 27353, 24851, 22524, }, + /* LAT: -70 */ { 14984, 13584, 12456, 11491, 10620, 9786, 8942, 8053, 7100, 6080, 5010, 3917, 2834, 1780, 760, -250, -1288, -2397, -3602, -4906, -6291, -7725, -9176,-10620,-12048,-13465,-14895,-16384,-18013,-19935,-22470,-26290, 30623, 24105, 19619, 16855, 14984, }, // WARNING! black out zone + /* LAT: -60 */ { 8447, 8196, 7910, 7631, 7373, 7115, 6804, 6368, 5750, 4928, 3926, 2814, 1697, 674, -202, -955, -1683, -2509, -3521, -4728, -6067, -7442, -8766, -9977,-11041,-11938,-12652,-13146,-13318,-12869,-10759, -3420, 5009, 7718, 8471, 8588, 8447, }, // WARNING! black out zone + /* LAT: -50 */ { 5505, 5539, 5480, 5387, 5309, 5269, 5231, 5101, 4754, 4087, 3073, 1799, 460, -712, -1571, -2123, -2513, -2956, -3654, -4686, -5945, -7230, -8373, -9272, -9864,-10094, -9891, -9124, -7608, -5240, -2330, 419, 2529, 3955, 4829, 5303, 5505, }, + /* LAT: -40 */ { 3969, 4061, 4066, 4018, 3954, 3917, 3920, 3907, 3731, 3191, 2161, 714, -848, -2152, -2999, -3436, -3607, -3661, -3840, -4436, -5440, -6527, -7410, -7939, -8033, -7641, -6738, -5348, -3640, -1945, -490, 730, 1775, 2645, 3305, 3737, 3969, }, + /* LAT: -30 */ { 2996, 3082, 3108, 3090, 3027, 2946, 2884, 2849, 2720, 2236, 1191, -337, -1942, -3184, -3907, -4238, -4303, -4077, -3640, -3446, -3847, -4610, -5303, -5639, -5497, -4896, -3928, -2724, -1526, -591, 80, 679, 1306, 1914, 2429, 2795, 2996, }, + /* LAT: -20 */ { 2353, 2398, 2412, 2409, 2362, 2264, 2154, 2076, 1930, 1429, 361, -1137, -2613, -3659, -4174, -4274, -4056, -3481, -2614, -1835, -1590, -1966, -2618, -3076, -3099, -2729, -2088, -1276, -510, -31, 225, 524, 969, 1455, 1884, 2197, 2353, }, + /* LAT: -10 */ { 1960, 1953, 1927, 1918, 1886, 1797, 1683, 1589, 1404, 849, -223, -1606, -2873, -3686, -3932, -3687, -3100, -2313, -1469, -723, -274, -324, -804, -1306, -1510, -1413, -1097, -601, -112, 120, 161, 318, 694, 1141, 1542, 1836, 1960, }, + /* LAT: 0 */ { 1746, 1712, 1651, 1638, 1622, 1549, 1438, 1320, 1065, 441, -604, -1833, -2878, -3448, -3427, -2912, -2139, -1359, -712, -180, 227, 327, 38, -379, -628, -681, -583, -324, -40, 41, -28, 57, 404, 856, 1285, 1612, 1746, }, + /* LAT: 10 */ { 1607, 1614, 1568, 1580, 1602, 1548, 1421, 1229, 852, 134, -888, -1961, -2778, -3102, -2882, -2266, -1488, -780, -271, 107, 430, 572, 402, 79, -152, -259, -286, -210, -111, -158, -305, -281, 25, 489, 979, 1393, 1607, }, + /* LAT: 20 */ { 1417, 1565, 1623, 1713, 1796, 1772, 1612, 1305, 760, -95, -1135, -2080, -2666, -2762, -2424, -1817, -1106, -460, -4, 302, 559, 700, 604, 360, 163, 43, -54, -125, -211, -405, -647, -707, -468, -13, 534, 1057, 1417, }, + /* LAT: 30 */ { 1107, 1473, 1732, 1955, 2114, 2122, 1931, 1506, 778, -247, -1359, -2226, -2630, -2559, -2155, -1573, -917, -302, 157, 459, 688, 831, 806, 656, 508, 382, 218, -2, -292, -670, -1041, -1200, -1036, -605, -25, 586, 1107, }, + /* LAT: 40 */ { 739, 1324, 1819, 2212, 2462, 2506, 2290, 1756, 847, -375, -1602, -2457, -2769, -2612, -2163, -1570, -917, -288, 218, 580, 848, 1042, 1132, 1119, 1043, 892, 615, 193, -355, -965, -1485, -1725, -1605, -1186, -587, 82, 739, }, + /* LAT: 50 */ { 436, 1181, 1863, 2424, 2794, 2903, 2677, 2026, 892, -593, -2001, -2904, -3192, -2999, -2508, -1862, -1153, -456, 157, 660, 1076, 1427, 1703, 1870, 1886, 1693, 1237, 514, -390, -1294, -1967, -2247, -2118, -1678, -1047, -322, 436, }, + /* LAT: 60 */ { 222, 1071, 1877, 2577, 3088, 3309, 3103, 2305, 811, -1123, -2820, -3793, -4046, -3788, -3218, -2475, -1651, -813, -8, 741, 1431, 2059, 2598, 2988, 3141, 2943, 2288, 1153, -273, -1591, -2449, -2750, -2583, -2093, -1409, -619, 222, }, + /* LAT: 70 */ { -47, 883, 1779, 2581, 3202, 3508, 3276, 2184, 17, -2630, -4567, -5406, -5432, -4962, -4202, -3275, -2256, -1194, -123, 934, 1957, 2920, 3785, 4488, 4927, 4947, 4315, 2811, 591, -1519, -2807, -3244, -3086, -2561, -1823, -964, -47, }, // WARNING! black out zone + /* LAT: 80 */ { -864, 55, 904, 1595, 1987, 1837, 743, -1642, -4648, -6750, -7562, -7492, -6908, -6022, -4952, -3769, -2514, -1217, 102, 1425, 2737, 4018, 5245, 6384, 7382, 8144, 8486, 8024, 6018, 2070, -1618, -3319, -3658, -3315, -2633, -1784, -864, }, // WARNING! black out zone + /* LAT: 90 */ { -29433,-27688,-25942,-24197,-22451,-20706,-18961,-17215,-15470,-13725,-11980,-10234, -8489, -6744, -4999, -3254, -1509, 236, 1982, 3727, 5472, 7217, 8962, 10708, 12453, 14199, 15944, 17689, 19435, 21180, 22926, 24671, 26417, 28162, 29908,-31178,-29433, }, // WARNING! black out zone }; +static constexpr float WMM_DECLINATION_MIN_RAD = -3.118; // latitude: 90, longitude: 170 +static constexpr float WMM_DECLINATION_MAX_RAD = 3.120; // latitude: -90, longitude: 150 + // Magnetic inclination data in radians * 10^-4 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.3287, +// Date: 2023.6083, 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 */ { -12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12566,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567,-12567, }, - /* LAT: -80 */ { -13650,-13516,-13355,-13175,-12981,-12780,-12576,-12376,-12186,-12010,-11853,-11719,-11609,-11523,-11459,-11418,-11398,-11400,-11427,-11481,-11565,-11680,-11825,-11999,-12197,-12414,-12644,-12877,-13105,-13317,-13503,-13653,-13756,-13807,-13803,-13749,-13650, }, - /* LAT: -70 */ { -14098,-13779,-13460,-13137,-12805,-12462,-12107,-11750,-11408,-11100,-10848,-10666,-10554,-10502,-10487,-10490,-10495,-10504,-10526,-10582,-10690,-10865,-11113,-11429,-11802,-12221,-12669,-13134,-13600,-14052,-14469,-14813,-14999,-14943,-14712,-14414,-14098, }, - /* LAT: -60 */ { -13514,-13160,-12821,-12488,-12145,-11773,-11358,-10904,-10437,-10008, -9680, -9507, -9506, -9644, -9847,-10037,-10158,-10198,-10182,-10166,-10214,-10374,-10667,-11078,-11579,-12136,-12723,-13319,-13908,-14469,-14967,-15255,-15075,-14689,-14282,-13887,-13514, }, - /* LAT: -50 */ { -12494,-12151,-11819,-11497,-11173,-10827,-10428, -9957, -9428, -8909, -8522, -8402, -8607, -9070, -9633,-10141,-10494,-10649,-10613,-10456,-10314,-10324,-10550,-10972,-11517,-12111,-12697,-13234,-13679,-13976,-14084,-14010,-13806,-13522,-13193,-12845,-12494, }, - /* LAT: -40 */ { -11239,-10889,-10541,-10195, -9856, -9518, -9158, -8733, -8213, -7650, -7229, -7196, -7664, -8491, -9412,-10237,-10890,-11316,-11445,-11268,-10921,-10653,-10654,-10943,-11409,-11914,-12358,-12676,-12833,-12837,-12748,-12612,-12434,-12202,-11915,-11587,-11239, }, - /* LAT: -30 */ { -9602, -9221, -8839, -8448, -8056, -7681, -7327, -6938, -6426, -5816, -5370, -5466, -6249, -7458, -8708, -9798,-10705,-11407,-11793,-11770,-11389,-10882,-10555,-10563,-10819,-11141,-11394,-11503,-11438,-11263,-11090,-10959,-10816,-10610,-10329, -9982, -9602, }, - /* LAT: -20 */ { -7373, -6927, -6506, -6077, -5633, -5204, -4816, -4406, -3842, -3158, -2719, -2998, -4128, -5750, -7375, -8735, -9790,-10555,-10981,-11002,-10628,-10009, -9454, -9218, -9271, -9434, -9573, -9586, -9411, -9140, -8950, -8869, -8770, -8566, -8249, -7835, -7373, }, - /* LAT: -10 */ { -4418, -3874, -3416, -2976, -2519, -2069, -1658, -1209, -588, 111, 462, 14, -1338, -3267, -5238, -6831, -7910, -8530, -8784, -8710, -8290, -7597, -6935, -6596, -6560, -6653, -6768, -6789, -6599, -6305, -6156, -6175, -6144, -5932, -5548, -5017, -4418, }, - /* LAT: 0 */ { -910, -278, 193, 599, 1017, 1434, 1820, 2257, 2834, 3405, 3595, 3081, 1765, -159, -2199, -3831, -4819, -5232, -5281, -5102, -4651, -3927, -3222, -2856, -2797, -2865, -2988, -3060, -2929, -2696, -2648, -2804, -2879, -2699, -2274, -1640, -910, }, - /* LAT: 10 */ { 2558, 3192, 3631, 3975, 4330, 4697, 5045, 5426, 5870, 6233, 6260, 5757, 4660, 3083, 1395, 35, -752, -984, -884, -642, -223, 423, 1056, 1389, 1453, 1412, 1313, 1222, 1264, 1365, 1282, 1003, 799, 866, 1219, 1825, 2558, }, - /* LAT: 20 */ { 5414, 5947, 6330, 6627, 6941, 7284, 7624, 7966, 8294, 8488, 8393, 7922, 7077, 5976, 4853, 3954, 3437, 3330, 3487, 3735, 4070, 4540, 5001, 5254, 5313, 5299, 5253, 5195, 5181, 5160, 4980, 4639, 4333, 4239, 4412, 4838, 5414, }, - /* LAT: 30 */ { 7568, 7943, 8262, 8546, 8855, 9202, 9557, 9894, 10167, 10276, 10126, 9699, 9067, 8354, 7696, 7191, 6906, 6872, 7019, 7234, 7483, 7785, 8074, 8248, 8306, 8322, 8327, 8317, 8292, 8207, 7981, 7623, 7263, 7041, 7025, 7221, 7568, }, - /* LAT: 40 */ { 9266, 9487, 9743, 10029, 10355, 10715, 11081, 11418, 11667, 11745, 11593, 11231, 10757, 10285, 9891, 9611, 9466, 9467, 9581, 9745, 9922, 10108, 10281, 10406, 10484, 10547, 10604, 10637, 10619, 10505, 10261, 9908, 9538, 9252, 9110, 9122, 9266, }, - /* LAT: 50 */ { 10802, 10923, 11124, 11393, 11715, 12068, 12421, 12735, 12955, 13012, 12872, 12574, 12210, 11867, 11593, 11409, 11319, 11318, 11387, 11492, 11607, 11724, 11841, 11957, 12076, 12200, 12314, 12387, 12376, 12248, 12000, 11671, 11331, 11046, 10856, 10776, 10802, }, - /* LAT: 60 */ { 12320, 12391, 12540, 12757, 13026, 13325, 13626, 13891, 14066, 14093, 13958, 13710, 13423, 13153, 12933, 12778, 12689, 12659, 12677, 12726, 12795, 12881, 12988, 13121, 13281, 13457, 13622, 13730, 13734, 13612, 13386, 13106, 12828, 12592, 12422, 12330, 12320, }, - /* LAT: 70 */ { 13757, 13798, 13892, 14032, 14210, 14412, 14619, 14800, 14905, 14887, 14753, 14553, 14337, 14134, 13961, 13827, 13733, 13679, 13661, 13674, 13717, 13790, 13894, 14031, 14199, 14387, 14571, 14710, 14755, 14681, 14519, 14320, 14126, 13961, 13841, 13772, 13757, }, - /* LAT: 80 */ { 14994, 15005, 15041, 15099, 15174, 15258, 15336, 15381, 15367, 15292, 15182, 15060, 14938, 14825, 14725, 14644, 14582, 14542, 14523, 14527, 14554, 14604, 14676, 14769, 14881, 15008, 15144, 15276, 15384, 15427, 15386, 15297, 15201, 15115, 15050, 15009, 14994, }, - /* LAT: 90 */ { 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, 15398, }, + /* LAT: -90 */ { -12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565,-12565, }, + /* LAT: -80 */ { -13648,-13514,-13353,-13173,-12979,-12778,-12574,-12374,-12184,-12008,-11852,-11718,-11608,-11521,-11458,-11416,-11396,-11399,-11426,-11480,-11564,-11678,-11824,-11997,-12196,-12413,-12643,-12876,-13104,-13316,-13502,-13652,-13755,-13805,-13801,-13747,-13648, }, + /* LAT: -70 */ { -14095,-13776,-13457,-13134,-12803,-12459,-12105,-11749,-11406,-11099,-10848,-10665,-10554,-10502,-10487,-10489,-10494,-10502,-10524,-10580,-10688,-10864,-11112,-11428,-11802,-12221,-12670,-13134,-13600,-14052,-14469,-14812,-14997,-14941,-14709,-14411,-14095, }, // WARNING! black out zone + /* LAT: -60 */ { -13512,-13158,-12819,-12486,-12143,-11771,-11356,-10902,-10436,-10007, -9680, -9508, -9508, -9646, -9849,-10038,-10158,-10196,-10179,-10163,-10211,-10372,-10666,-11078,-11580,-12137,-12725,-13321,-13910,-14471,-14968,-15257,-15076,-14688,-14281,-13886,-13512, }, // WARNING! black out zone + /* LAT: -50 */ { -12493,-12150,-11818,-11495,-11172,-10826,-10427, -9956, -9428, -8909, -8524, -8405, -8612, -9076, -9639,-10145,-10496,-10649,-10609,-10451,-10309,-10320,-10549,-10972,-11519,-12113,-12699,-13236,-13681,-13978,-14084,-14011,-13807,-13523,-13193,-12844,-12493, }, + /* LAT: -40 */ { -11239,-10889,-10540,-10194, -9854, -9517, -9157, -8733, -8213, -7651, -7232, -7201, -7672, -8502, -9422,-10245,-10896,-11319,-11444,-11263,-10915,-10649,-10653,-10944,-11412,-11917,-12360,-12677,-12833,-12837,-12748,-12612,-12434,-12203,-11916,-11587,-11239, }, + /* LAT: -30 */ { -9602, -9220, -8837, -8446, -8054, -7680, -7325, -6937, -6426, -5816, -5373, -5474, -6262, -7473, -8722, -9810,-10716,-11414,-11796,-11768,-11384,-10876,-10552,-10563,-10821,-11143,-11395,-11502,-11436,-11261,-11089,-10959,-10817,-10611,-10330, -9983, -9602, }, + /* LAT: -20 */ { -7373, -6926, -6504, -6073, -5629, -5201, -4813, -4405, -3842, -3159, -2723, -3009, -4145, -5770, -7395, -8751, -9803,-10564,-10985,-11002,-10624,-10004, -9450, -9215, -9271, -9434, -9572, -9584, -9408, -9137, -8948, -8869, -8772, -8568, -8252, -7837, -7373, }, + /* LAT: -10 */ { -4418, -3872, -3412, -2971, -2514, -2065, -1655, -1208, -588, 110, 456, 1, -1358, -3291, -5260, -6848, -7922, -8538, -8787, -8710, -8286, -7591, -6929, -6592, -6557, -6651, -6765, -6785, -6595, -6300, -6153, -6175, -6147, -5936, -5552, -5020, -4418, }, + /* LAT: 0 */ { -911, -276, 198, 604, 1023, 1439, 1825, 2259, 2833, 3402, 3589, 3068, 1745, -183, -2221, -3848, -4830, -5238, -5283, -5101, -4647, -3920, -3215, -2851, -2793, -2860, -2983, -3054, -2923, -2690, -2645, -2805, -2883, -2704, -2279, -1644, -911, }, + /* LAT: 10 */ { 2557, 3194, 3635, 3980, 4336, 4702, 5049, 5427, 5869, 6230, 6253, 5746, 4643, 3064, 1376, 20, -760, -988, -885, -641, -219, 429, 1062, 1394, 1458, 1418, 1320, 1229, 1271, 1371, 1286, 1003, 795, 860, 1213, 1821, 2557, }, + /* LAT: 20 */ { 5413, 5949, 6333, 6631, 6945, 7287, 7627, 7967, 8292, 8485, 8388, 7913, 7065, 5962, 4840, 3943, 3431, 3327, 3487, 3736, 4073, 4544, 5005, 5258, 5317, 5305, 5259, 5201, 5187, 5164, 4983, 4639, 4330, 4235, 4408, 4835, 5413, }, + /* LAT: 30 */ { 7568, 7944, 8264, 8548, 8857, 9203, 9558, 9894, 10165, 10272, 10121, 9692, 9058, 8346, 7688, 7184, 6902, 6870, 7019, 7234, 7485, 7788, 8078, 8251, 8310, 8327, 8332, 8322, 8297, 8210, 7983, 7623, 7262, 7039, 7023, 7220, 7568, }, + /* LAT: 40 */ { 9266, 9487, 9744, 10029, 10355, 10715, 11081, 11416, 11664, 11742, 11588, 11225, 10751, 10279, 9886, 9608, 9464, 9466, 9581, 9746, 9924, 10110, 10283, 10408, 10487, 10550, 10608, 10641, 10622, 10508, 10262, 9908, 9538, 9252, 9109, 9122, 9266, }, + /* LAT: 50 */ { 10802, 10923, 11123, 11392, 11714, 12067, 12419, 12733, 12952, 13009, 12868, 12570, 12206, 11863, 11590, 11407, 11317, 11317, 11387, 11493, 11609, 11726, 11843, 11960, 12079, 12203, 12318, 12390, 12378, 12250, 12001, 11671, 11331, 11046, 10856, 10776, 10802, }, + /* LAT: 60 */ { 12320, 12391, 12539, 12756, 13024, 13323, 13623, 13888, 14063, 14090, 13955, 13707, 13420, 13150, 12931, 12777, 12688, 12659, 12677, 12726, 12796, 12883, 12990, 13123, 13283, 13460, 13625, 13733, 13736, 13613, 13386, 13107, 12829, 12592, 12422, 12330, 12320, }, + /* LAT: 70 */ { 13757, 13798, 13891, 14030, 14208, 14409, 14616, 14796, 14901, 14884, 14751, 14552, 14336, 14133, 13960, 13826, 13733, 13680, 13662, 13675, 13719, 13792, 13896, 14034, 14201, 14389, 14573, 14712, 14756, 14682, 14519, 14321, 14126, 13962, 13841, 13772, 13757, }, // WARNING! black out zone + /* LAT: 80 */ { 14993, 15004, 15039, 15097, 15171, 15255, 15332, 15378, 15364, 15290, 15181, 15059, 14938, 14825, 14726, 14644, 14583, 14542, 14524, 14529, 14556, 14605, 14677, 14771, 14883, 15010, 15146, 15279, 15386, 15429, 15387, 15298, 15201, 15115, 15049, 15008, 14993, }, // WARNING! black out zone + /* LAT: 90 */ { 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, 15399, }, // 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 + // Magnetic strength data in milli-Gauss * 10 // Model: WMM-2020, // Version: 0.5.1.11, -// Date: 2023.3287, +// Date: 2023.6083, 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 */ { 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, 5447, }, - /* LAT: -80 */ { 6053, 5989, 5910, 5818, 5714, 5602, 5484, 5363, 5240, 5120, 5006, 4901, 4807, 4728, 4665, 4620, 4596, 4595, 4616, 4663, 4734, 4828, 4943, 5075, 5219, 5369, 5519, 5662, 5793, 5907, 5999, 6068, 6112, 6131, 6126, 6100, 6053, }, - /* LAT: -70 */ { 6298, 6164, 6013, 5847, 5668, 5476, 5270, 5056, 4837, 4622, 4419, 4237, 4080, 3949, 3846, 3769, 3722, 3710, 3739, 3816, 3945, 4126, 4355, 4623, 4919, 5226, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6546, 6496, 6410, 6298, }, - /* LAT: -60 */ { 6183, 5990, 5788, 5578, 5359, 5124, 4867, 4589, 4297, 4007, 3741, 3514, 3335, 3201, 3103, 3029, 2979, 2959, 2985, 3077, 3246, 3499, 3827, 4214, 4637, 5072, 5493, 5877, 6201, 6449, 6612, 6690, 6691, 6627, 6513, 6361, 6183, }, - /* LAT: -50 */ { 5841, 5610, 5377, 5146, 4913, 4668, 4397, 4094, 3765, 3434, 3132, 2893, 2733, 2642, 2594, 2559, 2524, 2498, 2503, 2576, 2752, 3044, 3442, 3915, 4423, 4929, 5403, 5821, 6160, 6401, 6538, 6578, 6535, 6424, 6261, 6062, 5841, }, - /* LAT: -40 */ { 5391, 5145, 4899, 4659, 4424, 4186, 3931, 3647, 3333, 3007, 2709, 2488, 2372, 2346, 2365, 2386, 2389, 2376, 2363, 2393, 2525, 2804, 3225, 3741, 4289, 4812, 5278, 5665, 5957, 6143, 6231, 6233, 6163, 6031, 5849, 5631, 5391, }, - /* LAT: -30 */ { 4877, 4636, 4396, 4161, 3934, 3715, 3496, 3265, 3009, 2733, 2476, 2294, 2226, 2251, 2318, 2389, 2454, 2502, 2522, 2534, 2604, 2807, 3173, 3664, 4196, 4692, 5109, 5424, 5625, 5725, 5751, 5724, 5644, 5511, 5331, 5114, 4877, }, - /* LAT: -20 */ { 4320, 4107, 3897, 3692, 3496, 3313, 3144, 2981, 2802, 2604, 2413, 2280, 2241, 2286, 2376, 2487, 2614, 2741, 2827, 2861, 2887, 2986, 3230, 3618, 4069, 4495, 4842, 5074, 5178, 5185, 5155, 5108, 5026, 4898, 4731, 4534, 4320, }, - /* LAT: -10 */ { 3790, 3628, 3475, 3328, 3192, 3072, 2968, 2876, 2778, 2662, 2540, 2442, 2398, 2424, 2512, 2642, 2797, 2955, 3076, 3137, 3151, 3179, 3306, 3561, 3888, 4210, 4473, 4634, 4668, 4615, 4548, 4484, 4396, 4272, 4122, 3958, 3790, }, - /* LAT: 0 */ { 3412, 3318, 3234, 3161, 3105, 3067, 3040, 3021, 2995, 2946, 2867, 2773, 2694, 2665, 2709, 2813, 2945, 3080, 3194, 3268, 3299, 3321, 3398, 3557, 3768, 3981, 4161, 4265, 4270, 4202, 4114, 4022, 3911, 3779, 3645, 3520, 3412, }, - /* LAT: 10 */ { 3282, 3251, 3230, 3226, 3250, 3297, 3352, 3404, 3438, 3428, 3359, 3244, 3119, 3025, 3001, 3044, 3125, 3224, 3324, 3409, 3472, 3535, 3624, 3743, 3880, 4019, 4138, 4208, 4209, 4147, 4037, 3893, 3731, 3572, 3436, 3339, 3282, }, - /* LAT: 20 */ { 3399, 3401, 3427, 3480, 3571, 3692, 3820, 3936, 4016, 4028, 3954, 3813, 3647, 3509, 3435, 3424, 3460, 3533, 3630, 3728, 3819, 3918, 4029, 4141, 4251, 4363, 4466, 4531, 4541, 4482, 4344, 4141, 3913, 3703, 3540, 3439, 3399, }, - /* LAT: 30 */ { 3722, 3728, 3781, 3880, 4022, 4192, 4367, 4523, 4630, 4656, 4584, 4429, 4241, 4078, 3975, 3930, 3935, 3987, 4074, 4173, 4271, 4377, 4494, 4612, 4732, 4859, 4981, 5067, 5092, 5033, 4876, 4636, 4361, 4107, 3907, 3778, 3722, }, - /* LAT: 40 */ { 4222, 4218, 4282, 4404, 4570, 4757, 4939, 5096, 5200, 5226, 5158, 5011, 4825, 4652, 4526, 4451, 4427, 4450, 4511, 4591, 4678, 4776, 4891, 5025, 5176, 5338, 5490, 5600, 5638, 5582, 5426, 5187, 4914, 4654, 4443, 4297, 4222, }, - /* LAT: 50 */ { 4832, 4822, 4876, 4985, 5130, 5289, 5438, 5560, 5634, 5644, 5581, 5455, 5293, 5131, 4994, 4898, 4844, 4833, 4859, 4910, 4980, 5071, 5190, 5341, 5516, 5701, 5867, 5984, 6027, 5981, 5850, 5654, 5430, 5215, 5035, 4905, 4832, }, - /* LAT: 60 */ { 5392, 5378, 5404, 5465, 5550, 5644, 5731, 5798, 5832, 5824, 5771, 5678, 5558, 5430, 5312, 5217, 5151, 5118, 5116, 5144, 5201, 5288, 5406, 5554, 5720, 5888, 6035, 6138, 6181, 6158, 6075, 5950, 5804, 5662, 5540, 5448, 5392, }, - /* LAT: 70 */ { 5726, 5704, 5700, 5711, 5732, 5758, 5782, 5798, 5798, 5780, 5742, 5685, 5616, 5540, 5467, 5403, 5356, 5328, 5324, 5344, 5389, 5459, 5551, 5660, 5777, 5891, 5989, 6062, 6100, 6103, 6074, 6021, 5954, 5884, 5819, 5765, 5726, }, - /* LAT: 80 */ { 5790, 5772, 5757, 5744, 5734, 5724, 5715, 5703, 5689, 5671, 5649, 5624, 5598, 5571, 5547, 5527, 5513, 5509, 5514, 5529, 5555, 5590, 5633, 5681, 5730, 5778, 5821, 5855, 5879, 5892, 5894, 5887, 5873, 5853, 5832, 5810, 5790, }, + /* LAT: -90 */ { 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, 5445, }, + /* LAT: -80 */ { 6052, 5987, 5908, 5816, 5712, 5600, 5482, 5360, 5238, 5118, 5004, 4899, 4805, 4726, 4663, 4619, 4595, 4593, 4615, 4661, 4732, 4827, 4942, 5074, 5218, 5368, 5518, 5661, 5792, 5906, 5998, 6067, 6111, 6130, 6125, 6098, 6052, }, + /* LAT: -70 */ { 6296, 6162, 6011, 5845, 5666, 5473, 5268, 5053, 4834, 4619, 4417, 4235, 4078, 3947, 3843, 3767, 3721, 3709, 3738, 3815, 3944, 4125, 4355, 4623, 4919, 5227, 5530, 5814, 6064, 6267, 6418, 6513, 6554, 6545, 6495, 6409, 6296, }, + /* LAT: -60 */ { 6182, 5988, 5786, 5576, 5356, 5121, 4864, 4586, 4294, 4005, 3738, 3512, 3333, 3200, 3101, 3028, 2977, 2957, 2984, 3075, 3246, 3499, 3828, 4215, 4638, 5073, 5494, 5878, 6202, 6450, 6612, 6690, 6691, 6627, 6512, 6360, 6182, }, + /* LAT: -50 */ { 5840, 5609, 5376, 5144, 4911, 4665, 4394, 4091, 3762, 3431, 3130, 2891, 2731, 2641, 2592, 2557, 2523, 2496, 2501, 2575, 2751, 3044, 3444, 3917, 4425, 4931, 5405, 5823, 6161, 6402, 6539, 6578, 6535, 6423, 6260, 6061, 5840, }, + /* LAT: -40 */ { 5390, 5144, 4898, 4657, 4422, 4184, 3929, 3645, 3331, 3005, 2707, 2486, 2370, 2346, 2365, 2385, 2388, 2374, 2361, 2391, 2524, 2805, 3227, 3744, 4292, 4815, 5280, 5667, 5958, 6144, 6232, 6234, 6163, 6031, 5849, 5630, 5390, }, + /* LAT: -30 */ { 4877, 4635, 4395, 4159, 3933, 3713, 3494, 3263, 3007, 2731, 2474, 2293, 2225, 2251, 2318, 2388, 2452, 2501, 2520, 2532, 2602, 2807, 3175, 3667, 4199, 4695, 5111, 5425, 5626, 5725, 5751, 5724, 5644, 5511, 5331, 5114, 4877, }, + /* LAT: -20 */ { 4320, 4106, 3896, 3691, 3494, 3312, 3143, 2979, 2801, 2602, 2411, 2279, 2240, 2286, 2376, 2487, 2614, 2740, 2826, 2859, 2885, 2986, 3232, 3620, 4072, 4498, 4844, 5075, 5179, 5186, 5155, 5108, 5026, 4898, 4731, 4534, 4320, }, + /* LAT: -10 */ { 3790, 3628, 3474, 3327, 3191, 3071, 2967, 2874, 2776, 2660, 2538, 2440, 2397, 2424, 2513, 2642, 2798, 2955, 3076, 3136, 3150, 3178, 3306, 3562, 3891, 4212, 4475, 4635, 4668, 4615, 4548, 4485, 4397, 4272, 4123, 3958, 3790, }, + /* LAT: 0 */ { 3412, 3318, 3233, 3160, 3104, 3066, 3039, 3019, 2993, 2944, 2865, 2771, 2693, 2665, 2709, 2814, 2946, 3081, 3194, 3268, 3298, 3321, 3398, 3558, 3769, 3983, 4162, 4266, 4271, 4203, 4114, 4022, 3911, 3780, 3645, 3520, 3412, }, + /* LAT: 10 */ { 3282, 3251, 3230, 3225, 3249, 3296, 3350, 3402, 3436, 3425, 3356, 3242, 3116, 3024, 3001, 3044, 3126, 3224, 3324, 3409, 3472, 3535, 3625, 3744, 3881, 4021, 4140, 4209, 4210, 4148, 4037, 3894, 3732, 3572, 3436, 3339, 3282, }, + /* LAT: 20 */ { 3399, 3401, 3426, 3479, 3570, 3690, 3818, 3934, 4013, 4025, 3951, 3810, 3644, 3507, 3434, 3424, 3460, 3534, 3631, 3729, 3820, 3919, 4031, 4143, 4253, 4365, 4468, 4534, 4543, 4483, 4345, 4142, 3914, 3704, 3540, 3439, 3399, }, + /* LAT: 30 */ { 3722, 3727, 3780, 3878, 4020, 4190, 4365, 4520, 4627, 4653, 4580, 4426, 4238, 4076, 3974, 3929, 3935, 3987, 4075, 4174, 4272, 4379, 4496, 4614, 4734, 4862, 4983, 5070, 5094, 5035, 4878, 4637, 4362, 4108, 3908, 3778, 3722, }, + /* LAT: 40 */ { 4222, 4218, 4281, 4402, 4568, 4754, 4937, 5093, 5197, 5223, 5155, 5008, 4823, 4651, 4525, 4451, 4427, 4451, 4513, 4593, 4680, 4778, 4893, 5027, 5178, 5341, 5493, 5602, 5640, 5584, 5427, 5188, 4914, 4655, 4444, 4298, 4222, }, + /* LAT: 50 */ { 4832, 4822, 4875, 4982, 5127, 5286, 5435, 5557, 5631, 5641, 5579, 5453, 5292, 5130, 4994, 4898, 4845, 4834, 4860, 4911, 4981, 5073, 5192, 5343, 5519, 5703, 5869, 5986, 6028, 5982, 5850, 5655, 5431, 5216, 5036, 4905, 4832, }, + /* LAT: 60 */ { 5393, 5377, 5402, 5463, 5547, 5641, 5728, 5795, 5830, 5822, 5770, 5677, 5557, 5430, 5312, 5217, 5152, 5119, 5117, 5146, 5203, 5290, 5408, 5556, 5722, 5890, 6037, 6139, 6182, 6159, 6076, 5950, 5805, 5663, 5541, 5448, 5393, }, + /* LAT: 70 */ { 5726, 5704, 5699, 5709, 5731, 5757, 5781, 5796, 5797, 5779, 5741, 5685, 5616, 5540, 5467, 5404, 5357, 5329, 5325, 5345, 5391, 5461, 5553, 5661, 5778, 5892, 5991, 6063, 6101, 6104, 6075, 6022, 5955, 5885, 5819, 5765, 5726, }, + /* LAT: 80 */ { 5790, 5772, 5756, 5744, 5733, 5724, 5714, 5703, 5688, 5671, 5649, 5625, 5598, 5572, 5547, 5528, 5514, 5510, 5515, 5530, 5556, 5591, 5634, 5682, 5732, 5780, 5822, 5856, 5880, 5893, 5895, 5888, 5873, 5854, 5832, 5810, 5790, }, /* LAT: 90 */ { 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, 5684, }, }; +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; + + diff --git a/src/lib/world_magnetic_model/test_geo_lookup.cpp b/src/lib/world_magnetic_model/test_geo_lookup.cpp index 0d3757a9e8..3d216c3378 100644 --- a/src/lib/world_magnetic_model/test_geo_lookup.cpp +++ b/src/lib/world_magnetic_model/test_geo_lookup.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,5049 +40,5049 @@ TEST(GeoLookupTest, declination) { - EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.50566, 0.40065 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -175), 31.69586, 0.39371 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -170), 31.70545, 0.38793 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -165), 31.58317, 0.38312 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -160), 31.37217, 0.3791 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -155), 31.11298, 0.37576 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -150), 30.84433, 0.37297 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -145), 30.6007, 0.37063 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -140), 30.40695, 0.36866 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -135), 30.2719, 0.36698 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -130), 30.18285, 0.36556 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -125), 30.10305, 0.36437 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -120), 29.97263, 0.36343 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -115), 29.71298, 0.36279 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -110), 29.23373, 0.36253 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -105), 28.44186, 0.36278 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -100), 27.25195, 0.36369 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -95), 25.59667, 0.36541 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -90), 23.43663, 0.3681 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -85), 20.7687, 0.37189 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -80), 17.63217, 0.37686 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -75), 14.11148, 0.38306 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -70), 10.33372, 0.3905 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -65), 6.45875, 0.39916 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -60), 2.6614, 0.40904 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -55), -0.89188, 0.42014 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -50), -4.06681, 0.43246 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -45), -6.77958, 0.44595 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -40), -9.00606, 0.46041 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -35), -10.78122, 0.47549 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -30), -12.19087, 0.4906 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -25), -13.35941, 0.50492 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -20), -14.43684, 0.51744 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -15), -15.5862, 0.52701 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -10), -16.96891, 0.53257 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, -5), -18.72493, 0.53335 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 0), -20.94835, 0.52906 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 5), -23.66615, 0.52004 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 10), -26.83055, 0.5072 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 15), -30.3309, 0.49184 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 20), -34.02043, 0.4754 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 25), -37.74655, 0.45921 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.37338, 0.44434 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -44.79206, 0.43151 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -47.92048, 0.42114 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 45), -50.69739, 0.41343 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 50), -53.07519, 0.40843 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 55), -55.01385, 0.40614 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 60), -56.47617, 0.40654 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 65), -57.42368, 0.40965 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 70), -57.8117, 0.41559 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 75), -57.58286, 0.42454 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 80), -56.65944, 0.43681 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 85), -54.93683, 0.45275 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 90), -52.28282, 0.4726 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 95), -48.54938, 0.4962 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 100), -43.60629, 0.52255 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 105), -37.40282, 0.54927 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 110), -30.04874, 0.57252 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 115), -21.872, 0.58786 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 120), -13.38769, 0.59212 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 125), -5.15993, 0.58499 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 130), 2.36154, 0.56894 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 135), 8.92368, 0.54763 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 140), 14.45182, 0.52438 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 145), 18.98528, 0.5015 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 150), 22.61798, 0.4803 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 155), 25.46159, 0.4614 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 160), 27.62788, 0.44494 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 165), 29.22237, 0.43087 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 170), 30.34257, 0.41896 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 175), 31.07731, 0.40898 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-50, 180), 31.50566, 0.40065 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -180), 26.54462, 0.37423 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -175), 26.847, 0.37033 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -170), 26.97658, 0.36708 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -165), 26.97301, 0.3644 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -160), 26.87004, 0.36219 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -155), 26.70027, 0.3604 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -150), 26.49855, 0.35898 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -145), 26.30152, 0.35791 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -140), 26.14273, 0.35715 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -135), 26.04516, 0.35668 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -130), 26.0138, 0.35648 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -125), 26.03015, 0.35656 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -120), 26.04902, 0.3569 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -115), 25.99799, 0.35755 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -110), 25.77998, 0.35854 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -105), 25.28033, 0.35997 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -100), 24.37909, 0.36198 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -95), 22.96755, 0.36477 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -90), 20.96594, 0.36852 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -85), 18.33982, 0.37341 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -80), 15.11302, 0.37957 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -75), 11.37546, 0.38703 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -70), 7.28267, 0.39577 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -65), 3.04267, 0.40574 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -60), -1.11141, 0.41692 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -55), -4.95856, 0.42939 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -50), -8.32604, 0.44328 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -45), -11.11345, 0.45871 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -40), -13.29901, 0.47572 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -35), -14.92951, 0.49414 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -30), -16.10029, 0.51358 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.93495, 0.53327 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -20), -17.57358, 0.55204 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -15), -18.17284, 0.56828 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -10), -18.91008, 0.58002 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, -5), -19.97497, 0.58539 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 0), -21.53612, 0.58312 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 5), -23.69082, 0.57307 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 10), -26.42614, 0.55632 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 15), -29.61873, 0.53495 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 20), -33.07448, 0.51143 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 25), -36.5834, 0.48806 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 30), -39.96198, 0.46662 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.07038, 0.44822 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -45.80923, 0.4334 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 45), -48.1074, 0.42227 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 50), -49.90971, 0.41471 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 55), -51.16869, 0.41049 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 60), -51.84019, 0.40933 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 65), -51.88085, 0.41095 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 70), -51.2453, 0.41513 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 75), -49.8826, 0.42161 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 80), -47.73459, 0.43011 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 85), -44.74228, 0.4402 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 90), -40.86717, 0.45115 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 95), -36.12859, 0.46181 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 100), -30.64382, 0.47071 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 105), -24.64227, 0.47643 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 110), -18.42818, 0.47814 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 115), -12.30241, 0.47587 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 120), -6.49124, 0.47039 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 125), -1.12282, 0.4627 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 130), 3.75132, 0.45369 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 135), 8.11995, 0.444 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 140), 11.98556, 0.4341 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 145), 15.35353, 0.42431 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 150), 18.23135, 0.41489 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 155), 20.63141, 0.40606 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 160), 22.57411, 0.39796 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 165), 24.09027, 0.39071 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 170), 25.22207, 0.38435 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 175), 26.02131, 0.37887 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-45, 180), 26.54462, 0.37423 + 2); - EXPECT_NEAR(get_mag_declination_degrees(-40, -180), 22.71498, 0.35481 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -175), 23.05656, 0.35275 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -170), 23.2434, 0.35109 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -165), 23.30893, 0.34976 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -160), 23.27698, 0.34874 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -155), 23.16806, 0.34798 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -150), 23.00622, 0.34748 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -145), 22.82175, 0.34723 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -140), 22.64818, 0.34726 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -135), 22.51567, 0.34756 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -130), 22.44349, 0.34818 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -125), 22.43337, 0.34912 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -120), 22.46298, 0.35039 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -115), 22.47908, 0.35198 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -110), 22.39227, 0.35393 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -105), 22.07785, 0.35627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -100), 21.38739, 0.35912 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -95), 20.17082, 0.36269 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -90), 18.30434, 0.3672 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -85), 15.71746, 0.37286 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -80), 12.41508, 0.37982 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -75), 8.49176, 0.38806 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -70), 4.13354, 0.39748 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -65), -0.40026, 0.40798 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -60), -4.8181, 0.41951 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -55), -8.85151, 0.43223 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -50), -12.30488, 0.4464 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -45), -15.08253, 0.46235 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -40), -17.18677, 0.48031 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -35), -18.69251, 0.50039 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -30), -19.70965, 0.52247 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -25), -20.34909, 0.54618 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -20), -20.70875, 0.57066 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -15), -20.88866, 0.59426 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -10), -21.027, 0.61441 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.32712, 0.62787 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -22.03749, 0.63163 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 5), -23.37224, 0.6241 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 10), -25.41464, 0.60588 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 15), -28.07341, 0.57962 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 20), -31.12299, 0.54904 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 25), -34.29177, 0.51789 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 30), -37.338, 0.48905 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.08195, 0.46426 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 40), -42.40118, 0.44425 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 45), -44.21122, 0.42907 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 50), -45.44851, 0.41837 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 55), -46.06128, 0.41161 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 60), -46.00747, 0.40819 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 65), -45.25514, 0.40751 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 70), -43.7822, 0.40893 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 75), -41.57495, 0.41183 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 80), -38.63071, 0.41553 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 85), -34.9723, 0.41926 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 90), -30.67638, 0.42217 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 95), -25.90127, 0.42342 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 100), -20.88545, 0.42252 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 105), -15.89795, 0.4195 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 110), -11.16119, 0.41495 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 115), -6.79574, 0.40963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 120), -2.81834, 0.4042 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 125), 0.8176, 0.39901 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 130), 4.17186, 0.39412 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 135), 7.28334, 0.38945 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 140), 10.16003, 0.3849 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 145), 12.78645, 0.3804 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 150), 15.13661, 0.37596 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 155), 17.18448, 0.37163 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 160), 18.91075, 0.36751 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 165), 20.30824, 0.3637 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 170), 21.38671, 0.36028 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 175), 22.17459, 0.35731 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-40, 180), 22.71498, 0.35481 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -180), 19.64538, 0.34006 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -175), 19.98261, 0.33914 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -170), 20.18647, 0.3385 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -165), 20.28919, 0.33809 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -160), 20.30753, 0.33788 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -155), 20.24982, 0.33785 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -150), 20.12602, 0.33799 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -145), 19.95412, 0.33833 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -140), 19.76004, 0.33888 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -135), 19.57302, 0.3397 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -130), 19.41997, 0.34081 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -125), 19.31963, 0.34227 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -120), 19.27425, 0.34407 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -115), 19.25657, 0.34622 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -110), 19.19503, 0.3487 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -105), 18.96555, 0.35154 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -100), 18.39975, 0.35483 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -95), 17.31199, 0.35877 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -90), 15.53826, 0.36361 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -85), 12.9755, 0.36958 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -80), 9.61364, 0.37677 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -75), 5.55621, 0.38514 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 1.02259, 0.39449 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.67843, 0.40463 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.20681, 0.41555 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -55), -12.26467, 0.42748 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -50), -15.65545, 0.44083 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -45), -18.30773, 0.45604 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -40), -20.26137, 0.47345 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -35), -21.626, 0.49325 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -30), -22.52674, 0.51555 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -25), -23.05659, 0.54035 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -20), -23.25769, 0.56733 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -15), -23.1461, 0.59543 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.77538, 0.62226 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.30688, 0.644 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -22.03029, 0.65605 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 5), -22.28628, 0.65479 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 10), -23.31435, 0.63933 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 15), -25.13254, 0.61197 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 20), -27.54053, 0.57725 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 25), -30.22695, 0.54023 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -32.88702, 0.50511 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 35), -35.28383, 0.47452 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 40), -37.25263, 0.44959 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 45), -38.68058, 0.43039 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 50), -39.48818, 0.41638 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 55), -39.62157, 0.40678 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 60), -39.05242, 0.40067 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 65), -37.77878, 0.39717 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 70), -35.82155, 0.39544 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 75), -33.21773, 0.39469 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 80), -30.01945, 0.3942 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 85), -26.30861, 0.3933 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 90), -22.22338, 0.39142 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 95), -17.97126, 0.38824 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 100), -13.79976, 0.38386 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 105), -9.92657, 0.37877 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 110), -6.47146, 0.37361 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 115), -3.43443, 0.36893 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 120), -0.72708, 0.36502 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 125), 1.76916, 0.36187 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 130), 4.15009, 0.35931 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 135), 6.46081, 0.35708 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 140), 8.69677, 0.35499 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 145), 10.8228, 0.3529 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 150), 12.79397, 0.35079 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 155), 14.56738, 0.34866 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 160), 16.10614, 0.34658 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 165), 17.38287, 0.3446 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 170), 18.38657, 0.34282 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 175), 19.12919, 0.34129 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-35, 180), 19.64538, 0.34006 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -180), 17.14231, 0.32874 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -175), 17.44506, 0.32853 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -170), 17.6342, 0.32854 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -165), 17.74583, 0.32875 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -160), 17.79426, 0.32912 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -155), 17.77838, 0.32963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -150), 17.69433, 0.33026 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -145), 17.54541, 0.33103 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -140), 17.3451, 0.33198 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -135), 17.11535, 0.33313 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -130), 16.88393, 0.33455 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -125), 16.6811, 0.33627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -120), 16.53065, 0.3383 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -115), 16.43141, 0.34064 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -110), 16.33258, 0.34326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -105), 16.11595, 0.34617 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -100), 15.60014, 0.34948 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -95), 14.57248, 0.35339 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -90), 12.83878, 0.35814 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -85), 10.2745, 0.36394 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -80), 6.86541, 0.37084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -75), 2.732, 0.3787 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -70), -1.87174, 0.38724 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -65), -6.59707, 0.39627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -60), -11.07574, 0.40584 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -55), -15.00505, 0.41629 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -50), -18.2065, 0.42813 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -45), -20.63988, 0.44182 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -40), -22.37764, 0.45766 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -35), -23.5529, 0.47575 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -30), -24.29561, 0.49615 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -25), -24.67749, 0.51894 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.69123, 0.54411 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.28099, 0.57118 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.42018, 0.59856 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.20679, 0.62295 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 0), -20.92076, 0.6397 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 5), -19.97782, 0.64424 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 10), -19.7678, 0.63426 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 15), -20.47422, 0.61084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 20), -22.00821, 0.57793 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 25), -24.08579, 0.54074 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 30), -26.36162, 0.50405 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 35), -28.52597, 0.4712 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 40), -30.33929, 0.44384 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 45), -31.62993, 0.42231 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 50), -32.28459, 0.40613 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 55), -32.24366, 0.39439 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 60), -31.49925, 0.38611 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 65), -30.08808, 0.38032 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 70), -28.0752, 0.37616 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 75), -25.53452, 0.37294 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 80), -22.54219, 0.37007 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 85), -19.19449, 0.36705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 90), -15.63897, 0.36353 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 95), -12.08452, 0.35939 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 100), -8.7614, 0.35482 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 105), -5.84482, 0.35025 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 110), -3.39284, 0.34613 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 115), -1.33882, 0.34278 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 120), 0.46321, 0.34029 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 125), 2.17064, 0.33855 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 130), 3.8945, 0.33733 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 135), 5.67317, 0.33639 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 140), 7.48191, 0.33553 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 145), 9.26404, 0.33464 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 150), 10.96024, 0.33369 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 155), 12.52055, 0.33269 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 160), 13.9023, 0.33169 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 165), 15.06804, 0.33072 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 170), 15.99249, 0.32987 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 175), 16.6745, 0.32919 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-30, 180), 17.14231, 0.32874 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -180), 15.10099, 0.32015 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -175), 15.34739, 0.32037 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -170), 15.49484, 0.32081 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -165), 15.5876, 0.32144 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -160), 15.64233, 0.32221 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -155), 15.65272, 0.32311 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -150), 15.60376, 0.32411 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -145), 15.4847, 0.32522 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -140), 15.29507, 0.32645 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -135), 15.04606, 0.32785 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -130), 14.76158, 0.32944 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -125), 14.47799, 0.33125 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -120), 14.23512, 0.3333 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -115), 14.05284, 0.33557 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -110), 13.89739, 0.33804 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -105), 13.65554, 0.34074 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -100), 13.13673, 0.34377 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -95), 12.11054, 0.34736 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -90), 10.36648, 0.35173 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -85), 7.7742, 0.35702 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -80), 4.32959, 0.36321 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -75), 0.17819, 0.37007 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.39456, 0.37729 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.01679, 0.3847 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.31721, 0.39246 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -55), -17.01027, 0.40102 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -50), -19.94378, 0.41093 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -45), -22.10053, 0.42262 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -40), -23.56568, 0.43626 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -35), -24.47345, 0.45176 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -30), -24.94405, 0.46892 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -25), -25.02975, 0.48757 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -20), -24.69665, 0.50763 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -15), -23.85957, 0.52881 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -10), -22.46246, 0.55014 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, -5), -20.57325, 0.56946 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 0), -18.44488, 0.58344 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 5), -16.49012, 0.58854 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 10), -15.15272, 0.58255 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 15), -14.73778, 0.56558 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 20), -15.30734, 0.54004 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 25), -16.69135, 0.50969 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 30), -18.57671, 0.47839 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 35), -20.60705, 0.44926 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 40), -22.45459, 0.42416 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 45), -23.86027, 0.40376 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 50), -24.65253, 0.38789 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 55), -24.75257, 0.3759 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 60), -24.16653, 0.36695 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 65), -22.96238, 0.36026 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 70), -21.23414, 0.35513 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 75), -19.06893, 0.35102 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 80), -16.53896, 0.3475 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 85), -13.72827, 0.3442 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 90), -10.77405, 0.34084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 95), -7.87926, 0.33732 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 100), -5.26702, 0.33374 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 105), -3.09663, 0.33038 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 110), -1.39813, 0.32755 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 115), -0.06765, 0.32542 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 120), 1.07919, 0.32402 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 125), 2.22855, 0.32321 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 130), 3.5015, 0.32281 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 135), 4.92571, 0.32259 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 140), 6.45282, 0.32239 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 145), 8.00292, 0.32213 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 150), 9.50373, 0.3218 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 155), 10.90297, 0.32142 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 160), 12.15868, 0.32102 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 165), 13.22931, 0.32063 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 170), 14.07918, 0.32031 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 175), 14.6956, 0.32013 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-25, 180), 15.10099, 0.32015 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -180), 13.46013, 0.31389 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -175), 13.63597, 0.31439 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -170), 13.7204, 0.31509 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -165), 13.76899, 0.31598 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -160), 13.80502, 0.31702 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -155), 13.8214, 0.31818 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -150), 13.79558, 0.31943 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -145), 13.70507, 0.32078 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -140), 13.53638, 0.32221 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -135), 13.28954, 0.32375 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -130), 12.9826, 0.32541 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -125), 12.65405, 0.32722 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -120), 12.3536, 0.32915 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -115), 12.11403, 0.33121 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -110), 11.91056, 0.33337 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -105), 11.63021, 0.3357 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -100), 11.07525, 0.33834 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -95), 10.00726, 0.34148 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -90), 8.21456, 0.34533 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -85), 5.57772, 0.34994 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -80), 2.11583, 0.3552 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -75), -1.99632, 0.36079 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -70), -6.4509, 0.36641 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -10.87318, 0.37198 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -14.91072, 0.37774 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -55), -18.30672, 0.3842 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -50), -20.93123, 0.39191 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -45), -22.77287, 0.40124 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -40), -23.90676, 0.41223 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -35), -24.44922, 0.4246 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -30), -24.50509, 0.43785 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -25), -24.12248, 0.4515 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -20), -23.27948, 0.46516 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -15), -21.91822, 0.47851 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -20.01509, 0.49099 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.65266, 0.50152 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -15.0555, 0.50852 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.56569, 0.51027 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 10), -10.562, 0.50559 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 15), -9.35679, 0.49432 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 20), -9.11305, 0.47752 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 25), -9.8068, 0.4571 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 30), -11.23806, 0.43529 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 35), -13.08234, 0.41412 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 40), -14.96786, 0.39506 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 45), -16.55704, 0.37889 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 50), -17.60911, 0.36575 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 55), -18.0091, 0.3554 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 60), -17.7596, 0.34735 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 65), -16.94117, 0.3411 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 70), -15.65736, 0.33621 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 75), -13.99055, 0.33231 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 80), -11.99557, 0.32911 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 85), -9.73608, 0.32634 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 90), -7.33538, 0.32374 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 95), -4.99271, 0.32122 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 100), -2.93428, 0.31879 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 105), -1.32121, 0.31663 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 110), -0.17571, 0.3149 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 115), 0.62684, 0.31373 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 120), 1.29953, 0.3131 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 125), 2.05333, 0.31292 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 130), 3.01838, 0.31302 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 135), 4.21162, 0.31323 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 140), 5.56097, 0.31343 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 145), 6.96329, 0.31355 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 150), 8.33402, 0.31359 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 155), 9.62029, 0.31357 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 160), 10.78354, 0.31352 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 165), 11.78096, 0.31347 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 170), 12.5685, 0.31348 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 175), 13.12269, 0.3136 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-20, 180), 13.46013, 0.31389 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -180), 12.17599, 0.30974 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -175), 12.27639, 0.3104 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -170), 12.28483, 0.31126 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -165), 12.27048, 0.31231 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -160), 12.26681, 0.3135 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -155), 12.26987, 0.31481 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -150), 12.25289, 0.3162 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -145), 12.18407, 0.31766 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -140), 12.03873, 0.31918 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -135), 11.80717, 0.32075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -130), 11.50199, 0.32239 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -125), 11.16183, 0.32408 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -120), 10.84056, 0.32582 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -115), 10.57493, 0.32759 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -110), 10.3398, 0.3294 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -105), 10.01723, 0.33135 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -100), 9.40473, 0.33359 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -95), 8.26591, 0.33631 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -90), 6.40248, 0.33963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -85), 3.7199, 0.34354 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -80), 0.26869, 0.34781 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -75), -3.74946, 0.35209 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -70), -8.01796, 0.35611 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -65), -12.17767, 0.35987 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -60), -15.90732, 0.36366 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -55), -18.97833, 0.36801 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -50), -21.27131, 0.37342 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -45), -22.76478, 0.38018 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -40), -23.51073, 0.38826 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -35), -23.60322, 0.39725 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -30), -23.14119, 0.4065 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -25), -22.1951, 0.41532 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -20), -20.7958, 0.42314 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -15), -18.95724, 0.42963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.72086, 0.43461 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -14.19221, 0.43786 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 0), -11.54845, 0.43906 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 5), -9.01874, 0.43778 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 10), -6.85423, 0.43363 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 15), -5.29643, 0.42643 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 20), -4.53688, 0.41643 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 25), -4.66317, 0.40436 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 30), -5.60962, 0.3912 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 35), -7.14648, 0.37795 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 40), -8.92808, 0.36546 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 45), -10.5868, 0.35431 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 50), -11.83135, 0.34479 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 55), -12.50701, 0.33691 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 60), -12.5978, 0.33053 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 65), -12.17881, 0.32539 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 70), -11.34721, 0.3213 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 75), -10.17029, 0.31805 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 80), -8.68066, 0.31546 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 85), -6.91974, 0.31335 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 90), -4.99534, 0.31154 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 95), -3.10156, 0.3099 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 100), -1.46926, 0.30844 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 105), -0.26817, 0.30721 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 110), 0.47639, 0.30633 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 115), 0.89856, 0.30585 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 120), 1.2319, 0.30576 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 125), 1.70805, 0.306 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 130), 2.46575, 0.30643 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 135), 3.51331, 0.30693 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 140), 4.75817, 0.3074 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 145), 6.0765, 0.30778 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 150), 7.37238, 0.30808 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 155), 8.59207, 0.30832 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 160), 9.70011, 0.30852 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 165), 10.65281, 0.30871 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 170), 11.39833, 0.30894 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 175), 11.90259, 0.30926 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-15, 180), 12.17599, 0.30974 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -180), 11.20567, 0.3075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -175), 11.23754, 0.30829 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -170), 11.16848, 0.30925 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -165), 11.08295, 0.31038 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -160), 11.02776, 0.31164 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -155), 11.00547, 0.313 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -150), 10.98758, 0.31442 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -145), 10.93441, 0.31587 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -140), 10.81127, 0.31735 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -135), 10.60036, 0.31883 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -130), 10.31033, 0.32033 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -125), 9.97937, 0.32182 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -120), 9.66119, 0.32329 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -115), 9.38888, 0.32475 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -110), 9.12938, 0.32623 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -105), 8.75691, 0.32785 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -100), 8.06834, 0.32977 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -95), 6.83937, 0.33215 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -90), 4.89696, 0.33503 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -85), 2.18049, 0.3383 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -80), -1.22659, 0.34166 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -75), -5.10404, 0.34476 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -70), -9.14121, 0.34737 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -65), -13.00662, 0.34954 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -60), -16.41182, 0.35159 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -55), -19.14806, 0.35401 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -50), -21.09527, 0.35723 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -45), -22.21543, 0.36149 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -40), -22.53974, 0.36669 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -35), -22.15109, 0.37245 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -30), -21.16041, 0.37812 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -25), -19.68194, 0.38298 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -20), -17.81862, 0.38653 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -15), -15.66487, 0.38854 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.31844, 0.38909 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.88577, 0.38844 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.47702, 0.38683 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 5), -6.20377, 0.38436 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 10), -4.19116, 0.38093 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 15), -2.59472, 0.3764 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 20), -1.59263, 0.37069 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 25), -1.33319, 0.36397 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 30), -1.85554, 0.35658 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 35), -3.03393, 0.34893 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 40), -4.58925, 0.3414 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 45), -6.17163, 0.33433 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 50), -7.47337, 0.32796 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 55), -8.31345, 0.32241 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 60), -8.65525, 0.31769 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 65), -8.55975, 0.31376 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 70), -8.10988, 0.31055 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 75), -7.35395, 0.30798 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 80), -6.30187, 0.30597 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 85), -4.97271, 0.30441 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 90), -3.4563, 0.30319 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 95), -1.93639, 0.30221 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 100), -0.6432, 0.30144 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 105), 0.2462, 0.30089 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 110), 0.69886, 0.30061 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 115), 0.84987, 0.30063 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 120), 0.9441, 0.30092 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 125), 1.22759, 0.30144 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 130), 1.84622, 0.30211 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 135), 2.80318, 0.30282 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 140), 3.99157, 0.30351 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 145), 5.272, 0.30412 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 150), 6.53882, 0.30467 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 155), 7.73607, 0.30514 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 160), 8.82909, 0.30557 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 165), 9.77182, 0.30597 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 170), 10.50416, 0.30639 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 175), 10.98106, 0.30688 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-10, 180), 11.20567, 0.3075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -180), 10.49679, 0.30705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -175), 10.48121, 0.30796 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -170), 10.34735, 0.309 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -165), 10.19599, 0.31017 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -160), 10.09016, 0.31144 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -155), 10.04196, 0.31276 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -150), 10.02306, 0.31411 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -145), 9.98618, 0.31544 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -140), 9.88643, 0.31675 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -135), 9.69833, 0.31802 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -130), 9.42747, 0.31926 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -125), 9.11138, 0.32046 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -120), 8.80078, 0.32162 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -115), 8.52035, 0.32275 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -110), 8.22555, 0.32394 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -105), 7.78355, 0.3253 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -100), 6.99666, 0.327 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -95), 5.66155, 0.32912 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -90), 3.63864, 0.33166 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -85), 0.90466, 0.33441 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -80), -2.42826, 0.33703 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -75), -6.13236, 0.33917 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -70), -9.91456, 0.34065 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -65), -13.4753, 0.34155 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -60), -16.55367, 0.34218 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -55), -18.95179, 0.34298 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -50), -20.54476, 0.34432 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -45), -21.28361, 0.34638 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -40), -21.19304, 0.34909 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -35), -20.36206, 0.35212 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -30), -18.92628, 0.35495 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -25), -17.04549, 0.35704 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -20), -14.88314, 0.35799 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -15), -12.59118, 0.35771 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -10), -10.29709, 0.35639 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, -5), -8.0905, 0.35439 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -6.01742, 0.35205 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 5), -4.0956, 0.34956 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 10), -2.35372, 0.34694 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 15), -0.87493, 0.34405 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 20), 0.19106, 0.34077 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 25), 0.67293, 0.33707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 30), 0.4663, 0.333 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 35), -0.38599, 0.3287 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 40), -1.67664, 0.3243 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 45), -3.0973, 0.31996 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 50), -4.3505, 0.31584 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 55), -5.24676, 0.31206 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 60), -5.73581, 0.30869 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 65), -5.86528, 0.30577 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 70), -5.70407, 0.30331 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 75), -5.28365, 0.30131 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 80), -4.59442, 0.29974 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 85), -3.63484, 0.29858 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 90), -2.47486, 0.29775 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 95), -1.28164, 0.2972 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 100), -0.27637, 0.29688 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 105), 0.36293, 0.2968 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 110), 0.59604, 0.29693 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 115), 0.5527, 0.29729 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 120), 0.47934, 0.29785 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 125), 0.6281, 0.29858 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 130), 1.1494, 0.29942 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 135), 2.04548, 0.30033 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 140), 3.20333, 0.30123 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 145), 4.47513, 0.30209 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 150), 5.7482, 0.30289 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 155), 6.96259, 0.30363 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 160), 8.08122, 0.30431 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 165), 9.0533, 0.30495 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 170), 9.80943, 0.30559 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 175), 10.29255, 0.30627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(-5, 180), 10.49679, 0.30705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -180), 9.98214, 0.30823 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -175), 9.95611, 0.30928 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -170), 9.78659, 0.31041 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -165), 9.59055, 0.31161 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -160), 9.44936, 0.31283 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -155), 9.38761, 0.31406 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -150), 9.37881, 0.31526 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -145), 9.36802, 0.31638 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -140), 9.29891, 0.31742 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -135), 9.13717, 0.31839 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -130), 8.88467, 0.31928 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -125), 8.57734, 0.32012 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -120), 8.26118, 0.32092 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -115), 7.95091, 0.32173 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -110), 7.5913, 0.32266 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -105), 7.04685, 0.32382 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -100), 6.13226, 0.32536 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -95), 4.67237, 0.32731 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -90), 2.56538, 0.32957 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -85), -0.17557, 0.33191 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -80), -3.41643, 0.33394 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -75), -6.93141, 0.33536 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -70), -10.4508, 0.336 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -65), -13.70519, 0.33596 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -60), -16.45501, 0.33552 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -55), -18.51003, 0.33505 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -50), -19.74591, 0.33489 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -45), -20.11667, 0.3352 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -40), -19.65772, 0.33595 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -35), -18.47632, 0.33692 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -30), -16.73199, 0.33776 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -25), -14.61293, 0.33811 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -20), -12.31327, 0.33771 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -15), -10.01085, 0.33653 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -10), -7.84231, 0.33473 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, -5), -5.88003, 0.3326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 0), -4.12581, 0.3304 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 5), -2.53552, 0.3283 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 10), -1.07238, 0.32634 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 15), 0.23535, 0.32444 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 20), 1.2696, 0.32249 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 25), 1.85824, 0.32039 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 30), 1.86254, 0.31813 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 35), 1.26802, 0.31571 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 40), 0.22114, 0.31314 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 45), -1.01573, 0.31049 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 50), -2.168, 0.30783 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 55), -3.04992, 0.30527 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 60), -3.60518, 0.30287 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 65), -3.87279, 0.30071 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 70), -3.91245, 0.29882 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 75), -3.74518, 0.29724 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 80), -3.34832, 0.29599 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 85), -2.70392, 0.29508 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 90), -1.86191, 0.29449 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 95), -0.96782, 0.2942 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 100), -0.22512, 0.29416 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 105), 0.1958, 0.29436 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 110), 0.25118, 0.29476 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 115), 0.06173, 0.29535 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 120), -0.13366, 0.29609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 125), -0.08571, 0.29697 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 130), 0.35724, 0.29797 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 135), 1.20078, 0.29905 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 140), 2.33464, 0.30018 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 145), 3.61086, 0.30131 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 150), 4.91322, 0.30241 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 155), 6.17654, 0.30346 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 160), 7.35813, 0.30446 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 165), 8.40018, 0.3054 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 170), 9.22292, 0.30632 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 175), 9.75608, 0.30725 + 1); - EXPECT_NEAR(get_mag_declination_degrees(0, 180), 9.98214, 0.30823 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -180), 9.57862, 0.31085 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -175), 9.59512, 0.3121 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -170), 9.43695, 0.31334 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -165), 9.23472, 0.31455 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -160), 9.08863, 0.31572 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -155), 9.03792, 0.31681 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -150), 9.06002, 0.31778 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -145), 9.09287, 0.31863 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -140), 9.06774, 0.31935 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -135), 8.93965, 0.31995 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -130), 8.70448, 0.32046 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -125), 8.39437, 0.32092 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -120), 8.04929, 0.32135 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -115), 7.6749, 0.32186 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -110), 7.20919, 0.32255 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -105), 6.52037, 0.32355 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -100), 5.44232, 0.32495 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -95), 3.8329, 0.32674 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -90), 1.62939, 0.32878 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -85), -1.12159, 0.33076 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -80), -4.26986, 0.33234 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -75), -7.59614, 0.33322 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -70), -10.85374, 0.33326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -65), -13.7992, 0.33255 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -60), -16.21209, 0.33132 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -55), -17.915, 0.32992 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -50), -18.79705, 0.32864 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -45), -18.83122, 0.32765 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -40), -18.0754, 0.32699 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -35), -16.65642, 0.32654 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -30), -14.74532, 0.32609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.53416, 0.32544 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.21809, 0.32442 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.97636, 0.323 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -10), -5.94517, 0.32127 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, -5), -4.18852, 0.3194 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 0), -2.68719, 0.31757 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 5), -1.3636, 0.3159 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 10), -0.14023, 0.31443 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 15), 0.99489, 0.31312 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 20), 1.95436, 0.31188 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 25), 2.57754, 0.31066 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 30), 2.71191, 0.30938 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 35), 2.30642, 0.308 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 40), 1.46031, 0.3065 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 45), 0.39384, 0.30485 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 50), -0.64441, 0.30311 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 55), -1.47761, 0.30133 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 60), -2.04777, 0.2996 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 65), -2.38911, 0.29797 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 70), -2.55777, 0.29649 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 75), -2.57216, 0.29521 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 80), -2.40499, 0.29417 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 85), -2.02811, 0.2934 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 90), -1.47348, 0.29294 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 95), -0.86213, 0.29277 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 100), -0.37274, 0.29287 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 105), -0.15965, 0.29323 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 110), -0.26473, 0.29378 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 115), -0.57744, 0.2945 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 120), -0.87333, 0.29536 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 125), -0.91378, 0.29635 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 130), -0.54906, 0.29747 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 135), 0.23333, 0.29873 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 140), 1.33407, 0.30009 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 145), 2.61293, 0.30151 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 150), 3.95417, 0.30295 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 155), 5.28704, 0.30437 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 160), 6.56099, 0.30575 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 165), 7.70973, 0.30707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 170), 8.64296, 0.30835 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 175), 9.2766, 0.3096 + 1); - EXPECT_NEAR(get_mag_declination_degrees(5, 180), 9.57862, 0.31085 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -180), 9.19161, 0.3146 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -175), 9.31654, 0.31611 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -170), 9.23386, 0.3175 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -165), 9.08181, 0.31876 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -160), 8.97651, 0.31985 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -155), 8.97225, 0.32077 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -150), 9.0525, 0.3215 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -145), 9.15039, 0.32205 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -140), 9.18574, 0.32243 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -135), 9.10174, 0.32267 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -130), 8.88542, 0.32282 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -125), 8.56192, 0.32291 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -120), 8.16364, 0.32304 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -115), 7.68898, 0.32328 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -110), 7.07428, 0.32378 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -105), 6.19795, 0.32463 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -100), 4.91846, 0.3259 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -95), 3.12922, 0.32753 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -90), 0.80592, 0.32932 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -85), -1.9742, 0.33096 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -80), -5.04716, 0.33213 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -75), -8.19892, 0.33257 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -70), -11.201, 0.33215 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -65), -13.83244, 0.33096 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -60), -15.89553, 0.32919 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -55), -17.23766, 0.32713 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -50), -17.77713, 0.32507 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -45), -17.51805, 0.3232 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -40), -16.54384, 0.32159 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -35), -14.99298, 0.32022 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -30), -13.03137, 0.31899 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -25), -10.83402, 0.31779 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -20), -8.57702, 0.31651 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -15), -6.42706, 0.31511 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -10), -4.51662, 0.3136 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, -5), -2.91034, 0.31206 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 0), -1.58646, 0.31059 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 5), -0.45583, 0.30927 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 10), 0.58086, 0.30813 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 15), 1.56414, 0.30718 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 20), 2.43487, 0.30636 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 25), 3.05087, 0.30563 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 30), 3.25948, 0.30491 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 35), 2.9883, 0.30414 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 40), 2.3009, 0.30326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 45), 1.38092, 0.30222 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 50), 0.45142, 0.30104 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 55), -0.32112, 0.29976 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 60), -0.8782, 0.29845 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 65), -1.25089, 0.29717 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 70), -1.49547, 0.29595 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 75), -1.63447, 0.29486 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 80), -1.64436, 0.29394 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 85), -1.49403, 0.29324 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 90), -1.20127, 0.29281 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 95), -0.86203, 0.29268 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 100), -0.62568, 0.29283 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 105), -0.6238, 0.29324 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 110), -0.89088, 0.29385 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 115), -1.32539, 0.29463 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 120), -1.72105, 0.29554 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 125), -1.85507, 0.29659 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 130), -1.58204, 0.29781 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 135), -0.88011, 0.29921 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 140), 0.16812, 0.30079 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 145), 1.43639, 0.3025 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 150), 2.81317, 0.30428 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 155), 4.22273, 0.3061 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 160), 5.60582, 0.3079 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 165), 6.88771, 0.30966 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 170), 7.96904, 0.31136 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 175), 8.75307, 0.31301 + 1); - EXPECT_NEAR(get_mag_declination_degrees(10, 180), 9.19161, 0.3146 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -180), 8.72698, 0.31909 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -175), 9.0317, 0.32091 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -170), 9.10111, 0.3225 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -165), 9.07121, 0.32382 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -160), 9.0664, 0.32486 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -155), 9.15292, 0.3256 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -150), 9.32185, 0.32609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -145), 9.50565, 0.32635 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -140), 9.61665, 0.32643 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -135), 9.58749, 0.32638 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -130), 9.39433, 0.32625 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -125), 9.05182, 0.3261 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -120), 8.58273, 0.32601 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -115), 7.97955, 0.32609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -110), 7.18127, 0.32647 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -105), 6.0815, 0.32723 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -100), 4.56643, 0.32838 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -95), 2.56553, 0.32982 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -90), 0.09128, 0.33132 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -85), -2.74986, 0.33259 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -80), -5.7787, 0.33333 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -75), -8.78099, 0.33331 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -70), -11.54016, 0.33249 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -65), -13.85656, 0.3309 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -60), -15.56285, 0.32875 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -55), -16.54511, 0.32629 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -50), -16.76383, 0.32375 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -45), -16.25977, 0.32133 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -40), -15.13762, 0.31914 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -35), -13.53613, 0.31722 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -30), -11.60221, 0.31554 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -25), -9.48149, 0.31406 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -20), -7.32148, 0.31269 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -15), -5.27021, 0.31137 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -10), -3.45504, 0.31008 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, -5), -1.94644, 0.30883 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 0), -0.73183, 0.30766 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 5), 0.27404, 0.30661 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 10), 1.1776, 0.30573 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 15), 2.03802, 0.30502 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 20), 2.82144, 0.30446 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 25), 3.40846, 0.30403 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 30), 3.65599, 0.30366 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 35), 3.48171, 0.30327 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 40), 2.92258, 0.30278 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 45), 2.13002, 0.30212 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 50), 1.302, 0.30127 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 55), 0.59373, 0.30029 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 60), 0.06387, 0.29922 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 65), -0.31528, 0.29813 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 70), -0.60128, 0.29707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 75), -0.82472, 0.29607 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 80), -0.97224, 0.29519 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 85), -1.01645, 0.2945 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 90), -0.96564, 0.29405 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 95), -0.89207, 0.29388 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 100), -0.91392, 0.294 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 105), -1.1357, 0.29439 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 110), -1.57971, 0.29499 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 115), -2.15068, 0.29575 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 120), -2.6607, 0.29665 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 125), -2.90477, 0.29771 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 130), -2.74313, 0.29898 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 135), -2.14395, 0.30048 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 140), -1.17047, 0.30222 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 145), 0.0683, 0.30417 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 150), 1.46761, 0.30626 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 155), 2.94785, 0.30844 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 160), 4.44157, 0.31066 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 165), 5.86722, 0.31286 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 170), 7.11998, 0.31502 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 175), 8.09432, 0.31711 + 1); - EXPECT_NEAR(get_mag_declination_degrees(15, 180), 8.72698, 0.31909 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -180), 8.11069, 0.32386 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -175), 8.66065, 0.326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -170), 8.96138, 0.32782 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -165), 9.13409, 0.32923 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -160), 9.29908, 0.33023 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -155), 9.52701, 0.33084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -150), 9.81604, 0.33112 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -145), 10.10308, 0.33117 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -140), 10.29977, 0.33105 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -135), 10.33268, 0.33084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -130), 10.16748, 0.33059 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -125), 9.80584, 0.33037 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -120), 9.25863, 0.33025 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -115), 8.51245, 0.33034 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -110), 7.51104, 0.33074 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -105), 6.16541, 0.3315 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -100), 4.39017, 0.33258 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -95), 2.1496, 0.33385 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -90), -0.50865, 0.33504 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -85), -3.44874, 0.33587 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -80), -6.47225, 0.33609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -75), -9.3582, 0.33555 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -70), -11.89694, 0.33426 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -65), -13.9113, 0.3323 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -60), -15.27215, 0.32986 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -55), -15.91426, 0.32714 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -50), -15.84636, 0.32435 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -45), -15.14455, 0.32166 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -40), -13.92731, 0.31919 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -35), -12.32437, 0.31699 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.45676, 0.3151 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -25), -8.43725, 0.3135 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -20), -6.38329, 0.31211 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -15), -4.4227, 0.3109 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -10), -2.67626, 0.3098 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, -5), -1.22114, 0.30878 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 0), -0.05948, 0.30786 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 5), 0.88023, 0.30705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 10), 1.70027, 0.30637 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 15), 2.46971, 0.30585 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 20), 3.17661, 0.3055 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 25), 3.727, 0.30529 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 30), 3.99484, 0.30516 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 35), 3.89717, 0.30504 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 40), 3.45156, 0.30481 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 45), 2.78005, 0.30438 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 50), 2.05431, 0.30373 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 55), 1.41571, 0.30289 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 60), 0.92169, 0.30192 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 65), 0.54846, 0.3009 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 70), 0.23823, 0.29987 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 75), -0.04823, 0.29888 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 80), -0.31077, 0.29799 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 85), -0.53046, 0.29725 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 90), -0.71046, 0.29673 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 95), -0.90203, 0.29649 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 100), -1.19239, 0.29653 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 105), -1.65673, 0.29684 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 110), -2.30135, 0.29736 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 115), -3.03326, 0.29805 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 120), -3.68027, 0.29888 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 125), -4.05437, 0.29988 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 130), -4.02203, 0.30112 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 135), -3.54263, 0.30263 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 140), -2.66108, 0.30444 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 145), -1.4692, 0.30652 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 150), -0.06431, 0.30882 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 155), 1.47116, 0.31127 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 160), 3.0629, 0.31381 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 165), 4.62546, 0.3164 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 170), 6.05373, 0.31898 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 175), 7.24, 0.32149 + 1); - EXPECT_NEAR(get_mag_declination_degrees(20, 180), 8.11069, 0.32386 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -180), 7.30991, 0.32852 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -175), 8.15263, 0.33094 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -170), 8.7526, 0.33295 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -165), 9.20419, 0.33447 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -160), 9.60878, 0.33548 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -155), 10.03023, 0.33602 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -150), 10.47022, 0.3362 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -145), 10.87412, 0.33616 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -140), 11.16058, 0.336 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -135), 11.25702, 0.33582 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -130), 11.12191, 0.33568 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -125), 10.74381, 0.33563 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -120), 10.12018, 0.33574 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -115), 9.23053, 0.33609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -110), 8.02281, 0.33673 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -105), 6.42461, 0.33768 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -100), 4.37702, 0.33884 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -95), 1.87667, 0.34001 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -90), -0.99492, 0.3409 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -85), -4.07099, 0.34123 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -80), -7.12945, 0.34082 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -75), -9.93756, 0.33964 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -70), -12.29068, 0.33776 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -65), -14.03655, 0.33536 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -60), -15.0887, 0.33262 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -55), -15.43299, 0.32973 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -50), -15.12404, 0.32685 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -45), -14.26681, 0.32408 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -40), -12.98643, 0.32154 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -35), -11.39955, 0.31927 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -30), -9.60191, 0.31733 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -25), -7.67716, 0.3157 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -20), -5.71631, 0.31436 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -15), -3.82788, 0.31326 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -10), -2.12415, 0.31233 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, -5), -0.68664, 0.31154 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 0), 0.4673, 0.31086 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 5), 1.39036, 0.31027 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 10), 2.17264, 0.3098 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 15), 2.88554, 0.30947 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 20), 3.53512, 0.3093 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 25), 4.05408, 0.30928 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 30), 4.33921, 0.30936 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 35), 4.3146, 0.30944 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 40), 3.9845, 0.30939 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 45), 3.44313, 0.30911 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 50), 2.83286, 0.30855 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 55), 2.27635, 0.30774 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 60), 1.82647, 0.30676 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 65), 1.46308, 0.30569 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 70), 1.13035, 0.3046 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 75), 0.7828, 0.30355 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 80), 0.40799, 0.30258 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 85), 0.01471, 0.30176 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 90), -0.39812, 0.30115 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 95), -0.8637, 0.3008 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 100), -1.43968, 0.30073 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 105), -2.17115, 0.30092 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 110), -3.04509, 0.30131 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 115), -3.9661, 0.30185 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 120), -4.7733, 0.30253 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 125), -5.29326, 0.30338 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 130), -5.39925, 0.30447 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 135), -5.04501, 0.30587 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 140), -4.26119, 0.30761 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 145), -3.12564, 0.30967 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 150), -1.7295, 0.31201 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 155), -0.15753, 0.31457 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 160), 1.51102, 0.31729 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 165), 3.19011, 0.32012 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 170), 4.78002, 0.32299 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 175), 6.17871, 0.32583 + 1); - EXPECT_NEAR(get_mag_declination_degrees(25, 180), 7.30991, 0.32852 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -180), 6.34636, 0.33287 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -175), 7.50305, 0.33544 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -170), 8.44472, 0.33759 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -165), 9.23109, 0.33921 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -160), 9.93209, 0.34029 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -155), 10.59298, 0.34088 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -150), 11.21283, 0.34113 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -145), 11.7462, 0.3412 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -140), 12.12431, 0.34123 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -135), 12.28206, 0.34133 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -130), 12.17577, 0.34157 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -125), 11.78334, 0.34201 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -120), 11.08901, 0.34267 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -115), 10.06406, 0.3436 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -110), 8.65832, 0.34481 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -105), 6.81262, 0.34624 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -100), 4.49073, 0.34771 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -95), 1.71849, 0.34894 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -90), -1.38964, 0.3496 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -85), -4.63392, 0.3494 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -80), -7.76519, 0.34826 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -75), -10.53694, 0.34625 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -70), -12.75048, 0.34362 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -65), -14.28067, 0.34062 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -60), -15.08339, 0.3375 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -55), -15.19045, 0.33442 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -50), -14.69334, 0.33147 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -45), -13.71666, 0.32873 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -40), -12.387, 0.32624 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -35), -10.80885, 0.32404 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -30), -9.05888, 0.32216 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.19995, 0.32061 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -20), -5.30317, 0.31937 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -15), -3.46016, 0.31841 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -10), -1.7727, 0.31767 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, -5), -0.32246, 0.31711 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 0), 0.86095, 0.31668 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 5), 1.81053, 0.31636 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 10), 2.59938, 0.31613 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 15), 3.2951, 0.31603 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 20), 3.91734, 0.31607 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 25), 4.42497, 0.31625 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 30), 4.74124, 0.31651 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 35), 4.80321, 0.31675 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 40), 4.60668, 0.31683 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 45), 4.21887, 0.31661 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 50), 3.74895, 0.31606 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 55), 3.29448, 0.3152 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 60), 2.89855, 0.31411 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 65), 2.54314, 0.31292 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 70), 2.17659, 0.31171 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 75), 1.75141, 0.31054 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 80), 1.24625, 0.30947 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 85), 0.66125, 0.30854 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 90), -0.00347, 0.30783 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 95), -0.76503, 0.30737 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 100), -1.65302, 0.30716 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 105), -2.68187, 0.30718 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 110), -3.81616, 0.30736 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 115), -4.95303, 0.30766 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 120), -5.93801, 0.30807 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 125), -6.60983, 0.30863 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 130), -6.84996, 0.30943 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 135), -6.61132, 0.31054 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 140), -5.91665, 0.31202 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 145), -4.83518, 0.31387 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 150), -3.45442, 0.31606 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 155), -1.8612, 0.31852 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 160), -0.13769, 0.32122 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 165), 1.63262, 0.32409 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 170), 3.36006, 0.32705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 175), 4.95502, 0.33002 + 1); - EXPECT_NEAR(get_mag_declination_degrees(30, 180), 6.34636, 0.33287 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -180), 5.29334, 0.33707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -175), 6.75696, 0.33963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -170), 8.04826, 0.34182 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -165), 9.19136, 0.34354 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -160), 10.21897, 0.34477 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -155), 11.14991, 0.34559 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -150), 11.974, 0.34614 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -145), 12.65193, 0.34659 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -140), 13.12812, 0.34712 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -135), 13.34806, 0.34783 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -130), 13.26991, 0.34881 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -125), 12.8644, 0.35009 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -120), 12.1045, 0.3517 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -115), 10.95296, 0.35363 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -110), 9.35911, 0.35582 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -105), 7.27296, 0.35813 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -100), 4.67667, 0.36026 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -95), 1.6225, 0.36181 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -90), -1.74236, 0.36235 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -85), -5.18328, 0.3616 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -80), -8.42232, 0.35957 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -75), -11.20008, 0.35652 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -70), -13.32657, 0.35288 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -65), -14.70474, 0.34905 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -60), -15.32868, 0.34534 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -55), -15.26645, 0.34191 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -50), -14.63451, 0.33883 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -45), -13.56774, 0.33609 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -40), -12.19083, 0.33369 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -35), -10.5998, 0.33161 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.86092, 0.32986 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -25), -7.02614, 0.32845 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -20), -5.15438, 0.32737 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.32364, 0.3266 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -10), -1.62382, 0.3261 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, -5), -0.13216, 0.32582 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 0), 1.11435, 0.3257 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 5), 2.13137, 0.32569 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 10), 2.97356, 0.32578 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 15), 3.70023, 0.32596 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 20), 4.33987, 0.32625 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 25), 4.87484, 0.32664 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 30), 5.25543, 0.32707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 35), 5.4348, 0.32744 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 40), 5.40365, 0.32758 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 45), 5.20287, 0.32739 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 50), 4.90512, 0.32679 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 55), 4.57677, 0.32582 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 60), 4.24531, 0.32461 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 65), 3.89203, 0.32327 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 70), 3.47073, 0.32192 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 75), 2.93628, 0.32064 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 80), 2.26372, 0.31948 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 85), 1.44858, 0.31848 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 90), 0.49383, 0.31768 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 95), -0.60162, 0.3171 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 100), -1.83951, 0.31672 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 105), -3.20226, 0.3165 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 110), -4.62856, 0.31637 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 115), -6.00326, 0.31627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 120), -7.17422, 0.31622 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 125), -7.99119, 0.31627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 130), -8.34684, 0.31655 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 135), -8.19962, 0.31716 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 140), -7.57183, 0.31816 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 145), -6.53008, 0.31956 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 150), -5.16102, 0.32136 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 155), -3.55317, 0.32349 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 160), -1.78946, 0.32592 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 165), 0.05155, 0.32858 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 170), 1.89265, 0.3314 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 175), 3.66016, 0.33427 + 1); - EXPECT_NEAR(get_mag_declination_degrees(35, 180), 5.29334, 0.33707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -180), 4.25264, 0.34177 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -175), 5.99295, 0.34419 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -170), 7.60786, 0.34635 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -165), 9.09162, 0.34819 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -160), 10.44271, 0.3497 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -155), 11.65244, 0.35097 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -150), 12.69716, 0.35211 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -145), 13.53817, 0.35331 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -140), 14.12852, 0.35471 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -135), 14.42206, 0.35645 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -130), 14.37906, 0.3586 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -125), 13.96495, 0.36121 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -120), 13.14327, 0.36427 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -115), 11.86867, 0.36775 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -110), 10.08826, 0.3715 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -105), 7.75824, 0.37524 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -100), 4.8764, 0.37851 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -95), 1.52056, 0.38071 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -90), -2.12769, 0.38127 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -85), -5.79637, 0.37989 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -80), -9.17837, 0.37672 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -75), -12.0042, 0.37226 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -70), -14.09614, 0.36721 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -65), -15.38528, 0.36216 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -60), -15.89815, 0.3575 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -55), -15.72883, 0.35343 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -50), -15.00787, 0.34996 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -45), -13.87285, 0.34706 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -40), -12.44479, 0.34464 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -35), -10.81525, 0.34264 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -30), -9.04789, 0.34103 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -25), -7.19246, 0.3398 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -20), -5.30267, 0.33893 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -15), -3.44648, 0.33841 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -10), -1.70153, 0.33821 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, -5), -0.13699, 0.33827 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 0), 1.20811, 0.33853 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 5), 2.33652, 0.33892 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 10), 3.28571, 0.33939 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 15), 4.10425, 0.33992 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 20), 4.82443, 0.3405 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 25), 5.4469, 0.34112 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 30), 5.94573, 0.3417 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 35), 6.28959, 0.34214 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 40), 6.46536, 0.34231 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 45), 6.48877, 0.3421 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 50), 6.39507, 0.34146 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 55), 6.21595, 0.34045 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 60), 5.9585, 0.33918 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 65), 5.59966, 0.3378 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 70), 5.098, 0.33643 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 75), 4.41346, 0.33516 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 80), 3.52242, 0.33403 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 85), 2.42101, 0.33306 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 90), 1.11911, 0.33224 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 95), -0.36565, 0.33155 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 100), -2.00468, 0.33095 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 105), -3.74512, 0.33035 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 110), -5.4954, 0.32968 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 115), -7.12393, 0.3289 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 120), -8.47872, 0.32807 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 125), -9.42188, 0.32729 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 130), -9.86223, 0.32672 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 135), -9.77107, 0.3265 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 140), -9.1777, 0.32672 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 145), -8.15147, 0.32742 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 150), -6.78028, 0.32856 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 155), -5.15345, 0.33012 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 160), -3.35268, 0.33203 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 165), -1.45034, 0.33425 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 170), 0.48866, 0.33668 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 175), 2.40604, 0.33922 + 1); - EXPECT_NEAR(get_mag_declination_degrees(40, 180), 4.25264, 0.34177 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -180), 3.31743, 0.34827 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -175), 5.29105, 0.35048 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -170), 7.18051, 0.35263 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -165), 8.95989, 0.35469 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -160), 10.60283, 0.35666 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -155), 12.07853, 0.35864 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -150), 13.34959, 0.36073 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -145), 14.3728, 0.36309 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -140), 15.10252, 0.36587 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -135), 15.49435, 0.36919 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -130), 15.50644, 0.37314 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -125), 15.09707, 0.37777 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -120), 14.21972, 0.38306 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -115), 12.82001, 0.38892 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -110), 10.84106, 0.39509 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -105), 8.24346, 0.40109 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -100), 5.04049, 0.40619 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -95), 1.33868, 0.40947 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -90), -2.64011, 0.41014 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -85), -6.58126, 0.40787 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -80), -10.14755, 0.40303 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -75), -13.06306, 0.39649 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -70), -15.16665, 0.38932 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -65), -16.41814, 0.38235 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -60), -16.87192, 0.37613 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -55), -16.64048, 0.37087 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -50), -15.8611, 0.36657 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -45), -14.66969, 0.36314 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -40), -13.18313, 0.36043 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -35), -11.4914, 0.35834 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -30), -9.66045, 0.35677 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -25), -7.74323, 0.35568 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -20), -5.79283, 0.35502 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -15), -3.87019, 0.35479 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -10), -2.04133, 0.35493 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, -5), -0.36524, 0.35538 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 0), 1.12141, 0.35607 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 5), 2.41344, 0.35691 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 10), 3.53422, 0.35781 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 15), 4.52062, 0.35873 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 20), 5.40363, 0.35962 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 25), 6.19467, 0.36046 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 30), 6.88436, 0.36117 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 35), 7.45242, 0.36167 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 40), 7.8813, 0.36188 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 45), 8.16392, 0.36171 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 50), 8.30092, 0.36117 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 55), 8.28995, 0.36032 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 60), 8.11527, 0.35928 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 65), 7.74528, 0.35819 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 70), 7.13991, 0.35717 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 75), 6.26354, 0.35627 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 80), 5.09641, 0.3555 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 85), 3.63997, 0.35483 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 90), 1.91666, 0.35417 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 95), -0.03111, 0.35343 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 100), -2.13807, 0.35249 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 105), -4.30946, 0.35123 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 110), -6.41786, 0.34959 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 115), -8.31209, 0.34761 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 120), -9.84094, 0.34543 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 125), -10.88322, 0.34327 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 130), -11.37072, 0.34135 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 135), -11.2951, 0.33986 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 140), -10.69908, 0.33892 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 145), -9.65872, 0.33855 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 150), -8.26416, 0.33875 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 155), -6.60441, 0.33946 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 160), -4.75841, 0.34062 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 165), -2.79238, 0.34217 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 170), -0.76123, 0.34402 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 175), 1.28865, 0.34608 + 1); - EXPECT_NEAR(get_mag_declination_degrees(45, 180), 3.31743, 0.34827 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -180), 2.53747, 0.35868 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -175), 4.69971, 0.36073 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -170), 6.80822, 0.363 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -165), 8.82691, 0.36546 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -160), 10.71582, 0.36817 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -155), 12.43068, 0.3712 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -150), 13.92354, 0.37469 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -145), 15.14414, 0.37877 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -140), 16.04168, 0.3836 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -135), 16.56567, 0.38932 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -130), 16.66503, 0.39603 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -125), 16.28519, 0.40381 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -120), 15.36478, 0.41262 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -115), 13.83577, 0.42227 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -110), 11.63303, 0.43233 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -105), 8.719, 0.44199 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -100), 5.12456, 0.45003 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -95), 0.99358, 0.455 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -90), -3.39841, 0.45572 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -85), -7.68216, 0.45188 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -80), -11.48607, 0.44424 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -75), -14.53163, 0.43429 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -70), -16.68054, 0.42366 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -65), -17.92469, 0.41356 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -60), -18.34549, 0.40469 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -55), -18.07074, 0.39731 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -50), -17.24164, 0.39139 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -45), -15.99137, 0.38678 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -40), -14.43327, 0.38327 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -35), -12.65717, 0.38068 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -30), -10.73272, 0.37887 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -25), -8.7174, 0.37772 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -20), -6.66523, 0.37717 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -15), -4.63169, 0.37714 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -10), -2.67235, 0.37758 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, -5), -0.83552, 0.3784 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 0), 0.8471, 0.37949 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 5), 2.36653, 0.38075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 10), 3.73529, 0.38206 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 15), 4.97845, 0.38335 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 20), 6.12072, 0.38455 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 25), 7.17544, 0.38561 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 30), 8.14006, 0.38648 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 35), 8.9985, 0.38714 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 40), 9.72723, 0.38754 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 45), 10.30018, 0.3877 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 50), 10.68977, 0.38764 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 55), 10.86434, 0.38746 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 60), 10.78543, 0.38729 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 65), 10.40843, 0.38723 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 70), 9.6882, 0.38735 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 75), 8.58825, 0.38765 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 80), 7.09079, 0.38802 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 85), 5.20477, 0.3883 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 90), 2.97136, 0.38822 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 95), 0.46721, 0.38755 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 100), -2.19403, 0.38604 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 105), -4.86444, 0.38356 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 110), -7.37295, 0.38014 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 115), -9.54698, 0.37601 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 120), -11.23992, 0.3715 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 125), -12.35452, 0.36703 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 130), -12.85358, 0.36295 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 135), -12.75598, 0.3595 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 140), -12.12272, 0.35682 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 145), -11.03896, 0.35493 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 150), -9.59742, 0.3538 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 155), -7.88585, 0.35336 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 160), -5.97979, 0.35353 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 165), -3.94034, 0.35422 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 170), -1.81576, 0.35536 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 175), 0.35485, 0.35687 + 1); - EXPECT_NEAR(get_mag_declination_degrees(50, 180), 2.53747, 0.35868 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.90149, 0.37644 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.21643, 0.37851 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.49718, 0.38112 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -165), 8.70496, 0.38431 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -160), 10.79547, 0.38817 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -155), 12.71918, 0.39279 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -150), 14.42236, 0.39833 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -145), 15.84817, 0.40496 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -140), 16.93728, 0.41288 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -135), 17.62728, 0.42229 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -130), 17.8508, 0.43336 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -125), 17.53262, 0.44619 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -120), 16.58781, 0.46075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -115), 14.925, 0.47674 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -110), 12.46103, 0.49339 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -105), 9.15389, 0.50926 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -100), 5.05453, 0.5222 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -95), 0.35968, 0.52976 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -90), -4.57575, 0.53011 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -85), -9.30418, 0.52305 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -80), -13.41007, 0.51021 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -75), -16.61755, 0.49423 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -70), -18.82236, 0.47763 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -65), -20.05741, 0.46216 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -60), -20.43569, 0.44873 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -55), -20.10214, 0.43763 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -50), -19.20317, 0.42875 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.87065, 0.42185 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.21548, 0.41663 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.32731, 0.41281 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.27816, 0.41017 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -25), -10.12812, 0.4085 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -20), -7.93079, 0.40767 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -15), -5.73633, 0.40756 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.59093, 0.40805 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, -5), -1.53313, 0.40901 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 0), 0.41094, 0.41032 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 5), 2.23068, 0.41184 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 10), 3.93008, 0.41344 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 15), 5.52232, 0.41502 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 20), 7.02166, 0.41651 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 25), 8.43537, 0.41787 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 30), 9.75852, 0.4191 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 35), 10.97263, 0.42022 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 40), 12.04718, 0.42129 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 45), 12.94214, 0.42239 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 50), 13.60997, 0.42363 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 55), 13.99662, 0.42515 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 60), 14.04266, 0.42707 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 65), 13.6859, 0.42947 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 70), 12.86682, 0.43234 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 75), 11.53753, 0.43553 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 80), 9.67366, 0.43871 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 85), 7.28832, 0.44139 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 90), 4.44552, 0.44293 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 95), 1.26863, 0.44271 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 100), -2.0618, 0.44028 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 105), -5.32608, 0.43554 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 110), -8.29787, 0.42881 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 115), -10.78176, 0.42075 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 120), -12.64234, 0.41217 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 125), -13.81583, 0.4038 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 130), -14.30439, 0.39619 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 135), -14.16001, 0.38965 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 140), -13.46525, 0.38432 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 145), -12.31587, 0.38018 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 150), -10.80741, 0.37716 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 155), -9.02618, 0.37513 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 160), -7.04462, 0.374 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 165), -4.92032, 0.37364 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 170), -2.69807, 0.37397 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 175), -0.41369, 0.37492 + 1); - EXPECT_NEAR(get_mag_declination_degrees(55, 180), 1.90149, 0.37644 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -180), 1.33743, 0.40753 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -175), 3.78298, 0.40988 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -170), 6.20569, 0.41325 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -165), 8.56811, 0.41771 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -160), 10.82706, 0.42339 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -155), 12.93326, 0.43047 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -150), 14.83147, 0.43915 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -145), 16.46038, 0.44971 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -140), 17.75192, 0.46246 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -135), 18.62934, 0.47773 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -130), 19.00433, 0.49584 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -125), 18.7737, 0.51705 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -120), 17.81822, 0.54137 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -115), 16.00846, 0.56832 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.22638, 0.59655 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -105), 9.41246, 0.62339 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -100), 4.63955, 0.64478 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -95), -0.81631, 0.65618 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -90), -6.47502, 0.65461 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -85), -11.77029, 0.64045 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.23041, 0.61735 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -75), -19.59616, 0.59014 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -70), -21.81968, 0.56291 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -65), -22.99236, 0.53812 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.2689, 0.51687 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.81669, 0.49936 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.79051, 0.48531 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.32291, 0.4743 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.52294, 0.46585 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.47873, 0.45952 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.26165, 0.45497 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.93079, 0.45188 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.53664, 0.45 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -15), -7.12329, 0.44915 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.72866, 0.44911 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, -5), -2.38338, 0.44973 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 0), -0.10913, 0.45084 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 5), 2.0821, 0.45229 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 10), 4.18682, 0.45395 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.20667, 0.45573 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 20), 8.1434, 0.45757 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 25), 9.99363, 0.45949 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 30), 11.74465, 0.46152 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 35), 13.372, 0.46379 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 40), 14.83885, 0.46642 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 45), 16.09653, 0.46963 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 50), 17.08552, 0.47361 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 55), 17.7364, 0.47859 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 60), 17.9712, 0.48475 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 65), 17.70588, 0.49218 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 70), 16.8556, 0.50081 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 75), 15.34491, 0.51026 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 80), 13.12489, 0.51978 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 85), 10.19809, 0.52819 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 90), 6.64694, 0.53393 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 95), 2.65349, 0.53548 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 100), -1.50767, 0.53185 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 105), -5.51361, 0.52305 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 110), -9.06133, 0.51016 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 115), -11.92791, 0.49489 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 120), -13.99654, 0.47899 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 125), -15.24773, 0.46381 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 130), -15.73177, 0.45021 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 135), -15.5391, 0.43857 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 140), -14.77684, 0.42897 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 145), -13.55299, 0.42132 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 150), -11.96696, 0.41542 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 155), -10.10456, 0.41109 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 160), -8.0363, 0.40814 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 165), -5.81808, 0.4064 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 170), -3.49362, 0.40577 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 175), -1.09818, 0.40616 + 1); - EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.33743, 0.40753 + 1); + EXPECT_NEAR(get_mag_declination_degrees(-50, -180), 31.5, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -175), 31.7, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -170), 31.7, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -165), 31.6, 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, -155), 31.1, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-50, -120), 30.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -115), 29.7, 0.36 + 1.0); + 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, -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, -75), 14.1, 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, -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, -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.8, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -30), -12.2, 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, -20), -14.4, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, -15), -15.6, 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, 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.8, 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.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 25), -37.8, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 30), -41.4, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 35), -44.8, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 40), -48.0, 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.1, 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, 60), -56.5, 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.8, 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, 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, 105), -37.4, 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, 120), -13.4, 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, 130), 2.4, 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, 140), 14.5, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 145), 19.0, 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, 155), 25.5, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 160), 27.7, 0.44 + 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, 175), 31.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-50, 180), 31.5, 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(-45, -170), 27.0, 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, -155), 26.7, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-45, -125), 26.0, 0.36 + 1.0); + 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, -100), 24.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -95), 23.0, 0.36 + 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, -80), 15.1, 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, -65), 3.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -60), -1.1, 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, -50), -8.3, 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.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -30), -16.1, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, -25), -16.9, 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.9, 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, 5), -23.7, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 10), -26.4, 0.56 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 15), -29.6, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 20), -33.1, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 25), -36.6, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 30), -40.0, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 35), -43.1, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 40), -45.9, 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, 55), -51.2, 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, 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, 115), -12.3, 0.48 + 1.8); + EXPECT_NEAR(get_mag_declination_degrees(-45, 120), -6.5, 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, 130), 3.8, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 135), 8.1, 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, 150), 18.3, 0.41 + 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.6, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-45, 165), 24.1, 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(-40, -180), 22.7, 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, -170), 23.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -165), 23.3, 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, -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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-40, -125), 22.4, 0.35 + 1.0); + 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, -95), 20.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -90), 18.3, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -85), 15.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -80), 12.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -75), 8.5, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -70), 4.1, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -65), -0.4, 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, -50), -12.3, 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.7, 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.7, 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), -21.0, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, -5), -21.3, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 0), -22.0, 0.63 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 5), -23.4, 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.3, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 30), -37.4, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 35), -40.1, 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, 50), -45.5, 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, 60), -46.0, 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.6, 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, 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, 95), -25.9, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 100), -20.9, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 105), -15.9, 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, 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, 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, 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, 160), 18.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 165), 20.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-40, 170), 21.4, 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, 180), 22.7, 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, -160), 20.3, 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.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -145), 20.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -140), 19.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -135), 19.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -130), 19.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -125), 19.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -120), 19.3, 0.34 + 1.0); + 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), 19.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -100), 18.4, 0.35 + 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, -85), 12.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -80), 9.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -75), 5.5, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -70), 1.0, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -65), -3.7, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -60), -8.3, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -55), -12.3, 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, -45), -18.3, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -40), -20.3, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -35), -21.6, 0.49 + 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.1, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -10), -22.7, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, -5), -22.2, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 0), -22.0, 0.66 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 5), -22.3, 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, 25), -30.3, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 30), -32.9, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 35), -35.3, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 40), -37.3, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 45), -38.7, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 50), -39.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 55), -39.6, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 60), -39.1, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 65), -37.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 70), -35.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 75), -33.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 80), -30.0, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 85), -26.3, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 90), -22.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 95), -17.9, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 100), -13.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 105), -9.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 110), -6.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 115), -3.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 120), -0.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-35, 125), 1.8, 0.36 + 1.0); + 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, 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.1, 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, 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, -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.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -140), 17.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -135), 17.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -130), 16.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -125), 16.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -120), 16.5, 0.34 + 1.0); + 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, -95), 14.6, 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.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -70), -1.9, 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.1, 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, -50), -18.2, 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, -25), -24.7, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -20), -24.7, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -15), -24.2, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -10), -23.4, 0.60 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, -5), -22.1, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 0), -20.9, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 5), -19.9, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 10), -19.7, 0.63 + 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.0, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 25), -24.1, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 30), -26.4, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 35), -28.6, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 40), -30.4, 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, 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, 70), -28.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 75), -25.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 80), -22.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 85), -19.2, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 90), -15.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-30, 95), -12.1, 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, 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.5, 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, 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, 180), 17.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -180), 15.1, 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.5, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-25, -145), 15.5, 0.33 + 1.0); + 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.8, 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, -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, -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, -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.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -75), 0.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -70), -4.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -65), -9.1, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -60), -13.4, 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, -45), -22.1, 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, -20), -24.7, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -15), -23.8, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -10), -22.4, 0.55 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, -5), -20.5, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 0), -18.4, 0.58 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 5), -16.4, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 10), -15.1, 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, 25), -16.7, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 30), -18.6, 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, 40), -22.5, 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, 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, 65), -22.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 70), -21.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 75), -19.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 80), -16.5, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 85), -13.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 90), -10.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 95), -7.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 100), -5.3, 0.33 + 1.0); + 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(-25, 140), 6.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 145), 8.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 150), 9.5, 0.32 + 1.0); + 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.2, 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, 175), 14.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-25, 180), 15.1, 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.7, 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, -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, -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, -105), 11.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -100), 11.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -95), 10.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -90), 8.2, 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.1, 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.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -65), -10.9, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -60), -15.0, 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, -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, -25), -24.1, 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.9, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -10), -19.9, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, -5), -17.6, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 0), -15.0, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 5), -12.5, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 10), -10.5, 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); + EXPECT_NEAR(get_mag_declination_degrees(-20, 25), -9.8, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 30), -11.3, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 35), -13.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 40), -15.0, 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, 60), -17.8, 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), -14.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 80), -12.0, 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, 95), -5.0, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(-20, 140), 5.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 145), 7.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 150), 8.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 155), 9.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 160), 10.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 165), 11.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 170), 12.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-20, 175), 13.1, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-15, -150), 12.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -145), 12.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -140), 12.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -135), 11.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -130), 11.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -125), 11.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -120), 10.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -115), 10.6, 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, -100), 9.4, 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.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -85), 3.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -80), 0.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -75), -3.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -70), -8.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -65), -12.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -60), -16.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -55), -19.0, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -50), -21.3, 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.2, 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.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -10), -16.7, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, -5), -14.1, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 0), -11.5, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 5), -9.0, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 10), -6.8, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 15), -5.3, 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, 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, 50), -11.8, 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); + EXPECT_NEAR(get_mag_declination_degrees(-15, 65), -12.2, 0.33 + 1.0); + 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.7, 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, 90), -5.0, 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, 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, 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, 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, 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, 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, 175), 11.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-15, 180), 12.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -180), 11.2, 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, -160), 11.0, 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); + EXPECT_NEAR(get_mag_declination_degrees(-10, -145), 10.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -140), 10.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -135), 10.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -130), 10.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -125), 10.0, 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.4, 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, -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, -90), 4.9, 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, -80), -1.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -75), -5.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -70), -9.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -65), -13.1, 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, -55), -19.2, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -50), -21.1, 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.8, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -15), -15.6, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -10), -13.3, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, -5), -10.8, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 0), -8.4, 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.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 20), -1.6, 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.0, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(-10, 60), -8.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 65), -8.6, 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, 85), -5.0, 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); + EXPECT_NEAR(get_mag_declination_degrees(-10, 100), -0.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 105), 0.2, 0.30 + 1.0); + 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, 130), 1.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 135), 2.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 140), 4.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 145), 5.3, 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, 165), 9.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-10, 170), 10.5, 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, 180), 11.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -180), 10.5, 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, -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, -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); + EXPECT_NEAR(get_mag_declination_degrees(-5, -145), 10.0, 0.32 + 1.0); + 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, -120), 8.8, 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, -105), 7.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -100), 7.0, 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, -90), 3.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -85), 0.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -80), -2.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -75), -6.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -70), -10.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -65), -13.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -60), -16.6, 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, -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.2, 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, -30), -18.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -25), -17.0, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -20), -14.8, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -15), -12.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -10), -10.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, -5), -8.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 0), -6.0, 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, 10), -2.3, 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, 20), 0.2, 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); + EXPECT_NEAR(get_mag_declination_degrees(-5, 35), -0.4, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 40), -1.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 45), -3.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 50), -4.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 55), -5.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 60), -5.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(-5, 65), -5.9, 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.3, 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, 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, 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.4, 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.5, 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, 145), 4.5, 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.8, 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, 180), 10.5, 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, -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, -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, -125), 8.6, 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, -110), 7.6, 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, -100), 6.1, 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, -85), -0.2, 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, -75), -7.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -70), -10.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -65), -13.7, 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, -55), -18.5, 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, -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.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -25), -14.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -20), -12.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -15), -10.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -10), -7.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, -5), -5.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 0), -4.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 5), -2.5, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(0, 40), 0.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 45), -1.0, 0.31 + 1.0); + 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(0, 85), -2.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 90), -1.9, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 95), -1.0, 0.29 + 1.0); + 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, 120), -0.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 125), -0.1, 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, 135), 1.2, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(0, 160), 7.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 165), 8.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(0, 170), 9.2, 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(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); + EXPECT_NEAR(get_mag_declination_degrees(5, -155), 9.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -150), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -145), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -140), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -135), 8.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -130), 8.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -125), 8.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -120), 8.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -115), 7.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -110), 7.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -105), 6.5, 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, -95), 3.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -90), 1.6, 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, -80), -4.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -75), -7.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -70), -10.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -65), -13.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -60), -16.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -55), -17.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -50), -18.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -45), -18.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -40), -18.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -35), -16.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -30), -14.7, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -25), -12.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -20), -10.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -15), -7.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, -10), -5.9, 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, 10), -0.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 15), 1.0, 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, 30), 2.7, 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); + EXPECT_NEAR(get_mag_declination_degrees(5, 45), 0.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 50), -0.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(5, 55), -1.5, 0.30 + 1.0); + 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(5, 95), -0.9, 0.29 + 1.0); + 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, 125), -0.9, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(5, 160), 6.6, 0.31 + 1.0); + 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(10, -175), 9.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -170), 9.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -165), 9.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -160), 9.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -155), 9.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -150), 9.1, 0.32 + 1.0); + 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, -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.7, 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, -105), 6.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -100), 4.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -95), 3.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -90), 0.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -85), -2.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -80), -5.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -75), -8.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -70), -11.2, 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, -55), -17.2, 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), -15.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -30), -13.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -25), -10.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -20), -8.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -15), -6.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -10), -4.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, -5), -2.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 0), -1.6, 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, 10), 0.6, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(10, 35), 3.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 40), 2.3, 0.30 + 1.0); + 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(10, 80), -1.6, 0.29 + 1.0); + 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, 105), -0.6, 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, 115), -1.3, 0.29 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 120), -1.7, 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, 130), -1.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 135), -0.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 140), 0.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 145), 1.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 150), 2.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 155), 4.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(10, 160), 5.6, 0.31 + 1.0); + 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(15, -180), 8.7, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -175), 9.0, 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, -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, -125), 9.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -120), 8.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -115), 8.0, 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, -90), 0.1, 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, -80), -5.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -75), -8.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -70), -11.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -65), -13.9, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -60), -15.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -55), -16.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -50), -16.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -45), -16.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -40), -15.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -35), -13.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -30), -11.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -25), -9.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -20), -7.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -15), -5.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -10), -3.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, -5), -1.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 0), -0.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 5), 0.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 10), 1.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 15), 2.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 20), 2.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 25), 3.4, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(15, 60), 0.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 65), -0.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 70), -0.6, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 75), -0.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 80), -1.0, 0.30 + 1.0); + 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, 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, 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, 125), -2.9, 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, 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, 145), 0.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(15, 150), 1.5, 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.32 + 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.7, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(20, -145), 10.1, 0.33 + 1.0); + 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.2, 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, -120), 9.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -115), 8.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -110), 7.5, 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, -100), 4.4, 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, -90), -0.5, 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, -80), -6.5, 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, -70), -11.9, 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, -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, -40), -13.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -35), -12.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -30), -10.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, -25), -8.4, 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.4, 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, -5), -1.2, 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, 5), 0.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 10), 1.7, 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, 25), 3.7, 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, 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, 60), 0.9, 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.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 75), -0.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 80), -0.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 85), -0.5, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 90), -0.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 95), -0.9, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 100), -1.2, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 105), -1.7, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 110), -2.3, 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.7, 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, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(20, 175), 7.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(20, 180), 8.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -180), 7.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -175), 8.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -170), 8.8, 0.33 + 1.0); + 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, -140), 11.2, 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, -110), 8.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -105), 6.4, 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.0, 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, -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.4, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -30), -9.6, 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.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -15), -3.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, -10), -2.1, 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, 0), 0.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 5), 1.4, 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, 15), 2.9, 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); + EXPECT_NEAR(get_mag_declination_degrees(25, 30), 4.4, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 35), 4.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 40), 4.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 45), 3.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 50), 2.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 55), 2.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 60), 1.8, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 65), 1.5, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 70), 1.1, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 75), 0.8, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 80), 0.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 85), 0.0, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 90), -0.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 95), -0.9, 0.30 + 1.0); + 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, 125), -5.3, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 130), -5.4, 0.30 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 135), -5.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 140), -4.3, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 145), -3.1, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 150), -1.7, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 155), -0.2, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 160), 1.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 165), 3.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 170), 4.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 175), 6.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(25, 180), 7.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -180), 6.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -175), 7.5, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -170), 8.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -165), 9.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -160), 9.9, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -155), 10.6, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -150), 11.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -145), 11.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -140), 12.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -135), 12.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -130), 12.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -125), 11.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -120), 11.1, 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.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -105), 6.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -100), 4.5, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -95), 1.7, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -90), -1.4, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -85), -4.7, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -80), -7.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -75), -10.5, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -70), -12.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -65), -14.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -60), -15.1, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -55), -15.2, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -50), -14.7, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -45), -13.7, 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.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -30), -9.0, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -25), -7.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -20), -5.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, -15), -3.4, 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, -5), -0.3, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 0), 0.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 5), 1.8, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 10), 2.6, 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, 20), 3.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 25), 4.4, 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, 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, 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.2, 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, 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, 110), -3.8, 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, 120), -6.0, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 125), -6.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 130), -6.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 135), -6.6, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 140), -5.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 145), -4.9, 0.31 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 150), -3.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 155), -1.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 160), -0.1, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 165), 1.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(30, 170), 3.4, 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, -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, -155), 11.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -150), 12.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -145), 12.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -140), 13.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -135), 13.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -130), 13.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -125), 12.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -120), 12.1, 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, -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, -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, -80), -8.4, 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, -50), -14.6, 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.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -30), -8.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -25), -7.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -20), -5.1, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -15), -3.3, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -10), -1.6, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, -5), -0.1, 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, 10), 3.0, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 15), 3.7, 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, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(35, 70), 3.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 75), 2.9, 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, 100), -1.9, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 105), -3.2, 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.0, 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, 125), -8.0, 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.2, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 140), -7.6, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 145), -6.5, 0.32 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(35, 150), -5.2, 0.32 + 1.0); + 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, 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(40, -180), 4.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -175), 6.0, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -170), 7.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -165), 9.1, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -160), 10.4, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -155), 11.6, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -150), 12.7, 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, -130), 14.4, 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, -115), 11.8, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -110), 10.1, 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.9, 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, -90), -2.1, 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, -65), -15.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -60), -15.9, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -55), -15.7, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -50), -15.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -45), -13.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -40), -12.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -35), -10.8, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -30), -9.0, 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.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -15), -3.4, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -10), -1.7, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, -5), -0.1, 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), 3.3, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 15), 4.1, 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, 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, 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, 55), 6.2, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(40, 85), 2.4, 0.33 + 1.0); + 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.0, 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, 110), -5.5, 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.5, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 125), -9.4, 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, 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); + EXPECT_NEAR(get_mag_declination_degrees(40, 150), -6.8, 0.33 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(40, 155), -5.2, 0.33 + 1.0); + 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.5, 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, 180), 4.2, 0.34 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -180), 3.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -175), 5.3, 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, -160), 10.6, 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, -145), 14.3, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -140), 15.1, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -135), 15.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -130), 15.5, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -125), 15.1, 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, -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); + EXPECT_NEAR(get_mag_declination_degrees(45, -85), -6.6, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -80), -10.1, 0.40 + 1.0); + 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.4, 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, -55), -16.6, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -50), -15.8, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -45), -14.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -40), -13.1, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -35), -11.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -30), -9.6, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -25), -7.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -20), -5.7, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -15), -3.8, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -10), -2.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, -5), -0.3, 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, 20), 5.4, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 25), 6.2, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 30), 6.9, 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, 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, 60), 8.1, 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.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 90), 1.9, 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, 105), -4.3, 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, 115), -8.3, 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, 125), -10.9, 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, 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, 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, 175), 1.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(45, 180), 3.3, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -180), 2.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -175), 4.7, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -170), 6.8, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -165), 8.8, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -160), 10.7, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -155), 12.4, 0.37 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -150), 13.9, 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, -125), 16.3, 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, -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.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -85), -7.7, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -80), -11.5, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -75), -14.5, 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.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -60), -18.3, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -55), -18.0, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -50), -17.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -45), -15.9, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -40), -14.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -35), -12.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -30), -10.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -25), -8.7, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -20), -6.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -15), -4.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -10), -2.6, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, -5), -0.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 0), 0.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 5), 2.4, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 10), 3.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 15), 5.0, 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, 25), 7.2, 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, 35), 9.0, 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.3, 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, 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, 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, 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, 100), -2.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 105), -4.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 110), -7.4, 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, 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.1, 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, 165), -4.0, 0.35 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(50, 170), -1.8, 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, 180), 2.5, 0.36 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -180), 1.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -175), 4.2, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -170), 6.4, 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.7, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -150), 14.4, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -145), 15.8, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -140), 16.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -135), 17.6, 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, -125), 17.5, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -120), 16.6, 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, -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.6, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -85), -9.3, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -80), -13.4, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -75), -16.6, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -70), -18.8, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -65), -20.0, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -60), -20.4, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -55), -20.0, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -50), -19.1, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -45), -17.8, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -40), -16.1, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -35), -14.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -30), -12.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -25), -10.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -20), -7.9, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -15), -5.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -10), -3.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, -5), -1.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 0), 0.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 5), 2.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 10), 4.0, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 15), 5.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 20), 7.1, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 25), 8.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 30), 9.8, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 35), 11.0, 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, 50), 13.6, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 55), 14.0, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 60), 14.1, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 65), 13.7, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 70), 12.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 75), 11.5, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 80), 9.7, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 85), 7.3, 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, 100), -2.1, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 105), -5.4, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 110), -8.3, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 115), -10.8, 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, 125), -13.8, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 130), -14.3, 0.40 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 135), -14.2, 0.39 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 140), -13.5, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 145), -12.3, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 150), -10.8, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(55, 155), -9.0, 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.7, 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.9, 0.38 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -180), 1.3, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -175), 3.7, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -170), 6.1, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -165), 8.5, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -160), 10.8, 0.42 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -155), 12.9, 0.43 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -150), 14.8, 0.44 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -145), 16.4, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -140), 17.7, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -135), 18.6, 0.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -130), 19.0, 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, -120), 17.8, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -115), 16.0, 0.57 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -110), 13.2, 0.60 + 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.6, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -95), -0.8, 0.66 + 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.7, 0.64 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -80), -16.2, 0.62 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -75), -19.5, 0.59 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -70), -21.7, 0.56 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -65), -22.9, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -60), -23.2, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -55), -22.7, 0.50 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -50), -21.7, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -45), -20.2, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -40), -18.4, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -35), -16.4, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -30), -14.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -25), -11.9, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -20), -9.5, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -15), -7.1, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, -10), -4.7, 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, 0), -0.0, 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, 10), 4.2, 0.45 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 15), 6.3, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 20), 8.2, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 25), 10.0, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 30), 11.8, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 35), 13.4, 0.46 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 40), 14.9, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 45), 16.1, 0.47 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 50), 17.1, 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.48 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 65), 17.7, 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); + EXPECT_NEAR(get_mag_declination_degrees(60, 80), 13.1, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 85), 10.2, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 90), 6.6, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 95), 2.6, 0.54 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 100), -1.6, 0.53 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 105), -5.6, 0.52 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 110), -9.1, 0.51 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 115), -12.0, 0.49 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 120), -14.0, 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, 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, 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, 155), -10.1, 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.5, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 175), -1.2, 0.41 + 1.0); + EXPECT_NEAR(get_mag_declination_degrees(60, 180), 1.3, 0.41 + 1.0); } TEST(GeoLookupTest, inclination) { - EXPECT_NEAR(get_mag_inclination_degrees(-50, -180), -71.58401, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -175), -70.59455, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -170), -69.62019, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -165), -68.66205, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -160), -67.7198, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -155), -66.79138, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -150), -65.87195, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -145), -64.95256, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -140), -64.01931, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -135), -63.05368, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -130), -62.03405, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -125), -60.9385, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -120), -59.74839, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -115), -58.4524, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -110), -57.05104, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -105), -55.56121, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -100), -54.02015, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -95), -52.4877, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -90), -51.04547, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -85), -49.79158, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -80), -48.83025, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -75), -48.25641, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -70), -48.13796, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -65), -48.4997, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -60), -49.31498, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -55), -50.50837, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -50), -51.96942, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -45), -53.57229, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -40), -55.19487, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -35), -56.73229, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -30), -58.10283, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -25), -59.24766, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -20), -60.12742, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -15), -60.71924, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -10), -61.01671, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, -5), -61.03317, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 0), -60.8064, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 5), -60.40159, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 10), -59.90942, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 15), -59.43749, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 20), -59.09544, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 25), -58.97782, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 30), -59.15001, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 35), -59.64167, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 40), -60.44829, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 45), -61.53839, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 50), -62.86303, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 55), -64.36512, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 60), -65.98713, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 65), -67.67666, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 70), -69.38941, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 75), -71.08956, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 80), -72.74763, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 85), -74.33668, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 90), -75.82805, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 95), -77.18814, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 100), -78.37761, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 105), -79.35416, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 110), -80.07912, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 115), -80.52685, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 120), -80.69297, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 125), -80.59617, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 130), -80.27176, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 135), -79.76126, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 140), -79.10403, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 145), -78.3335, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 150), -77.47696, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 155), -76.55686, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 160), -75.59229, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 165), -74.59983, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 170), -73.59372, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 175), -72.58558, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-50, 180), -71.58401, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -180), -68.19244, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -175), -67.20464, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -170), -66.22442, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -165), -65.25331, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -160), -64.2922, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -155), -63.34191, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -150), -62.40227, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -145), -61.47011, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -140), -60.53727, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -135), -59.58959, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -130), -58.60724, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -125), -57.56626, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -120), -56.4411, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -115), -55.20869, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -110), -53.85454, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -105), -52.38098, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -100), -50.8162, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -95), -49.22159, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -90), -47.69421, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -85), -46.36165, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -80), -45.36751, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -75), -44.84792, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -70), -44.90232, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -65), -45.56704, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -60), -46.80192, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -55), -48.49755, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -50), -50.50087, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -45), -52.64828, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -40), -54.79352, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -35), -56.82278, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -30), -58.65608, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -25), -60.23891, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -20), -61.53041, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -15), -62.49492, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -10), -63.10173, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, -5), -63.33356, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 0), -63.19989, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 5), -62.7491, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 10), -62.07378, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 15), -61.30526, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 20), -60.59578, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 25), -60.09128, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 30), -59.90414, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 35), -60.09625, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 40), -60.67671, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 45), -61.61099, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 50), -62.83526, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 55), -64.27102, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 60), -65.83749, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 65), -67.46063, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 70), -69.0783, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 75), -70.64114, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 80), -72.10957, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 85), -73.44875, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 90), -74.62433, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 95), -75.60213, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 100), -76.35324, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 105), -76.86222, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 110), -77.13335, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 115), -77.18955, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 120), -77.06399, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 125), -76.78947, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 130), -76.39146, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 135), -75.88671, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 140), -75.2858, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 145), -74.59661, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 150), -73.82711, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 155), -72.98695, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 160), -72.08813, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 165), -71.14475, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 170), -70.17196, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 175), -69.18404, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-45, 180), -68.19244, 0.21 + 2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -180), -64.39611, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -175), -63.39272, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -170), -62.39141, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -165), -61.39218, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -160), -60.39466, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -155), -59.40048, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -150), -58.41316, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -145), -57.43589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -140), -56.46851, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -135), -55.50519, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -130), -54.53301, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -125), -53.53113, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -120), -52.4708, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -115), -51.31766, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -110), -50.03878, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -105), -48.61542, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -100), -47.05949, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -95), -45.42862, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -90), -43.8338, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -85), -42.43478, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -80), -41.42028, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -75), -40.97348, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -70), -41.22804, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -65), -42.22798, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -60), -43.90989, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -55), -46.11894, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -50), -48.65233, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -45), -51.30923, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -40), -53.92718, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -35), -56.3963, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -30), -58.6533, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -25), -60.66236, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -20), -62.39255, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -15), -63.80193, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -10), -64.83535, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, -5), -65.43742, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 0), -65.5749, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 5), -65.2589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 10), -64.55896, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 15), -63.60503, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 20), -62.57433, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 25), -61.66129, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 30), -61.03733, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 35), -60.81697, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 40), -61.04333, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 45), -61.69385, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 50), -62.69787, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 55), -63.95768, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 60), -65.36791, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 65), -66.8305, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 70), -68.26367, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 75), -69.60421, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 80), -70.8039, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 85), -71.82365, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 90), -72.63039, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 95), -73.20046, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 100), -73.52806, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 105), -73.63195, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 110), -73.55291, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 115), -73.34114, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 120), -73.04051, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 125), -72.67784, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 130), -72.26125, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 135), -71.78563, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 140), -71.24084, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 145), -70.6182, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 150), -69.91369, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 155), -69.12883, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 160), -68.27068, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 165), -67.3516, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 170), -66.38807, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 175), -65.39782, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-40, 180), -64.39611, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -180), -60.05607, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -175), -59.01756, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -170), -57.98021, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -165), -56.94105, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -160), -55.8958, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -155), -54.84387, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -150), -53.79006, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -145), -52.74258, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -140), -51.70951, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -135), -50.69537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -130), -49.69784, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -125), -48.70383, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -120), -47.68525, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -115), -46.59797, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -110), -45.38897, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -105), -44.01452, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -100), -42.46614, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -95), -40.79583, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -90), -39.1301, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -85), -37.66521, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -80), -36.64037, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -75), -36.28944, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -70), -36.77921, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -65), -38.15449, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.31785, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -55), -43.05936, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.12197, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.26874, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.32548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.19193, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -57.82743, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.22136, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -20), -62.36043, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -15), -64.2055, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -10), -65.68664, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, -5), -66.71826, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 0), -67.22767, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 5), -67.18498, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 10), -66.62317, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 15), -65.64546, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 20), -64.42115, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 25), -63.16548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 30), -62.09944, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 35), -61.40111, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 40), -61.17013, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 45), -61.41842, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 50), -62.08426, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 55), -63.05903, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 60), -64.21624, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 65), -65.4356, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 70), -66.61797, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 75), -67.68916, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 80), -68.59503, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 85), -69.29399, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 90), -69.75494, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 95), -69.96416, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 100), -69.93607, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 105), -69.71701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 110), -69.37463, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 115), -68.97674, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 120), -68.57054, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 125), -68.17227, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 130), -67.77033, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 135), -67.33776, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 140), -66.84624, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 145), -66.27508, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 150), -65.6134, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 155), -64.85849, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 160), -64.01449, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 165), -63.09254, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 170), -62.11082, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 175), -61.0917, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-35, 180), -60.05607, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -180), -55.01714, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -175), -53.92062, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -170), -52.83143, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -165), -51.74312, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -160), -50.6451, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -155), -49.53132, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -150), -48.40444, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -145), -47.27491, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -140), -46.15729, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -135), -45.06619, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -130), -44.01138, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -125), -42.99019, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -120), -41.97786, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -115), -40.92189, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -110), -39.74901, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -105), -38.38982, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -100), -36.81626, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -95), -35.07807, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -90), -33.32276, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -85), -31.78894, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -80), -30.76972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -75), -30.54853, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -70), -31.31919, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -65), -33.11826, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -60), -35.80637, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -55), -39.11487, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -42.73271, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -45), -46.38758, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -40), -49.89177, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.14974, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -30), -56.13732, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -25), -58.86468, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -20), -61.33658, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -15), -63.52392, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -10), -65.35624, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, -5), -66.73652, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 0), -67.5715, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 5), -67.80466, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 10), -67.43861, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 15), -66.54321, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 20), -65.25628, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 25), -63.77793, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 30), -62.3465, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 35), -61.19075, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 40), -60.47491, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 45), -60.26393, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 50), -60.52287, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 55), -61.14472, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 60), -61.99008, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 65), -62.92317, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 70), -63.83355, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 75), -64.64062, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 80), -65.28524, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 85), -65.71987, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 90), -65.90828, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 95), -65.83794, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 100), -65.53529, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 105), -65.06844, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 110), -64.52955, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 115), -64.004, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 120), -63.54239, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 125), -63.14871, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 130), -62.78863, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 135), -62.41082, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 140), -61.96895, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 145), -61.43349, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 150), -60.79132, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 155), -60.03943, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 160), -59.18087, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 165), -58.22587, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 170), -57.19451, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 175), -56.1152, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-30, 180), -55.01714, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -180), -49.12399, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -175), -47.94167, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -170), -46.78244, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -165), -45.63671, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -160), -44.48578, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -155), -43.31487, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -150), -42.12082, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -145), -40.91271, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -140), -39.70855, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -135), -38.53094, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -130), -37.40083, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -125), -36.32609, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -120), -35.28582, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -115), -34.21969, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -110), -33.03531, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -105), -31.64078, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -100), -29.99464, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -95), -28.15291, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -90), -26.29239, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -85), -24.69836, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -80), -23.71541, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -75), -23.66717, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -24.76215, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.02032, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.26103, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.16451, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.37291, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -42.57879, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.57229, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.2475, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.58, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.58782, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.28888, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.66944, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.67376, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, -5), -65.21615, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 0), -66.2084, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 5), -66.58985, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 10), -66.34811, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 15), -65.52711, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 20), -64.23077, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 25), -62.62735, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 30), -60.94349, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 35), -59.42973, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 40), -58.29938, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 45), -57.67012, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 50), -57.54184, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 55), -57.81897, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 60), -58.3589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 65), -59.01963, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 70), -59.68766, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 75), -60.28078, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 80), -60.73466, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 85), -60.99031, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 90), -60.99746, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 95), -60.73531, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 100), -60.23537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 105), -59.58519, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 110), -58.90356, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 115), -58.29749, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 120), -57.82335, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 125), -57.47157, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 130), -57.18086, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 135), -56.87146, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 140), -56.47773, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 145), -55.96353, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 150), -55.31768, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 155), -54.54062, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 160), -53.63615, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 165), -52.61386, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 170), -51.49607, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 175), -50.3191, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-25, 180), -49.12399, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -180), -42.24206, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -175), -40.94139, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -170), -39.69111, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -165), -38.47974, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -160), -37.27911, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -155), -36.06266, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -150), -34.81772, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -145), -33.54842, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -140), -32.27289, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -135), -31.01881, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -130), -29.81565, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -125), -28.67934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -120), -27.59095, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -115), -26.4819, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -110), -25.24344, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -105), -23.76848, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -100), -22.01322, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -95), -20.05158, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -90), -18.09663, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -85), -16.47823, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -80), -15.57947, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -75), -15.74429, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -70), -17.17918, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -19.88508, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.65448, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.13813, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -32.94585, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -37.73464, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.25832, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.37885, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.04952, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -25), -53.28002, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -20), -56.09537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -15), -58.50274, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -10), -60.47713, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, -5), -61.96758, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 0), -62.91612, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 5), -63.27846, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 10), -63.03868, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 15), -62.21952, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 20), -60.89425, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 25), -59.20126, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 30), -57.34905, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 35), -55.59184, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 40), -54.1691, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 45), -53.23373, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 50), -52.81241, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 55), -52.82097, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 60), -53.11874, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 65), -53.5656, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 70), -54.05306, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 75), -54.50287, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 80), -54.84664, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 85), -55.01065, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 90), -54.92429, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 95), -54.55147, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 100), -53.92366, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 105), -53.14688, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 110), -52.36972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 115), -51.72572, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 120), -51.27989, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 125), -51.00783, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 130), -50.81672, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 135), -50.59355, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 140), -50.25119, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 145), -49.74806, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 150), -49.07817, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 155), -48.24855, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 160), -47.26546, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 165), -46.13832, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 170), -44.89247, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 175), -43.57462, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-20, 180), -42.24206, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -180), -34.29388, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -175), -32.84176, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -170), -31.47968, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -165), -30.19614, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -160), -28.95262, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -155), -27.70792, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -150), -26.43636, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -145), -25.13391, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -140), -23.81627, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -135), -22.51361, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -130), -21.26062, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -125), -20.07716, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -120), -18.94256, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -115), -17.77939, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -110), -16.46764, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -105), -14.89602, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -100), -13.03215, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -95), -10.978, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -90), -8.9832, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -85), -7.40871, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -80), -6.65113, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -75), -7.0475, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -8.7871, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -11.85916, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.05669, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.03298, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.38537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -31.73549, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -36.78553, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.34368, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.32077, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -48.70467, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.52342, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -15), -53.81131, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -10), -55.58911, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, -5), -56.86003, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 0), -57.61448, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 5), -57.8361, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 10), -57.50895, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 15), -56.6324, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 20), -55.24714, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 25), -53.46468, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 30), -51.48126, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 35), -49.55536, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 40), -47.94294, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 45), -46.81589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 50), -46.21238, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 55), -46.05045, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 60), -46.18936, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 65), -46.49424, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 70), -46.86775, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 75), -47.24127, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 80), -47.54556, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 85), -47.69214, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 90), -47.58717, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 95), -47.17487, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 100), -46.48268, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 105), -45.63345, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 110), -44.80735, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 115), -44.16801, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 120), -43.79158, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 125), -43.63793, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 130), -43.57916, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 135), -43.465, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 140), -43.18441, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 145), -42.69035, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 150), -41.98369, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 155), -41.08003, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 160), -39.98941, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 165), -38.72187, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 170), -37.30701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 175), -35.80489, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-15, 180), -34.29388, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -180), -25.31126, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -175), -23.68453, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -170), -22.19803, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -165), -20.8427, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -160), -19.56947, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -155), -18.32016, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -150), -17.05281, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -145), -15.75257, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -140), -14.43109, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -135), -13.11959, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -130), -11.85525, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -125), -10.65766, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -120), -9.50081, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -115), -8.29929, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -110), -6.92953, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -105), -5.28892, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -100), -3.36839, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -95), -1.30124, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -90), 0.63721, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -85), 2.07953, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -80), 2.6469, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -75), 2.03462, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -70), 0.07845, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.21342, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -7.66467, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -12.96384, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -18.7179, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.51866, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.00879, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -34.92958, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.13781, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -25), -42.59223, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -20), -45.32175, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -15), -47.39104, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -10), -48.87421, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, -5), -49.83743, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 0), -50.32584, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 5), -50.35348, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 10), -49.90365, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 15), -48.94947, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 20), -47.49581, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 25), -45.62667, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 30), -43.52996, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 35), -41.47417, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 40), -39.73323, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 45), -38.49202, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 50), -37.78975, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 55), -37.5368, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 60), -37.58663, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 65), -37.80883, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 70), -38.12004, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 75), -38.46586, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 80), -38.78053, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 85), -38.9635, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 90), -38.89827, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 95), -38.50767, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 100), -37.81189, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 105), -36.94731, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 110), -36.12381, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 115), -35.53524, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 120), -35.26971, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 125), -35.27188, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 130), -35.37822, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 135), -35.40001, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 140), -35.2009, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 145), -34.72714, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 150), -33.98589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 155), -33.00152, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 160), -31.78856, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 165), -30.35858, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 170), -28.74703, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 175), -27.03006, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-10, 180), -25.31126, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -180), -15.49028, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -175), -13.69091, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -170), -12.08656, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -165), -10.67334, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -160), -9.39285, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -155), -8.16895, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -150), -6.94107, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -145), -5.68144, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -140), -4.39661, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -135), -3.11829, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -130), -1.88459, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -125), -0.7123, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -120), 0.43088, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -115), 1.6344, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -110), 3.01585, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -105), 4.65811, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -100), 6.54169, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -95), 8.50898, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -90), 10.27931, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -85), 11.50716, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -80), 11.85803, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -75), 11.07488, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 9.0185, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.68225, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 1.19296, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.1945, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.11738, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.15373, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -21.89614, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.02292, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -30), -31.33749, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -34.76781, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -20), -37.33968, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -15), -39.14281, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -10), -40.29934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, -5), -40.9341, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 0), -41.14402, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 5), -40.97224, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 10), -40.40131, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 15), -39.37863, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 20), -37.8731, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 25), -35.94037, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 30), -33.75972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 35), -31.61024, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 40), -29.78399, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 45), -28.47719, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 50), -27.72662, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 55), -27.43108, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 60), -27.43479, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 65), -27.6097, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 70), -27.88649, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 75), -28.22756, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 80), -28.57527, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 85), -28.82252, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 90), -28.83475, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 95), -28.51532, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 100), -27.87515, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 105), -27.05843, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 110), -26.29732, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 115), -25.80989, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 120), -25.69444, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 125), -25.88305, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 130), -26.18168, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 135), -26.36731, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 140), -26.27919, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 145), -25.8547, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 150), -25.10404, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 155), -24.05823, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 160), -22.73596, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 165), -21.15133, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 170), -19.34697, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 175), -17.41644, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(-5, 180), -15.49028, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -180), -5.21365, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -175), -3.28107, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -170), -1.59209, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -165), -0.15079, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -160), 1.10723, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -155), 2.27418, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -150), 3.42932, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -145), 4.6145, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -140), 5.82915, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -135), 7.04174, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -130), 8.21381, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -125), 9.3312, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -120), 10.42951, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -115), 11.59518, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -110), 12.93063, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -105), 14.49303, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -100), 16.23763, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -95), 17.99707, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -90), 19.50752, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -85), 20.46729, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -80), 20.60029, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -75), 19.70066, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -70), 17.6514, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.42868, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 10.11014, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -55), 4.89129, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -0.90913, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -45), -6.88115, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -40), -12.5975, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -17.69877, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -30), -21.94961, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -25), -25.24952, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -20), -27.61272, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -15), -29.13844, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -10), -29.97954, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, -5), -30.30537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 0), -30.25665, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 5), -29.90402, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 10), -29.23232, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 15), -28.16826, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 20), -26.64899, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 25), -24.70295, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 30), -22.49746, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 35), -20.31522, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 40), -18.45853, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 45), -17.13001, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 50), -16.36411, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 55), -16.05114, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 60), -16.02825, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 65), -16.16904, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 70), -16.41619, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 75), -16.74886, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 80), -17.12191, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 85), -17.43032, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 90), -17.53125, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 95), -17.31539, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 100), -16.78427, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 105), -16.08143, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 110), -15.44718, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 115), -15.11053, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 120), -15.17469, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 125), -15.56474, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 130), -16.0684, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 135), -16.43965, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 140), -16.49799, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 145), -16.16951, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 150), -15.46166, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 155), -14.40734, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 160), -13.02783, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 165), -11.34033, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 170), -9.39497, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 175), -7.30122, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(0, 180), -5.21365, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -180), 5.00455, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -175), 6.99463, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -170), 8.71188, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -165), 10.14177, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -160), 11.34891, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -155), 12.43587, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -150), 13.49728, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -145), 14.58829, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -140), 15.71535, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -135), 16.84842, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -130), 17.94904, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -125), 19.00332, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -120), 20.04428, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -115), 21.1472, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -110), 22.3927, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -105), 23.81212, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -100), 25.34411, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -95), 26.82684, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -90), 28.02919, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -85), 28.70338, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -80), 28.63336, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -75), 27.66094, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -70), 25.69049, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -65), 22.68936, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.70223, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -55), 13.87849, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -50), 8.48936, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -45), 2.9064, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.46493, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.26857, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -30), -11.25725, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -25), -14.30972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -20), -16.41941, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -15), -17.67484, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -10), -18.23585, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, -5), -18.29591, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 0), -18.02731, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 5), -17.52556, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 10), -16.78323, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 15), -15.71609, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 20), -14.23885, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 25), -12.35622, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 30), -10.21787, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 35), -8.09606, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 40), -6.28723, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 45), -4.99077, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 50), -4.23978, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 55), -3.92427, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 60), -3.88186, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 65), -3.98935, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 70), -4.19777, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 75), -4.49974, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 80), -4.86549, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 85), -5.20249, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 90), -5.37419, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 95), -5.27031, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 100), -4.88531, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 105), -4.35212, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 110), -3.90026, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 115), -3.75097, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 120), -4.0046, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 125), -4.58684, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 130), -5.28512, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 135), -5.84805, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 140), -6.08431, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 145), -5.90734, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 150), -5.31472, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 155), -4.33446, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 160), -2.98741, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 165), -1.2934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 170), 0.69159, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 175), 2.84765, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(5, 180), 5.00455, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -180), 14.65459, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -175), 16.60735, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -170), 18.28725, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -165), 19.66757, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -160), 20.80598, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -155), 21.80662, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -150), 22.77261, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -145), 23.76936, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -140), 24.81081, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -135), 25.87055, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -130), 26.9108, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -125), 27.91523, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -120), 28.90819, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -115), 29.94711, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -110), 31.0878, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -105), 32.33921, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -100), 33.63247, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -95), 34.82159, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -90), 35.71446, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -85), 36.11662, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -80), 35.86602, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -75), 34.84671, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -70), 32.98688, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -65), 30.2592, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -60), 26.69739, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.424, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.66681, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.74096, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.99393, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.7376, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -30), 0.19985, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -25), -2.49165, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -20), -4.30693, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -15), -5.30831, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -10), -5.63984, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, -5), -5.49582, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 0), -5.06285, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 5), -4.45491, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 10), -3.67758, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 15), -2.64898, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 20), -1.27496, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 25), 0.45762, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 30), 2.42353, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 35), 4.37838, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 40), 6.05087, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 45), 7.25612, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 50), 7.96121, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 55), 8.26658, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 60), 8.32403, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 65), 8.25029, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 70), 8.09017, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 75), 7.84361, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 80), 7.52553, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 85), 7.20719, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 90), 7.00385, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 95), 7.01301, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 100), 7.24272, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 105), 7.5789, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 110), 7.82066, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 115), 7.77253, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 120), 7.34494, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 125), 6.60457, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 130), 5.74626, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 135), 5.00741, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 140), 4.57835, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 145), 4.55701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 150), 4.96158, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 155), 5.77582, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 160), 6.9833, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 165), 8.56349, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 170), 10.45846, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 175), 12.54662, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(10, 180), 14.65459, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -180), 23.37517, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -175), 25.199, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -170), 26.78074, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -165), 28.08178, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -160), 29.14648, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -155), 30.07089, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -150), 30.95757, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -145), 31.87695, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -140), 32.84984, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -135), 33.85548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -130), 34.85825, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -125), 35.83792, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -120), 36.80628, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -115), 37.79947, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -110), 38.84866, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -105), 39.94476, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -100), 41.01751, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -95), 41.94059, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -90), 42.55929, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -85), 42.72537, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -80), 42.32186, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -75), 41.27008, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -70), 39.52594, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -65), 37.08077, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -60), 33.97548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.3213, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 26.3085, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 22.18864, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.2323, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.68231, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.72364, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -25), 9.47374, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -20), 7.97925, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -15), 7.20837, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -10), 7.04759, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, -5), 7.32286, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 0), 7.8532, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 5), 8.51706, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 10), 9.29366, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 15), 10.25, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 20), 11.47426, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 25), 12.98983, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 30), 14.70052, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 35), 16.40389, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 40), 17.86939, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 45), 18.93569, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 50), 19.56948, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 55), 19.85398, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 60), 19.92165, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 65), 19.87879, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 70), 19.76901, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 75), 19.59119, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 80), 19.34918, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 85), 19.08972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 90), 18.89721, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 95), 18.84578, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 100), 18.94062, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 105), 19.08971, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 110), 19.1307, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 115), 18.90477, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 120), 18.33852, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 125), 17.48736, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 130), 16.51681, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 135), 15.63569, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 140), 15.02205, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 145), 14.78083, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 150), 14.94792, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 155), 15.52312, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 160), 16.49794, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 165), 17.8539, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 170), 19.53614, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 175), 21.43101, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(15, 180), 23.37517, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -180), 31.01932, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -175), 32.64095, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -170), 34.07574, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -165), 35.27629, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -160), 36.27043, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -155), 37.13802, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -150), 37.97183, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -145), 38.84055, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -140), 39.76945, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -135), 40.74438, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -130), 41.73363, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -125), 42.71359, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -120), 43.68292, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -115), 44.65666, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -110), 45.64366, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -105), 46.62075, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -100), 47.51907, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -95), 48.23079, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -90), 48.63254, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -85), 48.61286, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -80), 48.09086, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -75), 47.02071, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -70), 45.38841, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -65), 43.21163, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -60), 40.5479, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -55), 37.5047, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 34.23933, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.94149, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -40), 27.80398, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -35), 24.9963, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -30), 22.65215, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -25), 20.8671, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -20), 19.69304, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -15), 19.12285, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -10), 19.0773, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, -5), 19.41567, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 0), 19.97934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 5), 20.65143, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 10), 21.39814, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 15), 22.26413, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 20), 23.3203, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 25), 24.59235, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 30), 26.01261, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 35), 27.42641, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 40), 28.6514, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 45), 29.55506, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 50), 30.10473, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 55), 30.36334, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 60), 30.43907, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 65), 30.4243, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 70), 30.36176, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 75), 30.25339, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 80), 30.0968, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 85), 29.91677, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 90), 29.76548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 95), 29.68972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 100), 29.68773, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.68729, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 110), 29.56464, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 115), 29.19915, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 120), 28.53535, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 125), 27.61778, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 130), 26.58, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 135), 25.59639, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 140), 24.82531, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 145), 24.37274, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 150), 24.28932, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 155), 24.59085, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 160), 25.27809, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 165), 26.3369, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 170), 27.71958, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 175), 29.32792, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(20, 180), 31.01932, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -180), 37.62767, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -175), 38.99854, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -170), 40.25168, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -165), 41.33675, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -160), 42.26565, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -155), 43.09738, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -150), 43.90722, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -145), 44.75497, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -140), 45.6661, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -135), 46.63216, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -130), 47.62648, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -125), 48.62393, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -120), 49.61237, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -115), 50.58897, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -110), 51.5443, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -105), 52.44444, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -100), 53.22257, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -95), 53.78588, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -90), 54.03494, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -85), 53.88601, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -80), 53.28639, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -75), 52.21859, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.69742, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.76735, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -60), 46.50294, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -55), 44.00881, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.41283, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.85094, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.4498, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.31632, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -30), 32.53756, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -25), 31.18356, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -20), 30.30169, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -15), 29.8992, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -10), 29.92619, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, -5), 30.27823, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 0), 30.8278, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 5), 31.47196, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 10), 32.16907, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 15), 32.94064, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 20), 33.83517, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 25), 34.87435, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 30), 36.01402, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 35), 37.14461, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 40), 38.13177, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 45), 38.87352, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 50), 39.34029, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 55), 39.57609, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 60), 39.66394, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 65), 39.67979, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 70), 39.66357, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 75), 39.62082, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 80), 39.54667, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 85), 39.44937, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 90), 39.35373, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 95), 39.28126, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 100), 39.22157, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 105), 39.1172, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 110), 38.87656, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 115), 38.41233, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 120), 37.68594, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 125), 36.73393, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 130), 35.66203, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 135), 34.61193, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 140), 33.71986, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 145), 33.08718, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 150), 32.77332, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 155), 32.80612, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 160), 33.19436, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 165), 33.92888, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 170), 34.97133, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 175), 36.24277, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(25, 180), 37.62767, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -180), 43.36043, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -175), 44.45891, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -170), 45.51173, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -165), 46.47234, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -160), 47.3404, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -155), 48.15326, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -150), 48.96415, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -145), 49.8179, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -140), 50.73506, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -135), 51.70999, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -130), 52.72076, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -125), 53.74269, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -120), 54.75665, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -115), 55.74697, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -110), 56.69101, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -105), 57.54744, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -100), 58.25151, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -95), 58.72174, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -90), 58.8762, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -85), 58.65154, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -80), 58.0168, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -75), 56.97756, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -70), 55.57258, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -65), 53.86745, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -60), 51.94822, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -55), 49.91408, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -50), 47.86739, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -45), 45.90228, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -40), 44.09663, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -35), 42.5119, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -30), 41.19957, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -25), 40.20643, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -20), 39.56975, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -15), 39.30159, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -10), 39.37247, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, -5), 39.70956, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 0), 40.21693, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 5), 40.8101, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 10), 41.44522, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 15), 42.12478, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 20), 42.87558, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 25), 43.71195, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 30), 44.60661, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 35), 45.48757, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 40), 46.26319, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 45), 46.86097, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 50), 47.25716, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 55), 47.48075, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 60), 47.59149, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 65), 47.64772, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 70), 47.68343, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 75), 47.70573, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 80), 47.70867, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 85), 47.68931, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 90), 47.65224, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 95), 47.59897, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 100), 47.51105, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 105), 47.34095, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 110), 47.0213, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 115), 46.49113, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 120), 45.72666, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 125), 44.76002, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 130), 43.67589, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 135), 42.58893, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 140), 41.61418, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 145), 40.84428, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 150), 40.34087, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 155), 40.13814, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 160), 40.25001, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 165), 40.6718, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 170), 41.37475, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 175), 42.29982, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(30, 180), 43.36043, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -180), 48.43609, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -175), 49.2684, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -170), 50.12084, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -165), 50.95582, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -160), 51.76578, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -155), 52.56902, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -150), 53.39612, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -145), 54.27376, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -140), 55.21327, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -135), 56.20813, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -130), 57.23934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -125), 58.28358, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -120), 59.31827, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -115), 60.32017, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -110), 61.25863, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -105), 62.0883, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -100), 62.74694, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -95), 63.16185, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -90), 63.26412, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -85), 63.00572, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -80), 62.3722, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -75), 61.38651, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -70), 60.10401, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -65), 58.60278, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -60), 56.97259, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -55), 55.3041, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -50), 53.67879, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -45), 52.1616, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -40), 50.79904, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -35), 49.62404, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -30), 48.66451, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -25), 47.94882, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -20), 47.50235, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -15), 47.33542, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -10), 47.43022, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, -5), 47.7374, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 0), 48.18798, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 5), 48.71613, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 10), 49.28038, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 15), 49.87048, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 20), 50.496, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 25), 51.16354, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 30), 51.85697, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 35), 52.53301, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 40), 53.13497, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 45), 53.61658, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 50), 53.96172, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 55), 54.18871, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 60), 54.33753, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 65), 54.44899, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 70), 54.54821, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 75), 54.64051, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 80), 54.71837, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 85), 54.7713, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 90), 54.79009, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 95), 54.76221, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 100), 54.66313, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 105), 54.45224, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 110), 54.08001, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 115), 53.50593, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 120), 52.71905, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 125), 51.7503, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 130), 50.66987, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 135), 49.57143, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 140), 48.55111, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 145), 47.69032, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 150), 47.04741, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 155), 46.65769, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 160), 46.5372, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 165), 46.68483, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 170), 47.08076, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 175), 47.68428, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(35, 180), 48.43609, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -180), 53.0875, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -175), 53.68544, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -170), 54.35615, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -165), 55.07296, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -160), 55.82549, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -155), 56.61786, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -150), 57.46133, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -145), 58.36511, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -140), 59.32962, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -135), 60.34458, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -130), 61.39161, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -125), 62.44834, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -120), 63.49088, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -115), 64.49229, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -110), 65.4182, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -105), 66.22221, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -100), 66.84546, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -95), 67.22287, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -90), 67.29618, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -85), 67.02955, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -80), 66.42111, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -75), 65.5055, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -70), 64.34701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -65), 63.02748, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -60), 61.63344, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -55), 60.24492, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -50), 58.92715, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -45), 57.7266, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -40), 56.67225, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -35), 55.78176, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -30), 55.06921, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -25), 54.54985, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -20), 54.23803, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -15), 54.13906, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -10), 54.24027, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, -5), 54.50782, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 0), 54.89315, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 5), 55.34685, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 10), 55.83273, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 15), 56.33415, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 20), 56.84939, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 25), 57.37914, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 30), 57.91449, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 35), 58.43266, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 40), 58.90338, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 45), 59.30165, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 50), 59.61914, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 55), 59.86723, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 60), 60.07012, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 65), 60.2525, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 70), 60.42883, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 75), 60.59946, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 80), 60.75363, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 85), 60.87505, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 90), 60.94533, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 95), 60.9429, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 100), 60.83981, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 105), 60.60127, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 110), 60.1918, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 115), 59.58745, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 120), 58.78883, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 125), 57.82808, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 130), 56.76583, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 135), 55.67959, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 140), 54.64888, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 145), 53.74266, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 150), 53.01236, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 155), 52.49071, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 160), 52.1938, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 165), 52.12331, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 170), 52.26738, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 175), 52.60089, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(40, 180), 53.0875, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -180), 57.52337, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -175), 57.93647, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -170), 58.45759, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -165), 59.06985, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -160), 59.76295, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -155), 60.53235, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -150), 61.37584, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -145), 62.28941, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -140), 63.26404, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -135), 64.28493, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -130), 65.33265, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -125), 66.38488, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -120), 67.41701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -115), 68.40051, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -110), 69.29972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -105), 70.06912, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -100), 70.65389, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -95), 70.99622, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -90), 71.04743, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -85), 70.78222, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -80), 70.20851, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -75), 69.36789, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -70), 68.32709, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -65), 67.16502, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -60), 65.96019, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -55), 64.78122, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -50), 63.68097, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -45), 62.69493, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -40), 61.84356, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -35), 61.13767, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -30), 60.5843, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -25), 60.19022, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -20), 59.96106, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -15), 59.89666, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -10), 59.9858, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, -5), 60.20412, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 0), 60.51745, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 5), 60.88966, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 10), 61.29131, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 15), 61.70457, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 20), 62.1222, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 25), 62.5418, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 30), 62.9592, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 35), 63.36526, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 40), 63.74781, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 45), 64.09716, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 50), 64.4113, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 55), 64.69725, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 60), 64.96739, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 65), 65.23277, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 70), 65.49697, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 75), 65.75335, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 80), 65.98618, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 85), 66.17384, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 90), 66.2915, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 95), 66.31218, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 100), 66.20704, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 105), 65.94737, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 110), 65.51001, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 115), 64.88542, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 120), 64.08494, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 125), 63.14327, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 130), 62.11462, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 135), 61.06372, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 140), 60.05548, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 145), 59.14626, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 150), 58.37894, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 155), 57.78179, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 160), 57.36978, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 165), 57.1469, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 170), 57.1079, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 175), 57.23975, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(45, 180), 57.52337, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -180), 61.89099, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -175), 62.17383, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -170), 62.58157, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -165), 63.10434, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -160), 63.73323, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -155), 64.45982, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -150), 65.27489, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -145), 66.16696, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -140), 67.12136, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -135), 68.12016, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -130), 69.14275, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -125), 70.16638, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -120), 71.16577, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -115), 72.11147, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -110), 72.9675, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -105), 73.68972, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -100), 74.2273, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -95), 74.52916, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -90), 74.55554, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -85), 74.29071, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -80), 73.75044, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -75), 72.98004, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -70), 72.04431, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -65), 71.01509, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -60), 69.96049, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -55), 68.93791, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -50), 67.99053, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -45), 67.14711, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -40), 66.42422, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -35), 65.83017, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -30), 65.3689, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -25), 65.04236, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -20), 64.85043, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -15), 64.78884, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -10), 64.84666, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, -5), 65.00541, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 0), 65.24085, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 5), 65.52725, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 10), 65.84229, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 15), 66.17045, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 20), 66.50354, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 25), 66.83858, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 30), 67.17464, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 35), 67.51065, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 40), 67.84519, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 45), 68.1778, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 50), 68.51031, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 55), 68.84679, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 60), 69.1913, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 65), 69.54446, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 70), 69.90032, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 75), 70.24483, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 80), 70.55643, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 85), 70.80815, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 90), 70.97024, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 95), 71.01259, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 100), 70.90741, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 105), 70.63275, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 110), 70.17711, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 115), 69.544, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 120), 68.75449, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 125), 67.84618, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 130), 66.86841, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 135), 65.87544, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 140), 64.91941, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 145), 64.04501, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 150), 63.28648, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 155), 62.66711, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 160), 62.20035, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 165), 61.8918, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 170), 61.7411, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 175), 61.74347, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(50, 180), 61.89099, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -180), 66.25235, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -175), 66.45182, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -170), 66.77707, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -165), 67.22227, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -160), 67.78022, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -155), 68.44191, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -150), 69.19617, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -145), 70.02934, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -140), 70.92524, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -135), 71.86538, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -130), 72.82925, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -125), 73.7942, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -120), 74.73472, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -115), 75.62086, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -110), 76.41654, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -105), 77.07872, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -100), 77.55964, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -95), 77.81384, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -90), 77.80944, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -85), 77.53942, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -80), 77.02624, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -75), 76.3169, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -70), 75.472, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -65), 74.55438, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -60), 73.62107, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -55), 72.71889, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -50), 71.88288, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -45), 71.13682, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -40), 70.49505, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -35), 69.96494, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -30), 69.54931, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -25), 69.24788, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -20), 69.05773, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -15), 68.9727, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -10), 68.98266, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, -5), 69.07347, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 0), 69.22811, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 5), 69.42897, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 10), 69.66049, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 15), 69.91125, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 20), 70.17481, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 25), 70.44915, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 30), 70.73538, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 35), 71.03629, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 40), 71.35532, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 45), 71.69603, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 50), 72.06154, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 55), 72.45351, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 60), 72.87054, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 65), 73.3063, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 70), 73.74787, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 75), 74.17502, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 80), 74.56082, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 85), 74.87337, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 90), 75.07882, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 95), 75.14513, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 100), 75.04659, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 105), 74.76829, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 110), 74.30963, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 115), 73.68563, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 120), 72.92558, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 125), 72.06917, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 130), 71.16148, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 135), 70.24784, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 140), 69.36963, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 145), 68.56147, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 150), 67.84993, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 155), 67.2536, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 160), 66.78421, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 165), 66.44812, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 170), 66.24783, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 175), 66.18323, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(55, 180), 66.25235, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -180), 70.58559, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -175), 70.73325, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -170), 70.99602, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -165), 71.37007, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -160), 71.84967, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -155), 72.42694, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -150), 73.09173, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -145), 73.83159, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -140), 74.6319, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -135), 75.47603, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -130), 76.34527, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -125), 77.21841, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -120), 78.07083, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -115), 78.87308, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -110), 79.58938, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -105), 80.17748, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -100), 80.59164, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -95), 80.79046, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -90), 80.74878, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -85), 80.46753, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -80), 79.97507, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -75), 79.31903, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -70), 78.55453, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -65), 77.73477, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -60), 76.9058, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -55), 76.10447, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -50), 75.3584, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -45), 74.68677, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -40), 74.10175, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -35), 73.60996, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -30), 73.21387, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -25), 72.91277, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -20), 72.70328, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -15), 72.57957, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -10), 72.53345, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, -5), 72.55476, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 0), 72.63223, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 5), 72.75466, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 10), 72.91236, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 15), 73.09825, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 20), 73.30849, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 25), 73.54247, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 30), 73.8022, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 35), 74.09138, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 40), 74.41439, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 45), 74.77519, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 50), 75.17619, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 55), 75.61701, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 60), 76.09309, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 65), 76.59441, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 70), 77.10437, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 75), 77.59928, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 80), 78.04872, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 85), 78.41719, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 90), 78.66747, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 95), 78.7659, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 100), 78.68854, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 105), 78.42662, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 110), 77.98882, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 115), 77.3994, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 120), 76.69352, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 125), 75.91155, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 130), 75.09428, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 135), 74.27941, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 140), 73.49954, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 145), 72.78115, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 150), 72.14454, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 155), 71.60429, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 160), 71.17026, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 165), 70.84865, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 170), 70.64305, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 175), 70.55524, 0.21 + 1.2); - EXPECT_NEAR(get_mag_inclination_degrees(60, 180), 70.58559, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -180), -71.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -175), -70.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -170), -69.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -165), -68.7, 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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -130), -62.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -125), -60.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -120), -59.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -115), -58.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -110), -57.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -105), -55.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -100), -54.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -95), -52.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -90), -51.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -85), -49.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -80), -48.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -75), -48.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -70), -48.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -65), -48.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -60), -49.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -55), -50.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -50), -52.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -45), -53.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -40), -55.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -35), -56.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -30), -58.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -25), -59.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, -20), -60.1, 0.21 + 1.2); + 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, 5), -60.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 10), -59.9, 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, 20), -59.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 25), -59.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 30), -59.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 35), -59.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 40), -60.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 45), -61.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 50), -62.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 55), -64.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 60), -66.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 65), -67.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 70), -69.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 75), -71.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 80), -72.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 85), -74.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 90), -75.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 95), -77.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 100), -78.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 105), -79.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 110), -80.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 115), -80.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 120), -80.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 125), -80.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 130), -80.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 135), -79.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 140), -79.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 145), -78.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 150), -77.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 155), -76.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 160), -75.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 165), -74.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 170), -73.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 175), -72.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-50, 180), -71.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -180), -68.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -175), -67.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -170), -66.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -165), -65.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -160), -64.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -155), -63.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -150), -62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -145), -61.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -140), -60.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -135), -59.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -130), -58.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -125), -57.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -120), -56.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -115), -55.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -110), -53.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -105), -52.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -100), -50.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -95), -49.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -90), -47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -85), -46.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -80), -45.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -75), -44.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -70), -44.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -65), -45.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -60), -46.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -55), -48.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -50), -50.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -45), -52.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, -40), -54.8, 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, -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, 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.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 20), -60.6, 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, 30), -59.9, 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, 45), -61.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 50), -62.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 55), -64.3, 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, 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, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 110), -77.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 115), -77.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 120), -77.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 125), -76.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 130), -76.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 135), -75.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 140), -75.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 145), -74.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 150), -73.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 155), -73.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 160), -72.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 165), -71.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 170), -70.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 175), -69.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-45, 180), -68.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -180), -64.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -175), -63.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -170), -62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -165), -61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -160), -60.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -155), -59.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -150), -58.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -145), -57.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -140), -56.5, 0.21 + 1.2); + 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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -100), -47.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -95), -45.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -90), -43.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -85), -42.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -80), -41.4, 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, -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, -55), -46.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -50), -48.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -45), -51.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -40), -54.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -35), -56.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -30), -58.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -25), -60.7, 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, -15), -63.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -10), -64.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, -5), -65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 0), -65.6, 0.21 + 1.2); + 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.6, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 55), -64.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 60), -65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 65), -66.8, 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, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 105), -73.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 110), -73.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 115), -73.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 120), -73.0, 0.21 + 1.2); + 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 160), -68.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 165), -67.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 170), -66.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 175), -65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-40, 180), -64.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -180), -60.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -175), -59.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -170), -58.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -165), -56.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -160), -55.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -155), -54.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -150), -53.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -145), -52.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -140), -51.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -135), -50.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -130), -49.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -125), -48.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -120), -47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -115), -46.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -110), -45.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -105), -44.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -100), -42.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -95), -40.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -90), -39.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -85), -37.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -80), -36.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -75), -36.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -70), -36.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -65), -38.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -60), -40.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -55), -43.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -50), -46.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -45), -49.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -40), -52.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -35), -55.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -30), -57.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -25), -60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -20), -62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -15), -64.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -10), -65.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, -5), -66.7, 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, 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, 20), -64.4, 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, 30), -62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 35), -61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 40), -61.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 45), -61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 50), -62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 55), -63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 60), -64.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 65), -65.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 70), -66.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 75), -67.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 80), -68.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 85), -69.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 90), -69.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 95), -70.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 100), -69.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 105), -69.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 110), -69.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 115), -69.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 120), -68.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 125), -68.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 130), -67.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 135), -67.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 140), -66.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 145), -66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 150), -65.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 155), -64.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 160), -64.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 165), -63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 170), -62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 175), -61.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-35, 180), -60.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -180), -55.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -175), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -170), -52.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -165), -51.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -160), -50.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -155), -49.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -150), -48.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -145), -47.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -140), -46.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -135), -45.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -130), -44.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -125), -43.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -120), -42.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -115), -40.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -110), -39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -105), -38.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -100), -36.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -95), -35.1, 0.21 + 1.2); + 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, -65), -33.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -60), -35.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -55), -39.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -50), -42.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -45), -46.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -40), -50.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -35), -53.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -30), -56.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -25), -58.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -20), -61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -15), -63.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, -10), -65.4, 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, 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, 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, 35), -61.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 40), -60.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 45), -60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 50), -60.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 55), -61.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 60), -62.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 65), -62.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 70), -63.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 75), -64.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 80), -65.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 85), -65.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 90), -65.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 95), -65.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 100), -65.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 105), -65.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 110), -64.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 115), -64.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 120), -63.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 125), -63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 130), -62.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 135), -62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 140), -62.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 145), -61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 150), -60.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-30, 155), -60.0, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -180), -49.1, 0.21 + 1.2); + 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, -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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -120), -35.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -115), -34.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -110), -33.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -105), -31.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -100), -30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -95), -28.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -90), -26.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -85), -24.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -80), -23.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -75), -23.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -70), -24.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -65), -27.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -60), -30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -55), -34.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -50), -38.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -45), -42.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -40), -46.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -35), -50.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -30), -53.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -25), -56.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -20), -59.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -15), -61.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, -10), -63.7, 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, 0), -66.2, 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, 25), -62.6, 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, 40), -58.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 45), -57.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 50), -57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 55), -57.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 60), -58.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 65), -59.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 70), -59.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 75), -60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 80), -60.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 85), -61.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 90), -61.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 95), -60.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 100), -60.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 105), -59.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 110), -58.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 115), -58.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 120), -57.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 125), -57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 130), -57.2, 0.21 + 1.2); + 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, 155), -54.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-25, 160), -53.6, 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, 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, -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.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -160), -37.3, 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, -145), -33.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -140), -32.3, 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, -130), -29.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -125), -28.7, 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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -100), -22.0, 0.21 + 1.2); + 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.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -65), -20.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -60), -23.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -55), -28.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -50), -33.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -45), -37.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -40), -42.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -35), -46.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -30), -50.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -25), -53.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -20), -56.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -15), -58.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -10), -60.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, -5), -62.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 0), -62.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 5), -63.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 10), -63.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 15), -62.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 20), -60.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 25), -59.2, 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, 35), -55.6, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 50), -52.8, 0.21 + 1.2); + 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 90), -54.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 95), -54.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 100), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 105), -53.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 110), -52.4, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 140), -50.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 145), -49.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 150), -49.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 155), -48.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-20, 160), -47.3, 0.21 + 1.2); + 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(-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, -160), -28.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -155), -27.7, 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, -130), -21.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -125), -20.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -120), -18.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -115), -17.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -110), -16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -105), -14.9, 0.21 + 1.2); + 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, -75), -7.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -70), -8.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -65), -11.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -60), -16.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -55), -21.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -50), -26.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -45), -31.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -40), -36.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -35), -41.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -30), -45.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -25), -48.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -20), -51.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -15), -53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -10), -55.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, -5), -56.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 0), -57.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 5), -57.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 10), -57.5, 0.21 + 1.2); + 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, 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.9, 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, 90), -47.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 95), -47.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-15, 100), -46.5, 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, 115), -44.2, 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, 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, 165), -38.7, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -175), -23.7, 0.21 + 1.2); + 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, -135), -13.1, 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.5, 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, -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.1, 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), 2.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -70), 0.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -65), -3.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -60), -7.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -55), -13.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -50), -18.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -45), -24.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -40), -30.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -35), -35.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -30), -39.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -25), -42.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -20), -45.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -15), -47.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, -10), -48.9, 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, 0), -50.3, 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); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 15), -48.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 20), -47.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 25), -45.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 30), -43.5, 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, 40), -39.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 45), -38.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 50), -37.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 55), -37.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 60), -37.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 65), -37.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 70), -38.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 75), -38.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 80), -38.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 85), -38.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 90), -38.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 95), -38.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 100), -37.8, 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, 115), -35.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 120), -35.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 125), -35.3, 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, 145), -34.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 150), -34.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 155), -33.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 160), -31.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 165), -30.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 170), -28.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-10, 175), -27.0, 0.21 + 1.2); + 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, -165), -10.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -160), -9.4, 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, -145), -5.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -140), -4.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -135), -3.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -130), -1.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -125), -0.7, 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, -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.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -85), 11.5, 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, -75), 11.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -70), 8.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -65), 5.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -60), 1.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -55), -4.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -50), -10.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -45), -16.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -40), -22.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -35), -27.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -30), -31.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -25), -34.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -20), -37.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -15), -39.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -10), -40.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, -5), -41.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 0), -41.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 5), -41.0, 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, 15), -39.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 20), -37.9, 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, 35), -31.6, 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, 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, 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, 70), -27.9, 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, 80), -28.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 85), -28.8, 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, 95), -28.5, 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, 110), -26.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 115), -25.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 120), -25.7, 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, 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, 150), -25.1, 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, 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, 170), -19.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(-5, 175), -17.4, 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, -165), -0.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -160), 1.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -155), 2.3, 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, -145), 4.6, 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, -130), 8.2, 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, -110), 12.9, 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, -85), 20.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -80), 20.6, 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.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -65), 14.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -60), 10.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -55), 4.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -50), -1.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -45), -7.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -40), -12.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -35), -17.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -30), -22.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -25), -25.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -20), -27.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -15), -29.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -10), -30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, -5), -30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 0), -30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 5), -29.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 10), -29.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 15), -28.2, 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.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 30), -22.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 35), -20.3, 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, 45), -17.1, 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, 65), -16.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 70), -16.4, 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, 80), -17.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 85), -17.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 90), -17.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 95), -17.3, 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, 115), -15.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 120), -15.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(0, 125), -15.6, 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.5, 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, 155), -14.4, 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.4, 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, 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, -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.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -145), 14.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -140), 15.7, 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, -125), 19.0, 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); + EXPECT_NEAR(get_mag_inclination_degrees(5, -110), 22.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -105), 23.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -100), 25.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -95), 26.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -90), 28.0, 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, -80), 28.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -75), 27.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -70), 25.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -65), 22.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -60), 18.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -55), 13.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -50), 8.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -45), 2.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -40), -2.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -35), -7.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -30), -11.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -25), -14.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -20), -16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, -15), -17.7, 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), -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, 30), -10.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 35), -8.1, 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, 45), -5.0, 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, 60), -3.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 65), -4.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 70), -4.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 75), -4.5, 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, 85), -5.2, 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, 110), -3.9, 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, 120), -4.0, 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, 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, 145), -5.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 150), -5.3, 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, 160), -3.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 165), -1.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 170), 0.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 175), 2.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(5, 180), 5.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -180), 14.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -175), 16.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -170), 18.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -165), 19.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -160), 20.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -155), 21.8, 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, -140), 24.8, 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, -130), 26.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -125), 27.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -120), 28.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -115), 30.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -110), 31.1, 0.21 + 1.2); + 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, -85), 36.1, 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, -75), 34.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -70), 32.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -65), 30.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -60), 26.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -55), 22.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -50), 17.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -45), 12.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -40), 7.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, -35), 3.6, 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.6, 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, -15), -5.3, 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); + EXPECT_NEAR(get_mag_inclination_degrees(10, 0), -5.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 5), -4.5, 0.21 + 1.2); + 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.3, 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, 35), 4.4, 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, 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, 70), 8.1, 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.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 90), 7.0, 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, 105), 7.6, 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, 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.6, 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, 155), 5.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 160), 7.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 165), 8.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 170), 10.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 175), 12.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(10, 180), 14.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -180), 23.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -175), 25.2, 0.21 + 1.2); + 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, -125), 35.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -120), 36.8, 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, -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, -80), 42.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -75), 41.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -70), 39.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -65), 37.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -60), 33.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -55), 30.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -50), 26.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -45), 22.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -40), 18.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -35), 14.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -30), 11.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, -25), 9.4, 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, -15), 7.2, 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); + EXPECT_NEAR(get_mag_inclination_degrees(15, 0), 7.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 5), 8.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 10), 9.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 15), 10.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 20), 11.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 25), 13.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 30), 14.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 35), 16.4, 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, 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, 60), 19.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 65), 19.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 70), 19.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 75), 19.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 80), 19.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 85), 19.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 90), 18.9, 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, 105), 19.1, 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), 18.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 120), 18.4, 0.21 + 1.2); + 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, 145), 14.8, 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, 155), 15.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 160), 16.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 165), 17.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 170), 19.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 175), 21.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(15, 180), 23.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -180), 31.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -175), 32.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -170), 34.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -165), 35.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -160), 36.3, 0.21 + 1.2); + 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, -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.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -120), 43.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -115), 44.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -110), 45.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -105), 46.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -100), 47.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -95), 48.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -90), 48.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -85), 48.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -80), 48.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -75), 47.0, 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.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -60), 40.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -55), 37.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -50), 34.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -45), 30.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -40), 27.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -35), 24.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -30), 22.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -25), 20.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, -20), 19.7, 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, -10), 19.1, 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); + EXPECT_NEAR(get_mag_inclination_degrees(20, 5), 20.7, 0.21 + 1.2); + 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.3, 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, 30), 26.0, 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.1, 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, 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, 80), 30.1, 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, 90), 29.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 95), 29.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 100), 29.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 105), 29.7, 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, 115), 29.2, 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); + EXPECT_NEAR(get_mag_inclination_degrees(20, 130), 26.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 135), 25.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 140), 24.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 145), 24.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 150), 24.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 155), 24.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 160), 25.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 165), 26.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 170), 27.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 175), 29.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(20, 180), 31.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -180), 37.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -175), 39.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -170), 40.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -165), 41.3, 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, -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.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -130), 47.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -125), 48.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -120), 49.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -115), 50.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -110), 51.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -105), 52.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -100), 53.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -95), 53.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -90), 54.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -85), 53.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -80), 53.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -75), 52.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -70), 50.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -65), 48.7, 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, -55), 43.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -50), 41.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -45), 38.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -40), 36.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -35), 34.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -30), 32.5, 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, -20), 30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -15), 29.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -10), 29.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, -5), 30.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 0), 30.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 5), 31.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 10), 32.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 15), 32.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 20), 33.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 25), 34.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 30), 36.0, 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, 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, 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, 95), 39.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 100), 39.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 105), 39.1, 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, 115), 38.4, 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.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 130), 35.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 135), 34.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 140), 33.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 145), 33.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 150), 32.8, 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, 165), 33.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 170), 35.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 175), 36.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(25, 180), 37.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -180), 43.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -175), 44.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -170), 45.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -165), 46.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -160), 47.3, 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, -140), 50.7, 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, -125), 53.7, 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.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -110), 56.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -105), 57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -100), 58.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -95), 58.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -90), 58.9, 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), 58.0, 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, -65), 53.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -60), 51.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -55), 49.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -50), 47.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -45), 45.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -40), 44.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -35), 42.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -30), 41.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, -25), 40.2, 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, -10), 39.4, 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, 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, 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, 65), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 70), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 75), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 80), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 85), 47.7, 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, 95), 47.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 100), 47.5, 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.0, 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, 120), 45.7, 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); + EXPECT_NEAR(get_mag_inclination_degrees(30, 135), 42.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 140), 41.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 145), 40.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(30, 150), 40.3, 0.21 + 1.2); + 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.7, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(35, -175), 49.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -170), 50.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -165), 51.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -160), 51.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -155), 52.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -150), 53.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -145), 54.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -140), 55.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -135), 56.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -130), 57.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -125), 58.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -120), 59.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -115), 60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -110), 61.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -105), 62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -100), 62.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -95), 63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -90), 63.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -85), 63.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -80), 62.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -75), 61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -70), 60.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -65), 58.6, 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.3, 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, -40), 50.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, -35), 49.6, 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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(35, 0), 48.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 5), 48.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 10), 49.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 15), 49.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 20), 50.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 25), 51.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 30), 51.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 35), 52.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 40), 53.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 45), 53.6, 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, 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.7, 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, 90), 54.8, 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, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(35, 140), 48.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 145), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 150), 47.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 155), 46.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 160), 46.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 165), 46.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 170), 47.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 175), 47.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(35, 180), 48.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -180), 53.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -175), 53.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -170), 54.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -165), 55.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -160), 55.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -155), 56.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -150), 57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -145), 58.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -140), 59.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -135), 60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -130), 61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -125), 62.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -120), 63.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -115), 64.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -110), 65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -105), 66.2, 0.21 + 1.2); + 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.3, 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, -80), 66.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -75), 65.5, 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, -65), 63.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -60), 61.6, 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, -50), 58.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -45), 57.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -40), 56.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -35), 55.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -30), 55.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -25), 54.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -20), 54.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -15), 54.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, -10), 54.2, 0.21 + 1.2); + 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.3, 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, 15), 56.3, 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, 35), 58.4, 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, 50), 59.6, 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); + EXPECT_NEAR(get_mag_inclination_degrees(40, 65), 60.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 70), 60.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 75), 60.6, 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, 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, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(40, 145), 53.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 150), 53.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 155), 52.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 160), 52.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 165), 52.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 170), 52.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 175), 52.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(40, 180), 53.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -180), 57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -175), 57.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -170), 58.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -165), 59.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -160), 59.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -155), 60.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -150), 61.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -145), 62.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -140), 63.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -135), 64.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -130), 65.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -125), 66.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -120), 67.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -115), 68.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -110), 69.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -105), 70.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -100), 70.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -95), 71.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -90), 71.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -85), 70.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -80), 70.2, 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, -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, -55), 64.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -50), 63.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, -45), 62.7, 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.6, 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, -20), 60.0, 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); + EXPECT_NEAR(get_mag_inclination_degrees(45, -5), 60.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 0), 60.5, 0.21 + 1.2); + 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, 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, 60), 65.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 65), 65.2, 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, 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, 95), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 100), 66.2, 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, 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.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 130), 62.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 135), 61.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 140), 60.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 145), 59.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 150), 58.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 155), 57.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 160), 57.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 165), 57.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 170), 57.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 175), 57.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(45, 180), 57.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -180), 61.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -175), 62.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -170), 62.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -165), 63.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -160), 63.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -155), 64.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -150), 65.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -145), 66.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -140), 67.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -135), 68.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -130), 69.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -125), 70.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -120), 71.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -115), 72.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -110), 73.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -105), 73.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -100), 74.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -95), 74.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -90), 74.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -85), 74.3, 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), 73.0, 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, -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), 68.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -45), 67.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -40), 66.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -35), 65.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -30), 65.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -25), 65.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -20), 64.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -15), 64.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -10), 64.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, -5), 65.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 0), 65.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 5), 65.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 10), 65.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 15), 66.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 20), 66.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 25), 66.8, 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, 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, 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, 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.8, 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.0, 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, 105), 70.6, 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); + EXPECT_NEAR(get_mag_inclination_degrees(50, 120), 68.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 125), 67.9, 0.21 + 1.2); + 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(50, 165), 61.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 170), 61.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 175), 61.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(50, 180), 61.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -180), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -175), 66.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -170), 66.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -165), 67.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -160), 67.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -155), 68.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -150), 69.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -145), 70.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -140), 70.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -135), 71.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -130), 72.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -125), 73.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -120), 74.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -115), 75.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -110), 76.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -105), 77.1, 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, -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, -70), 75.5, 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, -50), 71.9, 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, -35), 70.0, 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); + EXPECT_NEAR(get_mag_inclination_degrees(55, -20), 69.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -15), 69.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -10), 69.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, -5), 69.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 0), 69.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 5), 69.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 10), 69.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 15), 69.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 20), 70.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 25), 70.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 30), 70.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 35), 71.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 40), 71.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 45), 71.7, 0.21 + 1.2); + 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(55, 85), 74.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 90), 75.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 95), 75.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 100), 75.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 105), 74.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 110), 74.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 115), 73.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 120), 72.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 125), 72.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 130), 71.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 135), 70.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 140), 69.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 145), 68.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 150), 67.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 155), 67.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 160), 66.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 165), 66.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 170), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 175), 66.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(55, 180), 66.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -180), 70.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -175), 70.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -170), 71.0, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -165), 71.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -160), 71.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -155), 72.4, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -150), 73.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -145), 73.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -140), 74.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -135), 75.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -130), 76.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -125), 77.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -120), 78.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -115), 78.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -110), 79.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -105), 80.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -100), 80.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -95), 80.8, 0.21 + 1.2); + 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), 80.0, 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, -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, -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, -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); + EXPECT_NEAR(get_mag_inclination_degrees(60, -25), 72.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -20), 72.7, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -15), 72.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -10), 72.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, -5), 72.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 0), 72.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 5), 72.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 10), 72.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 15), 73.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 20), 73.3, 0.21 + 1.2); + 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, 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.6, 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, 80), 78.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 85), 78.4, 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, 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); + EXPECT_NEAR(get_mag_inclination_degrees(60, 125), 75.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 130), 75.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 135), 74.3, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 140), 73.5, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 145), 72.8, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 150), 72.1, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 155), 71.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 160), 71.2, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 165), 70.9, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 170), 70.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 175), 70.6, 0.21 + 1.2); + EXPECT_NEAR(get_mag_inclination_degrees(60, 180), 70.6, 0.21 + 1.2); } TEST(GeoLookupTest, strength) { - EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58412.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57264.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56103.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54938.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53774.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52615.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51460.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50302.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49130.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47928.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46678.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45362.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43969.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42492.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40936.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39313.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37649.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35977, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34335.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32769.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31322, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 30032.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28930.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 28028.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27325.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26800, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26420.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26145.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25935.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25757.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25587.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25415.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25244.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25088.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24975.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24941, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 25028.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25286.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25762.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26497, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27517.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28834.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30442.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32317.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34424.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36719.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39151.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41670.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44228.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46780.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49287.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51715.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54033.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56209.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58214.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60019.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61598.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62932, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64007.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64822, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65380.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65694.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65780.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65658, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65346.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64865.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64236.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63477.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62609.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61650.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60619.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59535.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58412.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56255.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55053.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53842.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52631.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51427.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50235.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49054.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47881.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46706.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45511.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44277.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42985.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41617.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40165.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38628.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 37015.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35349.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33662.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31999.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30411.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28952.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27673.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26614.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25795.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25212.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24840.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24631.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24533.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24491, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24461.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24415.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24338.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24231, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24103.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23980.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23896.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23902, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24055, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24420.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25058.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 26014.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27312.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28950.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30903, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33124.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35556.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38133.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40791.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43467.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46109.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48675, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51128.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53442.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55588.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57539.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59270.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60758.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 61987.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62950.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63650.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64098.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64308.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64301.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64094.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63707, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63156.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62461.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61637.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60704.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59680.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58584.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57437.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56255.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53912.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52682.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51447.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50214, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48989.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47779.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46585.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45407.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44237.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43061.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41862, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40618.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39312.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37932.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36471.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34932.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33331.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31697, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30073.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28517, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27092.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25862, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24876.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24161.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23715.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23502.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23464.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23536.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23654.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23772.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23859.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23901.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23894.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23844.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23762.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23677.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23632, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23689.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23926.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24425.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25254.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26456.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28044.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 29993.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32253.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34754.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37414.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40151.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42888.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45561.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48122.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50537.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52778.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54824, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56650.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58236.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59565.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60630.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61434.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 61988.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62310.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62419.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62331.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62064.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61630, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61042.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60314.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59459.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58494.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57437.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56307.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55126.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53912.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51412.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50180.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48945.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47714.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46491.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45282.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44090.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42919.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41765.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40619, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39466.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38292, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37077.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35807.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34469.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33058.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31580.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30055.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28527.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27054.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25707.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24560.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23671.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23072.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22758.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22688.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22796.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 23008.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23260.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23509.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23729.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23910, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24046.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24132.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24165.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24154.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24126.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24138.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24272.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24621.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25279.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26313.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27758, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29603.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31803.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34281.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 36942.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39686.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42419.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45066.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47571.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49896.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52016.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53910, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55557.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56943, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58059.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58911.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59518.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59903.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60092.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60105.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59954.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59646.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59188.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58587.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 150) * 1e9, 57852.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56992.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56022.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54958.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53820.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52631.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51412.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48770.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47562.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46355, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45152.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43957.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42774.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41607.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40462.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39340.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38239.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37150.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36061.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34959.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33826.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32645.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31400.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30086.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28718.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27331.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25985.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24756.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23721.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22944.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22458, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22257.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22299.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22514.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22828.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23182.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23541.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23889.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24223.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24535, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24808.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 25021.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25157.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25224.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25261.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25342, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25565.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 26037, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26850.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28066.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29703.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31731.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34077.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36639.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39303.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 41960.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44522.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46923.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49121, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51087.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52798.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54235.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55386.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56253.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56861.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57247.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57452.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57509.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57436.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57235.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56904.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56438.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55838.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55110.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54264.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53310, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52262.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51141.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49969.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48770.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46014.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44858, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43707.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42564.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41428.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40301.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39190.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38100.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37038.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 36005.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 35000.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 34016.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33043.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32064.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31058.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 30001.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28878.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27696.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26486.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25304.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24224.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23321.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22658, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22269.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22152.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22266.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22546.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22927.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23357.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23809.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24278.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24765.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25262.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25740.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26161.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26485.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26696.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26813.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26896.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 27033.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27334.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27907.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28841.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30186.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31941.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34050.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36411.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38904.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41407.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43821.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46072.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48112.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49905.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51423.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52639.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53544.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54152.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54506.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54667.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54694.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54628.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54482.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54246.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53901.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53434.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52842.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52130.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51308.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50386.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49375.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48294.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47165.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46014.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43203.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42129.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41067, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 40015.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38973.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37939.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36921, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35924.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34957.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 34025.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33130.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32272.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31443.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30629.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29805.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28943.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 28025, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27048.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 26038.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 25044.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24130.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23364.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22804.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22485.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22410.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22551.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22857.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23273.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23757.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24289.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24865.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25487.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26144, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26801.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27407.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27908, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28269.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28491.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28612.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28705.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28871.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29221.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29864.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30881.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32304.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34099.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36176.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38415.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40690.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42895.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 44952.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46807.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48416.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49739.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50743.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51416.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51781.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51900.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51854.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51722.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51550.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51344, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51080.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50726.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50259.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49675.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48982.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48191.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47313.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 165) * 1e9, 46357.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 170) * 1e9, 45338.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 175) * 1e9, 44279.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43203.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40446.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39485.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38542, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37614.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36699.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35795.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34907.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34044.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33212.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32417.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31665, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30955.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30285.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29641.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 29000.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28336, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27624.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26860.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26060.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25261.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24513.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23872.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23389.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23101.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23020.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23139, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -50) * 1e9, 23426.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23844.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24357.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24946.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25601.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26318.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27081.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27853.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28581, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29205.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29683.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 30000.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30174.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30258.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30334.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30510.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30901.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 35) * 1e9, 31610.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32696, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34148.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35896.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37825.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39815.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41760.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43584.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45229.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46644.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47780, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48593.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49067.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49231.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49157.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48945.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48685.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48427.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48170.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47880.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47514, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47042.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46463, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45786.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45030.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44204, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43317, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42381.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 175) * 1e9, 41417.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40446.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37897.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37078.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36281.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35505.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34745.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 34000.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33277.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32581.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31921, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31298.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30718.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30180.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29682.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29214.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28757, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28285.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27777.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27221.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26623.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 26006, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25403.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24860, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24418.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24116.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23980.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24024.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24243.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24617.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25120.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25726.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26415.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27171.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27970.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28777.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29545.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30220.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30761.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31143.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31369.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31470.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31509.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31578.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31792.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32261.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33058.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34192.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35608.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37208, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38882.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40537.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42100.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43517.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44733.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45691.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46341.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46661.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46681.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46476.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46152.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45802.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45475.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45169.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44843.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44451.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43963.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43379.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42716.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 41993, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41223.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40415.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39581.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 175) * 1e9, 38736.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37897.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35735.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35079.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34451.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33844, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33255.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32688.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32150.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31647.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31183.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30760.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30375.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30028.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29714.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29427.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29151.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28866.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28550.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28184.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27763.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27295.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26802.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26317.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25877.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25522.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25290.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25213.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25311.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25587.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26024.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26590.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27254.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 27983.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28750.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29520.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30257.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30918.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31470, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31885.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32156.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32296.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32348.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32388.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32519.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32844.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33437, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34314.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35436.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36724, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38086.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39446.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40744.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41927.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42944, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43735.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44250.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44463.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44399.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44128.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43747.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43342.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42957.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42591.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42207.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41765.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41241.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40639.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39979.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39284.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38570.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37848.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37127.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36419.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35735.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34118.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33637, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33183.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32751.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32339.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31954.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31606.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31305.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31051.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30842, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30668.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30523.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30403.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30300.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30205.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30097.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29953.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29747.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29464.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29102.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28674.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28206.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27734.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27298.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26943.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26714.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26653.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26780.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27091.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27554.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28129.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28774.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29454.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30140.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30802.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31412.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31943.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32371.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32682.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32878.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32988.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33071.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33210.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33493.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33978.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34681.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35571.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36591.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37675.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38765.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39814.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40777.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41606.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42247.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42653.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42799.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42702.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42417.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42021.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41583.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41139.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40691.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40218.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39693.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39105.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38463.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37791.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37112.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36447, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35806.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35201.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34638.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34118.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33142.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32827, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32541.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32276.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32032.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31821.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31659.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31556.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31512.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31519.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31561.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31625.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31704, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31789.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31870.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31927.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31931.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31852.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31666.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31365.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30959.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30472.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29941.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29406.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28917.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28529.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28295, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28247.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28391.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28703.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29141, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29659.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30223.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30804.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31381.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31933.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32439, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32875.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33226.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33491.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33690, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33867.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34086.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34407.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34868.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35474.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36202.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37015.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37873.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38740.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39580.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40357.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41027.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41543, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41865.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 41973.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41876.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41608.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41219.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40753.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40238.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39683.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39085.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38440.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37753.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37042.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36332.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35648.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35012.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34438.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 170) * 1e9, 33936.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 175) * 1e9, 33506.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33142.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32824.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32648.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32509.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32392.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32300.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32249.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32259.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32342.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32499.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32715, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32968.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33240.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33517.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33789.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 34040.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34249.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34380.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34399.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34276.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 34002.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33585.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 33052.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32443.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31802.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31185.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30650.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30254.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30038.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30011.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30156.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30436.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30812.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31252, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31732.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32237.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32746.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33238.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33690.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34085.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34423.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34721.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35015.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35347.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35750.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36238.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36804.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37431.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38100.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38795.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39498.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40186.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40826.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41380.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41808.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42077.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42171.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42090, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41847.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41466.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40966, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40365.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39681.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38930.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38132, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37311.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36497.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35717, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 34997.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34359.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33820.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 170) * 1e9, 33389.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 175) * 1e9, 33063, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32824.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33126.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33049.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33022.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33028.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33069, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33161.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33328.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33583.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33924.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34332.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34783.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35252.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35720, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36169.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36581.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36927.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37168.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37264.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37185.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36918, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36474, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35883.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35189.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34446.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33713.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33055.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32529, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32173.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 32001.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31996.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32130.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32371.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32698.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33093.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33544.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34029.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34523.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 34998.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35437.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35842.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36230.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36631.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37070.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37558.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38090.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38650.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39223.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39805.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40398.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41000.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41597.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42159.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42651.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43036.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43287.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43388.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43330.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43110.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42727, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42182.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41488.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40665, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39747.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38778.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37800.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36853.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35970.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35180.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34504.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33960.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33555.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 175) * 1e9, 33285, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33126.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33991.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33966.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34013, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34113.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34267.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34489.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34799.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35208.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35712.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36291.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36917.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37560.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38197.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38805.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39359.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39825.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40160.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40322.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40277.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 40013.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39542.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38897.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38127.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37294.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36466.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35711.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35087.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34629.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34346.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34224, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34238.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -25) * 1e9, 34368.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34600.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34926.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35334.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35804.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36303.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36801.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37278.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37734.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38187.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38663.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39176.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39725.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40293.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40859.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41412.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 41957.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42507.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43068.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43632.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44174.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44657, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45046.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45313.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45440.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45410.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45207.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44815.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44223.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43437.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42484, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41410, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40273.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39132.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38040.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37034.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36147.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35401, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34811.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34387.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34121.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33991.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35368.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35351.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35436.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35604.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35852.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36189.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36628.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37174.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37819.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38541.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39309.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40094.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40867, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41601.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42268.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42828.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43238.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43453.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43440.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43185, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42698.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 42015.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41189.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40290.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39391.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38565.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37868.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37333.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36966.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36754.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36676.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36716.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36870.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37134.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37501.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37949.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38444.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38951.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39447.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39932.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40420.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40932.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41480, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42056, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42642, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43218.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43778.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44329.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44887.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45461.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46045.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46614, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47131.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47561.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47872.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48041.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48043.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47855.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47451.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46817, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45956.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44899, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43698.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42422.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41140.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39913.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38786.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37794.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36960.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36299.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35819.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35515.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35368.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37223.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37184, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37277.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37489.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37811.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38246.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38796.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39458.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40217.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 41049, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41922, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42805.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43670.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44487.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45225.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45844.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46301.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46552.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46564.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46322.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45835.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45138.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44286.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43350.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42409.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41533.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40781.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40183.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39745.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39455.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39298.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39262.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39345.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39548.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39865.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40274.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40743.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41236.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41729.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42216.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42710.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43225.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43773.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44349, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44939.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45530.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46118.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46710.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47319.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 47950.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48594.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49225.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49806.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50299.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50669.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50885.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50921, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50745.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50334, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49671.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48762.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47639.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46359, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 44992.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43613.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42288.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41068.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 39987.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39070.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38333.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37782, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37416, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37223.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39524, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39449.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39535, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39768.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40142.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40650.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41285.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 42032.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42870.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43772, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44705.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45639.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46545.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47394.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48155.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48789.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49257.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49519, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49542.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49312.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48838.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48150.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47302.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46362.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45406.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44504.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43710, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43055.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42550.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42187.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41954.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41844.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41857, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 41992.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42244.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42594.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43011.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43463.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43927.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44393.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44868.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45366.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45895.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46458.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47050.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47663.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48297.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 48955.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49643.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50359.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51087.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51799.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52455.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53017.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53445.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53707.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53770.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53607.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53194.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52521.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51597.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50455.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49151.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47755.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46341.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44976, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43709.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42578.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41607.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40810.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40197.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39770, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39524, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42220.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42114.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42183.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42421.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42819.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43365, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 44042.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44830.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45701.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46624.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47567.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48500, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49394.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50223.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50957.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51562.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 52003.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52245.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52261.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 52039, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51583.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50923.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50106.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49192.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48250.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47344.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46525, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45824.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45257.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44822, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44514.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -25) * 1e9, 44329.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44267.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44326.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44499.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44771, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45114.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45502.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45914.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46340.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46784, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47254.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47762, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48314.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48913.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49557.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50247.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 50983, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51760.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52568.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53384.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54176, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54902.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55523, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 55998.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56293.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56378, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56226.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55822.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55161.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54256.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53143.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51874.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50516, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49135.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47794.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46542.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45415, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44434.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43617.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42972.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42505.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42220.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45209.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45085.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45137.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45362.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45750.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46288.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46956.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47728.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48573.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49460.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50358, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51236.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 52068.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52829.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53492.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 54029.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54410.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54608.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54600.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54378.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53946.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53328.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52563, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51701.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50801.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49918.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49096.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48369.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47753.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47256.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46879.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46621.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46481.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46458.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46543.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46723.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 46979.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47288.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47635.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48012.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48418.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48863.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49356.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49907.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50523.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51204.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 51951.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52757.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53613.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54498.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55384.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56234.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57009.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57666.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58169.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58483.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58582.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58445.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58064, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57439.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56591.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55554.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54376.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53116.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51833.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50583.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49408.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48341.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47405.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46614.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45979.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45509, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45209.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48318.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48190.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48223.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48415.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48761, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49244.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49845.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50540.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51298.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52089.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52885.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53658.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54383.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 55036.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55595.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 56036.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56336.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56474.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56435, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56211.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55807.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55243.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54548.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53762.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52932, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52100.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51306.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50580.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49943.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49406.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -30) * 1e9, 48975.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48653.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48441.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48337.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48334.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48422.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48588.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48816.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49096.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49422.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49795.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50221, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50708.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51267.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51902.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52616.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53405.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54259.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55162.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56088.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57006.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57879.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58667.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59331.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59838.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60156.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60266.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60152.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59811.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59253.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58498.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57579.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56540.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55430.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54299.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53193.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52148.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51194.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50349.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49629.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49045.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48605.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48318.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51309.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51183.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51192.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51336.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51607.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51995.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52482.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 53046.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53664.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54309.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54957, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55582.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56164.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56682.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57116.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57447.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57657.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57733, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57662.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57441, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57074.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56575.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55966.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55277.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54541.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53791.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53060.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52372.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51749.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51204.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50748.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50385.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50119.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49948.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49870.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49879, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49965.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50122.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50343.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50626.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 50970.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51382.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51867.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52431.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53077.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53804.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54605.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55466.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56369.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57286.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58186.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59034.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59795, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60433.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60920.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61232.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61352.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61273.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 60995.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60531.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59902.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59137.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58273.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57351, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56409, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55483.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54605.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53797.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53078.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52461, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51954.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51568.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51309.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53924.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53799.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53777.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53858.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 54037.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54304.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54648.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 55052, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55497.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55965.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56435.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56889.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57309.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57677.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57977.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58195.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58318.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58336.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58241.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 58031.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57710.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57286.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56777, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56200.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55579.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54939.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54303.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53692.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -40) * 1e9, 53124, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52612.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52167.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51798.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51509.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51301.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51176.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51130.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51162, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51267.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51444.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51692.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52013, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52408.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52881.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53433.8, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54064.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54769.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55538.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56356.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57204.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58056.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58884.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59660, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60352.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60933.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61381.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61677.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61810.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61777.2, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61580.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61232.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60752.6, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60164.3, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59496.7, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58780.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58044.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57317.1, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56621.4, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 55976.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55396.9, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54894, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54476.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54151.5, 145 + 500); - EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53924.4, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-50, -180) * 1e9, 58400, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-50, -175) * 1e9, 57251, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-50, -170) * 1e9, 56088, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(-50, -165) * 1e9, 54921, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-50, -160) * 1e9, 53756, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(-50, -155) * 1e9, 52596, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-50, -150) * 1e9, 51439, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-50, -145) * 1e9, 50280, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-50, -140) * 1e9, 49107, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-50, -135) * 1e9, 47904, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-50, -130) * 1e9, 46653, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-50, -125) * 1e9, 45337, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-50, -120) * 1e9, 43943, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-50, -115) * 1e9, 42466, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(-50, -110) * 1e9, 40909, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-50, -105) * 1e9, 39287, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-50, -100) * 1e9, 37623, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-50, -95) * 1e9, 35951, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-50, -90) * 1e9, 34310, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-50, -85) * 1e9, 32745, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-50, -80) * 1e9, 31299, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-50, -75) * 1e9, 30012, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-50, -70) * 1e9, 28911, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-50, -65) * 1e9, 28011, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-50, -60) * 1e9, 27309, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-50, -55) * 1e9, 26786, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-50, -50) * 1e9, 26407, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-50, -45) * 1e9, 26132, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-50, -40) * 1e9, 25923, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-50, -35) * 1e9, 25744, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, -30) * 1e9, 25573, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-50, -25) * 1e9, 25399, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-50, -20) * 1e9, 25227, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-50, -15) * 1e9, 25070, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-50, -10) * 1e9, 24956, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, -5) * 1e9, 24922, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-50, 0) * 1e9, 25010, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-50, 5) * 1e9, 25270, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-50, 10) * 1e9, 25748, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-50, 15) * 1e9, 26486, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-50, 20) * 1e9, 27511, 145 + 275); + EXPECT_NEAR(get_mag_strength_tesla(-50, 25) * 1e9, 28833, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-50, 30) * 1e9, 30445, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(-50, 35) * 1e9, 32325, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-50, 40) * 1e9, 34436, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-50, 45) * 1e9, 36734, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-50, 50) * 1e9, 39168, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-50, 55) * 1e9, 41689, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-50, 60) * 1e9, 44248, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-50, 65) * 1e9, 46800, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(-50, 70) * 1e9, 49307, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(-50, 75) * 1e9, 51734, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-50, 80) * 1e9, 54051, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-50, 85) * 1e9, 56226, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(-50, 90) * 1e9, 58230, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(-50, 95) * 1e9, 60034, 145 + 600); + EXPECT_NEAR(get_mag_strength_tesla(-50, 100) * 1e9, 61612, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 105) * 1e9, 62944, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-50, 110) * 1e9, 64018, 145 + 640); + EXPECT_NEAR(get_mag_strength_tesla(-50, 115) * 1e9, 64831, 145 + 648); + EXPECT_NEAR(get_mag_strength_tesla(-50, 120) * 1e9, 65388, 145 + 654); + EXPECT_NEAR(get_mag_strength_tesla(-50, 125) * 1e9, 65700, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 130) * 1e9, 65785, 145 + 658); + EXPECT_NEAR(get_mag_strength_tesla(-50, 135) * 1e9, 65660, 145 + 657); + EXPECT_NEAR(get_mag_strength_tesla(-50, 140) * 1e9, 65347, 145 + 653); + EXPECT_NEAR(get_mag_strength_tesla(-50, 145) * 1e9, 64865, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-50, 150) * 1e9, 64234, 145 + 642); + EXPECT_NEAR(get_mag_strength_tesla(-50, 155) * 1e9, 63474, 145 + 635); + EXPECT_NEAR(get_mag_strength_tesla(-50, 160) * 1e9, 62604, 145 + 626); + EXPECT_NEAR(get_mag_strength_tesla(-50, 165) * 1e9, 61644, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(-50, 170) * 1e9, 60612, 145 + 606); + EXPECT_NEAR(get_mag_strength_tesla(-50, 175) * 1e9, 59525, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(-50, 180) * 1e9, 58400, 145 + 584); + EXPECT_NEAR(get_mag_strength_tesla(-45, -180) * 1e9, 56245, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-45, -175) * 1e9, 55042, 145 + 936); + EXPECT_NEAR(get_mag_strength_tesla(-45, -170) * 1e9, 53829, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-45, -165) * 1e9, 52617, 145 + 894); + EXPECT_NEAR(get_mag_strength_tesla(-45, -160) * 1e9, 51411, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-45, -155) * 1e9, 50217, 145 + 854); + EXPECT_NEAR(get_mag_strength_tesla(-45, -150) * 1e9, 49035, 145 + 834); + EXPECT_NEAR(get_mag_strength_tesla(-45, -145) * 1e9, 47862, 145 + 814); + EXPECT_NEAR(get_mag_strength_tesla(-45, -140) * 1e9, 46685, 145 + 794); + EXPECT_NEAR(get_mag_strength_tesla(-45, -135) * 1e9, 45489, 145 + 773); + EXPECT_NEAR(get_mag_strength_tesla(-45, -130) * 1e9, 44255, 145 + 752); + EXPECT_NEAR(get_mag_strength_tesla(-45, -125) * 1e9, 42962, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-45, -120) * 1e9, 41594, 145 + 707); + EXPECT_NEAR(get_mag_strength_tesla(-45, -115) * 1e9, 40141, 145 + 682); + EXPECT_NEAR(get_mag_strength_tesla(-45, -110) * 1e9, 38603, 145 + 656); + EXPECT_NEAR(get_mag_strength_tesla(-45, -105) * 1e9, 36990, 145 + 629); + EXPECT_NEAR(get_mag_strength_tesla(-45, -100) * 1e9, 35324, 145 + 601); + EXPECT_NEAR(get_mag_strength_tesla(-45, -95) * 1e9, 33638, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-45, -90) * 1e9, 31976, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(-45, -85) * 1e9, 30389, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-45, -80) * 1e9, 28931, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, -75) * 1e9, 27654, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-45, -70) * 1e9, 26597, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-45, -65) * 1e9, 25780, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(-45, -60) * 1e9, 25199, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-45, -55) * 1e9, 24828, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-45, -50) * 1e9, 24621, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(-45, -45) * 1e9, 24523, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-45, -40) * 1e9, 24480, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -35) * 1e9, 24449, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-45, -30) * 1e9, 24402, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, -25) * 1e9, 24323, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(-45, -20) * 1e9, 24214, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-45, -15) * 1e9, 24084, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-45, -10) * 1e9, 23960, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-45, -5) * 1e9, 23876, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-45, 0) * 1e9, 23881, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-45, 5) * 1e9, 24035, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-45, 10) * 1e9, 24403, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-45, 15) * 1e9, 25045, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-45, 20) * 1e9, 26007, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-45, 25) * 1e9, 27310, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-45, 30) * 1e9, 28955, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-45, 35) * 1e9, 30912, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(-45, 40) * 1e9, 33138, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-45, 45) * 1e9, 35574, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(-45, 50) * 1e9, 38155, 145 + 649); + EXPECT_NEAR(get_mag_strength_tesla(-45, 55) * 1e9, 40814, 145 + 694); + EXPECT_NEAR(get_mag_strength_tesla(-45, 60) * 1e9, 43491, 145 + 739); + EXPECT_NEAR(get_mag_strength_tesla(-45, 65) * 1e9, 46133, 145 + 784); + EXPECT_NEAR(get_mag_strength_tesla(-45, 70) * 1e9, 48697, 145 + 828); + EXPECT_NEAR(get_mag_strength_tesla(-45, 75) * 1e9, 51149, 145 + 870); + EXPECT_NEAR(get_mag_strength_tesla(-45, 80) * 1e9, 53461, 145 + 909); + EXPECT_NEAR(get_mag_strength_tesla(-45, 85) * 1e9, 55606, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-45, 90) * 1e9, 57556, 145 + 978); + EXPECT_NEAR(get_mag_strength_tesla(-45, 95) * 1e9, 59285, 145 + 1008); + EXPECT_NEAR(get_mag_strength_tesla(-45, 100) * 1e9, 60772, 145 + 1033); + EXPECT_NEAR(get_mag_strength_tesla(-45, 105) * 1e9, 62000, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-45, 110) * 1e9, 62961, 145 + 1070); + EXPECT_NEAR(get_mag_strength_tesla(-45, 115) * 1e9, 63660, 145 + 1082); + EXPECT_NEAR(get_mag_strength_tesla(-45, 120) * 1e9, 64106, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 125) * 1e9, 64315, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 130) * 1e9, 64306, 145 + 1093); + EXPECT_NEAR(get_mag_strength_tesla(-45, 135) * 1e9, 64098, 145 + 1090); + EXPECT_NEAR(get_mag_strength_tesla(-45, 140) * 1e9, 63709, 145 + 1083); + EXPECT_NEAR(get_mag_strength_tesla(-45, 145) * 1e9, 63157, 145 + 1074); + EXPECT_NEAR(get_mag_strength_tesla(-45, 150) * 1e9, 62460, 145 + 1062); + EXPECT_NEAR(get_mag_strength_tesla(-45, 155) * 1e9, 61635, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-45, 160) * 1e9, 60700, 145 + 1032); + EXPECT_NEAR(get_mag_strength_tesla(-45, 165) * 1e9, 59675, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-45, 170) * 1e9, 58578, 145 + 996); + EXPECT_NEAR(get_mag_strength_tesla(-45, 175) * 1e9, 57429, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-45, 180) * 1e9, 56245, 145 + 956); + EXPECT_NEAR(get_mag_strength_tesla(-40, -180) * 1e9, 53904, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-40, -175) * 1e9, 52673, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-40, -170) * 1e9, 51436, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-40, -165) * 1e9, 50201, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-40, -160) * 1e9, 48975, 145 + 833); + EXPECT_NEAR(get_mag_strength_tesla(-40, -155) * 1e9, 47763, 145 + 812); + EXPECT_NEAR(get_mag_strength_tesla(-40, -150) * 1e9, 46568, 145 + 792); + EXPECT_NEAR(get_mag_strength_tesla(-40, -145) * 1e9, 45389, 145 + 772); + EXPECT_NEAR(get_mag_strength_tesla(-40, -140) * 1e9, 44218, 145 + 752); + EXPECT_NEAR(get_mag_strength_tesla(-40, -135) * 1e9, 43042, 145 + 732); + EXPECT_NEAR(get_mag_strength_tesla(-40, -130) * 1e9, 41841, 145 + 711); + EXPECT_NEAR(get_mag_strength_tesla(-40, -125) * 1e9, 40597, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-40, -120) * 1e9, 39291, 145 + 668); + EXPECT_NEAR(get_mag_strength_tesla(-40, -115) * 1e9, 37910, 145 + 644); + EXPECT_NEAR(get_mag_strength_tesla(-40, -110) * 1e9, 36449, 145 + 620); + EXPECT_NEAR(get_mag_strength_tesla(-40, -105) * 1e9, 34910, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(-40, -100) * 1e9, 33308, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(-40, -95) * 1e9, 31674, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(-40, -90) * 1e9, 30051, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-40, -85) * 1e9, 28496, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-40, -80) * 1e9, 27072, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-40, -75) * 1e9, 25844, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-40, -70) * 1e9, 24860, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(-40, -65) * 1e9, 24148, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-40, -60) * 1e9, 23704, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-40, -55) * 1e9, 23493, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -50) * 1e9, 23456, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(-40, -45) * 1e9, 23528, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-40, -40) * 1e9, 23646, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, -35) * 1e9, 23762, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -30) * 1e9, 23847, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -25) * 1e9, 23887, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, -20) * 1e9, 23878, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, -15) * 1e9, 23825, 145 + 405); + EXPECT_NEAR(get_mag_strength_tesla(-40, -10) * 1e9, 23742, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-40, -5) * 1e9, 23655, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 0) * 1e9, 23609, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(-40, 5) * 1e9, 23667, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-40, 10) * 1e9, 23906, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-40, 15) * 1e9, 24409, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(-40, 20) * 1e9, 25244, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-40, 25) * 1e9, 26454, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-40, 30) * 1e9, 28048, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(-40, 35) * 1e9, 30004, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(-40, 40) * 1e9, 32270, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(-40, 45) * 1e9, 34776, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(-40, 50) * 1e9, 37439, 145 + 636); + EXPECT_NEAR(get_mag_strength_tesla(-40, 55) * 1e9, 40178, 145 + 683); + EXPECT_NEAR(get_mag_strength_tesla(-40, 60) * 1e9, 42915, 145 + 730); + EXPECT_NEAR(get_mag_strength_tesla(-40, 65) * 1e9, 45588, 145 + 775); + EXPECT_NEAR(get_mag_strength_tesla(-40, 70) * 1e9, 48147, 145 + 819); + EXPECT_NEAR(get_mag_strength_tesla(-40, 75) * 1e9, 50560, 145 + 860); + EXPECT_NEAR(get_mag_strength_tesla(-40, 80) * 1e9, 52798, 145 + 898); + EXPECT_NEAR(get_mag_strength_tesla(-40, 85) * 1e9, 54841, 145 + 932); + EXPECT_NEAR(get_mag_strength_tesla(-40, 90) * 1e9, 56666, 145 + 963); + EXPECT_NEAR(get_mag_strength_tesla(-40, 95) * 1e9, 58250, 145 + 990); + EXPECT_NEAR(get_mag_strength_tesla(-40, 100) * 1e9, 59578, 145 + 1013); + EXPECT_NEAR(get_mag_strength_tesla(-40, 105) * 1e9, 60642, 145 + 1031); + EXPECT_NEAR(get_mag_strength_tesla(-40, 110) * 1e9, 61444, 145 + 1045); + EXPECT_NEAR(get_mag_strength_tesla(-40, 115) * 1e9, 61997, 145 + 1054); + EXPECT_NEAR(get_mag_strength_tesla(-40, 120) * 1e9, 62318, 145 + 1059); + EXPECT_NEAR(get_mag_strength_tesla(-40, 125) * 1e9, 62425, 145 + 1061); + EXPECT_NEAR(get_mag_strength_tesla(-40, 130) * 1e9, 62336, 145 + 1060); + EXPECT_NEAR(get_mag_strength_tesla(-40, 135) * 1e9, 62068, 145 + 1055); + EXPECT_NEAR(get_mag_strength_tesla(-40, 140) * 1e9, 61632, 145 + 1048); + EXPECT_NEAR(get_mag_strength_tesla(-40, 145) * 1e9, 61043, 145 + 1038); + EXPECT_NEAR(get_mag_strength_tesla(-40, 150) * 1e9, 60314, 145 + 1025); + EXPECT_NEAR(get_mag_strength_tesla(-40, 155) * 1e9, 59458, 145 + 1011); + EXPECT_NEAR(get_mag_strength_tesla(-40, 160) * 1e9, 58491, 145 + 994); + EXPECT_NEAR(get_mag_strength_tesla(-40, 165) * 1e9, 57432, 145 + 976); + EXPECT_NEAR(get_mag_strength_tesla(-40, 170) * 1e9, 56302, 145 + 957); + EXPECT_NEAR(get_mag_strength_tesla(-40, 175) * 1e9, 55119, 145 + 937); + EXPECT_NEAR(get_mag_strength_tesla(-40, 180) * 1e9, 53904, 145 + 916); + EXPECT_NEAR(get_mag_strength_tesla(-35, -180) * 1e9, 51406, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-35, -175) * 1e9, 50172, 145 + 853); + EXPECT_NEAR(get_mag_strength_tesla(-35, -170) * 1e9, 48936, 145 + 832); + EXPECT_NEAR(get_mag_strength_tesla(-35, -165) * 1e9, 47703, 145 + 811); + EXPECT_NEAR(get_mag_strength_tesla(-35, -160) * 1e9, 46479, 145 + 790); + EXPECT_NEAR(get_mag_strength_tesla(-35, -155) * 1e9, 45268, 145 + 770); + EXPECT_NEAR(get_mag_strength_tesla(-35, -150) * 1e9, 44075, 145 + 749); + EXPECT_NEAR(get_mag_strength_tesla(-35, -145) * 1e9, 42903, 145 + 729); + EXPECT_NEAR(get_mag_strength_tesla(-35, -140) * 1e9, 41748, 145 + 710); + EXPECT_NEAR(get_mag_strength_tesla(-35, -135) * 1e9, 40601, 145 + 690); + EXPECT_NEAR(get_mag_strength_tesla(-35, -130) * 1e9, 39448, 145 + 671); + EXPECT_NEAR(get_mag_strength_tesla(-35, -125) * 1e9, 38273, 145 + 651); + EXPECT_NEAR(get_mag_strength_tesla(-35, -120) * 1e9, 37058, 145 + 630); + EXPECT_NEAR(get_mag_strength_tesla(-35, -115) * 1e9, 35787, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(-35, -110) * 1e9, 34449, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(-35, -105) * 1e9, 33038, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(-35, -100) * 1e9, 31559, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(-35, -95) * 1e9, 30035, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-35, -90) * 1e9, 28507, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(-35, -85) * 1e9, 27034, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-35, -80) * 1e9, 25689, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-35, -75) * 1e9, 24544, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(-35, -70) * 1e9, 23657, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(-35, -65) * 1e9, 23061, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-35, -60) * 1e9, 22749, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -55) * 1e9, 22681, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-35, -50) * 1e9, 22790, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-35, -45) * 1e9, 23002, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(-35, -40) * 1e9, 23254, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-35, -35) * 1e9, 23501, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-35, -30) * 1e9, 23719, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-35, -25) * 1e9, 23898, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-35, -20) * 1e9, 24032, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(-35, -15) * 1e9, 24116, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -10) * 1e9, 24147, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, -5) * 1e9, 24133, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 0) * 1e9, 24103, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 5) * 1e9, 24115, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(-35, 10) * 1e9, 24250, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-35, 15) * 1e9, 24603, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-35, 20) * 1e9, 25267, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-35, 25) * 1e9, 26309, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(-35, 30) * 1e9, 27762, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-35, 35) * 1e9, 29615, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-35, 40) * 1e9, 31822, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(-35, 45) * 1e9, 34306, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(-35, 50) * 1e9, 36970, 145 + 628); + EXPECT_NEAR(get_mag_strength_tesla(-35, 55) * 1e9, 39716, 145 + 675); + EXPECT_NEAR(get_mag_strength_tesla(-35, 60) * 1e9, 42450, 145 + 722); + EXPECT_NEAR(get_mag_strength_tesla(-35, 65) * 1e9, 45095, 145 + 767); + EXPECT_NEAR(get_mag_strength_tesla(-35, 70) * 1e9, 47598, 145 + 809); + EXPECT_NEAR(get_mag_strength_tesla(-35, 75) * 1e9, 49920, 145 + 849); + EXPECT_NEAR(get_mag_strength_tesla(-35, 80) * 1e9, 52037, 145 + 885); + EXPECT_NEAR(get_mag_strength_tesla(-35, 85) * 1e9, 53927, 145 + 917); + EXPECT_NEAR(get_mag_strength_tesla(-35, 90) * 1e9, 55572, 145 + 945); + EXPECT_NEAR(get_mag_strength_tesla(-35, 95) * 1e9, 56955, 145 + 968); + EXPECT_NEAR(get_mag_strength_tesla(-35, 100) * 1e9, 58070, 145 + 987); + EXPECT_NEAR(get_mag_strength_tesla(-35, 105) * 1e9, 58922, 145 + 1002); + EXPECT_NEAR(get_mag_strength_tesla(-35, 110) * 1e9, 59527, 145 + 1012); + EXPECT_NEAR(get_mag_strength_tesla(-35, 115) * 1e9, 59911, 145 + 1018); + EXPECT_NEAR(get_mag_strength_tesla(-35, 120) * 1e9, 60100, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 125) * 1e9, 60111, 145 + 1022); + EXPECT_NEAR(get_mag_strength_tesla(-35, 130) * 1e9, 59959, 145 + 1019); + EXPECT_NEAR(get_mag_strength_tesla(-35, 135) * 1e9, 59650, 145 + 1014); + EXPECT_NEAR(get_mag_strength_tesla(-35, 140) * 1e9, 59191, 145 + 1006); + EXPECT_NEAR(get_mag_strength_tesla(-35, 145) * 1e9, 58589, 145 + 996); + EXPECT_NEAR(get_mag_strength_tesla(-35, 150) * 1e9, 57852, 145 + 983); + EXPECT_NEAR(get_mag_strength_tesla(-35, 155) * 1e9, 56992, 145 + 969); + EXPECT_NEAR(get_mag_strength_tesla(-35, 160) * 1e9, 56020, 145 + 952); + EXPECT_NEAR(get_mag_strength_tesla(-35, 165) * 1e9, 54955, 145 + 934); + EXPECT_NEAR(get_mag_strength_tesla(-35, 170) * 1e9, 53816, 145 + 915); + EXPECT_NEAR(get_mag_strength_tesla(-35, 175) * 1e9, 52626, 145 + 895); + EXPECT_NEAR(get_mag_strength_tesla(-35, 180) * 1e9, 51406, 145 + 874); + EXPECT_NEAR(get_mag_strength_tesla(-30, -180) * 1e9, 48765, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-30, -175) * 1e9, 47556, 145 + 476); + EXPECT_NEAR(get_mag_strength_tesla(-30, -170) * 1e9, 46346, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(-30, -165) * 1e9, 45143, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(-30, -160) * 1e9, 43946, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-30, -155) * 1e9, 42761, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(-30, -150) * 1e9, 41593, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(-30, -145) * 1e9, 40447, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-30, -140) * 1e9, 39325, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, -135) * 1e9, 38223, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(-30, -130) * 1e9, 37133, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-30, -125) * 1e9, 36044, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-30, -120) * 1e9, 34942, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-30, -115) * 1e9, 33808, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-30, -110) * 1e9, 32627, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-30, -105) * 1e9, 31382, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-30, -100) * 1e9, 30068, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(-30, -95) * 1e9, 28699, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-30, -90) * 1e9, 27312, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-30, -85) * 1e9, 25966, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, -80) * 1e9, 24739, 145 + 247); + EXPECT_NEAR(get_mag_strength_tesla(-30, -75) * 1e9, 23705, 145 + 237); + EXPECT_NEAR(get_mag_strength_tesla(-30, -70) * 1e9, 22930, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-30, -65) * 1e9, 22447, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-30, -60) * 1e9, 22250, 145 + 222); + EXPECT_NEAR(get_mag_strength_tesla(-30, -55) * 1e9, 22294, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-30, -50) * 1e9, 22510, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-30, -45) * 1e9, 22825, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-30, -40) * 1e9, 23178, 145 + 232); + EXPECT_NEAR(get_mag_strength_tesla(-30, -35) * 1e9, 23536, 145 + 235); + EXPECT_NEAR(get_mag_strength_tesla(-30, -30) * 1e9, 23883, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-30, -25) * 1e9, 24215, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-30, -20) * 1e9, 24525, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-30, -15) * 1e9, 24796, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-30, -10) * 1e9, 25006, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-30, -5) * 1e9, 25139, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-30, 0) * 1e9, 25203, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 5) * 1e9, 25238, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-30, 10) * 1e9, 25318, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-30, 15) * 1e9, 25544, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-30, 20) * 1e9, 26022, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-30, 25) * 1e9, 26843, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-30, 30) * 1e9, 28068, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(-30, 35) * 1e9, 29714, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-30, 40) * 1e9, 31750, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(-30, 45) * 1e9, 34102, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-30, 50) * 1e9, 36668, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-30, 55) * 1e9, 39335, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-30, 60) * 1e9, 41993, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-30, 65) * 1e9, 44553, 145 + 446); + EXPECT_NEAR(get_mag_strength_tesla(-30, 70) * 1e9, 46951, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-30, 75) * 1e9, 49145, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-30, 80) * 1e9, 51108, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 85) * 1e9, 52815, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(-30, 90) * 1e9, 54249, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-30, 95) * 1e9, 55397, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(-30, 100) * 1e9, 56263, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(-30, 105) * 1e9, 56870, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 110) * 1e9, 57255, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(-30, 115) * 1e9, 57459, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 120) * 1e9, 57515, 145 + 575); + EXPECT_NEAR(get_mag_strength_tesla(-30, 125) * 1e9, 57441, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(-30, 130) * 1e9, 57240, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(-30, 135) * 1e9, 56908, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(-30, 140) * 1e9, 56441, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(-30, 145) * 1e9, 55840, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(-30, 150) * 1e9, 55112, 145 + 551); + EXPECT_NEAR(get_mag_strength_tesla(-30, 155) * 1e9, 54264, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(-30, 160) * 1e9, 53309, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(-30, 165) * 1e9, 52260, 145 + 523); + EXPECT_NEAR(get_mag_strength_tesla(-30, 170) * 1e9, 51138, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-30, 175) * 1e9, 49965, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(-30, 180) * 1e9, 48765, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(-25, -180) * 1e9, 46010, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-25, -175) * 1e9, 44852, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(-25, -170) * 1e9, 43700, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-25, -165) * 1e9, 42555, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-25, -160) * 1e9, 41418, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, -155) * 1e9, 40290, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(-25, -150) * 1e9, 39177, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(-25, -145) * 1e9, 38087, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-25, -140) * 1e9, 37025, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(-25, -135) * 1e9, 35992, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(-25, -130) * 1e9, 34986, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(-25, -125) * 1e9, 34001, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-25, -120) * 1e9, 33028, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(-25, -115) * 1e9, 32049, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, -110) * 1e9, 31042, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(-25, -105) * 1e9, 29984, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-25, -100) * 1e9, 28861, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-25, -95) * 1e9, 27678, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-25, -90) * 1e9, 26467, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-25, -85) * 1e9, 25286, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-25, -80) * 1e9, 24206, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-25, -75) * 1e9, 23304, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-25, -70) * 1e9, 22644, 145 + 226); + EXPECT_NEAR(get_mag_strength_tesla(-25, -65) * 1e9, 22259, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-25, -60) * 1e9, 22145, 145 + 221); + EXPECT_NEAR(get_mag_strength_tesla(-25, -55) * 1e9, 22262, 145 + 223); + EXPECT_NEAR(get_mag_strength_tesla(-25, -50) * 1e9, 22544, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-25, -45) * 1e9, 22926, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-25, -40) * 1e9, 23356, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-25, -35) * 1e9, 23808, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-25, -30) * 1e9, 24276, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-25, -25) * 1e9, 24762, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-25, -20) * 1e9, 25257, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-25, -15) * 1e9, 25734, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-25, -10) * 1e9, 26151, 145 + 262); + EXPECT_NEAR(get_mag_strength_tesla(-25, -5) * 1e9, 26470, 145 + 265); + EXPECT_NEAR(get_mag_strength_tesla(-25, 0) * 1e9, 26678, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(-25, 5) * 1e9, 26792, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-25, 10) * 1e9, 26873, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(-25, 15) * 1e9, 27012, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-25, 20) * 1e9, 27318, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-25, 25) * 1e9, 27897, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-25, 30) * 1e9, 28840, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-25, 35) * 1e9, 30194, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-25, 40) * 1e9, 31958, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(-25, 45) * 1e9, 34073, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-25, 50) * 1e9, 36440, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(-25, 55) * 1e9, 38935, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-25, 60) * 1e9, 41439, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-25, 65) * 1e9, 43852, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(-25, 70) * 1e9, 46101, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(-25, 75) * 1e9, 48137, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(-25, 80) * 1e9, 49926, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(-25, 85) * 1e9, 51439, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-25, 90) * 1e9, 52651, 145 + 527); + EXPECT_NEAR(get_mag_strength_tesla(-25, 95) * 1e9, 53553, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(-25, 100) * 1e9, 54160, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 105) * 1e9, 54513, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 110) * 1e9, 54673, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 115) * 1e9, 54699, 145 + 547); + EXPECT_NEAR(get_mag_strength_tesla(-25, 120) * 1e9, 54633, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(-25, 125) * 1e9, 54486, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(-25, 130) * 1e9, 54250, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(-25, 135) * 1e9, 53904, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(-25, 140) * 1e9, 53437, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(-25, 145) * 1e9, 52844, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(-25, 150) * 1e9, 52132, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(-25, 155) * 1e9, 51309, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(-25, 160) * 1e9, 50386, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(-25, 165) * 1e9, 49375, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(-25, 170) * 1e9, 48292, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(-25, 175) * 1e9, 47163, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(-25, 180) * 1e9, 46010, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(-20, -180) * 1e9, 43200, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-20, -175) * 1e9, 42125, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-20, -170) * 1e9, 41060, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(-20, -165) * 1e9, 40007, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-20, -160) * 1e9, 38963, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(-20, -155) * 1e9, 37929, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-20, -150) * 1e9, 36909, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(-20, -145) * 1e9, 35912, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-20, -140) * 1e9, 34945, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-20, -135) * 1e9, 34013, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-20, -130) * 1e9, 33118, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-20, -125) * 1e9, 32259, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-20, -120) * 1e9, 31430, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-20, -115) * 1e9, 30615, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(-20, -110) * 1e9, 29790, 145 + 298); + EXPECT_NEAR(get_mag_strength_tesla(-20, -105) * 1e9, 28928, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, -100) * 1e9, 28008, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-20, -95) * 1e9, 27030, 145 + 270); + EXPECT_NEAR(get_mag_strength_tesla(-20, -90) * 1e9, 26019, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-20, -85) * 1e9, 25025, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-20, -80) * 1e9, 24111, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-20, -75) * 1e9, 23347, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -70) * 1e9, 22790, 145 + 228); + EXPECT_NEAR(get_mag_strength_tesla(-20, -65) * 1e9, 22475, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-20, -60) * 1e9, 22404, 145 + 224); + EXPECT_NEAR(get_mag_strength_tesla(-20, -55) * 1e9, 22548, 145 + 225); + EXPECT_NEAR(get_mag_strength_tesla(-20, -50) * 1e9, 22857, 145 + 229); + EXPECT_NEAR(get_mag_strength_tesla(-20, -45) * 1e9, 23275, 145 + 233); + EXPECT_NEAR(get_mag_strength_tesla(-20, -40) * 1e9, 23760, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-20, -35) * 1e9, 24291, 145 + 243); + EXPECT_NEAR(get_mag_strength_tesla(-20, -30) * 1e9, 24867, 145 + 249); + EXPECT_NEAR(get_mag_strength_tesla(-20, -25) * 1e9, 25489, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-20, -20) * 1e9, 26144, 145 + 261); + EXPECT_NEAR(get_mag_strength_tesla(-20, -15) * 1e9, 26799, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-20, -10) * 1e9, 27402, 145 + 274); + EXPECT_NEAR(get_mag_strength_tesla(-20, -5) * 1e9, 27898, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-20, 0) * 1e9, 28255, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-20, 5) * 1e9, 28473, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-20, 10) * 1e9, 28592, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-20, 15) * 1e9, 28685, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-20, 20) * 1e9, 28854, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-20, 25) * 1e9, 29209, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-20, 30) * 1e9, 29860, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(-20, 35) * 1e9, 30886, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-20, 40) * 1e9, 32317, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-20, 45) * 1e9, 34119, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(-20, 50) * 1e9, 36202, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(-20, 55) * 1e9, 38444, 145 + 384); + EXPECT_NEAR(get_mag_strength_tesla(-20, 60) * 1e9, 40721, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(-20, 65) * 1e9, 42925, 145 + 429); + EXPECT_NEAR(get_mag_strength_tesla(-20, 70) * 1e9, 44980, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-20, 75) * 1e9, 46832, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(-20, 80) * 1e9, 48436, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-20, 85) * 1e9, 49754, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(-20, 90) * 1e9, 50754, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(-20, 95) * 1e9, 51424, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(-20, 100) * 1e9, 51787, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(-20, 105) * 1e9, 51904, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 110) * 1e9, 51858, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(-20, 115) * 1e9, 51726, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(-20, 120) * 1e9, 51553, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(-20, 125) * 1e9, 51347, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(-20, 130) * 1e9, 51083, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(-20, 135) * 1e9, 50730, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(-20, 140) * 1e9, 50263, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(-20, 145) * 1e9, 49679, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(-20, 150) * 1e9, 48985, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(-20, 155) * 1e9, 48194, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-20, 160) * 1e9, 47314, 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, 44278, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-20, 180) * 1e9, 43200, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(-15, -180) * 1e9, 40444, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-15, -175) * 1e9, 39482, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-15, -170) * 1e9, 38536, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(-15, -165) * 1e9, 37607, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(-15, -160) * 1e9, 36690, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-15, -155) * 1e9, 35785, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(-15, -150) * 1e9, 34897, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(-15, -145) * 1e9, 34033, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-15, -140) * 1e9, 33201, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-15, -135) * 1e9, 32406, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-15, -130) * 1e9, 31654, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(-15, -125) * 1e9, 30944, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, -120) * 1e9, 30273, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, -115) * 1e9, 29628, 145 + 296); + EXPECT_NEAR(get_mag_strength_tesla(-15, -110) * 1e9, 28987, 145 + 290); + EXPECT_NEAR(get_mag_strength_tesla(-15, -105) * 1e9, 28321, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-15, -100) * 1e9, 27608, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(-15, -95) * 1e9, 26843, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-15, -90) * 1e9, 26041, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-15, -85) * 1e9, 25241, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-15, -80) * 1e9, 24494, 145 + 245); + EXPECT_NEAR(get_mag_strength_tesla(-15, -75) * 1e9, 23854, 145 + 239); + EXPECT_NEAR(get_mag_strength_tesla(-15, -70) * 1e9, 23374, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-15, -65) * 1e9, 23089, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -60) * 1e9, 23013, 145 + 230); + EXPECT_NEAR(get_mag_strength_tesla(-15, -55) * 1e9, 23135, 145 + 231); + EXPECT_NEAR(get_mag_strength_tesla(-15, -50) * 1e9, 23426, 145 + 234); + EXPECT_NEAR(get_mag_strength_tesla(-15, -45) * 1e9, 23847, 145 + 238); + EXPECT_NEAR(get_mag_strength_tesla(-15, -40) * 1e9, 24362, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-15, -35) * 1e9, 24952, 145 + 250); + EXPECT_NEAR(get_mag_strength_tesla(-15, -30) * 1e9, 25607, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-15, -25) * 1e9, 26324, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-15, -20) * 1e9, 27086, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(-15, -15) * 1e9, 27856, 145 + 279); + EXPECT_NEAR(get_mag_strength_tesla(-15, -10) * 1e9, 28580, 145 + 286); + EXPECT_NEAR(get_mag_strength_tesla(-15, -5) * 1e9, 29200, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-15, 0) * 1e9, 29674, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-15, 5) * 1e9, 29986, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-15, 10) * 1e9, 30157, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 15) * 1e9, 30240, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-15, 20) * 1e9, 30318, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-15, 25) * 1e9, 30497, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(-15, 30) * 1e9, 30894, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-15, 35) * 1e9, 31611, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-15, 40) * 1e9, 32704, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-15, 45) * 1e9, 34164, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-15, 50) * 1e9, 35918, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(-15, 55) * 1e9, 37851, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-15, 60) * 1e9, 39842, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(-15, 65) * 1e9, 41788, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-15, 70) * 1e9, 43610, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(-15, 75) * 1e9, 45252, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(-15, 80) * 1e9, 46663, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-15, 85) * 1e9, 47794, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(-15, 90) * 1e9, 48603, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(-15, 95) * 1e9, 49074, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(-15, 100) * 1e9, 49235, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 105) * 1e9, 49160, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(-15, 110) * 1e9, 48948, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(-15, 115) * 1e9, 48688, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(-15, 120) * 1e9, 48429, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(-15, 125) * 1e9, 48173, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(-15, 130) * 1e9, 47884, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(-15, 135) * 1e9, 47518, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(-15, 140) * 1e9, 47047, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(-15, 145) * 1e9, 46467, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-15, 150) * 1e9, 45791, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-15, 155) * 1e9, 45033, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(-15, 160) * 1e9, 44206, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(-15, 165) * 1e9, 43319, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(-15, 170) * 1e9, 42382, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(-15, 175) * 1e9, 41417, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(-15, 180) * 1e9, 40444, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, -180) * 1e9, 37896, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-10, -175) * 1e9, 37075, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-10, -170) * 1e9, 36277, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(-10, -165) * 1e9, 35499, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-10, -160) * 1e9, 34737, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(-10, -155) * 1e9, 33992, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(-10, -150) * 1e9, 33268, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(-10, -145) * 1e9, 32572, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(-10, -140) * 1e9, 31911, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-10, -135) * 1e9, 31289, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(-10, -130) * 1e9, 30708, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(-10, -125) * 1e9, 30170, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, -120) * 1e9, 29671, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-10, -115) * 1e9, 29202, 145 + 292); + EXPECT_NEAR(get_mag_strength_tesla(-10, -110) * 1e9, 28743, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(-10, -105) * 1e9, 28270, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(-10, -100) * 1e9, 27760, 145 + 278); + EXPECT_NEAR(get_mag_strength_tesla(-10, -95) * 1e9, 27203, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -90) * 1e9, 26604, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-10, -85) * 1e9, 25985, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-10, -80) * 1e9, 25383, 145 + 254); + EXPECT_NEAR(get_mag_strength_tesla(-10, -75) * 1e9, 24841, 145 + 248); + EXPECT_NEAR(get_mag_strength_tesla(-10, -70) * 1e9, 24401, 145 + 244); + EXPECT_NEAR(get_mag_strength_tesla(-10, -65) * 1e9, 24102, 145 + 241); + EXPECT_NEAR(get_mag_strength_tesla(-10, -60) * 1e9, 23971, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-10, -55) * 1e9, 24020, 145 + 240); + EXPECT_NEAR(get_mag_strength_tesla(-10, -50) * 1e9, 24242, 145 + 242); + EXPECT_NEAR(get_mag_strength_tesla(-10, -45) * 1e9, 24620, 145 + 246); + EXPECT_NEAR(get_mag_strength_tesla(-10, -40) * 1e9, 25126, 145 + 251); + EXPECT_NEAR(get_mag_strength_tesla(-10, -35) * 1e9, 25734, 145 + 257); + EXPECT_NEAR(get_mag_strength_tesla(-10, -30) * 1e9, 26424, 145 + 264); + EXPECT_NEAR(get_mag_strength_tesla(-10, -25) * 1e9, 27179, 145 + 272); + EXPECT_NEAR(get_mag_strength_tesla(-10, -20) * 1e9, 27977, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-10, -15) * 1e9, 28783, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-10, -10) * 1e9, 29548, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-10, -5) * 1e9, 30220, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(-10, 0) * 1e9, 30756, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(-10, 5) * 1e9, 31134, 145 + 311); + EXPECT_NEAR(get_mag_strength_tesla(-10, 10) * 1e9, 31357, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(-10, 15) * 1e9, 31456, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 20) * 1e9, 31495, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-10, 25) * 1e9, 31566, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-10, 30) * 1e9, 31785, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(-10, 35) * 1e9, 32260, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-10, 40) * 1e9, 33063, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(-10, 45) * 1e9, 34203, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(-10, 50) * 1e9, 35625, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(-10, 55) * 1e9, 37228, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(-10, 60) * 1e9, 38905, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(-10, 65) * 1e9, 40561, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-10, 70) * 1e9, 42124, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(-10, 75) * 1e9, 43538, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(-10, 80) * 1e9, 44750, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 85) * 1e9, 45704, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(-10, 90) * 1e9, 46350, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(-10, 95) * 1e9, 46668, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 100) * 1e9, 46685, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(-10, 105) * 1e9, 46479, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(-10, 110) * 1e9, 46154, 145 + 462); + EXPECT_NEAR(get_mag_strength_tesla(-10, 115) * 1e9, 45804, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(-10, 120) * 1e9, 45477, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(-10, 125) * 1e9, 45172, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(-10, 130) * 1e9, 44847, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(-10, 135) * 1e9, 44455, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-10, 140) * 1e9, 43968, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(-10, 145) * 1e9, 43385, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(-10, 150) * 1e9, 42721, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(-10, 155) * 1e9, 41997, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(-10, 160) * 1e9, 41227, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-10, 165) * 1e9, 40418, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(-10, 170) * 1e9, 39583, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(-10, 175) * 1e9, 38736, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(-10, 180) * 1e9, 37896, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, -180) * 1e9, 35735, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(-5, -175) * 1e9, 35078, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(-5, -170) * 1e9, 34447, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(-5, -165) * 1e9, 33838, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(-5, -160) * 1e9, 33249, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(-5, -155) * 1e9, 32681, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(-5, -150) * 1e9, 32142, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, -145) * 1e9, 31639, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(-5, -140) * 1e9, 31175, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(-5, -135) * 1e9, 30752, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(-5, -130) * 1e9, 30367, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(-5, -125) * 1e9, 30018, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(-5, -120) * 1e9, 29704, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(-5, -115) * 1e9, 29415, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(-5, -110) * 1e9, 29138, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(-5, -105) * 1e9, 28851, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(-5, -100) * 1e9, 28533, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(-5, -95) * 1e9, 28165, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(-5, -90) * 1e9, 27742, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(-5, -85) * 1e9, 27274, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -80) * 1e9, 26781, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(-5, -75) * 1e9, 26296, 145 + 263); + EXPECT_NEAR(get_mag_strength_tesla(-5, -70) * 1e9, 25858, 145 + 259); + EXPECT_NEAR(get_mag_strength_tesla(-5, -65) * 1e9, 25507, 145 + 255); + EXPECT_NEAR(get_mag_strength_tesla(-5, -60) * 1e9, 25279, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -55) * 1e9, 25206, 145 + 252); + EXPECT_NEAR(get_mag_strength_tesla(-5, -50) * 1e9, 25309, 145 + 253); + EXPECT_NEAR(get_mag_strength_tesla(-5, -45) * 1e9, 25590, 145 + 256); + EXPECT_NEAR(get_mag_strength_tesla(-5, -40) * 1e9, 26029, 145 + 260); + EXPECT_NEAR(get_mag_strength_tesla(-5, -35) * 1e9, 26598, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(-5, -30) * 1e9, 27262, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(-5, -25) * 1e9, 27992, 145 + 280); + EXPECT_NEAR(get_mag_strength_tesla(-5, -20) * 1e9, 28758, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(-5, -15) * 1e9, 29527, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(-5, -10) * 1e9, 30261, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(-5, -5) * 1e9, 30920, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(-5, 0) * 1e9, 31468, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(-5, 5) * 1e9, 31880, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(-5, 10) * 1e9, 32148, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(-5, 15) * 1e9, 32286, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 20) * 1e9, 32338, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(-5, 25) * 1e9, 32379, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(-5, 30) * 1e9, 32512, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(-5, 35) * 1e9, 32842, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(-5, 40) * 1e9, 33440, 145 + 334); + EXPECT_NEAR(get_mag_strength_tesla(-5, 45) * 1e9, 34323, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(-5, 50) * 1e9, 35450, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(-5, 55) * 1e9, 36741, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(-5, 60) * 1e9, 38106, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(-5, 65) * 1e9, 39467, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(-5, 70) * 1e9, 40764, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(-5, 75) * 1e9, 41946, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(-5, 80) * 1e9, 42960, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 85) * 1e9, 43748, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-5, 90) * 1e9, 44260, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(-5, 95) * 1e9, 44470, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(-5, 100) * 1e9, 44404, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(-5, 105) * 1e9, 44131, 145 + 441); + EXPECT_NEAR(get_mag_strength_tesla(-5, 110) * 1e9, 43750, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(-5, 115) * 1e9, 43344, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(-5, 120) * 1e9, 42959, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(-5, 125) * 1e9, 42594, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(-5, 130) * 1e9, 42211, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(-5, 135) * 1e9, 41769, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(-5, 140) * 1e9, 41246, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(-5, 145) * 1e9, 40645, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(-5, 150) * 1e9, 39985, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(-5, 155) * 1e9, 39289, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(-5, 160) * 1e9, 38575, 145 + 386); + EXPECT_NEAR(get_mag_strength_tesla(-5, 165) * 1e9, 37852, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(-5, 170) * 1e9, 37130, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(-5, 175) * 1e9, 36420, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(-5, 180) * 1e9, 35735, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(0, -180) * 1e9, 34118, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(0, -175) * 1e9, 33635, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(0, -170) * 1e9, 33180, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, -165) * 1e9, 32747, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, -160) * 1e9, 32334, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(0, -155) * 1e9, 31948, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, -150) * 1e9, 31600, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(0, -145) * 1e9, 31298, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(0, -140) * 1e9, 31044, 145 + 310); + EXPECT_NEAR(get_mag_strength_tesla(0, -135) * 1e9, 30834, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -130) * 1e9, 30659, 145 + 307); + EXPECT_NEAR(get_mag_strength_tesla(0, -125) * 1e9, 30514, 145 + 305); + EXPECT_NEAR(get_mag_strength_tesla(0, -120) * 1e9, 30392, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(0, -115) * 1e9, 30288, 145 + 303); + EXPECT_NEAR(get_mag_strength_tesla(0, -110) * 1e9, 30191, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(0, -105) * 1e9, 30081, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -100) * 1e9, 29934, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(0, -95) * 1e9, 29727, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(0, -90) * 1e9, 29443, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(0, -85) * 1e9, 29080, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(0, -80) * 1e9, 28651, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(0, -75) * 1e9, 28184, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(0, -70) * 1e9, 27714, 145 + 277); + EXPECT_NEAR(get_mag_strength_tesla(0, -65) * 1e9, 27280, 145 + 273); + EXPECT_NEAR(get_mag_strength_tesla(0, -60) * 1e9, 26928, 145 + 269); + EXPECT_NEAR(get_mag_strength_tesla(0, -55) * 1e9, 26704, 145 + 267); + EXPECT_NEAR(get_mag_strength_tesla(0, -50) * 1e9, 26648, 145 + 266); + EXPECT_NEAR(get_mag_strength_tesla(0, -45) * 1e9, 26780, 145 + 268); + EXPECT_NEAR(get_mag_strength_tesla(0, -40) * 1e9, 27094, 145 + 271); + EXPECT_NEAR(get_mag_strength_tesla(0, -35) * 1e9, 27560, 145 + 276); + EXPECT_NEAR(get_mag_strength_tesla(0, -30) * 1e9, 28136, 145 + 281); + EXPECT_NEAR(get_mag_strength_tesla(0, -25) * 1e9, 28782, 145 + 288); + EXPECT_NEAR(get_mag_strength_tesla(0, -20) * 1e9, 29462, 145 + 295); + EXPECT_NEAR(get_mag_strength_tesla(0, -15) * 1e9, 30147, 145 + 301); + EXPECT_NEAR(get_mag_strength_tesla(0, -10) * 1e9, 30807, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(0, -5) * 1e9, 31415, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(0, 0) * 1e9, 31944, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(0, 5) * 1e9, 32370, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(0, 10) * 1e9, 32678, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(0, 15) * 1e9, 32873, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(0, 20) * 1e9, 32982, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(0, 25) * 1e9, 33065, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(0, 30) * 1e9, 33206, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(0, 35) * 1e9, 33492, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(0, 40) * 1e9, 33981, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(0, 45) * 1e9, 34689, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(0, 50) * 1e9, 35583, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(0, 55) * 1e9, 36606, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(0, 60) * 1e9, 37693, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(0, 65) * 1e9, 38784, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(0, 70) * 1e9, 39833, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(0, 75) * 1e9, 40795, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(0, 80) * 1e9, 41622, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 85) * 1e9, 42261, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(0, 90) * 1e9, 42664, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 95) * 1e9, 42807, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(0, 100) * 1e9, 42708, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(0, 105) * 1e9, 42422, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(0, 110) * 1e9, 42025, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(0, 115) * 1e9, 41586, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(0, 120) * 1e9, 41142, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(0, 125) * 1e9, 40695, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(0, 130) * 1e9, 40222, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(0, 135) * 1e9, 39698, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(0, 140) * 1e9, 39110, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(0, 145) * 1e9, 38469, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(0, 150) * 1e9, 37796, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(0, 155) * 1e9, 37117, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(0, 160) * 1e9, 36451, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(0, 165) * 1e9, 35810, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(0, 170) * 1e9, 35204, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(0, 175) * 1e9, 34639, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(0, 180) * 1e9, 34118, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(5, -180) * 1e9, 33142, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(5, -175) * 1e9, 32825, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(5, -170) * 1e9, 32539, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(5, -165) * 1e9, 32272, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(5, -160) * 1e9, 32027, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(5, -155) * 1e9, 31816, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -150) * 1e9, 31653, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -145) * 1e9, 31549, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -140) * 1e9, 31505, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -135) * 1e9, 31511, 145 + 315); + EXPECT_NEAR(get_mag_strength_tesla(5, -130) * 1e9, 31552, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -125) * 1e9, 31616, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -120) * 1e9, 31692, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(5, -115) * 1e9, 31776, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -110) * 1e9, 31854, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -105) * 1e9, 31909, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -100) * 1e9, 31911, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, -95) * 1e9, 31830, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(5, -90) * 1e9, 31643, 145 + 316); + EXPECT_NEAR(get_mag_strength_tesla(5, -85) * 1e9, 31342, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(5, -80) * 1e9, 30935, 145 + 309); + EXPECT_NEAR(get_mag_strength_tesla(5, -75) * 1e9, 30449, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(5, -70) * 1e9, 29918, 145 + 299); + EXPECT_NEAR(get_mag_strength_tesla(5, -65) * 1e9, 29385, 145 + 294); + EXPECT_NEAR(get_mag_strength_tesla(5, -60) * 1e9, 28899, 145 + 289); + EXPECT_NEAR(get_mag_strength_tesla(5, -55) * 1e9, 28516, 145 + 285); + EXPECT_NEAR(get_mag_strength_tesla(5, -50) * 1e9, 28285, 145 + 283); + EXPECT_NEAR(get_mag_strength_tesla(5, -45) * 1e9, 28242, 145 + 282); + EXPECT_NEAR(get_mag_strength_tesla(5, -40) * 1e9, 28391, 145 + 284); + EXPECT_NEAR(get_mag_strength_tesla(5, -35) * 1e9, 28707, 145 + 287); + EXPECT_NEAR(get_mag_strength_tesla(5, -30) * 1e9, 29146, 145 + 291); + EXPECT_NEAR(get_mag_strength_tesla(5, -25) * 1e9, 29666, 145 + 297); + EXPECT_NEAR(get_mag_strength_tesla(5, -20) * 1e9, 30229, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(5, -15) * 1e9, 30810, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(5, -10) * 1e9, 31387, 145 + 314); + EXPECT_NEAR(get_mag_strength_tesla(5, -5) * 1e9, 31938, 145 + 319); + EXPECT_NEAR(get_mag_strength_tesla(5, 0) * 1e9, 32442, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(5, 5) * 1e9, 32876, 145 + 329); + EXPECT_NEAR(get_mag_strength_tesla(5, 10) * 1e9, 33226, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(5, 15) * 1e9, 33490, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(5, 20) * 1e9, 33688, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(5, 25) * 1e9, 33865, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 30) * 1e9, 34086, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(5, 35) * 1e9, 34409, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 40) * 1e9, 34873, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(5, 45) * 1e9, 35482, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(5, 50) * 1e9, 36213, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(5, 55) * 1e9, 37029, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(5, 60) * 1e9, 37890, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(5, 65) * 1e9, 38758, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(5, 70) * 1e9, 39599, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(5, 75) * 1e9, 40376, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(5, 80) * 1e9, 41044, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(5, 85) * 1e9, 41558, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 90) * 1e9, 41878, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 95) * 1e9, 41984, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(5, 100) * 1e9, 41885, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(5, 105) * 1e9, 41616, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(5, 110) * 1e9, 41225, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(5, 115) * 1e9, 40759, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(5, 120) * 1e9, 40243, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(5, 125) * 1e9, 39688, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(5, 130) * 1e9, 39091, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(5, 135) * 1e9, 38446, 145 + 384); + EXPECT_NEAR(get_mag_strength_tesla(5, 140) * 1e9, 37759, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(5, 145) * 1e9, 37048, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(5, 150) * 1e9, 36338, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(5, 155) * 1e9, 35653, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(5, 160) * 1e9, 35016, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(5, 165) * 1e9, 34441, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(5, 170) * 1e9, 33938, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(5, 175) * 1e9, 33507, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(5, 180) * 1e9, 33142, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(10, -180) * 1e9, 32824, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, -175) * 1e9, 32647, 145 + 326); + EXPECT_NEAR(get_mag_strength_tesla(10, -170) * 1e9, 32506, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -165) * 1e9, 32388, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -160) * 1e9, 32295, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -155) * 1e9, 32243, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -150) * 1e9, 32252, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -145) * 1e9, 32335, 145 + 323); + EXPECT_NEAR(get_mag_strength_tesla(10, -140) * 1e9, 32491, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(10, -135) * 1e9, 32705, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(10, -130) * 1e9, 32958, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -125) * 1e9, 33229, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, -120) * 1e9, 33504, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(10, -115) * 1e9, 33774, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(10, -110) * 1e9, 34023, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -105) * 1e9, 34229, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(10, -100) * 1e9, 34358, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, -95) * 1e9, 34375, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, -90) * 1e9, 34252, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(10, -85) * 1e9, 33976, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(10, -80) * 1e9, 33559, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(10, -75) * 1e9, 33027, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(10, -70) * 1e9, 32418, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(10, -65) * 1e9, 31779, 145 + 318); + EXPECT_NEAR(get_mag_strength_tesla(10, -60) * 1e9, 31164, 145 + 312); + EXPECT_NEAR(get_mag_strength_tesla(10, -55) * 1e9, 30633, 145 + 306); + EXPECT_NEAR(get_mag_strength_tesla(10, -50) * 1e9, 30241, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -45) * 1e9, 30030, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -40) * 1e9, 30008, 145 + 300); + EXPECT_NEAR(get_mag_strength_tesla(10, -35) * 1e9, 30156, 145 + 302); + EXPECT_NEAR(get_mag_strength_tesla(10, -30) * 1e9, 30439, 145 + 304); + EXPECT_NEAR(get_mag_strength_tesla(10, -25) * 1e9, 30816, 145 + 308); + EXPECT_NEAR(get_mag_strength_tesla(10, -20) * 1e9, 31256, 145 + 313); + EXPECT_NEAR(get_mag_strength_tesla(10, -15) * 1e9, 31738, 145 + 317); + EXPECT_NEAR(get_mag_strength_tesla(10, -10) * 1e9, 32242, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(10, -5) * 1e9, 32751, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(10, 0) * 1e9, 33243, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(10, 5) * 1e9, 33694, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(10, 10) * 1e9, 34088, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(10, 15) * 1e9, 34425, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 20) * 1e9, 34723, 145 + 347); + EXPECT_NEAR(get_mag_strength_tesla(10, 25) * 1e9, 35018, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 30) * 1e9, 35351, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(10, 35) * 1e9, 35756, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(10, 40) * 1e9, 36246, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(10, 45) * 1e9, 36814, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(10, 50) * 1e9, 37444, 145 + 374); + EXPECT_NEAR(get_mag_strength_tesla(10, 55) * 1e9, 38115, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 60) * 1e9, 38812, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(10, 65) * 1e9, 39516, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(10, 70) * 1e9, 40205, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(10, 75) * 1e9, 40846, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(10, 80) * 1e9, 41400, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(10, 85) * 1e9, 41826, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(10, 90) * 1e9, 42093, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 95) * 1e9, 42185, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(10, 100) * 1e9, 42102, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(10, 105) * 1e9, 41858, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(10, 110) * 1e9, 41475, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(10, 115) * 1e9, 40974, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(10, 120) * 1e9, 40373, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(10, 125) * 1e9, 39688, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(10, 130) * 1e9, 38936, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(10, 135) * 1e9, 38138, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(10, 140) * 1e9, 37317, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(10, 145) * 1e9, 36502, 145 + 365); + EXPECT_NEAR(get_mag_strength_tesla(10, 150) * 1e9, 35722, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(10, 155) * 1e9, 35001, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(10, 160) * 1e9, 34363, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(10, 165) * 1e9, 33823, 145 + 338); + EXPECT_NEAR(get_mag_strength_tesla(10, 170) * 1e9, 33391, 145 + 334); + EXPECT_NEAR(get_mag_strength_tesla(10, 175) * 1e9, 33063, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(10, 180) * 1e9, 32824, 145 + 328); + EXPECT_NEAR(get_mag_strength_tesla(15, -180) * 1e9, 33126, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -175) * 1e9, 33048, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -170) * 1e9, 33019, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -165) * 1e9, 33024, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -160) * 1e9, 33063, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -155) * 1e9, 33155, 145 + 332); + EXPECT_NEAR(get_mag_strength_tesla(15, -150) * 1e9, 33320, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(15, -145) * 1e9, 33574, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, -140) * 1e9, 33914, 145 + 339); + EXPECT_NEAR(get_mag_strength_tesla(15, -135) * 1e9, 34321, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(15, -130) * 1e9, 34771, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(15, -125) * 1e9, 35239, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -120) * 1e9, 35704, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(15, -115) * 1e9, 36152, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(15, -110) * 1e9, 36561, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(15, -105) * 1e9, 36905, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, -100) * 1e9, 37144, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, -95) * 1e9, 37239, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -90) * 1e9, 37158, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(15, -85) * 1e9, 36890, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, -80) * 1e9, 36446, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(15, -75) * 1e9, 35856, 145 + 359); + EXPECT_NEAR(get_mag_strength_tesla(15, -70) * 1e9, 35163, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, -65) * 1e9, 34421, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(15, -60) * 1e9, 33690, 145 + 337); + EXPECT_NEAR(get_mag_strength_tesla(15, -55) * 1e9, 33036, 145 + 330); + EXPECT_NEAR(get_mag_strength_tesla(15, -50) * 1e9, 32513, 145 + 325); + EXPECT_NEAR(get_mag_strength_tesla(15, -45) * 1e9, 32162, 145 + 322); + EXPECT_NEAR(get_mag_strength_tesla(15, -40) * 1e9, 31994, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -35) * 1e9, 31993, 145 + 320); + EXPECT_NEAR(get_mag_strength_tesla(15, -30) * 1e9, 32130, 145 + 321); + EXPECT_NEAR(get_mag_strength_tesla(15, -25) * 1e9, 32373, 145 + 324); + EXPECT_NEAR(get_mag_strength_tesla(15, -20) * 1e9, 32701, 145 + 327); + EXPECT_NEAR(get_mag_strength_tesla(15, -15) * 1e9, 33098, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(15, -10) * 1e9, 33549, 145 + 335); + EXPECT_NEAR(get_mag_strength_tesla(15, -5) * 1e9, 34035, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 0) * 1e9, 34528, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 5) * 1e9, 35004, 145 + 350); + EXPECT_NEAR(get_mag_strength_tesla(15, 10) * 1e9, 35443, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(15, 15) * 1e9, 35848, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(15, 20) * 1e9, 36236, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(15, 25) * 1e9, 36637, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(15, 30) * 1e9, 37078, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(15, 35) * 1e9, 37568, 145 + 376); + EXPECT_NEAR(get_mag_strength_tesla(15, 40) * 1e9, 38101, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(15, 45) * 1e9, 38662, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(15, 50) * 1e9, 39237, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(15, 55) * 1e9, 39821, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 60) * 1e9, 40416, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(15, 65) * 1e9, 41020, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(15, 70) * 1e9, 41618, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(15, 75) * 1e9, 42181, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 80) * 1e9, 42673, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 85) * 1e9, 43057, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 90) * 1e9, 43307, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(15, 95) * 1e9, 43406, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(15, 100) * 1e9, 43346, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(15, 105) * 1e9, 43124, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(15, 110) * 1e9, 42739, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(15, 115) * 1e9, 42193, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(15, 120) * 1e9, 41497, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(15, 125) * 1e9, 40673, 145 + 407); + EXPECT_NEAR(get_mag_strength_tesla(15, 130) * 1e9, 39755, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(15, 135) * 1e9, 38785, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(15, 140) * 1e9, 37806, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(15, 145) * 1e9, 36859, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(15, 150) * 1e9, 35976, 145 + 360); + EXPECT_NEAR(get_mag_strength_tesla(15, 155) * 1e9, 35185, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(15, 160) * 1e9, 34508, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(15, 165) * 1e9, 33963, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(15, 170) * 1e9, 33557, 145 + 336); + EXPECT_NEAR(get_mag_strength_tesla(15, 175) * 1e9, 33285, 145 + 333); + EXPECT_NEAR(get_mag_strength_tesla(15, 180) * 1e9, 33126, 145 + 331); + EXPECT_NEAR(get_mag_strength_tesla(20, -180) * 1e9, 33991, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -175) * 1e9, 33964, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -170) * 1e9, 34009, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(20, -165) * 1e9, 34108, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, -160) * 1e9, 34260, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -155) * 1e9, 34481, 145 + 345); + EXPECT_NEAR(get_mag_strength_tesla(20, -150) * 1e9, 34789, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, -145) * 1e9, 35197, 145 + 352); + EXPECT_NEAR(get_mag_strength_tesla(20, -140) * 1e9, 35700, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -135) * 1e9, 36278, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, -130) * 1e9, 36902, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(20, -125) * 1e9, 37544, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(20, -120) * 1e9, 38179, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, -115) * 1e9, 38785, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(20, -110) * 1e9, 39337, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(20, -105) * 1e9, 39801, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(20, -100) * 1e9, 40134, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(20, -95) * 1e9, 40294, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, -90) * 1e9, 40249, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(20, -85) * 1e9, 39985, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(20, -80) * 1e9, 39513, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(20, -75) * 1e9, 38868, 145 + 389); + EXPECT_NEAR(get_mag_strength_tesla(20, -70) * 1e9, 38099, 145 + 381); + EXPECT_NEAR(get_mag_strength_tesla(20, -65) * 1e9, 37267, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, -60) * 1e9, 36442, 145 + 364); + EXPECT_NEAR(get_mag_strength_tesla(20, -55) * 1e9, 35691, 145 + 357); + EXPECT_NEAR(get_mag_strength_tesla(20, -50) * 1e9, 35070, 145 + 351); + EXPECT_NEAR(get_mag_strength_tesla(20, -45) * 1e9, 34616, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -40) * 1e9, 34337, 145 + 343); + EXPECT_NEAR(get_mag_strength_tesla(20, -35) * 1e9, 34218, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -30) * 1e9, 34236, 145 + 342); + EXPECT_NEAR(get_mag_strength_tesla(20, -25) * 1e9, 34368, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(20, -20) * 1e9, 34602, 145 + 346); + EXPECT_NEAR(get_mag_strength_tesla(20, -15) * 1e9, 34930, 145 + 349); + EXPECT_NEAR(get_mag_strength_tesla(20, -10) * 1e9, 35340, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(20, -5) * 1e9, 35810, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(20, 0) * 1e9, 36310, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(20, 5) * 1e9, 36809, 145 + 368); + EXPECT_NEAR(get_mag_strength_tesla(20, 10) * 1e9, 37286, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(20, 15) * 1e9, 37742, 145 + 377); + EXPECT_NEAR(get_mag_strength_tesla(20, 20) * 1e9, 38196, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(20, 25) * 1e9, 38673, 145 + 387); + EXPECT_NEAR(get_mag_strength_tesla(20, 30) * 1e9, 39187, 145 + 392); + EXPECT_NEAR(get_mag_strength_tesla(20, 35) * 1e9, 39738, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(20, 40) * 1e9, 40307, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 45) * 1e9, 40874, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(20, 50) * 1e9, 41428, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 55) * 1e9, 41974, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(20, 60) * 1e9, 42526, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 65) * 1e9, 43089, 145 + 431); + EXPECT_NEAR(get_mag_strength_tesla(20, 70) * 1e9, 43655, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(20, 75) * 1e9, 44198, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(20, 80) * 1e9, 44681, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(20, 85) * 1e9, 45070, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(20, 90) * 1e9, 45336, 145 + 453); + EXPECT_NEAR(get_mag_strength_tesla(20, 95) * 1e9, 45461, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(20, 100) * 1e9, 45429, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(20, 105) * 1e9, 45224, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(20, 110) * 1e9, 44830, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(20, 115) * 1e9, 44236, 145 + 442); + EXPECT_NEAR(get_mag_strength_tesla(20, 120) * 1e9, 43448, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(20, 125) * 1e9, 42494, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(20, 130) * 1e9, 41419, 145 + 414); + EXPECT_NEAR(get_mag_strength_tesla(20, 135) * 1e9, 40281, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(20, 140) * 1e9, 39140, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(20, 145) * 1e9, 38046, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(20, 150) * 1e9, 37040, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(20, 155) * 1e9, 36152, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(20, 160) * 1e9, 35405, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(20, 165) * 1e9, 34815, 145 + 348); + EXPECT_NEAR(get_mag_strength_tesla(20, 170) * 1e9, 34389, 145 + 344); + EXPECT_NEAR(get_mag_strength_tesla(20, 175) * 1e9, 34122, 145 + 341); + EXPECT_NEAR(get_mag_strength_tesla(20, 180) * 1e9, 33991, 145 + 340); + EXPECT_NEAR(get_mag_strength_tesla(25, -180) * 1e9, 35368, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -175) * 1e9, 35349, 145 + 353); + EXPECT_NEAR(get_mag_strength_tesla(25, -170) * 1e9, 35431, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(25, -165) * 1e9, 35597, 145 + 356); + EXPECT_NEAR(get_mag_strength_tesla(25, -160) * 1e9, 35844, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, -155) * 1e9, 36179, 145 + 362); + EXPECT_NEAR(get_mag_strength_tesla(25, -150) * 1e9, 36616, 145 + 366); + EXPECT_NEAR(get_mag_strength_tesla(25, -145) * 1e9, 37160, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(25, -140) * 1e9, 37804, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, -135) * 1e9, 38524, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -130) * 1e9, 39291, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(25, -125) * 1e9, 40074, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(25, -120) * 1e9, 40845, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(25, -115) * 1e9, 41578, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(25, -110) * 1e9, 42243, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(25, -105) * 1e9, 42802, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(25, -100) * 1e9, 43210, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, -95) * 1e9, 43424, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -90) * 1e9, 43410, 145 + 434); + EXPECT_NEAR(get_mag_strength_tesla(25, -85) * 1e9, 43154, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, -80) * 1e9, 42668, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, -75) * 1e9, 41985, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(25, -70) * 1e9, 41160, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(25, -65) * 1e9, 40263, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(25, -60) * 1e9, 39367, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(25, -55) * 1e9, 38544, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, -50) * 1e9, 37851, 145 + 379); + EXPECT_NEAR(get_mag_strength_tesla(25, -45) * 1e9, 37320, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(25, -40) * 1e9, 36957, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(25, -35) * 1e9, 36748, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -30) * 1e9, 36673, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -25) * 1e9, 36716, 145 + 367); + EXPECT_NEAR(get_mag_strength_tesla(25, -20) * 1e9, 36871, 145 + 369); + EXPECT_NEAR(get_mag_strength_tesla(25, -15) * 1e9, 37138, 145 + 371); + EXPECT_NEAR(get_mag_strength_tesla(25, -10) * 1e9, 37507, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(25, -5) * 1e9, 37956, 145 + 380); + EXPECT_NEAR(get_mag_strength_tesla(25, 0) * 1e9, 38452, 145 + 385); + EXPECT_NEAR(get_mag_strength_tesla(25, 5) * 1e9, 38960, 145 + 390); + EXPECT_NEAR(get_mag_strength_tesla(25, 10) * 1e9, 39458, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(25, 15) * 1e9, 39943, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 20) * 1e9, 40432, 145 + 404); + EXPECT_NEAR(get_mag_strength_tesla(25, 25) * 1e9, 40946, 145 + 409); + EXPECT_NEAR(get_mag_strength_tesla(25, 30) * 1e9, 41494, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(25, 35) * 1e9, 42071, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(25, 40) * 1e9, 42658, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(25, 45) * 1e9, 43235, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(25, 50) * 1e9, 43795, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(25, 55) * 1e9, 44347, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(25, 60) * 1e9, 44906, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 65) * 1e9, 45483, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(25, 70) * 1e9, 46068, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(25, 75) * 1e9, 46639, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(25, 80) * 1e9, 47157, 145 + 472); + EXPECT_NEAR(get_mag_strength_tesla(25, 85) * 1e9, 47587, 145 + 476); + EXPECT_NEAR(get_mag_strength_tesla(25, 90) * 1e9, 47897, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 95) * 1e9, 48064, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 100) * 1e9, 48064, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(25, 105) * 1e9, 47873, 145 + 479); + EXPECT_NEAR(get_mag_strength_tesla(25, 110) * 1e9, 47468, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(25, 115) * 1e9, 46831, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(25, 120) * 1e9, 45969, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(25, 125) * 1e9, 44910, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(25, 130) * 1e9, 43708, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(25, 135) * 1e9, 42431, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(25, 140) * 1e9, 41148, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(25, 145) * 1e9, 39920, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(25, 150) * 1e9, 38793, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(25, 155) * 1e9, 37800, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(25, 160) * 1e9, 36965, 145 + 370); + EXPECT_NEAR(get_mag_strength_tesla(25, 165) * 1e9, 36303, 145 + 363); + EXPECT_NEAR(get_mag_strength_tesla(25, 170) * 1e9, 35822, 145 + 358); + EXPECT_NEAR(get_mag_strength_tesla(25, 175) * 1e9, 35516, 145 + 355); + EXPECT_NEAR(get_mag_strength_tesla(25, 180) * 1e9, 35368, 145 + 354); + EXPECT_NEAR(get_mag_strength_tesla(30, -180) * 1e9, 37222, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -175) * 1e9, 37180, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(30, -170) * 1e9, 37272, 145 + 373); + EXPECT_NEAR(get_mag_strength_tesla(30, -165) * 1e9, 37481, 145 + 375); + EXPECT_NEAR(get_mag_strength_tesla(30, -160) * 1e9, 37801, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, -155) * 1e9, 38234, 145 + 382); + EXPECT_NEAR(get_mag_strength_tesla(30, -150) * 1e9, 38782, 145 + 388); + EXPECT_NEAR(get_mag_strength_tesla(30, -145) * 1e9, 39442, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -140) * 1e9, 40199, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(30, -135) * 1e9, 41029, 145 + 410); + EXPECT_NEAR(get_mag_strength_tesla(30, -130) * 1e9, 41900, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(30, -125) * 1e9, 42782, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(30, -120) * 1e9, 43646, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, -115) * 1e9, 44462, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(30, -110) * 1e9, 45198, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(30, -105) * 1e9, 45816, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -100) * 1e9, 46272, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(30, -95) * 1e9, 46522, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -90) * 1e9, 46533, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(30, -85) * 1e9, 46291, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(30, -80) * 1e9, 45804, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(30, -75) * 1e9, 45108, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(30, -70) * 1e9, 44257, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(30, -65) * 1e9, 43324, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(30, -60) * 1e9, 42385, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(30, -55) * 1e9, 41513, 145 + 415); + EXPECT_NEAR(get_mag_strength_tesla(30, -50) * 1e9, 40764, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, -45) * 1e9, 40170, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(30, -40) * 1e9, 39736, 145 + 397); + EXPECT_NEAR(get_mag_strength_tesla(30, -35) * 1e9, 39449, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(30, -30) * 1e9, 39295, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -25) * 1e9, 39261, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -20) * 1e9, 39347, 145 + 393); + EXPECT_NEAR(get_mag_strength_tesla(30, -15) * 1e9, 39552, 145 + 396); + EXPECT_NEAR(get_mag_strength_tesla(30, -10) * 1e9, 39871, 145 + 399); + EXPECT_NEAR(get_mag_strength_tesla(30, -5) * 1e9, 40282, 145 + 403); + EXPECT_NEAR(get_mag_strength_tesla(30, 0) * 1e9, 40753, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(30, 5) * 1e9, 41247, 145 + 412); + EXPECT_NEAR(get_mag_strength_tesla(30, 10) * 1e9, 41741, 145 + 417); + EXPECT_NEAR(get_mag_strength_tesla(30, 15) * 1e9, 42230, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(30, 20) * 1e9, 42724, 145 + 427); + EXPECT_NEAR(get_mag_strength_tesla(30, 25) * 1e9, 43241, 145 + 432); + EXPECT_NEAR(get_mag_strength_tesla(30, 30) * 1e9, 43790, 145 + 438); + EXPECT_NEAR(get_mag_strength_tesla(30, 35) * 1e9, 44367, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(30, 40) * 1e9, 44958, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 45) * 1e9, 45549, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(30, 50) * 1e9, 46137, 145 + 461); + EXPECT_NEAR(get_mag_strength_tesla(30, 55) * 1e9, 46730, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(30, 60) * 1e9, 47340, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(30, 65) * 1e9, 47972, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(30, 70) * 1e9, 48618, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(30, 75) * 1e9, 49251, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(30, 80) * 1e9, 49833, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(30, 85) * 1e9, 50326, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(30, 90) * 1e9, 50695, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(30, 95) * 1e9, 50910, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(30, 100) * 1e9, 50943, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(30, 105) * 1e9, 50765, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(30, 110) * 1e9, 50351, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(30, 115) * 1e9, 49686, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(30, 120) * 1e9, 48775, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(30, 125) * 1e9, 47651, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(30, 130) * 1e9, 46369, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(30, 135) * 1e9, 45001, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(30, 140) * 1e9, 43622, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(30, 145) * 1e9, 42296, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(30, 150) * 1e9, 41076, 145 + 411); + EXPECT_NEAR(get_mag_strength_tesla(30, 155) * 1e9, 39994, 145 + 400); + EXPECT_NEAR(get_mag_strength_tesla(30, 160) * 1e9, 39077, 145 + 391); + EXPECT_NEAR(get_mag_strength_tesla(30, 165) * 1e9, 38338, 145 + 383); + EXPECT_NEAR(get_mag_strength_tesla(30, 170) * 1e9, 37785, 145 + 378); + EXPECT_NEAR(get_mag_strength_tesla(30, 175) * 1e9, 37417, 145 + 374); + EXPECT_NEAR(get_mag_strength_tesla(30, 180) * 1e9, 37222, 145 + 372); + EXPECT_NEAR(get_mag_strength_tesla(35, -180) * 1e9, 39523, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -175) * 1e9, 39446, 145 + 394); + EXPECT_NEAR(get_mag_strength_tesla(35, -170) * 1e9, 39528, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(35, -165) * 1e9, 39759, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, -160) * 1e9, 40130, 145 + 401); + EXPECT_NEAR(get_mag_strength_tesla(35, -155) * 1e9, 40636, 145 + 406); + EXPECT_NEAR(get_mag_strength_tesla(35, -150) * 1e9, 41268, 145 + 413); + EXPECT_NEAR(get_mag_strength_tesla(35, -145) * 1e9, 42013, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -140) * 1e9, 42850, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(35, -135) * 1e9, 43749, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -130) * 1e9, 44681, 145 + 447); + EXPECT_NEAR(get_mag_strength_tesla(35, -125) * 1e9, 45614, 145 + 456); + EXPECT_NEAR(get_mag_strength_tesla(35, -120) * 1e9, 46518, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, -115) * 1e9, 47367, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(35, -110) * 1e9, 48126, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -105) * 1e9, 48760, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(35, -100) * 1e9, 49227, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, -95) * 1e9, 49488, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(35, -90) * 1e9, 49511, 145 + 495); + EXPECT_NEAR(get_mag_strength_tesla(35, -85) * 1e9, 49281, 145 + 493); + EXPECT_NEAR(get_mag_strength_tesla(35, -80) * 1e9, 48807, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(35, -75) * 1e9, 48120, 145 + 481); + EXPECT_NEAR(get_mag_strength_tesla(35, -70) * 1e9, 47274, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(35, -65) * 1e9, 46337, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(35, -60) * 1e9, 45384, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, -55) * 1e9, 44485, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(35, -50) * 1e9, 43694, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, -45) * 1e9, 43044, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, -40) * 1e9, 42542, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(35, -35) * 1e9, 42181, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(35, -30) * 1e9, 41951, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -25) * 1e9, 41844, 145 + 418); + EXPECT_NEAR(get_mag_strength_tesla(35, -20) * 1e9, 41859, 145 + 419); + EXPECT_NEAR(get_mag_strength_tesla(35, -15) * 1e9, 41997, 145 + 420); + EXPECT_NEAR(get_mag_strength_tesla(35, -10) * 1e9, 42251, 145 + 423); + EXPECT_NEAR(get_mag_strength_tesla(35, -5) * 1e9, 42603, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 0) * 1e9, 43022, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(35, 5) * 1e9, 43476, 145 + 435); + EXPECT_NEAR(get_mag_strength_tesla(35, 10) * 1e9, 43941, 145 + 439); + EXPECT_NEAR(get_mag_strength_tesla(35, 15) * 1e9, 44409, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(35, 20) * 1e9, 44885, 145 + 449); + EXPECT_NEAR(get_mag_strength_tesla(35, 25) * 1e9, 45384, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(35, 30) * 1e9, 45914, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(35, 35) * 1e9, 46478, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(35, 40) * 1e9, 47070, 145 + 471); + EXPECT_NEAR(get_mag_strength_tesla(35, 45) * 1e9, 47684, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(35, 50) * 1e9, 48317, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(35, 55) * 1e9, 48976, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(35, 60) * 1e9, 49664, 145 + 497); + EXPECT_NEAR(get_mag_strength_tesla(35, 65) * 1e9, 50382, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(35, 70) * 1e9, 51112, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(35, 75) * 1e9, 51825, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(35, 80) * 1e9, 52483, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(35, 85) * 1e9, 53044, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(35, 90) * 1e9, 53472, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(35, 95) * 1e9, 53731, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(35, 100) * 1e9, 53792, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(35, 105) * 1e9, 53627, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(35, 110) * 1e9, 53211, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(35, 115) * 1e9, 52536, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(35, 120) * 1e9, 51610, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(35, 125) * 1e9, 50466, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(35, 130) * 1e9, 49161, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(35, 135) * 1e9, 47765, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(35, 140) * 1e9, 46351, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(35, 145) * 1e9, 44984, 145 + 450); + EXPECT_NEAR(get_mag_strength_tesla(35, 150) * 1e9, 43717, 145 + 437); + EXPECT_NEAR(get_mag_strength_tesla(35, 155) * 1e9, 42586, 145 + 426); + EXPECT_NEAR(get_mag_strength_tesla(35, 160) * 1e9, 41614, 145 + 416); + EXPECT_NEAR(get_mag_strength_tesla(35, 165) * 1e9, 40816, 145 + 408); + EXPECT_NEAR(get_mag_strength_tesla(35, 170) * 1e9, 40201, 145 + 402); + EXPECT_NEAR(get_mag_strength_tesla(35, 175) * 1e9, 39772, 145 + 398); + EXPECT_NEAR(get_mag_strength_tesla(35, 180) * 1e9, 39523, 145 + 395); + EXPECT_NEAR(get_mag_strength_tesla(40, -180) * 1e9, 42220, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -175) * 1e9, 42110, 145 + 421); + EXPECT_NEAR(get_mag_strength_tesla(40, -170) * 1e9, 42176, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(40, -165) * 1e9, 42411, 145 + 424); + EXPECT_NEAR(get_mag_strength_tesla(40, -160) * 1e9, 42806, 145 + 428); + EXPECT_NEAR(get_mag_strength_tesla(40, -155) * 1e9, 43349, 145 + 433); + EXPECT_NEAR(get_mag_strength_tesla(40, -150) * 1e9, 44024, 145 + 440); + EXPECT_NEAR(get_mag_strength_tesla(40, -145) * 1e9, 44810, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -140) * 1e9, 45678, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(40, -135) * 1e9, 46600, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, -130) * 1e9, 47541, 145 + 475); + EXPECT_NEAR(get_mag_strength_tesla(40, -125) * 1e9, 48473, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(40, -120) * 1e9, 49366, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(40, -115) * 1e9, 50195, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(40, -110) * 1e9, 50928, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -105) * 1e9, 51532, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(40, -100) * 1e9, 51973, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(40, -95) * 1e9, 52215, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -90) * 1e9, 52231, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(40, -85) * 1e9, 52009, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(40, -80) * 1e9, 51555, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(40, -75) * 1e9, 50896, 145 + 509); + EXPECT_NEAR(get_mag_strength_tesla(40, -70) * 1e9, 50080, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(40, -65) * 1e9, 49169, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(40, -60) * 1e9, 48230, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(40, -55) * 1e9, 47327, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, -50) * 1e9, 46511, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(40, -45) * 1e9, 45814, 145 + 458); + EXPECT_NEAR(get_mag_strength_tesla(40, -40) * 1e9, 45249, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(40, -35) * 1e9, 44817, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, -30) * 1e9, 44512, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, -25) * 1e9, 44330, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -20) * 1e9, 44270, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -15) * 1e9, 44332, 145 + 443); + EXPECT_NEAR(get_mag_strength_tesla(40, -10) * 1e9, 44507, 145 + 445); + EXPECT_NEAR(get_mag_strength_tesla(40, -5) * 1e9, 44781, 145 + 448); + EXPECT_NEAR(get_mag_strength_tesla(40, 0) * 1e9, 45126, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(40, 5) * 1e9, 45516, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(40, 10) * 1e9, 45929, 145 + 459); + EXPECT_NEAR(get_mag_strength_tesla(40, 15) * 1e9, 46357, 145 + 464); + EXPECT_NEAR(get_mag_strength_tesla(40, 20) * 1e9, 46801, 145 + 468); + EXPECT_NEAR(get_mag_strength_tesla(40, 25) * 1e9, 47273, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(40, 30) * 1e9, 47782, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 35) * 1e9, 48335, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(40, 40) * 1e9, 48934, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(40, 45) * 1e9, 49578, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(40, 50) * 1e9, 50269, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(40, 55) * 1e9, 51004, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(40, 60) * 1e9, 51782, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(40, 65) * 1e9, 52592, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(40, 70) * 1e9, 53409, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(40, 75) * 1e9, 54202, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(40, 80) * 1e9, 54929, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(40, 85) * 1e9, 55549, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(40, 90) * 1e9, 56023, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(40, 95) * 1e9, 56316, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(40, 100) * 1e9, 56398, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(40, 105) * 1e9, 56244, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(40, 110) * 1e9, 55837, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(40, 115) * 1e9, 55174, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(40, 120) * 1e9, 54268, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(40, 125) * 1e9, 53153, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(40, 130) * 1e9, 51884, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(40, 135) * 1e9, 50525, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(40, 140) * 1e9, 49144, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(40, 145) * 1e9, 47804, 145 + 478); + EXPECT_NEAR(get_mag_strength_tesla(40, 150) * 1e9, 46552, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(40, 155) * 1e9, 45424, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(40, 160) * 1e9, 44443, 145 + 444); + EXPECT_NEAR(get_mag_strength_tesla(40, 165) * 1e9, 43624, 145 + 436); + EXPECT_NEAR(get_mag_strength_tesla(40, 170) * 1e9, 42977, 145 + 430); + EXPECT_NEAR(get_mag_strength_tesla(40, 175) * 1e9, 42508, 145 + 425); + EXPECT_NEAR(get_mag_strength_tesla(40, 180) * 1e9, 42220, 145 + 422); + EXPECT_NEAR(get_mag_strength_tesla(45, -180) * 1e9, 45210, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(45, -175) * 1e9, 45082, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -170) * 1e9, 45130, 145 + 451); + EXPECT_NEAR(get_mag_strength_tesla(45, -165) * 1e9, 45351, 145 + 454); + EXPECT_NEAR(get_mag_strength_tesla(45, -160) * 1e9, 45736, 145 + 457); + EXPECT_NEAR(get_mag_strength_tesla(45, -155) * 1e9, 46271, 145 + 463); + EXPECT_NEAR(get_mag_strength_tesla(45, -150) * 1e9, 46936, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -145) * 1e9, 47706, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -140) * 1e9, 48549, 145 + 485); + EXPECT_NEAR(get_mag_strength_tesla(45, -135) * 1e9, 49435, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, -130) * 1e9, 50331, 145 + 503); + EXPECT_NEAR(get_mag_strength_tesla(45, -125) * 1e9, 51208, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(45, -120) * 1e9, 52040, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, -115) * 1e9, 52800, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, -110) * 1e9, 53463, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(45, -105) * 1e9, 54000, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(45, -100) * 1e9, 54382, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, -95) * 1e9, 54580, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, -90) * 1e9, 54573, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(45, -85) * 1e9, 54352, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, -80) * 1e9, 53921, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(45, -75) * 1e9, 53304, 145 + 533); + EXPECT_NEAR(get_mag_strength_tesla(45, -70) * 1e9, 52541, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(45, -65) * 1e9, 51682, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(45, -60) * 1e9, 50784, 145 + 508); + EXPECT_NEAR(get_mag_strength_tesla(45, -55) * 1e9, 49904, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, -50) * 1e9, 49085, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(45, -45) * 1e9, 48360, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, -40) * 1e9, 47747, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, -35) * 1e9, 47253, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(45, -30) * 1e9, 46878, 145 + 469); + EXPECT_NEAR(get_mag_strength_tesla(45, -25) * 1e9, 46622, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -20) * 1e9, 46485, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -15) * 1e9, 46464, 145 + 465); + EXPECT_NEAR(get_mag_strength_tesla(45, -10) * 1e9, 46552, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, -5) * 1e9, 46734, 145 + 467); + EXPECT_NEAR(get_mag_strength_tesla(45, 0) * 1e9, 46991, 145 + 470); + EXPECT_NEAR(get_mag_strength_tesla(45, 5) * 1e9, 47302, 145 + 473); + EXPECT_NEAR(get_mag_strength_tesla(45, 10) * 1e9, 47651, 145 + 477); + EXPECT_NEAR(get_mag_strength_tesla(45, 15) * 1e9, 48028, 145 + 480); + EXPECT_NEAR(get_mag_strength_tesla(45, 20) * 1e9, 48436, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 25) * 1e9, 48882, 145 + 489); + EXPECT_NEAR(get_mag_strength_tesla(45, 30) * 1e9, 49376, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 35) * 1e9, 49928, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(45, 40) * 1e9, 50544, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(45, 45) * 1e9, 51226, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(45, 50) * 1e9, 51973, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(45, 55) * 1e9, 52780, 145 + 528); + EXPECT_NEAR(get_mag_strength_tesla(45, 60) * 1e9, 53636, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(45, 65) * 1e9, 54522, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(45, 70) * 1e9, 55409, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(45, 75) * 1e9, 56260, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(45, 80) * 1e9, 57034, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(45, 85) * 1e9, 57691, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(45, 90) * 1e9, 58192, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(45, 95) * 1e9, 58503, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 100) * 1e9, 58599, 145 + 586); + EXPECT_NEAR(get_mag_strength_tesla(45, 105) * 1e9, 58460, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(45, 110) * 1e9, 58076, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(45, 115) * 1e9, 57450, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(45, 120) * 1e9, 56600, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(45, 125) * 1e9, 55562, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(45, 130) * 1e9, 54384, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(45, 135) * 1e9, 53124, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(45, 140) * 1e9, 51842, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(45, 145) * 1e9, 50592, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(45, 150) * 1e9, 49418, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(45, 155) * 1e9, 48352, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(45, 160) * 1e9, 47415, 145 + 474); + EXPECT_NEAR(get_mag_strength_tesla(45, 165) * 1e9, 46622, 145 + 466); + EXPECT_NEAR(get_mag_strength_tesla(45, 170) * 1e9, 45985, 145 + 460); + EXPECT_NEAR(get_mag_strength_tesla(45, 175) * 1e9, 45512, 145 + 455); + EXPECT_NEAR(get_mag_strength_tesla(45, 180) * 1e9, 45210, 145 + 452); + EXPECT_NEAR(get_mag_strength_tesla(50, -180) * 1e9, 48319, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -175) * 1e9, 48187, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -170) * 1e9, 48216, 145 + 482); + EXPECT_NEAR(get_mag_strength_tesla(50, -165) * 1e9, 48404, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -160) * 1e9, 48746, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -155) * 1e9, 49226, 145 + 492); + EXPECT_NEAR(get_mag_strength_tesla(50, -150) * 1e9, 49824, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, -145) * 1e9, 50516, 145 + 505); + EXPECT_NEAR(get_mag_strength_tesla(50, -140) * 1e9, 51272, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -135) * 1e9, 52062, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -130) * 1e9, 52857, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -125) * 1e9, 53630, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(50, -120) * 1e9, 54354, 145 + 544); + EXPECT_NEAR(get_mag_strength_tesla(50, -115) * 1e9, 55008, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(50, -110) * 1e9, 55568, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(50, -105) * 1e9, 56009, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(50, -100) * 1e9, 56310, 145 + 563); + EXPECT_NEAR(get_mag_strength_tesla(50, -95) * 1e9, 56449, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -90) * 1e9, 56411, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(50, -85) * 1e9, 56188, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(50, -80) * 1e9, 55786, 145 + 558); + EXPECT_NEAR(get_mag_strength_tesla(50, -75) * 1e9, 55223, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, -70) * 1e9, 54530, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(50, -65) * 1e9, 53747, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(50, -60) * 1e9, 52918, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(50, -55) * 1e9, 52089, 145 + 521); + EXPECT_NEAR(get_mag_strength_tesla(50, -50) * 1e9, 51298, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, -45) * 1e9, 50574, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(50, -40) * 1e9, 49939, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(50, -35) * 1e9, 49404, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(50, -30) * 1e9, 48975, 145 + 490); + EXPECT_NEAR(get_mag_strength_tesla(50, -25) * 1e9, 48656, 145 + 487); + EXPECT_NEAR(get_mag_strength_tesla(50, -20) * 1e9, 48446, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, -15) * 1e9, 48344, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -10) * 1e9, 48343, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(50, -5) * 1e9, 48433, 145 + 484); + EXPECT_NEAR(get_mag_strength_tesla(50, 0) * 1e9, 48600, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 5) * 1e9, 48830, 145 + 488); + EXPECT_NEAR(get_mag_strength_tesla(50, 10) * 1e9, 49111, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 15) * 1e9, 49438, 145 + 494); + EXPECT_NEAR(get_mag_strength_tesla(50, 20) * 1e9, 49812, 145 + 498); + EXPECT_NEAR(get_mag_strength_tesla(50, 25) * 1e9, 50239, 145 + 502); + EXPECT_NEAR(get_mag_strength_tesla(50, 30) * 1e9, 50728, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(50, 35) * 1e9, 51287, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(50, 40) * 1e9, 51923, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(50, 45) * 1e9, 52638, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(50, 50) * 1e9, 53427, 145 + 534); + EXPECT_NEAR(get_mag_strength_tesla(50, 55) * 1e9, 54282, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 60) * 1e9, 55185, 145 + 552); + EXPECT_NEAR(get_mag_strength_tesla(50, 65) * 1e9, 56112, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(50, 70) * 1e9, 57030, 145 + 570); + EXPECT_NEAR(get_mag_strength_tesla(50, 75) * 1e9, 57903, 145 + 579); + EXPECT_NEAR(get_mag_strength_tesla(50, 80) * 1e9, 58690, 145 + 587); + EXPECT_NEAR(get_mag_strength_tesla(50, 85) * 1e9, 59353, 145 + 594); + EXPECT_NEAR(get_mag_strength_tesla(50, 90) * 1e9, 59857, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(50, 95) * 1e9, 60174, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 100) * 1e9, 60280, 145 + 603); + EXPECT_NEAR(get_mag_strength_tesla(50, 105) * 1e9, 60163, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(50, 110) * 1e9, 59821, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(50, 115) * 1e9, 59261, 145 + 593); + EXPECT_NEAR(get_mag_strength_tesla(50, 120) * 1e9, 58504, 145 + 585); + EXPECT_NEAR(get_mag_strength_tesla(50, 125) * 1e9, 57585, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(50, 130) * 1e9, 56547, 145 + 565); + EXPECT_NEAR(get_mag_strength_tesla(50, 135) * 1e9, 55438, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(50, 140) * 1e9, 54309, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(50, 145) * 1e9, 53203, 145 + 532); + EXPECT_NEAR(get_mag_strength_tesla(50, 150) * 1e9, 52160, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(50, 155) * 1e9, 51205, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(50, 160) * 1e9, 50360, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(50, 165) * 1e9, 49639, 145 + 496); + EXPECT_NEAR(get_mag_strength_tesla(50, 170) * 1e9, 49053, 145 + 491); + EXPECT_NEAR(get_mag_strength_tesla(50, 175) * 1e9, 48610, 145 + 486); + EXPECT_NEAR(get_mag_strength_tesla(50, 180) * 1e9, 48319, 145 + 483); + EXPECT_NEAR(get_mag_strength_tesla(55, -180) * 1e9, 51311, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -175) * 1e9, 51180, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -170) * 1e9, 51185, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -165) * 1e9, 51324, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(55, -160) * 1e9, 51592, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, -155) * 1e9, 51976, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, -150) * 1e9, 52460, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, -145) * 1e9, 53022, 145 + 530); + EXPECT_NEAR(get_mag_strength_tesla(55, -140) * 1e9, 53638, 145 + 536); + EXPECT_NEAR(get_mag_strength_tesla(55, -135) * 1e9, 54282, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(55, -130) * 1e9, 54929, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(55, -125) * 1e9, 55555, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(55, -120) * 1e9, 56137, 145 + 561); + EXPECT_NEAR(get_mag_strength_tesla(55, -115) * 1e9, 56656, 145 + 567); + EXPECT_NEAR(get_mag_strength_tesla(55, -110) * 1e9, 57091, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(55, -105) * 1e9, 57423, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -100) * 1e9, 57635, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -95) * 1e9, 57711, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(55, -90) * 1e9, 57642, 145 + 576); + EXPECT_NEAR(get_mag_strength_tesla(55, -85) * 1e9, 57422, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, -80) * 1e9, 57057, 145 + 571); + EXPECT_NEAR(get_mag_strength_tesla(55, -75) * 1e9, 56560, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(55, -70) * 1e9, 55953, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(55, -65) * 1e9, 55266, 145 + 553); + EXPECT_NEAR(get_mag_strength_tesla(55, -60) * 1e9, 54531, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(55, -55) * 1e9, 53784, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, -50) * 1e9, 53054, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, -45) * 1e9, 52369, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(55, -40) * 1e9, 51747, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(55, -35) * 1e9, 51204, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(55, -30) * 1e9, 50750, 145 + 507); + EXPECT_NEAR(get_mag_strength_tesla(55, -25) * 1e9, 50389, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, -20) * 1e9, 50124, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(55, -15) * 1e9, 49956, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, -10) * 1e9, 49879, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, -5) * 1e9, 49889, 145 + 499); + EXPECT_NEAR(get_mag_strength_tesla(55, 0) * 1e9, 49977, 145 + 500); + EXPECT_NEAR(get_mag_strength_tesla(55, 5) * 1e9, 50136, 145 + 501); + EXPECT_NEAR(get_mag_strength_tesla(55, 10) * 1e9, 50358, 145 + 504); + EXPECT_NEAR(get_mag_strength_tesla(55, 15) * 1e9, 50641, 145 + 506); + EXPECT_NEAR(get_mag_strength_tesla(55, 20) * 1e9, 50987, 145 + 510); + EXPECT_NEAR(get_mag_strength_tesla(55, 25) * 1e9, 51400, 145 + 514); + EXPECT_NEAR(get_mag_strength_tesla(55, 30) * 1e9, 51886, 145 + 519); + EXPECT_NEAR(get_mag_strength_tesla(55, 35) * 1e9, 52451, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 40) * 1e9, 53097, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 45) * 1e9, 53825, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 50) * 1e9, 54626, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(55, 55) * 1e9, 55489, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 60) * 1e9, 56392, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 65) * 1e9, 57309, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(55, 70) * 1e9, 58209, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(55, 75) * 1e9, 59056, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 80) * 1e9, 59815, 145 + 598); + EXPECT_NEAR(get_mag_strength_tesla(55, 85) * 1e9, 60452, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 90) * 1e9, 60937, 145 + 609); + EXPECT_NEAR(get_mag_strength_tesla(55, 95) * 1e9, 61246, 145 + 612); + EXPECT_NEAR(get_mag_strength_tesla(55, 100) * 1e9, 61364, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(55, 105) * 1e9, 61282, 145 + 613); + EXPECT_NEAR(get_mag_strength_tesla(55, 110) * 1e9, 61002, 145 + 610); + EXPECT_NEAR(get_mag_strength_tesla(55, 115) * 1e9, 60537, 145 + 605); + EXPECT_NEAR(get_mag_strength_tesla(55, 120) * 1e9, 59907, 145 + 599); + EXPECT_NEAR(get_mag_strength_tesla(55, 125) * 1e9, 59143, 145 + 591); + EXPECT_NEAR(get_mag_strength_tesla(55, 130) * 1e9, 58280, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(55, 135) * 1e9, 57358, 145 + 574); + EXPECT_NEAR(get_mag_strength_tesla(55, 140) * 1e9, 56418, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(55, 145) * 1e9, 55494, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(55, 150) * 1e9, 54617, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(55, 155) * 1e9, 53810, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(55, 160) * 1e9, 53090, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(55, 165) * 1e9, 52471, 145 + 525); + EXPECT_NEAR(get_mag_strength_tesla(55, 170) * 1e9, 51963, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(55, 175) * 1e9, 51574, 145 + 516); + EXPECT_NEAR(get_mag_strength_tesla(55, 180) * 1e9, 51311, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -180) * 1e9, 53926, 145 + 539); + EXPECT_NEAR(get_mag_strength_tesla(60, -175) * 1e9, 53796, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -170) * 1e9, 53770, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -165) * 1e9, 53847, 145 + 538); + EXPECT_NEAR(get_mag_strength_tesla(60, -160) * 1e9, 54022, 145 + 540); + EXPECT_NEAR(get_mag_strength_tesla(60, -155) * 1e9, 54286, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -150) * 1e9, 54627, 145 + 546); + EXPECT_NEAR(get_mag_strength_tesla(60, -145) * 1e9, 55028, 145 + 550); + EXPECT_NEAR(get_mag_strength_tesla(60, -140) * 1e9, 55472, 145 + 555); + EXPECT_NEAR(get_mag_strength_tesla(60, -135) * 1e9, 55939, 145 + 559); + EXPECT_NEAR(get_mag_strength_tesla(60, -130) * 1e9, 56409, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, -125) * 1e9, 56864, 145 + 569); + EXPECT_NEAR(get_mag_strength_tesla(60, -120) * 1e9, 57284, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, -115) * 1e9, 57653, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -110) * 1e9, 57954, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -105) * 1e9, 58174, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -100) * 1e9, 58299, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -95) * 1e9, 58318, 145 + 583); + EXPECT_NEAR(get_mag_strength_tesla(60, -90) * 1e9, 58225, 145 + 582); + EXPECT_NEAR(get_mag_strength_tesla(60, -85) * 1e9, 58017, 145 + 580); + EXPECT_NEAR(get_mag_strength_tesla(60, -80) * 1e9, 57697, 145 + 577); + EXPECT_NEAR(get_mag_strength_tesla(60, -75) * 1e9, 57276, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, -70) * 1e9, 56768, 145 + 568); + EXPECT_NEAR(get_mag_strength_tesla(60, -65) * 1e9, 56193, 145 + 562); + EXPECT_NEAR(get_mag_strength_tesla(60, -60) * 1e9, 55574, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, -55) * 1e9, 54936, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, -50) * 1e9, 54301, 145 + 543); + EXPECT_NEAR(get_mag_strength_tesla(60, -45) * 1e9, 53691, 145 + 537); + EXPECT_NEAR(get_mag_strength_tesla(60, -40) * 1e9, 53124, 145 + 531); + EXPECT_NEAR(get_mag_strength_tesla(60, -35) * 1e9, 52614, 145 + 526); + EXPECT_NEAR(get_mag_strength_tesla(60, -30) * 1e9, 52171, 145 + 522); + EXPECT_NEAR(get_mag_strength_tesla(60, -25) * 1e9, 51804, 145 + 518); + EXPECT_NEAR(get_mag_strength_tesla(60, -20) * 1e9, 51516, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, -15) * 1e9, 51310, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, -10) * 1e9, 51185, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, -5) * 1e9, 51141, 145 + 511); + EXPECT_NEAR(get_mag_strength_tesla(60, 0) * 1e9, 51173, 145 + 512); + EXPECT_NEAR(get_mag_strength_tesla(60, 5) * 1e9, 51280, 145 + 513); + EXPECT_NEAR(get_mag_strength_tesla(60, 10) * 1e9, 51458, 145 + 515); + EXPECT_NEAR(get_mag_strength_tesla(60, 15) * 1e9, 51707, 145 + 517); + EXPECT_NEAR(get_mag_strength_tesla(60, 20) * 1e9, 52028, 145 + 520); + EXPECT_NEAR(get_mag_strength_tesla(60, 25) * 1e9, 52425, 145 + 524); + EXPECT_NEAR(get_mag_strength_tesla(60, 30) * 1e9, 52899, 145 + 529); + EXPECT_NEAR(get_mag_strength_tesla(60, 35) * 1e9, 53452, 145 + 535); + EXPECT_NEAR(get_mag_strength_tesla(60, 40) * 1e9, 54084, 145 + 541); + EXPECT_NEAR(get_mag_strength_tesla(60, 45) * 1e9, 54789, 145 + 548); + EXPECT_NEAR(get_mag_strength_tesla(60, 50) * 1e9, 55559, 145 + 556); + EXPECT_NEAR(get_mag_strength_tesla(60, 55) * 1e9, 56377, 145 + 564); + EXPECT_NEAR(get_mag_strength_tesla(60, 60) * 1e9, 57225, 145 + 572); + EXPECT_NEAR(get_mag_strength_tesla(60, 65) * 1e9, 58077, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 70) * 1e9, 58905, 145 + 589); + EXPECT_NEAR(get_mag_strength_tesla(60, 75) * 1e9, 59679, 145 + 597); + EXPECT_NEAR(get_mag_strength_tesla(60, 80) * 1e9, 60369, 145 + 604); + EXPECT_NEAR(get_mag_strength_tesla(60, 85) * 1e9, 60949, 145 + 609); + EXPECT_NEAR(get_mag_strength_tesla(60, 90) * 1e9, 61395, 145 + 614); + EXPECT_NEAR(get_mag_strength_tesla(60, 95) * 1e9, 61689, 145 + 617); + EXPECT_NEAR(get_mag_strength_tesla(60, 100) * 1e9, 61820, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 105) * 1e9, 61784, 145 + 618); + EXPECT_NEAR(get_mag_strength_tesla(60, 110) * 1e9, 61586, 145 + 616); + EXPECT_NEAR(get_mag_strength_tesla(60, 115) * 1e9, 61238, 145 + 612); + EXPECT_NEAR(get_mag_strength_tesla(60, 120) * 1e9, 60758, 145 + 608); + EXPECT_NEAR(get_mag_strength_tesla(60, 125) * 1e9, 60170, 145 + 602); + EXPECT_NEAR(get_mag_strength_tesla(60, 130) * 1e9, 59504, 145 + 595); + EXPECT_NEAR(get_mag_strength_tesla(60, 135) * 1e9, 58788, 145 + 588); + EXPECT_NEAR(get_mag_strength_tesla(60, 140) * 1e9, 58054, 145 + 581); + EXPECT_NEAR(get_mag_strength_tesla(60, 145) * 1e9, 57328, 145 + 573); + EXPECT_NEAR(get_mag_strength_tesla(60, 150) * 1e9, 56633, 145 + 566); + EXPECT_NEAR(get_mag_strength_tesla(60, 155) * 1e9, 55989, 145 + 560); + EXPECT_NEAR(get_mag_strength_tesla(60, 160) * 1e9, 55408, 145 + 554); + EXPECT_NEAR(get_mag_strength_tesla(60, 165) * 1e9, 54904, 145 + 549); + EXPECT_NEAR(get_mag_strength_tesla(60, 170) * 1e9, 54484, 145 + 545); + EXPECT_NEAR(get_mag_strength_tesla(60, 175) * 1e9, 54156, 145 + 542); + EXPECT_NEAR(get_mag_strength_tesla(60, 180) * 1e9, 53926, 145 + 539); } diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp index 3358340b7e..4707774eb0 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.cpp +++ b/src/modules/airspeed_selector/AirspeedValidator.cpp @@ -136,11 +136,11 @@ AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vec TAS_sum += _scale_check_TAS(i); } - const float TAS_to_grounspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_validated; - const float TAS_to_grounspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale(); + const float TAS_to_groundspeed_error_current = ground_speed_sum - TAS_sum * _CAS_scale_validated; + const float TAS_to_groundspeed_error_new = ground_speed_sum - TAS_sum * _wind_estimator.get_tas_scale(); // check passes if the average airspeed with the scale applied is closer to groundspeed than without - if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) { + if (fabsf(TAS_to_groundspeed_error_new) < fabsf(TAS_to_groundspeed_error_current)) { // constrain the scale update to max 0.05 at a time const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.05f, @@ -226,13 +226,13 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ || _tas_innov_integ_threshold <= 0.f) { _innovations_check_failed = false; _time_last_tas_pass = time_now; - _apsd_innov_integ_state = 0.f; + _aspd_innov_integ_state = 0.f; } else if (!lpos_valid || estimator_status_vel_test_ratio > 1.f || estimator_status_mag_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; - _apsd_innov_integ_state = 0.f; + _aspd_innov_integ_state = 0.f; } else { // nav velocity data is likely good so airspeed innovations are able to be used @@ -242,14 +242,14 @@ AirspeedValidator::check_airspeed_innovation(uint64_t time_now, float estimator_ const float tas_innov = fabsf(_TAS - air_vel.norm()); if (tas_innov > _tas_innov_threshold) { - _apsd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance + _aspd_innov_integ_state += dt_s * (tas_innov - _tas_innov_threshold); // integrate exceedance } else { // reset integrator used to trigger and record pass if integrator check is disabled - _apsd_innov_integ_state = 0.f; + _aspd_innov_integ_state = 0.f; } - if (_tas_innov_integ_threshold > 0.f && _apsd_innov_integ_state < _tas_innov_integ_threshold) { + if (_tas_innov_integ_threshold > 0.f && _aspd_innov_integ_state < _tas_innov_integ_threshold) { _time_last_tas_pass = time_now; } diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index c0b6db0377..ba8f5c1a30 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/src/modules/airspeed_selector/AirspeedValidator.hpp @@ -151,7 +151,7 @@ private: 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 _apsd_innov_integ_state{0.0f}; ///< integral of excess normalised airspeed innovation (sec) + 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) diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index cdbeeb2162..cdf5e4108e 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/src/modules/airspeed_selector/airspeed_selector_params.c @@ -1,6 +1,6 @@ /** - * Airspeed Selector: Wind estimator wind process noise noise spectral density + * Wind estimator wind process noise spectral density * * Wind process noise of the internal wind estimator(s) of the airspeed selector. * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second. @@ -14,7 +14,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f); /** - * Airspeed Selector: Wind estimator true airspeed scale process noise spectral density + * Wind estimator true airspeed scale process noise spectral density * * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector. * When unaided, the scale uncertainty (1-sigma, unitless) increases by this amount every second. @@ -28,7 +28,7 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f); PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f); /** - * Airspeed Selector: Wind estimator true airspeed measurement noise + * Wind estimator true airspeed measurement noise * * True airspeed measurement noise of the internal wind estimator(s) of the airspeed selector. * @@ -41,7 +41,7 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f); PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); /** - * Airspeed Selector: Wind estimator sideslip measurement noise + * Wind estimator sideslip measurement noise * * Sideslip measurement noise of the internal wind estimator(s) of the airspeed selector. * @@ -54,7 +54,7 @@ PARAM_DEFINE_FLOAT(ASPD_TAS_NOISE, 1.4f); PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); /** - * Airspeed Selector: Gate size for true airspeed fusion + * Gate size for true airspeed fusion * * Sets the number of standard deviations used by the innovation consistency test. * @@ -66,7 +66,7 @@ PARAM_DEFINE_FLOAT(ASPD_BETA_NOISE, 0.3f); PARAM_DEFINE_INT32(ASPD_TAS_GATE, 3); /** - * Airspeed Selector: Gate size for sideslip angle fusion + * Gate size for sideslip angle fusion * * Sets the number of standard deviations used by the innovation consistency test. * @@ -148,7 +148,6 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1); * Enable checks on airspeed sensors * * Controls which checks are run to check airspeed data for validity. Only applied if ASPD_PRIMARY > 0. - * Note that the data missing check is enabled if any of the options is set. * * @min 0 * @max 15 @@ -232,7 +231,7 @@ PARAM_DEFINE_INT32(ASPD_FS_T_START, -1); * Horizontal wind uncertainty threshold for synthetic airspeed. * * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid - * as soon and as long the horizontal wind uncertainty drops below this value. + * as soon and as long the horizontal wind uncertainty is below this value. * * @unit m/s * @min 0.001 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 89ff104279..b4da68f722 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -219,7 +219,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(gps.lat, gps.lon)); + update_mag_declination(get_mag_declination_radians((float)gps.latitude_deg, + (float)gps.longitude_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 0cea30cda2..90a9342bb5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -122,7 +122,7 @@ PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); * @group Attitude Q estimator * @boolean */ -PARAM_DEFINE_INT32(ATT_ACC_COMP, 1); +PARAM_DEFINE_INT32(ATT_ACC_COMP, 0); /** * Gyro bias limit diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp deleted file mode 100644 index af86b8b4ee..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ /dev/null @@ -1,174 +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. - * - ****************************************************************************/ - -#include "ArmStateMachine.hpp" - -#include - -constexpr bool -ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]; - -transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, - const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks, - const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason) -{ - // Double check that our static arrays are still valid - static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); - static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, - "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); - - transition_result_t ret = TRANSITION_DENIED; - bool feedback_provided = false; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == _arm_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][_arm_state]; - - // Preflight check - if (valid_transition - && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) - && fRunPreArmChecks - && !(status.hil_state == vehicle_status_s::HIL_STATE_ON) - && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) { - - checks.update(); - - if (!checks.canArm(status.nav_state)) { - feedback_provided = true; // Preflight checks report error messages - valid_transition = false; - } - } - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - /* enforce lockdown in HIL */ - armed.lockdown = true; - - /* recover from a prearm fail */ - if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - _arm_state = vehicle_status_s::ARMING_STATE_STANDBY; - } - - // HIL can always go to standby - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - valid_transition = true; - } - } - - // Finish up the state transition - if (valid_transition) { - ret = TRANSITION_CHANGED; - - // Record arm/disarm reason - if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition - status.latest_disarming_reason = (uint8_t)calling_reason; - - } else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition - status.latest_arming_reason = (uint8_t)calling_reason; - } - - // Switch state - _arm_state = new_arming_state; - - if (isArmed()) { - status.armed_time = hrt_absolute_time(); - - } else { - status.armed_time = 0; - } - } - } - - if (ret == TRANSITION_DENIED) { - /* print to MAVLink and console if we didn't provide any feedback yet */ - if (!feedback_provided) { - // FIXME: this catch-all does not provide helpful information to the user - mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t", - getArmStateName(_arm_state), getArmStateName(new_arming_state)); - events::send( - events::ID("commander_transition_denied"), events::Log::Critical, - "Arming state transition denied: {1} to {2}", - getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state)); - } - } - - return ret; -} - -const char *ArmStateMachine::getArmStateName(uint8_t arming_state) -{ - switch (arming_state) { - - case vehicle_status_s::ARMING_STATE_INIT: return "Init"; - - case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; - - case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; - - default: return "Unknown"; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, - "enum def mismatch"); -} - -events::px4::enums::arming_state_t ArmStateMachine::getArmStateEvent(uint8_t arming_state) -{ - switch (arming_state) { - case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; - - case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; - - case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, - "enum def mismatch"); - - return events::px4::enums::arming_state_t::init; -} diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp deleted file mode 100644 index e52c634b18..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ /dev/null @@ -1,95 +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. - * - ****************************************************************************/ - -#pragma once - -#include "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp" -#include -#include -#include -#include -#include - -typedef enum { - TRANSITION_DENIED = -1, - TRANSITION_NOT_CHANGED = 0, - TRANSITION_CHANGED -} transition_result_t; - -using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; - -class ArmStateMachine -{ -public: - ArmStateMachine() = default; - ~ArmStateMachine() = default; - - void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } - - transition_result_t - arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state, - actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason); - - // Getters - uint8_t getArmState() const { return _arm_state; } - - bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); } - bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); } - bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); } - bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); } - - static const char *getArmStateName(uint8_t arming_state); - const char *getArmStateName() const { return getArmStateName(_arm_state); } - -private: - static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state); - - uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT}; - - // This array defines the arming state transitions. The rows are the new state, and the columns - // are the current state. Using new state and current state you can index into the array which - // will be true for a valid transition or false for a invalid transition. In some cases even - // though the transition is marked as true additional checks must be made. See arming_state_transition - // code for those checks. - static constexpr bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] - = { - // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI - }; -}; diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp deleted file mode 100644 index f141f2ddcf..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ /dev/null @@ -1,273 +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. - * - ****************************************************************************/ - -#include -#include "ArmStateMachine.hpp" - -TEST(ArmStateMachineTest, ArmingStateTransitionTest) -{ - ArmStateMachine arm_state_machine; - - // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed - // to simulate machine state prior to testing an arming state transition. This structure is also - // use to represent the expected machine state after the transition has been requested. - typedef struct { - arming_state_t arming_state; // vehicle_status_s.arming_state - bool armed; // actuator_armed_s.armed - } ArmingTransitionVolatileState_t; - - // This structure represents a test case for arming_state_transition. It contains the machine - // state prior to transition, the requested state to transition to and finally the expected - // machine state after transition. - typedef struct { - const char *assertMsg; // Text to show when test case fails - ArmingTransitionVolatileState_t current_state; // Machine state prior to transition - hil_state_t hil_state; // Current vehicle_status_s.hil_state - bool safety_button_available; // Current safety_s.safety_button_available - bool safety_off; // Current safety_s.safety_off - arming_state_t requested_state; // Requested arming state to transition to - ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition - transition_result_t expected_transition_result; // Expected result from arming_state_transition - } ArmingTransitionTest_t; - - // We use these defines so that our test cases are more readable - static constexpr bool ATT_ARMED = true; - static constexpr bool ATT_DISARMED = false; - static constexpr bool ATT_SAFETY_AVAILABLE = true; - static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true; - static constexpr bool ATT_SAFETY_OFF = true; - static constexpr bool ATT_SAFETY_ON = false; - - // These are test cases for arming_state_transition - static const ArmingTransitionTest_t rgArmingTransitionTests[] = { - // TRANSITION_NOT_CHANGED tests - - { - "no transition: identical states", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED - }, - - // TRANSITION_CHANGED tests - - // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - - { - "transition: init to standby", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: init to standby error", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: init to reboot", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to init", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to standby error", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: armed to standby", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby error to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: in air restore to armed", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - { - "transition: in air restore to reboot", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - // hil on tests, standby error to standby not normally allowed - - { - "transition: standby error to standby, hil on", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - // Safety button arming tests - - { - "transition: standby to armed, no safety button", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to armed, safety button off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - // TRANSITION_DENIED tests - - // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - - { - "no transition: init to armed", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: armed to init", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED - }, - - { - "no transition: armed to reboot", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED - }, - - { - "no transition: standby error to armed", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: standby error to standby", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: reboot to armed", - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: in air restore to standby", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED - }, - - // Safety button arming tests - - { - "no transition: init to armed, safety button on", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED - }, - }; - - struct vehicle_status_s status {}; - struct actuator_armed_s armed {}; - - size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); - - for (size_t i = 0; i < cArmingTransitionTests; i++) { - const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i]; - - // Setup initial machine state - arm_state_machine.forceArmState(test->current_state.arming_state); - status.hil_state = test->hil_state; - - HealthAndArmingChecks health_and_arming_checks(nullptr, status); - - // Attempt transition - transition_result_t result = arm_state_machine.arming_state_transition( - status, - test->requested_state, - armed, - health_and_arming_checks, - true /* enable pre-arm checks */, - nullptr /* no mavlink_log_pub */, - arm_disarm_reason_t::unit_test); - - // Validate result of transition - EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg; - EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg; - EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg; - } -} diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index 37aa7473d9..067278f60a 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -32,4 +32,3 @@ ############################################################################ add_subdirectory(ArmAuthorization) -add_subdirectory(ArmStateMachine) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index fdc5e949b1..69b3ae4bd5 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -66,7 +66,6 @@ px4_add_module( health_and_arming_checks hysteresis ArmAuthorization - ArmStateMachine sensor_calibration world_magnetic_model mode_util diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c317f6bda..1397156c91 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -473,7 +473,7 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); + PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed"); PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]); PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]); PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no"); @@ -487,13 +487,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) return Commander::main(argc, argv); } -bool Commander::shutdownIfAllowed() -{ - return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks, - false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown); -} - static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { switch (calling_reason) { @@ -533,20 +526,37 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) { - // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming + if (isArmed()) { + return TRANSITION_NOT_CHANGED; + } + + if (_vehicle_status.calibration_enabled + || _vehicle_status.rc_calibration_in_progress + || _actuator_armed.in_esc_calibration_mode) { + + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: calibrating\t"); + events::send(events::ID("commander_arm_denied_calibrating"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: calibrating"); + tune_negative(true); + return TRANSITION_DENIED; + } + + // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidental in-air disarming if (calling_reason == arm_disarm_reason_t::rc_switch - && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + && ((_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) { + run_preflight_checks = false; } - if (run_preflight_checks && !_arm_state_machine.isArmed()) { + if (run_preflight_checks) { if (_vehicle_control_mode.flag_control_manual_enabled) { + if (_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t"); - events::send(events::ID("commander_arm_denied_throttle_center"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: throttle above center"); + events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: throttle above center"); tune_negative(true); return TRANSITION_DENIED; } @@ -554,10 +564,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); - events::send(events::ID("commander_arm_denied_throttle_high"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: high throttle"); + events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: high throttle"); tune_negative(true); return TRANSITION_DENIED; } @@ -565,35 +575,45 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } else if (calling_reason == arm_disarm_reason_t::rc_stick || calling_reason == arm_disarm_reason_t::rc_switch || calling_reason == arm_disarm_reason_t::rc_button) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t"); - events::send(events::ID("commander_arm_denied_not_manual"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: switch to manual mode first"); + events::send(events::ID("commander_arm_denied_not_manual"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: switch to manual mode first"); + tune_negative(true); + return TRANSITION_DENIED; + } + + _health_and_arming_checks.update(); + + if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) { tune_negative(true); return TRANSITION_DENIED; } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks, - &_mavlink_log_pub, calling_reason); + _vehicle_status.armed_time = hrt_absolute_time(); + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; + _vehicle_status.latest_arming_reason = (uint8_t)calling_reason; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); - events::send(events::ID("commander_armed_by"), events::Log::Info, - "Armed by {1}", calling_reason); + mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_armed_by"), events::Log::Info, + "Armed by {1}", calling_reason); - _status_changed = true; - - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); + if (_param_com_home_en.get()) { + _home_position.setHomePosition(); } - return arming_res; + _status_changed = true; + + return TRANSITION_CHANGED; } transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced) { + if (!isArmed()) { + return TRANSITION_NOT_CHANGED; + } + if (!forced) { const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed || is_ground_vehicle(_vehicle_status)); @@ -606,36 +626,43 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) { if (calling_reason != arm_disarm_reason_t::rc_stick) { - mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t"); - events::send(events::ID("commander_disarming_denied_not_landed"), + mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t"); + events::send(events::ID("commander_disarm_denied_not_landed"), {events::Log::Critical, events::LogInternal::Info}, - "Disarming denied, not landed"); + "Disarming denied: not landed"); } return TRANSITION_DENIED; } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false, - &_mavlink_log_pub, calling_reason); + _vehicle_status.armed_time = 0; + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; + _vehicle_status.latest_disarming_reason = (uint8_t)calling_reason; + _vehicle_status.takeoff_time = 0; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); - events::send(events::ID("commander_disarmed_by"), events::Log::Info, - "Disarmed by {1}", calling_reason); + _have_taken_off_since_arming = false; - if (_param_com_force_safety.get()) { - _safety.activateSafety(); - } + _last_disarmed_timestamp = hrt_absolute_time(); - _status_changed = true; + _user_mode_intention.onDisarm(); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_disarmed_by"), events::Log::Info, + "Disarmed by {1}", calling_reason); + + if (_param_com_force_safety.get()) { + _safety.activateSafety(); } - return arming_res; + // 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(); + + _status_changed = true; + + return TRANSITION_CHANGED; } Commander::Commander() : @@ -643,19 +670,15 @@ Commander::Commander() : { _vehicle_land_detected.landed = true; + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; _vehicle_status.system_id = 1; _vehicle_status.component_id = 1; - _vehicle_status.system_type = 0; _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - _vehicle_status.nav_state = _user_mode_intention.get(); _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); _vehicle_status.nav_state_timestamp = hrt_absolute_time(); - - /* mark all signals lost as long as they haven't been found */ _vehicle_status.gcs_connection_lost = true; - _vehicle_status.power_input_valid = true; // default for vtol is rotary wing @@ -696,8 +719,14 @@ Commander::handle_command(const vehicle_command_s &cmd) // to not require navigator and command to receive / process // the data at the exact same time. - // Check if a mode switch had been requested - if ((((uint32_t)cmd.param2) & 1) > 0) { + const uint32_t change_mode_flags = uint32_t(cmd.param2); + const bool mode_switch_not_requested = (change_mode_flags & 1) == 0; + const bool unsupported_bits_set = (change_mode_flags & ~1) != 0; + + if (mode_switch_not_requested || unsupported_bits_set) { + answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED); + + } else { if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; @@ -705,9 +734,6 @@ Commander::handle_command(const vehicle_command_s &cmd) printRejectMode(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - } else { - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } } break; @@ -858,16 +884,6 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { // Arm is forced (checks skipped) when param2 is set to a magic number. const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); - const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - - // Flick to in-air restore first if this comes from an onboard system and from IO - if (!forced && cmd_from_io - && (cmd.source_system == _vehicle_status.system_id) - && (cmd.source_component == _vehicle_status.component_id) - && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { - // TODO: replace with a proper allowed transition - _arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE); - } transition_result_t arming_res = TRANSITION_DENIED; arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external : @@ -886,47 +902,26 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - - /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) - && (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) - && (_param_com_home_en.get())) { - _home_position.setHomePosition(); - } } } } break; case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { - if (cmd.param1 > 1.5f) { - // Test termination command triggers lockdown but not actual termination. - if (!_lockdown_triggered) { - _actuator_armed.lockdown = true; - _lockdown_triggered = true; - PX4_WARN("forcing lockdown (motors off)"); - } + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; - } else if (cmd.param1 > 0.5f) { + if (cmd.param1 > 0.5f) { // Trigger real termination. - if (!_flight_termination_triggered) { - _actuator_armed.force_failsafe = true; - _flight_termination_triggered = true; - PX4_WARN("forcing failsafe (termination)"); - send_parachute_command(); + if (!isArmed()) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; + + } else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - } else { - _actuator_armed.force_failsafe = false; - _actuator_armed.lockdown = false; - - _lockdown_triggered = false; - _flight_termination_triggered = false; - - PX4_WARN("disabling failsafe and lockdown"); } - - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -999,6 +994,7 @@ Commander::handle_command(const vehicle_command_s &cmd) break; case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF: +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* ok, home set, use it to take off */ if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) { @@ -1009,6 +1005,9 @@ Commander::handle_command(const vehicle_command_s &cmd) cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } +#else + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED; +#endif // CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF break; case vehicle_command_s::VEHICLE_CMD_NAV_LAND: { @@ -1130,7 +1129,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1140,7 +1139,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(BOARD_HAS_POWER_CONTROL) - } else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) { + } else if ((param1 == 2) && !isArmed() && (px4_shutdown_request(400_ms) == 0)) { // 2: Shutdown autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1150,7 +1149,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1167,25 +1166,13 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks, - false /* fRunPreArmChecks */, &_mavlink_log_pub, - (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) - ) { - - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); - break; - - } - if ((int)(cmd.param1) == 1) { /* gyro calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1292,7 +1279,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { // Magnetometer quick calibration using world magnetic model and known heading - if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1327,7 +1314,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1424,7 +1411,7 @@ Commander::handle_command(const vehicle_command_s &cmd) unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) { - if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { + if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1504,7 +1491,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break; case action_request_s::ACTION_TOGGLE_ARMING: - if (_arm_state_machine.isArmed()) { + if (isArmed()) { disarm(arm_disarm_reason); } else { @@ -1540,7 +1527,6 @@ void Commander::executeActionRequest(const action_request_s &action_request) _status_changed = true; _actuator_armed.manual_lockdown = true; - send_parachute_command(); } break; @@ -1674,20 +1660,12 @@ void Commander::run() vtolStatusUpdate(); - _home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed); + _home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed); handleAutoDisarm(); battery_status_check(); - /* If in INIT state, try to proceed to STANDBY state */ - if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) { - - _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, - true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby); - } - checkForMissionUpdate(); manualControlCheck(); @@ -1754,40 +1732,30 @@ void Commander::run() } } - // check for arming state changes - if (_was_armed != _arm_state_machine.isArmed()) { - _status_changed = true; - } - - if (!_was_armed && _arm_state_machine.isArmed() && !_vehicle_land_detected.landed) { - _have_taken_off_since_arming = true; - } - - if (_was_armed && !_arm_state_machine.isArmed()) { - const int32_t flight_uuid = _param_flight_uuid.get() + 1; - _param_flight_uuid.set(flight_uuid); - _param_flight_uuid.commit_no_notification(); - - _last_disarmed_timestamp = hrt_absolute_time(); - - _user_mode_intention.onDisarm(); - _vehicle_status.takeoff_time = 0; - } - - if (!_arm_state_machine.isArmed()) { - /* Reset the flag if disarmed. */ - _have_taken_off_since_arming = false; - } - + // update actuator_armed + _actuator_armed.armed = isArmed(); _actuator_armed.prearmed = getPrearmState(); + _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); + _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) + || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)); + // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL + _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); + // _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION + + // if force_failsafe or manual_lockdown activated send parachute command + if ((!actuator_armed_prev.force_failsafe && _actuator_armed.force_failsafe) + || (!actuator_armed_prev.manual_lockdown && _actuator_armed.manual_lockdown) + ) { + if (isArmed()) { + send_parachute_command(); + } + } // publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { // publish actuator_armed first (used by output modules) - _actuator_armed.armed = _arm_state_machine.isArmed(); - _actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); _actuator_armed.timestamp = hrt_absolute_time(); _actuator_armed_pub.publish(_actuator_armed); @@ -1795,7 +1763,6 @@ void Commander::run() updateControlMode(); // vehicle_status publish (after prearm/preflight updates above) - _vehicle_status.arming_state = _arm_state_machine.getArmState(); _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); @@ -1822,11 +1789,9 @@ void Commander::run() _status_changed = false; - _was_armed = _arm_state_machine.isArmed(); - arm_auth_update(hrt_absolute_time(), params_updated); - px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed()); + px4_indicate_external_reset_lockout(LockoutComponent::Commander, isArmed()); perf_end(_loop_perf); @@ -1877,7 +1842,7 @@ void Commander::checkForMissionUpdate() } } - if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed + if (isArmed() && !_vehicle_land_detected.landed && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) { @@ -1901,6 +1866,10 @@ void Commander::checkForMissionUpdate() bool Commander::getPrearmState() const { + if (_vehicle_status.calibration_enabled) { + return false; + } + switch ((PrearmedMode)_param_com_prearm_mode.get()) { case PrearmedMode::DISABLED: /* skip prearmed state */ @@ -1936,7 +1905,7 @@ void Commander::handlePowerButtonState() if (_power_button_state_sub.copy(&button_state)) { if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { - if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) { + if (!isArmed() && (px4_shutdown_request() == 0)) { while (1) { px4_usleep(1); } } } @@ -1973,7 +1942,7 @@ void Commander::landDetectorUpdate() _vehicle_land_detected_sub.copy(&_vehicle_land_detected); // Only take actions if armed - if (_arm_state_machine.isArmed()) { + if (isArmed()) { if (!was_landed && _vehicle_land_detected.landed) { mavlink_log_info(&_mavlink_log_pub, "Landing detected\t"); events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected"); @@ -1987,9 +1956,8 @@ void Commander::landDetectorUpdate() // automatically set or update home position if (_param_com_home_en.get()) { - // set the home position when taking off, but only if we were previously disarmed - // and at least 500 ms from commander start spent to avoid setting home on in-air restart - if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + // set the home position when taking off + if (!_vehicle_land_detected.landed) { if (was_landed) { _home_position.setHomePosition(); @@ -2060,7 +2028,7 @@ void Commander::vtolStatusUpdate() void Commander::updateTunes() { // play arming and battery warning tunes - if (!_arm_tune_played && _arm_state_machine.isArmed()) { + if (!_arm_tune_played && isArmed()) { /* play tune when armed */ set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); @@ -2069,6 +2037,7 @@ void Commander::updateTunes() } else if (!_vehicle_status.usb_connected && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + /* play tune on battery critical */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); @@ -2077,7 +2046,7 @@ void Commander::updateTunes() /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); - } else if (_vehicle_status.failsafe && _arm_state_machine.isArmed()) { + } else if (_vehicle_status.failsafe && isArmed()) { tune_failsafe(true); } else { @@ -2085,7 +2054,7 @@ void Commander::updateTunes() } /* reset arm_tune_played when disarmed */ - if (!_arm_state_machine.isArmed()) { + if (!isArmed()) { // Notify the user that it is safe to approach the vehicle if (_arm_tune_played) { @@ -2119,7 +2088,7 @@ void Commander::checkWorkerThread() void Commander::handleAutoDisarm() { // Auto disarm when landed or kill switch engaged - if (_arm_state_machine.isArmed()) { + if (isArmed()) { // Check for auto-disarm on landing or pre-flight if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { @@ -2178,7 +2147,7 @@ bool Commander::handleModeIntentionAndFailsafe() const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction(); FailsafeBase::State state{}; - state.armed = _arm_state_machine.isArmed(); + state.armed = isArmed(); state.vtol_in_transition_mode = _vehicle_status.in_transition_mode; state.mission_finished = _mission_result_sub.get().finished; state.user_intended_mode = _user_mode_intention.get(); @@ -2210,13 +2179,6 @@ bool Commander::handleModeIntentionAndFailsafe() case FailsafeBase::Action::Terminate: _vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION; - _actuator_armed.force_failsafe = true; - - if (!_flight_termination_triggered) { - _flight_termination_triggered = true; - send_parachute_command(); - } - break; default: @@ -2293,14 +2255,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) uint8_t led_color = led_control_s::COLOR_WHITE; bool set_normal_color = false; - uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms; + uint64_t overload_warn_delay = isArmed() ? 1_ms : 250_ms; // set mode if (overload && (time_now_us >= _overload_start + overload_warn_delay)) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_PURPLE; - } else if (_arm_state_machine.isArmed()) { + } else if (isArmed()) { led_mode = led_control_s::MODE_ON; set_normal_color = true; @@ -2308,18 +2270,9 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; - } else if (_arm_state_machine.isStandby()) { + } else { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - - } else if (_arm_state_machine.isInit()) { - // if in init status it should not be in the error state - led_mode = led_control_s::MODE_OFF; - - } else { - // STANDBY_ERROR and other states - led_mode = led_control_s::MODE_BLINK_NORMAL; - led_color = led_control_s::COLOR_RED; } if (set_normal_color) { @@ -2352,7 +2305,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) - if (_arm_state_machine.isArmed()) { + if (isArmed()) { if (_vehicle_status.failsafe) { BOARD_ARMED_LED_OFF(); @@ -2368,7 +2321,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) BOARD_ARMED_LED_ON(); } - } else if (_arm_state_machine.isStandby()) { + } else if (_vehicle_status.pre_flight_checks_pass) { BOARD_ARMED_LED_OFF(); // ready to arm, blink at 1Hz @@ -2404,7 +2357,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) void Commander::updateControlMode() { _vehicle_control_mode = {}; - mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state, + mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state, _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); _vehicle_control_mode.timestamp = hrt_absolute_time(); _vehicle_control_mode_pub.publish(_vehicle_control_mode); @@ -2426,7 +2379,7 @@ void Commander::printRejectMode(uint8_t nav_state) /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ - tune_negative(_arm_state_machine.isArmed()); + tune_negative(isArmed()); _last_print_mode_reject_time = hrt_absolute_time(); } @@ -2698,7 +2651,7 @@ void Commander::battery_status_check() if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) - if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) { + if (!isArmed() && (px4_shutdown_request(60_s) == 0)) { mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t"); events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Dangerously low battery! Shutting system down"); @@ -2731,7 +2684,7 @@ void Commander::manualControlCheck() _is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f); _is_throttle_low = (manual_control_setpoint.throttle < -0.8f); - if (_arm_state_machine.isArmed()) { + if (isArmed()) { // Abort autonomous mode and switch to position mode if sticks are moved significantly // but only if actually in air. if (manual_control_setpoint.sticks_moving @@ -2794,10 +2747,9 @@ void Commander::send_parachute_command() vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE; vcmd.param1 = static_cast(vehicle_command_s::PARACHUTE_ACTION_RELEASE); - uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; - vcmd.source_system = vehicle_status_sub.get().system_id; - vcmd.target_system = vehicle_status_sub.get().system_id; - vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.source_system = _vehicle_status.system_id; + vcmd.target_system = _vehicle_status.system_id; + vcmd.source_component = _vehicle_status.component_id; vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3678b9ea9d..e6f79a1aca 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-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 @@ -34,7 +34,6 @@ #pragma once /* Helper classes */ -#include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "failure_detector/FailureDetector.hpp" #include "failsafe/failsafe.h" #include "Safety.hpp" @@ -87,6 +86,14 @@ using math::constrain; using systemlib::Hysteresis; +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED +} transition_result_t; + +using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; + using namespace time_literals; class Commander : public ModuleBase, public ModuleParams @@ -116,6 +123,8 @@ public: void enable_hil(); private: + bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); } + void answer_command(const vehicle_command_s &cmd, uint8_t result); transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); @@ -153,8 +162,6 @@ private: void updateControlMode(); - bool shutdownIfAllowed(); - void send_parachute_command(); void checkForMissionUpdate(); @@ -196,11 +203,9 @@ private: /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; - static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; vehicle_status_s _vehicle_status{}; - ArmStateMachine _arm_state_machine{}; Failsafe _failsafe_instance{this}; FailsafeBase &_failsafe{_failsafe_instance}; FailureDetector _failure_detector{this}; @@ -242,9 +247,6 @@ private: bool _failsafe_user_override_request{false}; ///< override request due to stick movements - bool _flight_termination_triggered{false}; - bool _lockdown_triggered{false}; - bool _open_drone_id_system_lost{true}; bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; @@ -257,7 +259,6 @@ private: bool _is_throttle_low{false}; bool _arm_tune_played{false}; - bool _was_armed{false}; bool _have_taken_off_since_arming{false}; bool _status_changed{true}; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 89dbf9fe19..b62c2805dc 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -34,34 +34,37 @@ px4_add_library(health_and_arming_checks Common.cpp HealthAndArmingChecks.cpp - checks/systemCheck.cpp - checks/magnetometerCheck.cpp + checks/accelerometerCheck.cpp - checks/gyroCheck.cpp - checks/baroCheck.cpp - checks/imuConsistencyCheck.cpp checks/airspeedCheck.cpp + checks/armPermissionCheck.cpp + checks/baroCheck.cpp + checks/batteryCheck.cpp + checks/cpuResourceCheck.cpp checks/distanceSensorChecks.cpp checks/escCheck.cpp - checks/rcCalibrationCheck.cpp - checks/powerCheck.cpp checks/estimatorCheck.cpp checks/failureDetectorCheck.cpp - checks/manualControlCheck.cpp - checks/modeCheck.cpp - checks/cpuResourceCheck.cpp - checks/sdcardCheck.cpp - checks/parachuteCheck.cpp - checks/batteryCheck.cpp - checks/windCheck.cpp - checks/geofenceCheck.cpp - checks/homePositionCheck.cpp checks/flightTimeCheck.cpp + checks/geofenceCheck.cpp + checks/gyroCheck.cpp + checks/homePositionCheck.cpp + checks/imuConsistencyCheck.cpp + checks/magnetometerCheck.cpp + checks/manualControlCheck.cpp checks/missionCheck.cpp - checks/rcAndDataLinkCheck.cpp - checks/vtolCheck.cpp + checks/modeCheck.cpp checks/offboardCheck.cpp checks/openDroneIDCheck.cpp + checks/parachuteCheck.cpp + checks/powerCheck.cpp + checks/rcAndDataLinkCheck.cpp + checks/rcCalibrationCheck.cpp + checks/sdcardCheck.cpp + checks/systemCheck.cpp + checks/vtolCheck.cpp + checks/windCheck.cpp + ) add_dependencies(health_and_arming_checks mode_util) diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index ac5ffbe567..f98ebb2c46 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -42,6 +42,7 @@ #include "checks/accelerometerCheck.hpp" #include "checks/airspeedCheck.hpp" +#include "checks/armPermissionCheck.hpp" #include "checks/baroCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" @@ -115,6 +116,7 @@ private: // all checks AccelerometerChecks _accelerometer_checks; AirspeedChecks _airspeed_checks; + ArmPermissionChecks _arm_permission_checks; BaroChecks _baro_checks; CpuResourceChecks _cpu_resource_checks; DistanceSensorChecks _distance_sensor_checks; @@ -145,6 +147,7 @@ private: HealthAndArmingCheckBase *_checks[31] = { &_accelerometer_checks, &_airspeed_checks, + &_arm_permission_checks, &_baro_checks, &_cpu_resource_checks, &_distance_sensor_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp index 2d65182c19..7c6ee617fb 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.cpp @@ -79,8 +79,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) } } - if (!context.isArmed() && _param_com_arm_arsp_en.get() - && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { + if (!context.isArmed() && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { /* EVENT * @description * Current airspeed reading too high. Check if wind is below maximum airspeed and redo airspeed @@ -89,7 +88,7 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter) * * Measured: {1:.1m/s}, limit: {2:.1m/s}. * - * This check can be configured via COM_ARM_ARSP_EN and FW_AIRSPD_MAX parameter. + * This check can be configured via FW_AIRSPD_MAX parameter. * */ reporter.armingCheckFailure(NavModes::None, health_component_t::differential_pressure, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp index 59aa82bc52..1c3ac53b12 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/airspeedCheck.hpp @@ -53,7 +53,6 @@ private: const param_t _param_fw_airspd_max_handle; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_cbrk_airspd_chk, - (ParamBool) _param_com_arm_arsp_en + (ParamInt) _param_cbrk_airspd_chk ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.cpp new file mode 100644 index 0000000000..d25cff98d5 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.cpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 "armPermissionCheck.hpp" + +void ArmPermissionChecks::checkAndReport(const Context &context, Report &reporter) +{ + if (_param_com_armable.get() < 1) { + /* EVENT + * @description + * Vehicle is in safety configuration and denies arming. + * + * + * This check can be configured via COM_ARMABLE parameter. + * + */ + reporter.armingCheckFailure(NavModes::All, health_component_t::system, + events::ID("check_armable_configuration"), + events::Log::Error, "Vehicle is in safety configuration"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Vehicle is in safety configuration"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.hpp new file mode 100644 index 0000000000..3e64e2c177 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/armPermissionCheck.hpp @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * 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 "../Common.hpp" + +class ArmPermissionChecks : public HealthAndArmingCheckBase +{ +public: + ArmPermissionChecks() = default; + ~ArmPermissionChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_armable + ) +}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp index 883fec1724..9bb33f2bd2 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp @@ -117,7 +117,7 @@ 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, - reporter.failsafeFlags()); + reporter.failsafeFlags(), reporter); lowPositionAccuracy(context, reporter, lpos); @@ -184,12 +184,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor /* EVENT * @description * + * Measured strength: {1:.3}, expected: {2:.3} ± EKF2_MAG_CHK_STR + * Measured inclination: {3:.3}, expected: {4:.3} ± EKF2_MAG_CHK_INC * This check can be configured via COM_ARM_MAG_STR and EKF2_MAG_CHECK parameters. * */ - reporter.armingCheckFailure(required_groups_mag, health_component_t::local_position_estimate, - events::ID("check_estimator_mag_interference"), - events::Log::Warning, "Strong magnetic interference"); + reporter.armingCheckFailure(required_groups_mag, + health_component_t::local_position_estimate, + events::ID("check_estimator_mag_interference"), + events::Log::Warning, "Strong magnetic interference", + estimator_status.mag_strength_gs, estimator_status.mag_strength_ref_gs, + estimator_status.mag_inclination_deg, estimator_status.mag_inclination_ref_deg); if (reporter.mavlink_log_pub()) { mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Strong magnetic interference"); @@ -726,7 +731,8 @@ 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, - const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags) + const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, + Report &reporter) { // The following flags correspond to mode requirements, and are reported in the corresponding mode checks vehicle_global_position_s gpos; @@ -739,7 +745,9 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f // run position and velocity accuracy checks // Check if quality checking of position accuracy and consistency is to be performed - float lpos_eph_threshold_relaxed = _param_com_pos_fs_eph.get(); + 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, @@ -762,11 +770,52 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f } failsafe_flags.global_position_invalid = - !checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp, + !checkPosVelValidity(now, xy_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp, _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 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; + + if (_estimator_status_flags_sub.copy(&estimator_status_flags)) { + + // only do the following if the estimator status flags are recent (less than 5 seconds old) + if (now - estimator_status_flags.timestamp < 5_s) { + const bool dead_reckoning = estimator_status_flags.cs_inertial_dead_reckoning + || estimator_status_flags.cs_wind_dead_reckoning; + + if (!failsafe_flags.global_position_invalid + && !_nav_failure_imminent_warned + && gpos.eph > gpos_critical_warning_thrld + && dead_reckoning) { + /* EVENT + * @description + * Switch to manual mode recommended. + * + * + * This warning is triggered when the position error estimate is 90% of (or only 10m below) COM_POS_FS_EPH parameter. + * + */ + events::send(events::ID("check_estimator_position_failure_imminent"), {events::Log::Error, events::LogInternal::Info}, + "Estimated position error is approaching the failsafe threshold"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), + "Estimated position error is approaching the failsafe threshold\t"); + } + + _nav_failure_imminent_warned = true; + + } else if (!dead_reckoning) { + _nav_failure_imminent_warned = false; + } + } + } + failsafe_flags.local_position_invalid = - !checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp, + !checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold, lpos.timestamp, _last_lpos_fail_time_us, !failsafe_flags.local_position_invalid); failsafe_flags.local_position_invalid_relaxed = diff --git a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp index 4dd45cd70e..f5b8073524 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp @@ -69,7 +69,7 @@ private: 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, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, - failsafe_flags_s &failsafe_flags); + failsafe_flags_s &failsafe_flags, Report &reporter); bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy, const float required_accuracy, @@ -103,6 +103,8 @@ private: bool _gps_was_fused{false}; + bool _nav_failure_imminent_warned{false}; + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sys_mc_est_group, (ParamInt) _param_sens_imu_mode, diff --git a/src/modules/commander/HomePosition.cpp b/src/modules/commander/HomePosition.cpp index 0f9766bebb..b53b7d6c30 100644 --- a/src/modules/commander/HomePosition.cpp +++ b/src/modules/commander/HomePosition.cpp @@ -152,10 +152,10 @@ void HomePosition::fillLocalHomePos(home_position_s &home, float x, float y, flo void HomePosition::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) { - fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt); + fillGlobalHomePos(home, gpos.lat, gpos.lon, (double)gpos.alt); } -void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) +void HomePosition::fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt) { home.lat = lat; home.lon = lon; @@ -189,7 +189,7 @@ void HomePosition::setInAirHomePosition() ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon); const float home_alt = gpos.alt + home.z; - fillGlobalHomePos(home, home_lat, home_lon, home_alt); + fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt); setHomePosValid(); home.timestamp = hrt_absolute_time(); @@ -206,8 +206,8 @@ void HomePosition::setInAirHomePosition() double home_lon; ref_pos.reproject(home.x - lpos.x, home.y - lpos.y, home_lat, home_lon); - const float home_alt = _gps_alt + home.z; - fillGlobalHomePos(home, home_lat, home_lon, home_alt); + const double home_alt = _gps_alt + (double)home.z; + fillGlobalHomePos(home, home_lat, home_lon, (double)home_alt); setHomePosValid(); home.timestamp = hrt_absolute_time(); @@ -304,9 +304,9 @@ void HomePosition::update(bool set_automatically, bool check_if_changed) sensor_gps_s vehicle_gps_position; _vehicle_gps_position_sub.copy(&vehicle_gps_position); - _gps_lat = static_cast(vehicle_gps_position.lat) * 1e-7; - _gps_lon = static_cast(vehicle_gps_position.lon) * 1e-7; - _gps_alt = static_cast(vehicle_gps_position.alt) * 1e-3f; + _gps_lat = vehicle_gps_position.latitude_deg; + _gps_lon = vehicle_gps_position.longitude_deg; + _gps_alt = vehicle_gps_position.altitude_msl_m; _gps_eph = vehicle_gps_position.eph; _gps_epv = vehicle_gps_position.epv; diff --git a/src/modules/commander/HomePosition.hpp b/src/modules/commander/HomePosition.hpp index 8456ddb6b8..6346dac42e 100644 --- a/src/modules/commander/HomePosition.hpp +++ b/src/modules/commander/HomePosition.hpp @@ -68,7 +68,7 @@ private: static void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos); static void fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading); static void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos); - static void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt); + static void fillGlobalHomePos(home_position_s &home, double lat, double lon, double alt); uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; @@ -83,7 +83,7 @@ private: bool _gps_position_for_home_valid{false}; double _gps_lat{0}; double _gps_lon{0}; - float _gps_alt{0.f}; + double _gps_alt{0}; float _gps_eph{0.f}; float _gps_epv{0.f}; }; diff --git a/src/modules/commander/ModeUtil/control_mode.cpp b/src/modules/commander/ModeUtil/control_mode.cpp index 0a07c2989d..a8b12edc50 100644 --- a/src/modules/commander/ModeUtil/control_mode.cpp +++ b/src/modules/commander/ModeUtil/control_mode.cpp @@ -47,6 +47,7 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type, vehicle_control_mode_s &vehicle_control_mode) { vehicle_control_mode.flag_armed = armed; + vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default switch (nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6321c457e2..e644e7723a 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,7 +180,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) /* save */ calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - param_save_default(); + param_save_default(true); feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 4d69f37311..2a027920a2 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -136,7 +136,7 @@ int do_baro_calibration(orb_advert_t *mavlink_log_pub) if ((hrt_elapsed_time(&sensor_gps.timestamp) < 1_s) && (sensor_gps.fix_type >= 2) && (sensor_gps.epv < 100)) { - float alt = sensor_gps.alt * 0.001f; + float alt = (float)sensor_gps.altitude_msl_m; if (PX4_ISFINITE(gps_altitude_sum)) { gps_altitude_sum += alt; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0c0a668079..d7ecb6b81c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,9 +45,7 @@ * Roll trim * * The trim value is the actuator control value the system needs - * for straight and level flight. It can be calibrated by - * flying manually straight and level using the RC trims and - * copying them using the GCS. + * for straight and level flight. * * @group Radio Calibration * @min -0.5 @@ -61,9 +59,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); * Pitch trim * * The trim value is the actuator control value the system needs - * for straight and level flight. It can be calibrated by - * flying manually straight and level using the RC trims and - * copying them using the GCS. + * for straight and level flight. * * @group Radio Calibration * @min -0.5 @@ -77,9 +73,7 @@ PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); * Yaw trim * * The trim value is the actuator control value the system needs - * for straight and level flight. It can be calibrated by - * flying manually straight and level using the RC trims and - * copying them using the GCS. + * for straight and level flight. * * @group Radio Calibration * @min -0.5 @@ -232,8 +226,6 @@ PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f); /** * Allow arming without GPS * - * The default allows the vehicle to arm without GPS signal. - * * @group Commander * @value 0 Require GPS lock to arm * @value 1 Allow arming without GPS @@ -270,8 +262,8 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0); * * Before entering failsafe (RTL, Land, Hold), wait COM_FAIL_ACT_T seconds in Hold mode * for the user to realize. - * During that time the user cannot take over control. - * Afterwards the configured failsafe action is triggered and the user may take over. + * During that time the user cannot take over control via the stick override feature see COM_RC_OVERRIDE. + * Afterwards the configured failsafe action is triggered and the user may use stick override. * * A zero value disables the delay and the user cannot take over via stick movements (switching modes is still allowed). * @@ -598,9 +590,6 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 2); * if position is unavailable Altitude mode. * Note: Only has an effect on multicopters, and VTOLs in multicopter mode. * - * This parameter is not considered in case of a GPS failure (Descend flight mode), where stick - * override is always enabled. - * * @min 0 * @max 3 * @bit 0 Enable override during auto modes (except for in critical battery reaction) @@ -719,8 +708,11 @@ PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1); * If the previous position error was below this threshold, there is an additional * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation). * + * Set to -1 to disable. + * * @unit m - * @min 0 + * @min -1 + * @max 400 * @decimal 1 * @group Commander */ @@ -904,10 +896,10 @@ PARAM_DEFINE_INT32(COM_PREARM_MODE, 0); PARAM_DEFINE_INT32(COM_FORCE_SAFETY, 0); /** - * Enable Motor Testing + * Enable Actuator Testing * - * If set, enables the motor test interface via MAVLink (DO_MOTOR_TEST), that - * allows spinning the motors for testing purposes. + * If set, enables the actuator test interface via MAVLink (ACTUATOR_TEST), that + * allows spinning the motors and moving the servos for testing purposes. * * @boolean * @group Commander @@ -966,18 +958,6 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1); */ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); -/** -* Enable preflight check for maximal allowed airspeed when arming. -* -* Deny arming if the current airspeed measurement is greater than half the cruise airspeed (FW_AIRSPD_TRIM). -* Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration. -* -* @group Commander -* @value 0 Disabled -* @value 1 Enabled -*/ -PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); - /** * Enable FMU SD card detection check * @@ -1099,8 +1079,21 @@ PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f); * * Set to -1 to disable. * - * @group Commander * @min -1 + * @max 1000 + * @group Commander * @unit m */ PARAM_DEFINE_FLOAT(COM_POS_LOW_EPH, -1.0f); + +/** + * Flag to allow arming + * + * Set 0 to prevent accidental use of the vehicle e.g. for safety or maintenance reasons. + * + * @boolean + * @value 0 Disallow arming + * @value 1 Allow arming + * @group Commander + */ +PARAM_DEFINE_INT32(COM_ARMABLE, 1); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index e2e8e99b4c..3e78578dff 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -58,18 +58,19 @@ using namespace time_literals; bool check_battery_disconnected(orb_advert_t *mavlink_log_pub) { - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); + uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)}; + battery_status_sub.update(); - if (battery.timestamp == 0) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "battery unavailable"); + const bool recent_battery_measurement = hrt_absolute_time() < (battery_status_sub.get().timestamp + 1_s); + + if (!recent_battery_measurement) { + // We have to send this message for now because "battery unavailable" gets ignored by QGC + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); return false; } - // Make sure battery is disconnected - // battery is not connected if the connected flag is not set and we have a recent battery measurement - if (!battery.connected && (hrt_elapsed_time(&battery.timestamp) < 500_ms)) { + // Make sure battery is reported to be disconnected + if (recent_battery_measurement && !battery_status_sub.get().connected) { return true; } @@ -93,66 +94,80 @@ static void set_motor_actuators(uORB::Publication &publisher, f int do_esc_calibration(orb_advert_t *mavlink_log_pub) { - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + // 1 Initialization + bool calibration_failed = false; - int return_code = PX4_OK; uORB::Publication actuator_test_pub{ORB_ID(actuator_test)}; // since we publish multiple at once, make sure the output driver subscribes before we publish actuator_test_pub.advertise(); - px4_usleep(10000); - // set motors to high + uORB::SubscriptionData battery_status_sub{ORB_ID(battery_status)}; + battery_status_sub.update(); + const bool battery_connected_before_calibration = battery_status_sub.get().connected; + const float current_before_calibration = battery_status_sub.get().current_a; + + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); + + px4_usleep(10_ms); + + // 2 Set motors to high set_motor_actuators(actuator_test_pub, 1.f, false); calibration_log_info(mavlink_log_pub, "[cal] Connect battery now"); - - uORB::SubscriptionData batt_sub{ORB_ID(battery_status)}; - const battery_status_s &battery = batt_sub.get(); - batt_sub.update(); - bool batt_connected = battery.connected; hrt_abstime timeout_start = hrt_absolute_time(); + // 3 Wait for user to connect power while (true) { - // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM - // sit high. - static constexpr hrt_abstime battery_connect_wait_timeout{20_s}; - static constexpr hrt_abstime pwm_high_timeout{3_s}; - hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; + hrt_abstime now = hrt_absolute_time(); + battery_status_sub.update(); - if (hrt_elapsed_time(&timeout_start) > timeout_wait) { - if (!batt_connected) { - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); - return_code = PX4_ERROR; - } - - // PWM was high long enough + if (now > (timeout_start + 1_s) && (battery_status_sub.get().current_a > current_before_calibration + 1.f)) { + // Safety termination, current rises immediately, user didn't unplug power before + calibration_failed = true; break; } - if (!batt_connected) { - if (batt_sub.update()) { - if (battery.connected) { - // Battery is connected, signal to user and start waiting again - batt_connected = true; - timeout_start = hrt_absolute_time(); - calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); - } - } + if (!battery_connected_before_calibration && battery_status_sub.get().connected) { + // Battery connection detected we can go to the next step immediately + break; } - px4_usleep(50000); + if (now > (timeout_start + 6_s)) { + // Timeout, we continue since maybe the battery cannot be detected properly + // If we abort here and the ESCs are infact connected and started calibrating + // they will measure the disarmed value as the lower limit instead of the fixed 1000us + break; + } + + px4_usleep(50_ms); } - if (return_code == PX4_OK) { - // set motors to low + // 4 Wait for ESCs to measure high signal + if (!calibration_failed) { + calibration_log_info(mavlink_log_pub, "[cal] Battery connected"); + px4_usleep(3_s); + } + + // 5 Set motors to low + if (!calibration_failed) { set_motor_actuators(actuator_test_pub, 0.f, false); - px4_usleep(4000000); - - // release control - set_motor_actuators(actuator_test_pub, 0.f, true); - - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); } - return return_code; + // 6 Wait for ESCs to measure low signal + if (!calibration_failed) { + px4_usleep(5_s); + } + + // 7 release control + set_motor_actuators(actuator_test_pub, 0.f, true); + + // 8 Report + if (calibration_failed) { + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); + return PX4_ERROR; + + } else { + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); + return PX4_OK; + } } diff --git a/src/modules/commander/factory_calibration_storage.cpp b/src/modules/commander/factory_calibration_storage.cpp index 143202f735..450f48c49d 100644 --- a/src/modules/commander/factory_calibration_storage.cpp +++ b/src/modules/commander/factory_calibration_storage.cpp @@ -43,18 +43,53 @@ static const char *CALIBRATION_STORAGE = "/fs/mtd_caldata"; +enum class FactoryCalibrationMode : uint32_t { + Disabled = 0, + AllSensors, + AllSensorsExceptMag, +}; + +static bool ends_with(const char *str, const char *suffix) +{ + if (!str || !suffix) { + return false; + } + + size_t len_str = strlen(str); + size_t len_suffix = strlen(suffix); + + if (len_suffix > len_str) { + return false; + } + + return strncmp(str + len_str - len_suffix, suffix, len_suffix) == 0; +} + +static FactoryCalibrationMode factory_calibration_mode{FactoryCalibrationMode::Disabled}; + static bool filter_calibration_params(param_t handle) { const char *name = param_name(handle); + + if (factory_calibration_mode == FactoryCalibrationMode::AllSensorsExceptMag) { + if (strncmp(name, "CAL_MAG", 7) == 0) { + return false; + } + } + // filter all non-calibration params - return (strncmp(name, "CAL_", 4) == 0 && strncmp(name, "CAL_MAG_SIDES", 13) != 0) || strncmp(name, "TC_", 3) == 0; + return (strncmp(name, "CAL_", 4) == 0 + && strcmp(name, "CAL_MAG_SIDES") != 0 + && !ends_with(name, "_PRIO")) + || strncmp(name, "TC_", 3) == 0; } FactoryCalibrationStorage::FactoryCalibrationStorage() { int32_t param = 0; param_get(param_find("SYS_FAC_CAL_MODE"), ¶m); - _enabled = param == 1; + _enabled = param >= 1; + factory_calibration_mode = (FactoryCalibrationMode)param; } int FactoryCalibrationStorage::open() diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index 2f79e92cff..f8d64308d4 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -354,7 +354,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard || rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming; - if (_param_com_rc_in_mode.get() != int32_t(offboard_loss_failsafe_mode::Land_mode) && !rc_loss_ignored) { + if (_param_com_rc_in_mode.get() != int32_t(RcInMode::StickInputDisabled) && !rc_loss_ignored) { CHECK_FAILSAFE(status_flags, manual_control_signal_lost, fromNavDllOrRclActParam(_param_nav_rcl_act.get()).causedBy(Cause::ManualControlLoss)); } @@ -382,7 +382,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, // If manual control loss and GCS connection loss are disabled and we lose both command links and the mission finished, // trigger RTL to avoid losing the vehicle - if ((_param_com_rc_in_mode.get() == int32_t(offboard_loss_failsafe_mode::Land_mode) || rc_loss_ignored_mission) + if ((_param_com_rc_in_mode.get() == int32_t(RcInMode::StickInputDisabled) || rc_loss_ignored_mission) && _param_nav_dll_act.get() == int32_t(gcs_connection_loss_failsafe_mode::Disabled) && state.mission_finished) { _last_state_mission_control_lost = checkFailsafe(_caller_id_mission_control_lost, _last_state_mission_control_lost, @@ -404,7 +404,8 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, primary_geofence_breached, fromGfActParam(_param_gf_action.get()).cannotBeDeferred()); // Battery - CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::BatteryLow)); + CHECK_FAILSAFE(status_flags, battery_low_remaining_time, + ActionOptions(Action::RTL).causedBy(Cause::BatteryLow).clearOn(ClearCondition::OnModeChangeOrDisarm)); CHECK_FAILSAFE(status_flags, battery_unhealthy, Action::Warn); switch (status_flags.battery_warning) { diff --git a/src/modules/commander/failsafe/failsafe.h b/src/modules/commander/failsafe/failsafe.h index 9cbb7d5764..753bf2b210 100644 --- a/src/modules/commander/failsafe/failsafe.h +++ b/src/modules/commander/failsafe/failsafe.h @@ -123,6 +123,15 @@ private: Hold_mode = 2, }; + // COM_RC_IN_MODE parameter values + enum class RcInMode : int32_t { + RcTransmitterOnly = 0, // RC Transmitter only + JoystickOnly = 1, // Joystick only + RcAndJoystickWithFallback = 2, // RC And Joystick with fallback + RcOrJoystickKeepFirst = 3, // RC or Joystick keep first + StickInputDisabled = 4 // input disabled + }; + static ActionOptions fromNavDllOrRclActParam(int param_value); static ActionOptions fromGfActParam(int param_value); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index fcb35ca782..3fff697d49 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -237,8 +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.lat / 1.e7; - const double lon = gps.lon / 1.e7; + 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); @@ -771,6 +771,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // FALLTHROUGH case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90 + + // FALLTHROUGH + case ROTATION_CUSTOM: // Skip, as we currently don't support detecting arbitrary euler angle orientation + MSE[r] = FLT_MAX; break; @@ -831,6 +835,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma worker_data.calibration[cur_mag].device_id(), worker_data.calibration[cur_mag].rotation_enum()); continue; + case ROTATION_CUSTOM: + PX4_INFO("[cal] External Mag: %d (%" PRIu32 "), not setting rotation enum since it's specified by Euler Angle", + cur_mag, worker_data.calibration[cur_mag].device_id()); + continue; // Continue to the next mag loop + default: break; } @@ -960,8 +969,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 = gps.lat / 1.e7f; - longitude = gps.lon / 1.e7f; + latitude = (float)gps.latitude_deg; + longitude = (float)gps.longitude_deg; mag_earth_available = true; } } diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index 501c483e91..bb73c1e6cc 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -158,7 +158,7 @@ void WorkerThread::threadEntry() break; case Request::ParamSaveDefault: - _ret_value = param_save_default(); + _ret_value = param_save_default(true); if (_ret_value != 0) { mavlink_log_critical(&_mavlink_log_pub, "Error saving settings\t"); @@ -175,7 +175,7 @@ void WorkerThread::threadEntry() case Request::ParamResetSensorFactory: { const char *reset_cal[] = { "CAL_ACC*", "CAL_GYRO*", "CAL_MAG*" }; param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0])); - _ret_value = param_save_default(); + _ret_value = param_save_default(true); #if defined(CONFIG_BOARDCTL_RESET) px4_reboot_request(false, 400_ms); #endif // CONFIG_BOARDCTL_RESET diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp index e8b2b3bea6..901c469746 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.cpp @@ -84,3 +84,21 @@ int ActuatorEffectiveness::Configuration::totalNumActuators() const return total_count; } + +void ActuatorEffectiveness::stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp) +{ + for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) { + const uint32_t motor_mask = (1u << actuator_idx); + + if (stoppable_motors_mask & motor_mask) { + + // Stop motor if its setpoint is below 2%. This value was determined empirically (RC stick inaccuracy) + if (fabsf(actuator_sp(actuator_idx)) < .02f) { + _stopped_motors_mask |= motor_mask; + + } else { + _stopped_motors_mask &= ~motor_mask; + } + } + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 1317011850..0ddd4988f3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -203,7 +203,7 @@ public: /** * Get a bitmask of motors to be stopped */ - virtual uint32_t getStoppedMotors() const { return 0; } + virtual uint32_t getStoppedMotors() const { return _stopped_motors_mask; } /** * Fill in the unallocated torque and thrust, customized by effectiveness type. @@ -211,6 +211,15 @@ public: */ virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {} + /** + * Stops motors which are masked by stoppable_motors_mask and whose demanded thrust is zero + * + * @param stoppable_motors_mask mask of motors that should be stopped if there's no thrust demand + * @param actuator_sp outcome of the allocation to determine if the motor should be stopped + */ + virtual void stopMaskedMotorsWithZeroThrust(uint32_t stoppable_motors_mask, ActuatorVector &actuator_sp); + protected: - FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; ///< Current flight phase + FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT}; + uint32_t _stopped_motors_mask{0}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp index ca991b5424..ead2043682 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.cpp @@ -48,9 +48,10 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration return false; } - // motors + // Motors _motors.enablePropellerTorque(false); const bool motors_added_successfully = _motors.addActuators(configuration); + _motors_mask = _motors.getMotors(); // Torque const bool torque_added_successfully = _torque.addActuators(configuration); @@ -58,3 +59,9 @@ ActuatorEffectivenessCustom::getEffectivenessMatrix(Configuration &configuration return (motors_added_successfully && torque_added_successfully); } +void ActuatorEffectivenessCustom::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp index e06278f08f..0cc1390bd2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessCustom.hpp @@ -45,9 +45,15 @@ public: bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + const char *name() const override { return "Custom"; } protected: ActuatorEffectivenessRotors _motors; ActuatorEffectivenessControlSurfaces _torque; + + uint32_t _motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 8e152183c8..c7c2bba7d7 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -53,6 +53,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat // Motors _rotors.enablePropellerTorque(false); const bool rotors_added_successfully = _rotors.addActuators(configuration); + _forwards_motors_mask = _rotors.getForwardsMotors(); // Control Surfaces _first_control_surface_idx = configuration.num_actuators_matrix[0]; @@ -61,6 +62,13 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat return (rotors_added_successfully && surfaces_added_successfully); } +void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); +} + void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 6195946866..5b4b7785b2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -51,6 +51,10 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; @@ -59,4 +63,6 @@ private: uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uint32_t _forwards_motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp index 9faae22ca4..f403424b67 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp @@ -37,8 +37,8 @@ using namespace matrix; using namespace time_literals; -ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent) - : ModuleParams(parent) +ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type) + : ModuleParams(parent), _tail_actuator_type(tail_actuator_type) { for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { char buffer[17]; @@ -104,8 +104,7 @@ void ActuatorEffectivenessHelicopter::updateParams() _geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f; } -bool -ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, +bool ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) { if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { @@ -115,8 +114,8 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura // As the allocation is non-linear, we use updateSetpoint() instead of the matrix configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); - // Tail (yaw) motor - configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); + // Tail (yaw) (either ESC or Servo) + configuration.addActuator(_tail_actuator_type, Vector3f{}, Vector3f{}); // N swash plate servos _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; @@ -221,7 +220,6 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) { - // Note: the values '-1', '1' and '0' are just to indicate a negative, // positive or no saturation to the rate controller. The actual magnitude is not used. if (_saturation_flags.roll_pos) { @@ -229,6 +227,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.roll_neg) { status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; } if (_saturation_flags.pitch_pos) { @@ -236,6 +237,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.pitch_neg) { status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; } if (_saturation_flags.yaw_pos) { @@ -243,6 +247,9 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.yaw_neg) { status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; } if (_saturation_flags.thrust_pos) { @@ -250,5 +257,8 @@ void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, co } else if (_saturation_flags.thrust_neg) { status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp index fc66e0044a..439e0f4ab9 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp @@ -66,7 +66,7 @@ public: float spoolup_time; }; - ActuatorEffectivenessHelicopter(ModuleParams *parent); + ActuatorEffectivenessHelicopter(ModuleParams *parent, ActuatorType tail_actuator_type); virtual ~ActuatorEffectivenessHelicopter() = default; bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; @@ -129,4 +129,6 @@ private: uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; bool _main_motor_engaged{true}; + + const ActuatorType _tail_actuator_type; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp new file mode 100644 index 0000000000..0c06f5963f --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.cpp @@ -0,0 +1,222 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessHelicopterCoaxial.hpp" +#include + +using namespace matrix; +using namespace time_literals; + +ActuatorEffectivenessHelicopterCoaxial::ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent) + : ModuleParams(parent) +{ + for (int i = 0; i < NUM_SWASH_PLATE_SERVOS_MAX; ++i) { + char buffer[17]; + snprintf(buffer, sizeof(buffer), "CA_SP0_ANG%u", i); + _param_handles.swash_plate_servos[i].angle = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i); + _param_handles.swash_plate_servos[i].arm_length = param_find(buffer); + snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i); + _param_handles.swash_plate_servos[i].trim = param_find(buffer); + } + + _param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT"); + _param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); +} + +void ActuatorEffectivenessHelicopterCoaxial::updateParams() +{ + ModuleParams::updateParams(); + + int32_t count = 0; + + if (param_get(_param_handles.num_swash_plate_servos, &count) != 0) { + PX4_ERR("param_get failed"); + return; + } + + _geometry.num_swash_plate_servos = math::constrain((int)count, 2, NUM_SWASH_PLATE_SERVOS_MAX); + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + float angle_deg{}; + param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg); + _geometry.swash_plate_servos[i].angle = math::radians(angle_deg); + param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length); + param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim); + } + + param_get(_param_handles.spoolup_time, &_geometry.spoolup_time); +} + +bool ActuatorEffectivenessHelicopterCoaxial::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // As the allocation is non-linear, we use updateSetpoint() instead of the matrix + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Clockwise rotor + configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{}); // Counter-clockwise rotor + + // N swash plate servos + _first_swash_plate_servo_index = configuration.num_actuators_matrix[0]; + + for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) { + configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{}); + configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim; + } + + return true; +} + +void ActuatorEffectivenessHelicopterCoaxial::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + _saturation_flags = {}; + + // throttle/collective pitch curve + const float throttle = -control_sp(ControlAxis::THRUST_Z) * throttleSpoolupProgress(); + const float yaw = control_sp(ControlAxis::YAW); + + // actuator mapping + actuator_sp(0) = throttle - yaw; // Clockwise + actuator_sp(1) = throttle + yaw; // Counter-clockwise + + // Saturation check for yaw + if ((actuator_sp(0) < actuator_min(0)) || (actuator_sp(1) > actuator_max(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos); + + } else if ((actuator_sp(0) > actuator_max(0)) || (actuator_sp(1) < actuator_min(1))) { + setSaturationFlag(1.f, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg); + } + + for (int i = 0; i < _geometry.num_swash_plate_servos; i++) { + float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length; + actuator_sp(_first_swash_plate_servo_index + i) = + + control_sp(ControlAxis::PITCH) * pitch_coeff + - control_sp(ControlAxis::ROLL) * roll_coeff + + _geometry.swash_plate_servos[i].trim; + + // Saturation check for roll & pitch + if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos); + + } else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) { + setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos); + setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg); + } + } +} + +float ActuatorEffectivenessHelicopterCoaxial::throttleSpoolupProgress() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f; + const float spoolup_progress = time_since_arming / _geometry.spoolup_time; + + if (_armed && spoolup_progress < 1.f) { + return spoolup_progress; + } + + return 1.f; +} + + +void ActuatorEffectivenessHelicopterCoaxial::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag) +{ + if (coeff > 0.f) { + // A positive change in given axis will increase saturation + positive_flag = true; + + } else if (coeff < 0.f) { + // A negative change in given axis will increase saturation + negative_flag = true; + } +} + +void ActuatorEffectivenessHelicopterCoaxial::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_saturation_flags.roll_pos) { + status.unallocated_torque[0] = 1.f; + + } else if (_saturation_flags.roll_neg) { + status.unallocated_torque[0] = -1.f; + + } else { + status.unallocated_torque[0] = 0.f; + } + + if (_saturation_flags.pitch_pos) { + status.unallocated_torque[1] = 1.f; + + } else if (_saturation_flags.pitch_neg) { + status.unallocated_torque[1] = -1.f; + + } else { + status.unallocated_torque[1] = 0.f; + } + + if (_saturation_flags.yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_saturation_flags.yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } + + if (_saturation_flags.thrust_pos) { + status.unallocated_thrust[2] = 1.f; + + } else if (_saturation_flags.thrust_neg) { + status.unallocated_thrust[2] = -1.f; + + } else { + status.unallocated_thrust[2] = 0.f; + } +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp new file mode 100644 index 0000000000..d5316bf498 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterCoaxial.hpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectiveness.hpp" + +#include + +#include +#include +#include + +class ActuatorEffectivenessHelicopterCoaxial : public ModuleParams, public ActuatorEffectiveness +{ +public: + + static constexpr int NUM_SWASH_PLATE_SERVOS_MAX = 4; + + struct SwashPlateGeometry { + float angle; + float arm_length; + float trim; + }; + + struct Geometry { + SwashPlateGeometry swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + int num_swash_plate_servos{0}; + float spoolup_time; + }; + + ActuatorEffectivenessHelicopterCoaxial(ModuleParams *parent); + virtual ~ActuatorEffectivenessHelicopterCoaxial() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + const char *name() const override { return "Helicopter"; } + + + const Geometry &geometry() const { return _geometry; } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; +private: + float throttleSpoolupProgress(); + + void updateParams() override; + + struct SaturationFlags { + bool roll_pos; + bool roll_neg; + bool pitch_pos; + bool pitch_neg; + bool yaw_pos; + bool yaw_neg; + bool thrust_pos; + bool thrust_neg; + }; + static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag); + + struct ParamHandlesSwashPlate { + param_t angle; + param_t arm_length; + param_t trim; + }; + struct ParamHandles { + ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX]; + param_t num_swash_plate_servos; + param_t spoolup_time; + }; + ParamHandles _param_handles{}; + + Geometry _geometry{}; + + int _first_swash_plate_servo_index{}; + SaturationFlags _saturation_flags; + + // Throttle spoolup state + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; + + uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp index c0f29f5a20..29ca1f1117 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp @@ -52,7 +52,7 @@ TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve) param_set(param, &values[i]); } - ActuatorEffectivenessHelicopter helicopter(nullptr); + ActuatorEffectivenessHelicopter helicopter(nullptr, ActuatorType::MOTORS); // run getEffectivenessMatrix with empty configuration to correctly initialize _first_swash_plate_servo_index ActuatorEffectiveness::Configuration configuration{}; EffectivenessUpdateReason external_update = EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index 41005d38a7..a8effa8cb3 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-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 @@ -55,7 +55,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts - int first_tilt_idx = configuration.num_actuators_matrix[0]; + _first_tilt_idx = configuration.num_actuators_matrix[0]; _tilts.updateTorqueSign(_mc_rotors.geometry()); const bool tilts_added_successfully = _tilts.addActuators(configuration); @@ -69,7 +69,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration if (delta_angle > FLT_EPSILON) { float trim = -1.f - 2.f * _tilts.config(i).min_angle / delta_angle; - _tilt_offsets(first_tilt_idx + i) = trim; + _tilt_offsets(_first_tilt_idx + i) = trim; } } @@ -82,4 +82,49 @@ void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } + } + } + + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; +} + +void ActuatorEffectivenessMCTilt::getUnallocatedControl(int matrix_index, control_allocator_status_s &status) +{ + // Note: the values '-1', '1' and '0' are just to indicate a negative, + // positive or no saturation to the rate controller. The actual magnitude is not used. + if (_yaw_tilt_saturation_flags.tilt_yaw_pos) { + status.unallocated_torque[2] = 1.f; + + } else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) { + status.unallocated_torque[2] = -1.f; + + } else { + status.unallocated_torque[2] = 0.f; + } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 15c12957ed..848c8b8853 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2021-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 @@ -61,8 +61,18 @@ public: const char *name() const override { return "MC Tilt"; } + void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; + protected: ActuatorVector _tilt_offsets; ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessTilts _tilts; + int _first_tilt_idx{0}; + + struct YawTiltSaturationFlags { + bool tilt_yaw_pos; + bool tilt_yaw_neg; + }; + + YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 1cf88c67b6..29a2dea9be 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -265,6 +265,17 @@ Vector3f ActuatorEffectivenessRotors::tiltedAxis(float tilt_angle, float tilt_di return Dcmf{Eulerf{0.f, -tilt_angle, tilt_direction}} * axis; } +uint32_t ActuatorEffectivenessRotors::getMotors() const +{ + uint32_t motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + motors |= 1u << i; + } + + return motors; +} + uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const { uint32_t upwards_motors = 0; @@ -280,6 +291,21 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const return upwards_motors; } +uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +{ + uint32_t forward_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { + forward_motors |= 1u << i; + } + } + + return forward_motors; +} + bool ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 8b6436cef1..14f96b7dab 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -126,7 +126,9 @@ public: void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } + uint32_t getMotors() const; uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; private: void updateParams() override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp index 37b3acb5e0..ed4d636809 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp @@ -45,7 +45,15 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi } configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f}); + _motors_mask = 1u << 0; configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{}); return true; } +void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} + diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp index c59ffc8741..294e453ec6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp @@ -43,6 +43,11 @@ public: bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + const char *name() const override { return "Rover (Ackermann)"; } private: + uint32_t _motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp index e9bc32477e..8e7c64baa8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp @@ -46,6 +46,14 @@ ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &co configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f}); configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f}); + _motors_mask = (1u << 0) | (1u << 1); return true; } +void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} + diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp index 6dde6b2bbd..fed0a7ddcf 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp @@ -43,6 +43,11 @@ public: bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + const char *name() const override { return "Rover (Differential)"; } private: + uint32_t _motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp index 8d7fed6939..79853c7518 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp @@ -53,7 +53,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu configuration.selected_matrix = 0; _rotors.enablePropellerTorqueNonUpwards(false); const bool mc_rotors_added_successfully = _rotors.addActuators(configuration); - _mc_motors_mask = _rotors.getUpwardsMotors(); + _upwards_motors_mask = _rotors.getUpwardsMotors(); + _forwards_motors_mask = _rotors.getForwardsMotors(); // Control Surfaces configuration.selected_matrix = 1; @@ -83,6 +84,15 @@ void ActuatorEffectivenessStandardVTOL::allocateAuxilaryControls(const float dt, } } +void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + if (matrix_index == 0) { + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + } +} + void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight_phase) { if (_flight_phase == flight_phase) { @@ -94,13 +104,13 @@ void ActuatorEffectivenessStandardVTOL::setFlightPhase(const FlightPhase &flight // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _mc_motors_mask; + _stopped_motors_mask |= _upwards_motors_mask; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _stopped_motors_mask &= ~_upwards_motors_mask; break; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index e49edc6176..0e5d1bfa44 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -75,20 +75,21 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; - void setFlightPhase(const FlightPhase &flight_phase) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; - uint32_t getStoppedMotors() const override { return _stopped_motors; } + void setFlightPhase(const FlightPhase &flight_phase) override; private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - uint32_t _mc_motors_mask{}; ///< mc motors (stopped during forward flight) - uint32_t _stopped_motors{}; ///< currently stopped motors + uint32_t _upwards_motors_mask{}; + uint32_t _forwards_motors_mask{}; int _first_control_surface_idx{0}; ///< applies to matrix 1 uORB::Subscription _flaps_setpoint_sub{ORB_ID(flaps_setpoint)}; uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; - }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 139dc36612..aad6d4391b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -86,7 +86,15 @@ void ActuatorEffectivenessTailsitterVTOL::allocateAuxilaryControls(const float d _control_surfaces.applySpoilers(spoilers_setpoint.normalized_setpoint, _first_control_surface_idx, dt, actuator_sp); } } +} +void ActuatorEffectivenessTailsitterVTOL::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + if (matrix_index == 0) { + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); + } } void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flight_phase) @@ -97,16 +105,17 @@ void ActuatorEffectivenessTailsitterVTOL::setFlightPhase(const FlightPhase &flig ActuatorEffectiveness::setFlightPhase(flight_phase); - // update stopped motors //TODO: add option to switch off certain motors in FW + // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = 0; + _forwards_motors_mask = _mc_rotors.getUpwardsMotors(); // allocation frame they stay upwards break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _forwards_motors_mask = 0; + _stopped_motors_mask = 0; break; } } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index f5a8ddb460..604f05f9e0 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -72,16 +72,20 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + void setFlightPhase(const FlightPhase &flight_phase) override; const char *name() const override { return "VTOL Tailsitter"; } - uint32_t getStoppedMotors() const override { return _stopped_motors; } protected: ActuatorEffectivenessRotors _mc_rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; - uint32_t _stopped_motors{}; ///< currently stopped motors + uint32_t _forwards_motors_mask{}; int _first_control_surface_idx{0}; ///< applies to matrix 1 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 84695a9bb2..5fe261b27f 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -48,8 +48,19 @@ ActuatorEffectivenessTiltrotorVTOL::ActuatorEffectivenessTiltrotorVTOL(ModulePar _mc_rotors(this, ActuatorEffectivenessRotors::AxisConfiguration::Configurable, true), _control_surfaces(this), _tilts(this) { + _param_handles.com_spoolup_time = param_find("COM_SPOOLUP_TIME"); + + updateParams(); setFlightPhase(FlightPhase::HOVER_FLIGHT); } + +void ActuatorEffectivenessTiltrotorVTOL::updateParams() +{ + ModuleParams::updateParams(); + + param_get(_param_handles.com_spoolup_time, &_param_spoolup_time); +} + bool ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) @@ -68,10 +79,11 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // scales are tilt-invariant. Note: configuration updates are only possible when disarmed. const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f : _last_collective_tilt_control; - _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied) - << configuration.num_actuators[(int)ActuatorType::MOTORS]; + _untiltable_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied) + << configuration.num_actuators[(int)ActuatorType::MOTORS]; const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); + _motors = _mc_rotors.getMotors(); // Control Surfaces configuration.selected_matrix = 1; @@ -118,7 +130,6 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector FLT_EPSILON) { + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_positive = false; + } + + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_negative = false; + } + + } else if (_tilts.getYawTorqueOfTilt(i) < -FLT_EPSILON) { + if (yaw_saturated_negative && actuator_sp(i + _first_tilt_idx) < actuator_max(i + _first_tilt_idx) - FLT_EPSILON) { + yaw_saturated_negative = false; + } + + if (yaw_saturated_positive && actuator_sp(i + _first_tilt_idx) > actuator_min(i + _first_tilt_idx) + FLT_EPSILON) { + yaw_saturated_positive = false; + } } } + _yaw_tilt_saturation_flags.tilt_yaw_neg = yaw_saturated_negative; + _yaw_tilt_saturation_flags.tilt_yaw_pos = yaw_saturated_positive; + // in FW directly use throttle sp if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { - for (int i = 0; i < _first_tilt_idx; ++i) { actuator_sp(i) = tiltrotor_extra_controls.collective_thrust_normalized_setpoint; } } } - } - // Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from - // the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags - // directly if the (normalized) yaw torque setpoint is outside of range (-1, 1). - if (matrix_index == 0 && _tilts.hasYawControl()) { - _yaw_tilt_saturation_flags.tilt_yaw_neg = false; - _yaw_tilt_saturation_flags.tilt_yaw_pos = false; - - if (control_sp(2) < -1.f) { - _yaw_tilt_saturation_flags.tilt_yaw_neg = true; - - } else if (control_sp(2) > 1.f) { - _yaw_tilt_saturation_flags.tilt_yaw_pos = true; + if (_flight_phase == FlightPhase::FORWARD_FLIGHT) { + stopMaskedMotorsWithZeroThrust(_motors & ~_untiltable_motors, actuator_sp); } } } @@ -180,13 +224,13 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh // update stopped motors switch (flight_phase) { case FlightPhase::FORWARD_FLIGHT: - _stopped_motors = _nontilted_motors; + _stopped_motors_mask |= _untiltable_motors; break; case FlightPhase::HOVER_FLIGHT: case FlightPhase::TRANSITION_FF_TO_HF: case FlightPhase::TRANSITION_HF_TO_FF: - _stopped_motors = 0; + _stopped_motors_mask = 0; break; } } @@ -210,3 +254,15 @@ void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, status.unallocated_torque[2] = 0.f; } } + +bool ActuatorEffectivenessTiltrotorVTOL::throttleSpoolupFinished() +{ + vehicle_status_s vehicle_status; + + if (_vehicle_status_sub.update(&vehicle_status)) { + _armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED; + _armed_time = vehicle_status.armed_time; + } + + return _armed && hrt_elapsed_time(&_armed_time) > _param_spoolup_time * 1_s; +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index 33d0df486d..310d937064 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -46,9 +46,11 @@ #include "ActuatorEffectivenessControlSurfaces.hpp" #include "ActuatorEffectivenessTilts.hpp" +#include + #include #include - +#include #include class ActuatorEffectivenessTiltrotorVTOL : public ModuleParams, public ActuatorEffectiveness @@ -84,8 +86,6 @@ public: const char *name() const override { return "VTOL Tiltrotor"; } - uint32_t getStoppedMotors() const override { return _stopped_motors; } - void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override; protected: @@ -94,8 +94,8 @@ protected: ActuatorEffectivenessControlSurfaces _control_surfaces; ActuatorEffectivenessTilts _tilts; - uint32_t _nontilted_motors{}; ///< motors that are not tiltable - uint32_t _stopped_motors{}; ///< currently stopped motors + uint32_t _motors{}; + uint32_t _untiltable_motors{}; int _first_control_surface_idx{0}; ///< applies to matrix 1 int _first_tilt_idx{0}; ///< applies to matrix 0 @@ -113,4 +113,22 @@ protected: YawTiltSaturationFlags _yaw_tilt_saturation_flags{}; uORB::Subscription _tiltrotor_extra_controls_sub{ORB_ID(tiltrotor_extra_controls)}; + +private: + + void updateParams() override; + + struct ParamHandles { + param_t com_spoolup_time; + }; + + ParamHandles _param_handles{}; + + float _param_spoolup_time{1.f}; + + // Tilt handling during motor spoolup: leave the tilts in their disarmed position unitil 1s after arming + bool throttleSpoolupFinished(); + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + bool _armed{false}; + uint64_t _armed_time{0}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp index a6f1733c10..d885a09155 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTilts.hpp @@ -79,6 +79,8 @@ public: bool hasYawControl() const; + float getYawTorqueOfTilt(int tilt_index) const { return _torque[tilt_index](2); } + private: void updateParams() override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp new file mode 100644 index 0000000000..e4c861a9d0 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.cpp @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectivenessUUV.hpp" + +using namespace matrix; + +ActuatorEffectivenessUUV::ActuatorEffectivenessUUV(ModuleParams *parent) + : ModuleParams(parent), + _rotors(this) +{ +} + +bool ActuatorEffectivenessUUV::getEffectivenessMatrix(Configuration &configuration, + EffectivenessUpdateReason external_update) +{ + if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) { + return false; + } + + // Motors + const bool rotors_added_successfully = _rotors.addActuators(configuration); + _motors_mask = _rotors.getMotors(); + + return rotors_added_successfully; +} + +void ActuatorEffectivenessUUV::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp); +} diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp new file mode 100644 index 0000000000..57e86f9436 --- /dev/null +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessUUV.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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 "ActuatorEffectiveness.hpp" +#include "ActuatorEffectivenessRotors.hpp" + +class ActuatorEffectivenessUUV : public ModuleParams, public ActuatorEffectiveness +{ +public: + ActuatorEffectivenessUUV(ModuleParams *parent); + virtual ~ActuatorEffectivenessUUV() = default; + + bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override; + + void getDesiredAllocationMethod(AllocationMethod allocation_method_out[MAX_NUM_MATRICES]) const override + { + allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; + } + + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + + const char *name() const override { return "UUV"; } + +protected: + ActuatorEffectivenessRotors _rotors; + + uint32_t _motors_mask{}; +}; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt index 31e338de82..994b566270 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt +++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt @@ -34,6 +34,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectiveness.cpp ActuatorEffectiveness.hpp + ActuatorEffectivenessUUV.cpp + ActuatorEffectivenessUUV.hpp ActuatorEffectivenessControlSurfaces.cpp ActuatorEffectivenessControlSurfaces.hpp ActuatorEffectivenessCustom.cpp @@ -42,6 +44,8 @@ px4_add_library(ActuatorEffectiveness ActuatorEffectivenessFixedWing.hpp ActuatorEffectivenessHelicopter.cpp ActuatorEffectivenessHelicopter.hpp + ActuatorEffectivenessHelicopterCoaxial.cpp + ActuatorEffectivenessHelicopterCoaxial.hpp ActuatorEffectivenessMCTilt.cpp ActuatorEffectivenessMCTilt.hpp ActuatorEffectivenessMultirotor.cpp diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 2b419ab782..0fad1b5f62 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -247,7 +247,7 @@ ControlAllocator::update_effectiveness_source() break; case EffectivenessSource::MOTORS_6DOF: // just a different UI from MULTIROTOR - tmp = new ActuatorEffectivenessRotors(this); + tmp = new ActuatorEffectivenessUUV(this); break; case EffectivenessSource::MULTIROTOR_WITH_TILT: @@ -258,8 +258,16 @@ ControlAllocator::update_effectiveness_source() tmp = new ActuatorEffectivenessCustom(this); break; - case EffectivenessSource::HELICOPTER: - tmp = new ActuatorEffectivenessHelicopter(this); + case EffectivenessSource::HELICOPTER_TAIL_ESC: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::MOTORS); + break; + + case EffectivenessSource::HELICOPTER_TAIL_SERVO: + tmp = new ActuatorEffectivenessHelicopter(this, ActuatorType::SERVOS); + break; + + case EffectivenessSource::HELICOPTER_COAXIAL: + tmp = new ActuatorEffectivenessHelicopterCoaxial(this); break; default: @@ -355,6 +363,14 @@ ControlAllocator::Run() } } + { + vehicle_control_mode_s vehicle_control_mode; + + if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) { + _publish_controls = vehicle_control_mode.flag_control_allocation_enabled; + } + } + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. const hrt_abstime now = hrt_absolute_time(); const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); @@ -637,6 +653,10 @@ ControlAllocator::publish_control_allocator_status(int matrix_index) void ControlAllocator::publish_actuator_controls() { + if (!_publish_controls) { + return; + } + actuator_motors_s actuator_motors; actuator_motors.timestamp = hrt_absolute_time(); actuator_motors.timestamp_sample = _timestamp_sample; diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 0281aa925e..25904f0551 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -51,7 +51,9 @@ #include #include #include +#include #include +#include #include #include @@ -72,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -153,7 +156,9 @@ private: MOTORS_6DOF = 7, MULTIROTOR_WITH_TILT = 8, CUSTOM = 9, - HELICOPTER = 10, + HELICOPTER_TAIL_ESC = 10, + HELICOPTER_TAIL_SERVO = 11, + HELICOPTER_COAXIAL = 12, }; enum class FailureMode { @@ -184,10 +189,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _failure_detector_status_sub{ORB_ID(failure_detector_status)}; matrix::Vector3f _torque_sp; matrix::Vector3f _thrust_sp; + bool _publish_controls{true}; // Reflects motor failures that are currently handled, not motor failures that are reported. // For example, the system might report two motor failures, but only the first one is handled by CA diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index e48570dc2b..2c506cc4e2 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -27,7 +27,9 @@ parameters: 7: Motors (6DOF) 8: Multirotor with Tilt 9: Custom - 10: Helicopter + 10: Helicopter (tail ESC) + 11: Helicopter (tail Servo) + 12: Helicopter (Coaxial) default: 0 CA_METHOD: @@ -123,7 +125,7 @@ parameters: default: 0 CA_ROTOR${i}_PX: description: - short: Position of rotor ${i} along X body axis + short: Position of rotor ${i} along X body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -134,7 +136,7 @@ parameters: default: 0.0 CA_ROTOR${i}_PY: description: - short: Position of rotor ${i} along Y body axis + short: Position of rotor ${i} along Y body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -145,7 +147,7 @@ parameters: default: 0.0 CA_ROTOR${i}_PZ: description: - short: Position of rotor ${i} along Z body axis + short: Position of rotor ${i} along Z body axis relative to center of gravity type: float decimal: 2 increment: 0.1 @@ -415,6 +417,7 @@ parameters: short: Number of swash plates servos type: enum values: + 2: '2' 3: '3' 4: '4' default: 3 @@ -1058,7 +1061,7 @@ mixer: advanced: true identifier: 'servo-scale-spoiler' - 10: # Helicopter + 10: # Helicopter (tail ESC) actuators: - actuator_type: 'motor' count: 1 @@ -1078,6 +1081,8 @@ mixer: - name: 'CA_SV_CS${i}_TRIM' label: 'Trim' parameters: + - label: 'Collective pitch offset to align least amount of rotor drag' + name: CA_HELI_YAW_CP_O - label: 'Yaw compensation scale based on collective pitch' name: CA_HELI_YAW_CP_S - label: 'Yaw compensation scale based on throttle' @@ -1087,3 +1092,51 @@ mixer: - label: 'Throttle spoolup time' name: COM_SPOOLUP_TIME + 11: # Helicopter (tail Servo) + actuators: + - actuator_type: 'motor' + count: 1 + item_label_prefix: ['Rotor'] + - actuator_type: 'servo' + item_label_prefix: ['Yaw tail Servo'] + count: 1 + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Yaw compensation scale based on collective pitch' + name: CA_HELI_YAW_CP_S + - label: 'Yaw compensation scale based on throttle' + name: CA_HELI_YAW_TH_S + - label: 'Main rotor turns counter-clockwise' + name: CA_HELI_YAW_CCW + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME + + 12: # Helicopter (Coaxial) + actuators: + - actuator_type: 'motor' + count: 2 + item_label_prefix: ['Clockwise Rotor', 'Counter-clockwise Rotor'] + - actuator_type: 'servo' + group_label: 'Swash plate servos' + count: 'CA_SP0_COUNT' + per_item_parameters: + extra: + - name: 'CA_SP0_ANG${i}' + label: 'Angle' + - name: 'CA_SP0_ARM_L${i}' + label: 'Arm Length (relative)' + - name: 'CA_SV_CS${i}_TRIM' + label: 'Trim' + parameters: + - label: 'Throttle spoolup time' + name: COM_SPOOLUP_TIME diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp index f74cec5cb6..fc42770a96 100644 --- a/src/modules/dataman/dataman.cpp +++ b/src/modules/dataman/dataman.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -52,13 +52,18 @@ #include #include +#include +#include +#include +#include + #include "dataman.h" __BEGIN_DECLS __EXPORT int dataman_main(int argc, char *argv[]); __END_DECLS -static constexpr int TASK_STACK_SIZE = 1320; +static constexpr int TASK_STACK_SIZE = 1420; /* Private File based Operations */ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count); @@ -117,74 +122,25 @@ static struct { bool silence = false; } dm_operations_data; -/** Types of function calls supported by the worker task */ -typedef enum { - dm_write_func = 0, - dm_read_func, - dm_clear_func, - dm_number_of_funcs -} dm_function_t; - -/** Work task work item */ -typedef struct { - sq_entry_t link; /**< list linkage */ - px4_sem_t wait_sem; - unsigned char first; - unsigned char func; - ssize_t result; - union { - struct { - dm_item_t item; - unsigned index; - const void *buf; - size_t count; - } write_params; - struct { - dm_item_t item; - unsigned index; - void *buf; - size_t count; - } read_params; - struct { - dm_item_t item; - } clear_params; - }; -} work_q_item_t; - -const size_t k_work_item_allocation_chunk_size = 8; - /* Usage statistics */ -static unsigned g_func_counts[dm_number_of_funcs]; - -/* table of maximum number of instances for each item type */ -static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { - DM_KEY_SAFE_POINTS_MAX, - DM_KEY_FENCE_POINTS_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, - DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, - DM_KEY_MISSION_STATE_MAX, - DM_KEY_COMPAT_MAX -}; +static unsigned g_func_counts[DM_NUMBER_OF_FUNCS]; #define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */ -/* Table of the len of each item type */ -static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { - sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct mission_s) + DM_SECTOR_HDR_SIZE, - sizeof(struct dataman_compat_s) + DM_SECTOR_HDR_SIZE +/* Table of the len of each item type including HDR size */ +static constexpr size_t g_per_item_size_with_hdr[DM_KEY_NUM_KEYS] = { + g_per_item_size[DM_KEY_SAFE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_FENCE_POINTS] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_1] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_MISSION_STATE] + DM_SECTOR_HDR_SIZE, + g_per_item_size[DM_KEY_COMPAT] + DM_SECTOR_HDR_SIZE }; /* Table of offset for index 0 of each item type */ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS]; -/* Item type lock mutexes */ -static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS]; -static px4_sem_t g_sys_state_mutex_mission; -static px4_sem_t g_sys_state_mutex_fence; +static uint8_t dataman_clients_count = 1; static perf_counter_t _dm_read_perf{nullptr}; static perf_counter_t _dm_write_perf{nullptr}; @@ -200,160 +156,12 @@ static enum { BACKEND_LAST } backend = BACKEND_NONE; -/* The data manager work queues */ - -typedef struct { - sq_queue_t q; /* Nuttx queue */ - px4_sem_t mutex; /* Mutual exclusion on work queue adds and deletes */ - unsigned size; /* Current size of queue */ - unsigned max_size; /* Maximum queue size reached */ -} work_q_t; - -static work_q_t g_free_q; /* queue of free work items. So that we don't always need to call malloc and free*/ -static work_q_t g_work_q; /* pending work items. To be consumed by worker thread */ - -static px4_sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */ static px4_sem_t g_init_sema; static bool g_task_should_exit; /**< if true, dataman task should exit */ -static void init_q(work_q_t *q) -{ - sq_init(&(q->q)); /* Initialize the NuttX queue structure */ - px4_sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */ - q->size = q->max_size = 0; /* Queue is initially empty */ -} - -static inline void -destroy_q(work_q_t *q) -{ - px4_sem_destroy(&(q->mutex)); /* Destroy the queue lock */ -} - -static inline void -lock_queue(work_q_t *q) -{ - px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */ -} - -static inline void -unlock_queue(work_q_t *q) -{ - px4_sem_post(&(q->mutex)); /* Release the queue lock */ -} - -static work_q_item_t * -create_work_item() -{ - work_q_item_t *item; - - /* Try to reuse item from free item queue */ - lock_queue(&g_free_q); - - if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q)))) { - g_free_q.size--; - } - - unlock_queue(&g_free_q); - - /* If we there weren't any free items then obtain memory for a new ones */ - if (item == nullptr) { - item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t)); - - if (item) { - item->first = 1; - lock_queue(&g_free_q); - - for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { - (item + i)->first = 0; - sq_addfirst(&(item + i)->link, &(g_free_q.q)); - } - - /* Update the queue size and potentially the maximum queue size */ - g_free_q.size += k_work_item_allocation_chunk_size - 1; - - if (g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); - } - } - - /* If we got one then lock the item*/ - if (item) { - px4_sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */ - - /* item->wait_sem use case is a signal */ - - px4_sem_setprotocol(&item->wait_sem, SEM_PRIO_NONE); - } - - /* return the item pointer, or nullptr if all failed */ - return item; -} - /* Work queue management functions */ -static inline void -destroy_work_item(work_q_item_t *item) -{ - px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */ - /* Return the item to the free item queue for later reuse */ - lock_queue(&g_free_q); - sq_addfirst(&item->link, &(g_free_q.q)); - - /* Update the queue size and potentially the maximum queue size */ - if (++g_free_q.size > g_free_q.max_size) { - g_free_q.max_size = g_free_q.size; - } - - unlock_queue(&g_free_q); -} - -static inline work_q_item_t * -dequeue_work_item() -{ - work_q_item_t *work; - - /* retrieve the 1st item on the work queue */ - lock_queue(&g_work_q); - - if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q))) { - g_work_q.size--; - } - - unlock_queue(&g_work_q); - return work; -} - -static int -enqueue_work_item_and_wait_for_result(work_q_item_t *item) -{ - /* put the work item at the end of the work queue */ - lock_queue(&g_work_q); - sq_addlast(&item->link, &(g_work_q.q)); - - /* Adjust the queue size and potentially the maximum queue size */ - if (++g_work_q.size > g_work_q.max_size) { - g_work_q.max_size = g_work_q.size; - } - - unlock_queue(&g_work_q); - - /* tell the work thread that work is available */ - px4_sem_post(&g_work_queued_sema); - - /* wait for the result */ - px4_sem_wait(&item->wait_sem); - - int result = item->result; - - destroy_work_item(item); - - return result; -} - static bool is_running() { return dm_operations_data.running; @@ -375,7 +183,7 @@ calculate_offset(dm_item_t item, unsigned index) } /* Calculate and return the item index based on type and index */ - return g_key_offsets[item] + (index * g_per_item_size[item]); + return g_key_offsets[item] + (index * g_per_item_size_with_hdr[item]); } /* Each data item is stored as follows @@ -392,6 +200,10 @@ calculate_offset(dm_item_t item, unsigned index) /* write to the data manager RAM buffer */ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_t count) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -401,7 +213,7 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -429,7 +241,11 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, const void *buf, size_ static ssize_t _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) { - unsigned char buffer[g_per_item_size[item]]; + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ const int offset = calculate_offset(item, index); @@ -440,7 +256,7 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) } /* Make sure caller has not given us more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -502,6 +318,10 @@ _file_write(dm_item_t item, unsigned index, const void *buf, size_t count) /* Retrieve from the data manager RAM buffer*/ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -511,7 +331,7 @@ static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -546,7 +366,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) return -1; } - unsigned char buffer[g_per_item_size[item]]; + unsigned char buffer[g_per_item_size_with_hdr[item]]; /* Get the offset for this item */ int offset = calculate_offset(item, index); @@ -557,7 +377,7 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) } /* Make sure the caller hasn't asked for more data than we can handle */ - if (count > (g_per_item_size[item] - DM_SECTOR_HDR_SIZE)) { + if (count > (g_per_item_size_with_hdr[item] - DM_SECTOR_HDR_SIZE)) { return -E2BIG; } @@ -610,6 +430,9 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) /* Looks good, copy it to the caller's buffer */ memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]); + + } else { + memset(buf, 0, count); } /* Return the number of bytes of caller data read */ @@ -618,6 +441,10 @@ _file_read(dm_item_t item, unsigned index, void *buf, size_t count) static int _ram_clear(dm_item_t item) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + int i; int result = 0; @@ -639,7 +466,7 @@ static int _ram_clear(dm_item_t item) } buf[0] = 0; - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } return result; @@ -648,6 +475,10 @@ static int _ram_clear(dm_item_t item) static int _file_clear(dm_item_t item) { + if (item >= DM_KEY_NUM_KEYS) { + return -1; + } + int i, result = 0; /* Get the offset of 1st item of this type */ @@ -687,7 +518,7 @@ _file_clear(dm_item_t item) } } - offset += g_per_item_size[item]; + offset += g_per_item_size_with_hdr[item]; } /* Make sure data is actually written to physical media */ @@ -698,30 +529,7 @@ _file_clear(dm_item_t item) static int _file_initialize(unsigned max_offset) { - /* See if the data manage file exists and is a multiple of the sector size */ - dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY); - - if (dm_operations_data.file.fd >= 0) { - // Read the mission state and check the hash - struct dataman_compat_s compat_state; - dm_operations_data.silence = true; - int ret = g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); - dm_operations_data.silence = false; - - bool incompat = true; - - if (ret == sizeof(compat_state)) { - if (compat_state.key == DM_COMPAT_KEY) { - incompat = false; - } - } - - close(dm_operations_data.file.fd); - - if (incompat) { - unlink(k_data_manager_device_path); - } - } + const bool file_existed = (access(k_data_manager_device_path, F_OK) == 0); /* Open or create the data manager file */ dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666); @@ -739,16 +547,43 @@ _file_initialize(unsigned max_offset) return -1; } - /* Write current compat info */ - struct dataman_compat_s compat_state; - compat_state.key = DM_COMPAT_KEY; - int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + dataman_compat_s compat_state{}; - if (ret != sizeof(compat_state)) { - PX4_ERR("Failed writing compat: %d", ret); + dm_operations_data.silence = true; + + g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + + dm_operations_data.silence = false; + + if (!file_existed || (compat_state.key != DM_COMPAT_KEY)) { + + /* Write current compat info */ + compat_state.key = DM_COMPAT_KEY; + int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state)); + + if (ret != sizeof(compat_state)) { + PX4_ERR("Failed writing compat: %d", ret); + } + + for (uint32_t item = DM_KEY_SAFE_POINTS; item <= DM_KEY_MISSION_STATE; ++item) { + g_dm_ops->clear((dm_item_t)item); + } + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; + + mission_stats_entry_s stats; + stats.num_items = 0; + stats.update_counter = 1; + + g_dm_ops->write(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + g_dm_ops->write(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); + g_dm_ops->write(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), sizeof(mission_stats_entry_s)); } - fsync(dm_operations_data.file.fd); dm_operations_data.running = true; return 0; @@ -787,156 +622,6 @@ _ram_shutdown() dm_operations_data.running = false; } -/** Write to the data manager file */ -__EXPORT ssize_t -dm_write(dm_item_t item, unsigned index, const void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_write_perf); - - /* get a work item and queue up a write request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_write create_work_item failed"); - perf_end(_dm_write_perf); - return -1; - } - - work->func = dm_write_func; - work->write_params.item = item; - work->write_params.index = index; - work->write_params.buf = buf; - work->write_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_write_perf); - return ret; -} - -/** Retrieve from the data manager file */ -__EXPORT ssize_t -dm_read(dm_item_t item, unsigned index, void *buf, size_t count) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - perf_begin(_dm_read_perf); - - /* get a work item and queue up a read request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_read create_work_item failed"); - perf_end(_dm_read_perf); - return -1; - } - - work->func = dm_read_func; - work->read_params.item = item; - work->read_params.index = index; - work->read_params.buf = buf; - work->read_params.count = count; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - ssize_t ret = (ssize_t)enqueue_work_item_and_wait_for_result(work); - perf_end(_dm_read_perf); - return ret; -} - -/** Clear a data Item */ -__EXPORT int -dm_clear(dm_item_t item) -{ - work_q_item_t *work; - - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return -1; - } - - /* get a work item and queue up a clear request */ - if ((work = create_work_item()) == nullptr) { - PX4_ERR("dm_clear create_work_item failed"); - return -1; - } - - work->func = dm_clear_func; - work->clear_params.item = item; - - /* Enqueue the item on the work queue and wait for the worker thread to complete processing it */ - return enqueue_work_item_and_wait_for_result(work); -} - -__EXPORT int -dm_lock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_wait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -__EXPORT int -dm_trylock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - errno = EINVAL; - return -1; - } - - if (item >= DM_KEY_NUM_KEYS) { - errno = EINVAL; - return -1; - } - - if (g_item_locks[item]) { - return px4_sem_trywait(g_item_locks[item]); - } - - errno = EINVAL; - return -1; -} - -/** Unlock a data Item */ -__EXPORT void -dm_unlock(dm_item_t item) -{ - /* Make sure data manager has been started and is not shutting down */ - if (!is_running() || g_task_should_exit) { - return; - } - - if (item >= DM_KEY_NUM_KEYS) { - return; - } - - if (g_item_locks[item]) { - px4_sem_post(g_item_locks[item]); - } -} - static int task_main(int argc, char *argv[]) { @@ -955,43 +640,28 @@ task_main(int argc, char *argv[]) return -1; } - work_q_item_t *work; - /* Initialize global variables */ g_key_offsets[0] = 0; for (int i = 0; i < ((int)DM_KEY_NUM_KEYS - 1); i++) { - g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size[i]); + g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * g_per_item_size_with_hdr[i]); } unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * - g_per_item_size[DM_KEY_NUM_KEYS - 1]); + g_per_item_size_with_hdr[DM_KEY_NUM_KEYS - 1]); - for (unsigned i = 0; i < dm_number_of_funcs; i++) { + for (unsigned i = 0; i < DM_NUMBER_OF_FUNCS; i++) { g_func_counts[i] = 0; } - /* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */ - px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */ - px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */ - - for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) { - g_item_locks[i] = nullptr; - } - - g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission; - g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence; - g_task_should_exit = false; - init_q(&g_work_q); - init_q(&g_free_q); + uORB::Publication dataman_response_pub{ORB_ID(dataman_response)}; + const int dataman_request_sub = orb_subscribe(ORB_ID(dataman_request)); - px4_sem_init(&g_work_queued_sema, 1, 0); - - /* g_work_queued_sema use case is a signal */ - - px4_sem_setprotocol(&g_work_queued_sema, SEM_PRIO_NONE); + if (dataman_request_sub < 0) { + PX4_ERR("Failed to subscribe (%i)", errno); + } _dm_read_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": read"); _dm_write_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": write"); @@ -1017,47 +687,109 @@ task_main(int argc, char *argv[]) break; } + px4_pollfd_struct_t fds; + fds.fd = dataman_request_sub; + fds.events = POLLIN; + /* Tell startup that the worker thread has completed its initialization */ px4_sem_post(&g_init_sema); /* Start the endless loop, waiting for then processing work requests */ while (true) { - /* do we need to exit ??? */ - if (!g_task_should_exit) { - /* wait for work */ - g_dm_ops->wait(&g_work_queued_sema); - } + ret = px4_poll(&fds, 1, 1000); - /* Empty the work queue */ - while ((work = dequeue_work_item())) { + if (ret > 0) { - /* handle each work item with the appropriate handler */ - switch (work->func) { - case dm_write_func: - g_func_counts[dm_write_func]++; - work->result = - g_dm_ops->write(work->write_params.item, work->write_params.index, work->write_params.buf, work->write_params.count); - break; + bool updated = false; + orb_check(dataman_request_sub, &updated); - case dm_read_func: - g_func_counts[dm_read_func]++; - work->result = - g_dm_ops->read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count); - break; + if (updated) { - case dm_clear_func: - g_func_counts[dm_clear_func]++; - work->result = g_dm_ops->clear(work->clear_params.item); - break; + dataman_request_s request; + orb_copy(ORB_ID(dataman_request), dataman_request_sub, &request); - default: /* should never happen */ - work->result = -1; - break; + dataman_response_s response{}; + response.client_id = request.client_id; + response.request_type = request.request_type; + response.item = request.item; + response.index = request.index; + response.status = dataman_response_s::STATUS_FAILURE_NO_DATA; + + ssize_t result; + + switch (request.request_type) { + + case DM_GET_ID: + if (dataman_clients_count < UINT8_MAX) { + response.client_id = dataman_clients_count++; + /* Send the timestamp of the request over the data buffer so that the "dataman client" + * can distinguish whether the request was made by it. */ + memcpy(response.data, &request.timestamp, sizeof(hrt_abstime)); + + } else { + PX4_ERR("Max Dataman clients reached!"); + } + + break; + + case DM_WRITE: + + g_func_counts[DM_WRITE]++; + perf_begin(_dm_write_perf); + result = g_dm_ops->write(static_cast(request.item), request.index, + &(request.data), request.data_length); + perf_end(_dm_write_perf); + + if (result > 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_WRITE_FAILED; + } + + break; + + case DM_READ: + + g_func_counts[DM_READ]++; + perf_begin(_dm_read_perf); + result = g_dm_ops->read(static_cast(request.item), request.index, + &(response.data), request.data_length); + + perf_end(_dm_read_perf); + + if (result >= 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_READ_FAILED; + } + + break; + + case DM_CLEAR: + + g_func_counts[DM_CLEAR]++; + result = g_dm_ops->clear(static_cast(request.item)); + + if (result == 0) { + response.status = dataman_response_s::STATUS_SUCCESS; + + } else { + response.status = dataman_response_s::STATUS_FAILURE_CLEAR_FAILED; + } + + break; + + default: + break; + + } + + response.timestamp = hrt_absolute_time(); + dataman_response_pub.publish(response); } - - /* Inform the caller that work is done */ - px4_sem_post(&work->wait_sem); } /* time to go???? */ @@ -1066,26 +798,12 @@ task_main(int argc, char *argv[]) } } + orb_unsubscribe(dataman_request_sub); + g_dm_ops->shutdown(); - /* The work queue is now empty, empty the free queue */ - for (;;) { - if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == nullptr) { - break; - } - - if (work->first) { - free(work); - } - } - end: backend = BACKEND_NONE; - destroy_q(&g_work_q); - destroy_q(&g_free_q); - px4_sem_destroy(&g_work_queued_sema); - px4_sem_destroy(&g_sys_state_mutex_mission); - px4_sem_destroy(&g_sys_state_mutex_fence); perf_free(_dm_read_perf); _dm_read_perf = nullptr; @@ -1127,10 +845,10 @@ static void status() { /* display usage statistics */ - PX4_INFO("Writes %u", g_func_counts[dm_write_func]); - PX4_INFO("Reads %u", g_func_counts[dm_read_func]); - PX4_INFO("Clears %u", g_func_counts[dm_clear_func]); - PX4_INFO("Max Q lengths work %u, free %u", g_work_q.max_size, g_free_q.max_size); + PX4_INFO("Writes %u", g_func_counts[DM_WRITE]); + PX4_INFO("Reads %u", g_func_counts[DM_READ]); + PX4_INFO("Clears %u", g_func_counts[DM_CLEAR]); + perf_print_counter(_dm_read_perf); perf_print_counter(_dm_write_perf); } @@ -1140,7 +858,6 @@ stop() { /* Tell the worker task to shut down */ g_task_should_exit = true; - px4_sem_post(&g_work_queued_sema); } static void @@ -1158,13 +875,11 @@ It is used to store structured data of different types: mission waypoints, missi Each type has a specific type and a fixed maximum amount of storage items, so that fast random access is possible. ### Implementation -Reading and writing a single item is always atomic. If multiple items need to be read/modified atomically, there is -an additional lock per item type via `dm_lock`. +Reading and writing a single item is always atomic. **DM_KEY_FENCE_POINTS** and **DM_KEY_SAFE_POINTS** items: the first data element is a `mission_stats_entry_s` struct, which stores the number of items for these types. These items are always updated atomically in one transaction (from -the mavlink mission manager). During that time, navigator will try to acquire the geofence item lock, fail, and will not -check for geofence violations. +the mavlink mission manager). )DESCR_STR"); @@ -1274,3 +989,11 @@ dataman_main(int argc, char *argv[]) return 0; } + +static_assert(sizeof(dataman_request_s::data) == sizeof(dataman_response_s::data), "request and response data are not the same size"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SAFE_POINT_SIZE, "mission_safe_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_FENCE_POINT_SIZE, "mission_fance_point_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_ITEM_SIZE, "mission_item_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= MISSION_SIZE, "mission_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= DATAMAN_COMPAT_SIZE, "dataman_compat_s can't fit in the response data"); +static_assert(sizeof(dataman_response_s::data) >= sizeof(hrt_abstime), "hrt_abstime can't fit in the response data"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index bcc5b2984a..48d048ca44 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,21 +42,27 @@ #include #include -#ifdef __cplusplus -extern "C" { -#endif - /** Types of items that the data manager can store */ typedef enum { - DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */ - DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */ - DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ - DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_SAFE_POINTS = 0, ///< Safe points coordinates, safe point 0 is home point + DM_KEY_FENCE_POINTS, ///< Fence vertex coordinates + DM_KEY_WAYPOINTS_OFFBOARD_0, ///< Mission way point coordinates sent over mavlink + DM_KEY_WAYPOINTS_OFFBOARD_1, ///< (alternate between 0 and 1) + DM_KEY_MISSION_STATE, ///< Persistent mission state DM_KEY_COMPAT, - DM_KEY_NUM_KEYS /* Total number of item types defined */ + DM_KEY_NUM_KEYS ///< Total number of item types defined } dm_item_t; +/** Types of function calls supported by the worker task */ +typedef enum { + DM_GET_ID = 0, ///< Get dataman client ID + DM_WRITE, ///< Write index for given item + DM_READ, ///< Read index for given item + DM_CLEAR, ///< Clear all index for given item + DM_NUMBER_OF_FUNCS +} dm_function_t; + +/** The maximum number of instances for each item type */ #if defined(MEMORY_CONSTRAINED_SYSTEM) enum { DM_KEY_SAFE_POINTS_MAX = 8, @@ -67,7 +73,6 @@ enum { DM_KEY_COMPAT_MAX = 1 }; #else -/** The maximum number of instances for each item type */ enum { DM_KEY_SAFE_POINTS_MAX = 8, DM_KEY_FENCE_POINTS_MAX = 64, @@ -78,68 +83,40 @@ enum { }; #endif +/* table of maximum number of instances for each item type */ +static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = { + DM_KEY_SAFE_POINTS_MAX, + DM_KEY_FENCE_POINTS_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_0_MAX, + DM_KEY_WAYPOINTS_OFFBOARD_1_MAX, + DM_KEY_MISSION_STATE_MAX, + DM_KEY_COMPAT_MAX +}; + +constexpr uint32_t MISSION_SAFE_POINT_SIZE = sizeof(struct mission_safe_point_s); +constexpr uint32_t MISSION_FENCE_POINT_SIZE = sizeof(struct mission_fence_point_s); +constexpr uint32_t MISSION_ITEM_SIZE = sizeof(struct mission_item_s); +constexpr uint32_t MISSION_SIZE = sizeof(struct mission_s); +constexpr uint32_t DATAMAN_COMPAT_SIZE = sizeof(struct mission_s); + +/** The table of the size of each item type */ +static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = { + MISSION_SAFE_POINT_SIZE, + MISSION_FENCE_POINT_SIZE, + MISSION_ITEM_SIZE, + MISSION_ITEM_SIZE, + MISSION_SIZE, + DATAMAN_COMPAT_SIZE +}; + struct dataman_compat_s { uint64_t key; }; /* increment this define whenever a binary incompatible change is performed */ -#define DM_COMPAT_VERSION 2ULL +#define DM_COMPAT_VERSION 3ULL #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \ (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \ (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \ sizeof(struct dataman_compat_s)) - -/** Retrieve from the data manager store */ -__EXPORT ssize_t -dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** write to the data manager store */ -__EXPORT ssize_t -dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ -); - -/** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ -__EXPORT int -dm_lock( - dm_item_t item /* The item type to lock */ -); - -/** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ -__EXPORT int -dm_trylock( - dm_item_t item /* The item type to lock */ -); - -/** Unlock all items of a type */ -__EXPORT void -dm_unlock( - dm_item_t item /* The item type to unlock */ -); - -/** Erase all items of this type */ -__EXPORT int -dm_clear( - dm_item_t item /* The item type to clear */ -); - -#ifdef __cplusplus -} -#endif diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index 9b7a6e493f..83d12e944a 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -68,7 +68,6 @@ add_subdirectory(Utility) set(EKF_SRCS) list(APPEND EKF_SRCS - EKF/baro_height_control.cpp EKF/bias_estimator.cpp EKF/control.cpp EKF/covariance.cpp @@ -84,11 +83,11 @@ list(APPEND EKF_SRCS EKF/gravity_fusion.cpp EKF/height_control.cpp EKF/imu_down_sampler.cpp - EKF/mag_control.cpp - EKF/mag_fusion.cpp EKF/output_predictor.cpp EKF/vel_pos_fusion.cpp + EKF/yaw_fusion.cpp EKF/zero_innovation_heading_update.cpp + EKF/zero_gyro_update.cpp EKF/zero_velocity_update.cpp ) @@ -100,6 +99,12 @@ if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp) endif() +if(CONFIG_EKF2_BAROMETER) + list(APPEND EKF_SRCS + EKF/baro_height_control.cpp + ) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS EKF/drag_fusion.cpp) endif() @@ -118,6 +123,15 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS EKF/gps_yaw_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 + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS EKF/optical_flow_control.cpp @@ -130,7 +144,6 @@ if(CONFIG_EKF2_RANGE_FINDER) EKF/range_finder_consistency_check.cpp EKF/range_height_control.cpp EKF/sensor_range_finder.cpp - EKF/terrain_estimator.cpp ) endif() @@ -138,6 +151,10 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS EKF/terrain_estimator.cpp) +endif() + px4_add_module( MODULE modules__ekf2 MAIN ekf2 diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 50cfd236fd..656bbcc1ce 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015-2018 ECL Development Team. All rights reserved. +# 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 @@ -12,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name ECL nor the names of its contributors may be +# 3. Neither the name PX4 nor the names of its contributors may be # used to endorse or promote products derived from this software # without specific prior written permission. # @@ -33,7 +33,6 @@ set(EKF_SRCS) list(APPEND EKF_SRCS - baro_height_control.cpp bias_estimator.cpp control.cpp covariance.cpp @@ -49,11 +48,11 @@ list(APPEND EKF_SRCS gravity_fusion.cpp height_control.cpp imu_down_sampler.cpp - mag_control.cpp - mag_fusion.cpp output_predictor.cpp vel_pos_fusion.cpp + yaw_fusion.cpp zero_innovation_heading_update.cpp + zero_gyro_update.cpp zero_velocity_update.cpp ) @@ -65,6 +64,12 @@ if(CONFIG_EKF2_AUXVEL) list(APPEND EKF_SRCS auxvel_fusion.cpp) endif() +if(CONFIG_EKF2_BAROMETER) + list(APPEND EKF_SRCS + baro_height_control.cpp + ) +endif() + if(CONFIG_EKF2_DRAG_FUSION) list(APPEND EKF_SRCS drag_fusion.cpp) endif() @@ -83,6 +88,15 @@ if(CONFIG_EKF2_GNSS_YAW) list(APPEND EKF_SRCS gps_yaw_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 + ) +endif() + if(CONFIG_EKF2_OPTICAL_FLOW) list(APPEND EKF_SRCS optical_flow_control.cpp @@ -95,7 +109,6 @@ if(CONFIG_EKF2_RANGE_FINDER) range_finder_consistency_check.cpp range_height_control.cpp sensor_range_finder.cpp - terrain_estimator.cpp ) endif() @@ -103,6 +116,10 @@ if(CONFIG_EKF2_SIDESLIP) list(APPEND EKF_SRCS sideslip_fusion.cpp) endif() +if(CONFIG_EKF2_TERRAIN) + list(APPEND EKF_SRCS terrain_estimator.cpp) +endif() + add_library(ecl_EKF ${EKF_SRCS} ) diff --git a/src/modules/ekf2/EKF/EKFGSF_yaw.h b/src/modules/ekf2/EKF/EKFGSF_yaw.h index f31f430330..c14ab4d9ff 100644 --- a/src/modules/ekf2/EKF/EKFGSF_yaw.h +++ b/src/modules/ekf2/EKF/EKFGSF_yaw.h @@ -39,7 +39,6 @@ #include #include "common.h" -#include "utils.hpp" using matrix::AxisAnglef; using matrix::Dcmf; diff --git a/src/modules/ekf2/EKF/Sensor.hpp b/src/modules/ekf2/EKF/Sensor.hpp index 5f18340c77..e1b83e708b 100644 --- a/src/modules/ekf2/EKF/Sensor.hpp +++ b/src/modules/ekf2/EKF/Sensor.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 62afda7447..5ad89e76cb 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -151,8 +151,6 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so aid_src.innovation = innov; aid_src.innovation_variance = innov_var; - aid_src.fusion_enabled = _control_status.flags.fuse_aspd; - aid_src.timestamp_sample = airspeed_sample.time_us; const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f); @@ -194,15 +192,15 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour _fault_status.flags.bad_airspeed = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector sym::ComputeAirspeedHAndK(getStateAtFusionHorizonAsVector(), P, innov_var, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, aid_src.innovation_variance, aid_src.innovation); @@ -258,7 +256,7 @@ void Ekf::resetWindCovarianceUsingAirspeed(const airspeedSample &airspeed_sample const float Wy = -_state.wind_vel(0) * sin_yaw + _state.wind_vel(1) * cos_yaw; // it is safer to remove all existing correlations to other states at this time - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); P(22, 22) = R_TAS * sq(cos_yaw) + R_yaw * sq(-Wx * sin_yaw - Wy * cos_yaw) + initial_wind_var_body_y * sq(sin_yaw); P(22, 23) = R_TAS * sin_yaw * cos_yaw + R_yaw * (-Wx * sin_yaw - Wy * cos_yaw) * (Wx * cos_yaw - Wy * sin_yaw) - diff --git a/src/modules/ekf2/EKF/auxvel_fusion.cpp b/src/modules/ekf2/EKF/auxvel_fusion.cpp index 680f40cc7d..dbe88ccb87 100644 --- a/src/modules/ekf2/EKF/auxvel_fusion.cpp +++ b/src/modules/ekf2/EKF/auxvel_fusion.cpp @@ -45,7 +45,6 @@ void Ekf::controlAuxVelFusion() updateVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel); if (isHorizontalAidingActive()) { - _aid_src_aux_vel.fusion_enabled = true; fuseVelocity(_aid_src_aux_vel); } } diff --git a/src/modules/ekf2/EKF/baro_height_control.cpp b/src/modules/ekf2/EKF/baro_height_control.cpp index fb649d5c5f..f3c5b0df64 100644 --- a/src/modules/ekf2/EKF/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/baro_height_control.cpp @@ -51,7 +51,12 @@ void Ekf::controlBaroHeightFusion() if (_baro_buffer && _baro_buffer->pop_first_older_than(_time_delayed_us, &baro_sample)) { +#if defined(CONFIG_EKF2_BARO_COMPENSATION) const float measurement = compensateBaroForDynamicPressure(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); @@ -103,7 +108,7 @@ void Ekf::controlBaroHeightFusion() if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + 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 @@ -116,7 +121,6 @@ void Ekf::controlBaroHeightFusion() && isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL); if (_control_status.flags.baro_hgt) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index 53f82dc03c..d810fc1d07 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -47,7 +47,8 @@ * @author Mathieu Bresciani */ -#pragma once +#ifndef EKF_BIAS_ESTIMATOR_HPP +#define EKF_BIAS_ESTIMATOR_HPP #include #include @@ -133,3 +134,5 @@ private: static constexpr float _innov_sequence_monitnoring_time_constant{10.f}; ///< in seconds static constexpr float _process_var_boost_gain{1.0e3f}; }; + +#endif // !EKF_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b9fef6e6fa..90c678ae7f 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -102,18 +102,15 @@ 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 - MAG_3D = 2, ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions - UNUSED = 3, ///< Not implemented - INDOOR = 4, ///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates. NONE = 5 ///< Do not use magnetometer under any circumstance.. }; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) enum TerrainFusionMask : uint8_t { TerrainFuseRangeFinder = (1 << 0), TerrainFuseOpticalFlow = (1 << 1) }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN enum HeightSensor : uint8_t { BARO = 0, @@ -155,6 +152,12 @@ enum class EvCtrl : uint8_t { YAW = (1<<3) }; +enum class MagCheckMask : uint8_t { + STRENGTH = (1 << 0), + INCLINATION = (1 << 1), + FORCE_WMM = (1 << 2) +}; + struct gpsMessage { uint64_t time_usec{}; int32_t lat{}; ///< Latitude in 1E-7 degrees @@ -270,8 +273,8 @@ struct stateSample { Quatf quat_nominal{}; ///< quaternion defining the rotation from body to earth frame Vector3f vel{}; ///< NED velocity in earth frame in m/s Vector3f pos{}; ///< NED position in earth frame in m - Vector3f delta_ang_bias{}; ///< delta angle bias estimate in rad - Vector3f delta_vel_bias{}; ///< delta velocity bias estimate in m/s + Vector3f gyro_bias{}; ///< gyro bias estimate in rad/s + Vector3f accel_bias{}; ///< accel bias estimate in m/s^2 Vector3f mag_I{}; ///< NED earth magnetic field in gauss Vector3f mag_B{}; ///< magnetometer bias estimate in body frame in gauss Vector2f wind_vel{}; ///< horizontal wind velocity in earth frame in m/s @@ -329,7 +332,7 @@ struct parameters { // magnetometer fusion float mag_heading_noise{3.0e-1f}; ///< measurement noise used for simple heading fusion (rad) - float mag_noise{5.0e-2f}; ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) + 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 heading_innov_gate{2.6f}; ///< heading fusion innovation consistency gate size (STD) float mag_innov_gate{3.0f}; ///< magnetometer fusion innovation consistency gate size (STD) @@ -359,18 +362,27 @@ struct parameters { const float beta_avg_ft_us{150000.0f}; ///< The average time between synthetic sideslip measurements (uSec) #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_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) +#endif // CONFIG_EKF2_TERRAIN || CONFIG_EKF2_OPTICAL_FLOW || CONFIG_EKF2_RANGE_FINDER + #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion int32_t rng_ctrl{RngCtrl::CONDITIONAL}; - int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | - TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator - 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_gnd_clearance{0.1f}; ///< minimum valid value for range when on ground (m) 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 @@ -382,11 +394,6 @@ struct parameters { float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check Vector3f rng_pos_body{}; ///< xyz position of range sensor in body frame (m) - - 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_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -416,6 +423,7 @@ struct parameters { float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor + int32_t flow_qual_min_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) @@ -485,7 +493,9 @@ struct parameters { // compute synthetic magnetomter Z value if possible int32_t synthesize_mag_z{0}; - int32_t check_mag_strength{0}; + int32_t mag_check{0}; + float mag_check_strength_tolerance_gs{0.2f}; + float mag_check_inclination_tolerance_deg{20.f}; // Parameters used to control when yaw is reset to the EKF-GSF yaw estimator value float EKFGSF_tas_default{15.0f}; ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s) @@ -592,6 +602,10 @@ union filter_control_status_u { 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 + } flags; uint64_t value; }; @@ -615,7 +629,7 @@ union ekf_solution_status_u { uint16_t value; }; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) union terrain_fusion_status_u { struct { bool range_finder : 1; ///< 0 - true if we are fusing range finder data @@ -623,7 +637,7 @@ union terrain_fusion_status_u { } flags; uint8_t value; }; -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN // define structure used to communicate information events union information_event_status_u { diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 20ff1dcde3..332aaf37b8 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -76,7 +76,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) _control_status.flags.tilt_align = true; // send alignment status message to the console - const char *height_source = nullptr; + const char *height_source = "unknown"; if (_control_status.flags.baro_hgt) { height_source = "baro"; @@ -89,21 +89,23 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) } else if (_control_status.flags.rng_hgt) { height_source = "rng"; - - } else { - height_source = "unknown"; - } - if (height_source) { - ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", + ECL_INFO("%llu: EKF aligned, (%s hgt, IMU buf: %i, OBS buf: %i)", (unsigned long long)imu_delayed.time_us, height_source, (int)_imu_buffer_length, (int)_obs_buffer_length); - } + + ECL_DEBUG("tilt aligned, roll: %.3f, pitch %.3f, yaw: %.3f", + (double)matrix::Eulerf(_state.quat_nominal).phi(), + (double)matrix::Eulerf(_state.quat_nominal).theta(), + (double)matrix::Eulerf(_state.quat_nominal).psi() + ); } } +#if defined(CONFIG_EKF2_MAGNETOMETER) // control use of observations for aiding controlMagFusion(); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_OPTICAL_FLOW) controlOpticalFlowFusion(imu_delayed); @@ -139,6 +141,7 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) controlZeroInnovationHeadingUpdate(); controlZeroVelocityUpdate(); + controlZeroGyroUpdate(imu_delayed); // Fake position measurement for constraining drift when no other velocity or position measurements controlFakePosFusion(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index b6726fdf6b..bbd7d3bec9 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -43,7 +43,7 @@ #include "ekf.h" #include "python/ekf_derivation/generated/predict_covariance.h" -#include "utils.hpp" +#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" #include #include @@ -54,70 +54,56 @@ void Ekf::initialiseCovariance() { P.zero(); - _delta_angle_bias_var_accum.setZero(); - _delta_vel_bias_var_accum.setZero(); - - const float dt = _dt_ekf_avg; - resetQuatCov(); // velocity - P(4,4) = sq(fmaxf(_params.gps_vel_noise, 0.01f)); - P(5,5) = P(4,4); - P(6,6) = sq(1.5f) * P(4,4); + const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f)); + P.uncorrelateCovarianceSetVariance(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var)); // position - P(7,7) = sq(fmaxf(_params.gps_pos_noise, 0.01f)); - P(8,8) = P(7,7); - P(9,9) = sq(fmaxf(_params.baro_noise, 0.01f)); + const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); + float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f)); if (_control_status.flags.gps_hgt) { - P(9,9) = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); + z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); } #if defined(CONFIG_EKF2_RANGE_FINDER) if (_control_status.flags.rng_hgt) { - P(9,9) = sq(fmaxf(_params.range_noise, 0.01f)); + 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)); + // gyro bias - _prev_delta_ang_bias_var(0) = P(10,10) = sq(_params.switch_on_gyro_bias * dt); - _prev_delta_ang_bias_var(1) = P(11,11) = P(10,10); - _prev_delta_ang_bias_var(2) = P(12,12) = P(10,10); + const float gyro_bias_var = sq(_params.switch_on_gyro_bias); + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, gyro_bias_var); + _prev_gyro_bias_var.setAll(gyro_bias_var); // accel bias - _prev_dvel_bias_var(0) = P(13,13) = sq(_params.switch_on_accel_bias * dt); - _prev_dvel_bias_var(1) = P(14,14) = P(13,13); - _prev_dvel_bias_var(2) = P(15,15) = P(13,13); + const float accel_bias_var = sq(_params.switch_on_accel_bias); + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, accel_bias_var); + _prev_accel_bias_var.setAll(accel_bias_var); resetMagCov(); // wind - P(22,22) = sq(_params.initial_wind_uncertainty); - P(23,23) = P(22,22); - + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, sq(_params.initial_wind_uncertainty)); } void Ekf::predictCovariance(const imuSample &imu_delayed) { // Use average update interval to reduce accumulated covariance prediction errors due to small single frame dt values const float dt = _dt_ekf_avg; - const float dt_inv = 1.f / dt; - - // convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update - const float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f); - - // convert rate of change of accelerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update - const float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f); // inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected // xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector const float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f); const float beta = 1.0f - alpha; - _ang_rate_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_ang.norm(), beta * _ang_rate_magnitude_filt); - _accel_magnitude_filt = fmaxf(dt_inv * imu_delayed.delta_vel.norm(), beta * _accel_magnitude_filt); - _accel_vec_filt = alpha * dt_inv * imu_delayed.delta_vel + beta * _accel_vec_filt; + _ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt, beta * _ang_rate_magnitude_filt); + _accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt); + _accel_vec_filt = alpha * imu_delayed.delta_vel / imu_delayed.delta_vel_dt + beta * _accel_vec_filt; const bool is_manoeuvre_level_high = _ang_rate_magnitude_filt > _params.acc_bias_learn_gyr_lim || _accel_magnitude_filt > _params.acc_bias_learn_acc_lim; @@ -125,8 +111,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) // gyro bias inhibit const bool do_inhibit_all_gyro_axes = !(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias)); - for (unsigned stateIndex = 10; stateIndex <= 12; stateIndex++) { - const unsigned index = stateIndex - 10; + for (unsigned index = 0; index < State::gyro_bias.dof; index++) { + const unsigned stateIndex = State::gyro_bias.idx + index; bool is_bias_observable = true; @@ -137,14 +123,14 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (do_inhibit_axis) { // store the bias state variances to be reinstated later if (!_gyro_bias_inhibit[index]) { - _prev_delta_ang_bias_var(index) = P(stateIndex, stateIndex); + _prev_gyro_bias_var(index) = P(stateIndex, stateIndex); _gyro_bias_inhibit[index] = true; } } else { if (_gyro_bias_inhibit[index]) { // reinstate the bias state variances - P(stateIndex, stateIndex) = _prev_delta_ang_bias_var(index); + P(stateIndex, stateIndex) = _prev_gyro_bias_var(index); _gyro_bias_inhibit[index] = false; } } @@ -155,8 +141,8 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) || is_manoeuvre_level_high || _fault_status.flags.bad_acc_vertical; - for (unsigned stateIndex = 13; stateIndex <= 15; stateIndex++) { - const unsigned index = stateIndex - 13; + for (unsigned index = 0; index < State::accel_bias.dof; index++) { + const unsigned stateIndex = State::accel_bias.idx + index; bool is_bias_observable = true; @@ -176,37 +162,35 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (do_inhibit_axis) { // store the bias state variances to be reinstated later if (!_accel_bias_inhibit[index]) { - _prev_dvel_bias_var(index) = P(stateIndex, stateIndex); + _prev_accel_bias_var(index) = P(stateIndex, stateIndex); _accel_bias_inhibit[index] = true; } } else { if (_accel_bias_inhibit[index]) { // reinstate the bias state variances - P(stateIndex, stateIndex) = _prev_dvel_bias_var(index); + P(stateIndex, stateIndex) = _prev_accel_bias_var(index); _accel_bias_inhibit[index] = false; } } } // Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_I_sig; + float mag_I_sig = 0.0f; - if (_control_status.flags.mag_3D && (P(16, 16) + P(17, 17) + P(18, 18)) < 0.1f) { + if (_control_status.flags.mag && P.trace(State::mag_I.idx) < 0.1f) { +#if defined(CONFIG_EKF2_MAGNETOMETER) mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f); - - } else { - mag_I_sig = 0.0f; +#endif // CONFIG_EKF2_MAGNETOMETER } // Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned - float mag_B_sig; + float mag_B_sig = 0.0f; - if (_control_status.flags.mag_3D && (P(19, 19) + P(20, 20) + P(21, 21)) < 0.1f) { + if (_control_status.flags.mag && P.trace(State::mag_B.idx) < 0.1f) { +#if defined(CONFIG_EKF2_MAGNETOMETER) mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f); - - } else { - mag_B_sig = 0.0f; +#endif // CONFIG_EKF2_MAGNETOMETER } float wind_vel_nsd_scaled; @@ -216,105 +200,122 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; // Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned - if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) { + if (_control_status.flags.wind && P.trace(State::wind_vel.idx) < sq(_params.initial_wind_uncertainty)) { wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); } else { wind_vel_nsd_scaled = 0.0f; } - // compute noise variance for stationary processes - Vector24f process_noise; - - // 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 - - // delta angle bias states - process_noise.slice<3, 1>(10, 0) = sq(d_ang_bias_sig); - // delta_velocity bias states - process_noise.slice<3, 1>(13, 0) = sq(d_vel_bias_sig); - // earth frame magnetic field states - process_noise.slice<3, 1>(16, 0) = sq(mag_I_sig); - // body frame magnetic field states - process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig); - // wind velocity states - process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt; - // assign IMU noise variances // inputs to the system are 3 delta angles and 3 delta velocities float gyro_noise = math::constrain(_params.gyro_noise, 0.0f, 1.0f); - const float d_ang_var = sq(dt * gyro_noise); + const float d_ang_var = sq(imu_delayed.delta_ang_dt * gyro_noise); float accel_noise = math::constrain(_params.accel_noise, 0.0f, 1.0f); Vector3f d_vel_var; - for (int i = 0; i <= 2; i++) { + for (unsigned i = 0; i < 3; i++) { if (_fault_status.flags.bad_acc_vertical || imu_delayed.delta_vel_clipping[i]) { // Increase accelerometer process noise if bad accel data is detected - d_vel_var(i) = sq(dt * BADACC_BIAS_PNOISE); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * BADACC_BIAS_PNOISE); } else { - d_vel_var(i) = sq(dt * accel_noise); + d_vel_var(i) = sq(imu_delayed.delta_vel_dt * accel_noise); } } // predict the covariance - SquareMatrix24f nextP; + SquareMatrixState nextP; // calculate variances and upper diagonal covariances for quaternion, velocity, position and gyro bias states - sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, imu_delayed.delta_vel, d_vel_var, imu_delayed.delta_ang, d_ang_var, dt, &nextP); + sym::PredictCovariance(getStateAtFusionHorizonAsVector(), P, + imu_delayed.delta_vel, imu_delayed.delta_vel_dt, d_vel_var, + imu_delayed.delta_ang, imu_delayed.delta_ang_dt, d_ang_var, + &nextP); - // process noise contribution for delta angle states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - for (unsigned i = 10; i <= 12; i++) { - const int index = i - 10; + // 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, or restore previous gyro bias var if state inhibited + const float gyro_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.f, 1.f); + 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]) { - // add process noise that is not from the IMU - // process noise contribution for delta velocity states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_angle_bias_var_accum(index)); + nextP(i, i) += gyro_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_delta_ang_bias_var(index)); - _delta_angle_bias_var_accum(index) = 0.f; + nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_gyro_bias_var(index)); } } - for (int i = 13; i <= 15; i++) { - const int index = i - 13; + // accel bias: add process noise, or restore previous accel bias var if state inhibited + const float accel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.f, 1.f); + 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]) { - // add process noise that is not from the IMU - // process noise contribution for delta velocity states can be very small compared to - // the variances, therefore use algorithm to minimise numerical error - nextP(i, i) = kahanSummation(nextP(i, i), process_noise(i), _delta_vel_bias_var_accum(index)); + nextP(i, i) += accel_bias_process_noise; } else { - nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_dvel_bias_var(index)); - _delta_vel_bias_var_accum(index) = 0.f; + nextP.uncorrelateCovarianceSetVariance<1>(i, _prev_accel_bias_var(index)); } } - // add process noise that is not from the IMU - for (unsigned i = 16; i <= 23; i++) { - nextP(i, i) += process_noise(i); + if (_control_status.flags.mag) { + const float mag_I_process_noise = sq(mag_I_sig); + for (unsigned index = 0; index < State::mag_I.dof; index++) { + unsigned i = State::mag_I.idx + index; + nextP(i, i) += mag_I_process_noise; + } + + const float mag_B_process_noise = sq(mag_B_sig); + for (unsigned index = 0; index < State::mag_B.dof; index++) { + unsigned i = State::mag_B.idx + index; + nextP(i, i) += mag_B_process_noise; + } + + } else { + // keep previous covariance + for (unsigned i = 0; i < State::mag_I.dof; i++) { + unsigned row = State::mag_I.idx + i; + for (unsigned col = 0; col < State::size; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); + } + } + + for (unsigned i = 0; i < State::mag_B.dof; i++) { + unsigned row = State::mag_B.idx + i; + for (unsigned col = 0; col < State::size; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); + } + } } - // stop position covariance growth if our total position variance reaches 100m - // this can happen if we lose gps for some time - if ((P(7, 7) + P(8, 8)) > 1e4f) { - for (uint8_t i = 7; i <= 8; i++) { - for (uint8_t j = 0; j < _k_num_states; j++) { - nextP(i, j) = P(i, j); - nextP(j, i) = P(j, i); + if (_control_status.flags.wind) { + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + unsigned i = State::wind_vel.idx + index; + nextP(i, i) += wind_vel_process_noise; + } + + } else { + // keep previous covariance + for (unsigned i = 0; i < State::wind_vel.dof; i++) { + unsigned row = State::wind_vel.idx + i; + for (unsigned col = 0 ; col < State::size; col++) { + nextP(row, col) = nextP(col, row) = P(row, col); } } } // covariance matrix is symmetrical, so copy upper half to lower half - for (unsigned row = 0; row <= 15; row++) { + for (unsigned row = 0; row < State::size; row++) { for (unsigned column = 0 ; column < row; column++) { P(row, column) = P(column, row) = nextP(column, row); } @@ -322,30 +323,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) P(row, row) = nextP(row, row); } - if (_control_status.flags.mag_3D) { - for (unsigned row = 16; row <= 21; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - } - - if (_control_status.flags.wind) { - for (unsigned row = 22; row <= 23; row++) { - for (unsigned column = 0 ; column < row; column++) { - P(row, column) = P(column, row) = nextP(column, row); - } - - P(row, row) = nextP(row, row); - } - } - // fix gross errors in the covariance matrix and ensure rows and // columns for un-used states are zero fixCovarianceErrors(false); - } void Ekf::fixCovarianceErrors(bool force_symmetry) @@ -355,39 +335,25 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // and set corresponding entries in Q to zero when states exceed 50% of the limit // Covariance diagonal limits. Use same values for states which // belong to the same group (e.g. vel_x, vel_y, vel_z) - float P_lim[8] = {}; - P_lim[0] = 1.0f; // quaternion max var - P_lim[1] = 1e6f; // velocity max var - P_lim[2] = 1e6f; // position max var - P_lim[3] = 1.0f; // gyro bias max var - P_lim[4] = 1.0f; // delta velocity z bias max var - P_lim[5] = 1.0f; // earth mag field max var - P_lim[6] = 1.0f; // body mag field max var - P_lim[7] = 1e6f; // wind max var + const float quat_var_max = 1.0f; + const float vel_var_max = 1e6f; + const float pos_var_max = 1e6f; + const float gyro_bias_var_max = 1.0f; + const float mag_I_var_max = 1.0f; + const float mag_B_var_max = 1.0f; + const float wind_vel_var_max = 1e6f; - for (int i = 0; i <= 3; i++) { - // quaternion states - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[0]); - } - - for (int i = 4; i <= 6; i++) { - // NED velocity states - P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[1]); - } - - for (int i = 7; i <= 9; i++) { - // NED position states - P(i, i) = math::constrain(P(i, i), 1e-6f, P_lim[2]); - } - - for (int i = 10; i <= 12; i++) { - // gyro bias states - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[3]); - } + constrainStateVar(State::quat_nominal, 0.f, quat_var_max); + constrainStateVar(State::vel, 1e-6f, vel_var_max); + constrainStateVar(State::pos, 1e-6f, pos_var_max); + constrainStateVar(State::gyro_bias, 0.f, gyro_bias_var_max); // force symmetry on the quaternion, velocity and position state covariances if (force_symmetry) { - P.makeRowColSymmetric<13>(0); + P.makeRowColSymmetric(State::quat_nominal.idx); + P.makeRowColSymmetric(State::vel.idx); + P.makeRowColSymmetric(State::pos.idx); + P.makeRowColSymmetric(State::gyro_bias.idx); //TODO: needed? } // the following states are optional and are deactivated when not required @@ -396,12 +362,14 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // accelerometer bias states if (!_accel_bias_inhibit[0] || !_accel_bias_inhibit[1] || !_accel_bias_inhibit[2]) { // Find the maximum delta velocity bias state variance and request a covariance reset if any variance is below the safe minimum - const float minSafeStateVar = 1e-9f; + const float minSafeStateVar = 1e-9f / sq(_dt_ekf_avg); float maxStateVar = minSafeStateVar; bool resetRequired = false; - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } @@ -417,28 +385,30 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // To ensure stability of the covariance matrix operations, the ratio of a max and min variance must // not exceed 100 and the minimum variance must not fall below the target minimum // Also limit variance to a maximum equivalent to a 0.1g uncertainty - const float minStateVarTarget = 5E-8f; + const float minStateVarTarget = 5E-8f / sq(_dt_ekf_avg); float minAllowedStateVar = fmaxf(0.01f * maxStateVar, minStateVarTarget); - for (uint8_t stateIndex = 13; stateIndex <= 15; stateIndex++) { - if (_accel_bias_inhibit[stateIndex - 13]) { + for (unsigned axis = 0; axis < State::accel_bias.dof; axis++) { + const unsigned stateIndex = State::accel_bias.idx + axis; + + if (_accel_bias_inhibit[axis]) { // Skip the check for the inhibited axis continue; } - P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, - sq(0.1f * CONSTANTS_ONE_G * _dt_ekf_avg)); + P(stateIndex, stateIndex) = math::constrain(P(stateIndex, stateIndex), minAllowedStateVar, sq(0.1f * CONSTANTS_ONE_G)); } // If any one axis has fallen below the safe minimum, all delta velocity covariance terms must be reset to zero if (resetRequired) { - P.uncorrelateCovariance<3>(13); + P.uncorrelateCovariance(State::accel_bias.idx); } // Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong // calculate accel bias term aligned with the gravity vector const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg; - const float down_dvel_bias = _state.delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); + const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg; + const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2))); // check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation bool bad_acc_bias = false; @@ -452,7 +422,11 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) #endif // CONFIG_EKF2_EXTERNAL_VISION if (bad_vz_gps || bad_vz_ev) { +#if defined(CONFIG_EKF2_BAROMETER) bool bad_z_baro = _control_status.flags.baro_hgt && (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.0f); +#else + bool bad_z_baro = false; +#endif bool bad_z_gps = _control_status.flags.gps_hgt && (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.0f); #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -486,7 +460,7 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) // the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue if (isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) { - P.uncorrelateCovariance<3>(13); + P.uncorrelateCovariance(State::accel_bias.idx); _time_acc_bias_check = _time_delayed_us; _fault_status.flags.bad_acc_bias = false; @@ -495,57 +469,53 @@ void Ekf::fixCovarianceErrors(bool force_symmetry) } else if (force_symmetry) { // ensure the covariance values are symmetrical - P.makeRowColSymmetric<3>(13); + P.makeRowColSymmetric(State::accel_bias.idx); } } // magnetic field states - if (!_control_status.flags.mag_3D) { - zeroMagCov(); + if (!_control_status.flags.mag) { + P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); + P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); } else { - // constrain variances - for (int i = 16; i <= 18; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[5]); - } + constrainStateVar(State::mag_I, 0.f, mag_I_var_max); + constrainStateVar(State::mag_B, 0.f, mag_B_var_max); - for (int i = 19; i <= 21; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[6]); - } - - // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<3>(16); - P.makeRowColSymmetric<3>(19); + P.makeRowColSymmetric(State::mag_I.idx); + P.makeRowColSymmetric(State::mag_B.idx); } - } // wind velocity states if (!_control_status.flags.wind) { - P.uncorrelateCovarianceSetVariance<2>(22, 0.0f); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, 0.0f); } else { - // constrain variances - for (int i = 22; i <= 23; i++) { - P(i, i) = math::constrain(P(i, i), 0.0f, P_lim[7]); - } + constrainStateVar(State::wind_vel, 0.f, wind_vel_var_max); - // force symmetry if (force_symmetry) { - P.makeRowColSymmetric<2>(22); + P.makeRowColSymmetric(State::wind_vel.idx); } } } +void Ekf::constrainStateVar(const IdxDof &state, float min, float max) +{ + for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { + P(i, i) = math::constrain(P(i, i), min, max); + } +} + // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) +bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP) { bool healthy = true; - for (int i = 0; i < _k_num_states; i++) { + for (int i = 0; i < State::size; i++) { if (P(i, i) < KHP(i, i)) { P.uncorrelateCovarianceSetVariance<1>(i, 0.0f); healthy = false; @@ -555,67 +525,50 @@ bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP) return healthy; } -void Ekf::resetMagRelatedCovariances() +void Ekf::resetQuatCov(const float yaw_noise) { - resetQuatCov(); - resetMagCov(); -} - -void Ekf::resetQuatCov() -{ - zeroQuatCov(); - - // define the initial angle uncertainty as variances for a rotation vector - Vector3f rot_vec_var; - rot_vec_var.setAll(sq(_params.initial_tilt_err)); - - initialiseQuatCovariances(rot_vec_var); + const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f)); + float yaw_var = sq(0.01f); // update the yaw angle variance using the variance of the measurement - if (_params.mag_fusion_type <= MagFuseType::MAG_3D) { + if (PX4_ISFINITE(yaw_noise)) { // using magnetic heading tuning parameter - increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); + yaw_var = math::max(sq(yaw_noise), yaw_var); } + + resetQuatCov(Vector3f(tilt_var, tilt_var, yaw_var)); } -void Ekf::zeroQuatCov() +void Ekf::resetQuatCov(const Vector3f &rot_var_ned) { - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); + matrix::SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(getStateAtFusionHorizonAsVector(), rot_var_ned, &q_cov); + q_cov.copyLowerToUpperTriangle(); + resetStateCovariance(q_cov); } void Ekf::resetMagCov() { - // reset the corresponding rows and columns in the covariance matrix and - // set the variances on the magnetic field states to the measurement variance - clearMagCov(); - - P.uncorrelateCovarianceSetVariance<3>(16, sq(_params.mag_noise)); - P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); - - if (!_control_status.flags.mag_3D) { - // save covariance data for re-use when auto-switching between heading and 3-axis fusion - // if already in 3-axis fusion mode, the covariances are automatically saved when switching out - // of this mode - saveMagCovData(); +#if defined(CONFIG_EKF2_MAGNETOMETER) + if (_mag_decl_cov_reset) { + ECL_INFO("reset mag covariance"); + _mag_decl_cov_reset = false; } + + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, sq(_params.mag_noise)); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, sq(_params.mag_noise)); + + saveMagCovData(); +#else + P.uncorrelateCovarianceSetVariance(State::mag_I.idx, 0.f); + P.uncorrelateCovarianceSetVariance(State::mag_B.idx, 0.f); + +#endif } -void Ekf::clearMagCov() +void Ekf::resetGyroBiasZCov() { - zeroMagCov(); - _mag_decl_cov_reset = false; -} + const float init_gyro_bias_var = sq(_params.switch_on_gyro_bias); -void Ekf::zeroMagCov() -{ - P.uncorrelateCovarianceSetVariance<3>(16, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(19, 0.0f); -} - -void Ekf::resetZDeltaAngBiasCov() -{ - const float init_delta_ang_bias_var = sq(_params.switch_on_gyro_bias * _dt_ekf_avg); - - P.uncorrelateCovarianceSetVariance<1>(12, init_delta_ang_bias_var); + P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, init_gyro_bias_var); } diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index ca15715b7c..a11a599be7 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -85,7 +85,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) _state.vel(2)); const Vector3f rel_wind_body = _state.quat_nominal.rotateVectorInverse(rel_wind_earth); const float rel_wind_speed = rel_wind_body.norm(); - const Vector24f state_vector_prev = getStateAtFusionHorizonAsVector(); + const VectorState state_vector_prev = getStateAtFusionHorizonAsVector(); Vector2f bcoef_inv; @@ -105,12 +105,12 @@ void Ekf::fuseDrag(const dragSample &drag_sample) bcoef_inv(1) = bcoef_inv(0); } - Vector24f Kfusion; + VectorState Kfusion; // perform sequential fusion of XY specific forces for (uint8_t axis_index = 0; axis_index < 2; axis_index++) { // measured drag acceleration corrected for sensor bias - const float mea_acc = drag_sample.accelXY(axis_index) - _state.delta_vel_bias(axis_index) / _dt_ekf_avg; + const float mea_acc = drag_sample.accelXY(axis_index) - _state.accel_bias(axis_index); // 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 diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f078a53690..bf96423bb4 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -45,9 +45,12 @@ bool Ekf::init(uint64_t timestamp) { - bool ret = initialise_interface(timestamp); - reset(); - return ret; + if (!_initialised) { + _initialised = initialise_interface(timestamp); + reset(); + } + + return _initialised; } void Ekf::reset() @@ -56,8 +59,8 @@ void Ekf::reset() _state.vel.setZero(); _state.pos.setZero(); - _state.delta_ang_bias.setZero(); - _state.delta_vel_bias.setZero(); + _state.gyro_bias.setZero(); + _state.accel_bias.setZero(); _state.mag_I.setZero(); _state.mag_B.setZero(); _state.wind_vel.setZero(); @@ -80,7 +83,8 @@ void Ekf::reset() _fault_status.value = 0; _innov_check_fail_status.value = 0; - _prev_dvel_bias_var.zero(); + _prev_gyro_bias_var.zero(); + _prev_accel_bias_var.zero(); resetGpsDriftCheckFilters(); @@ -105,14 +109,21 @@ void Ekf::reset() _gps_checks_passed = false; _gps_alt_ref = NAN; +#if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_MAGNETOMETER) _mag_counter = 0; +#endif // CONFIG_EKF2_MAGNETOMETER _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 @@ -138,8 +149,10 @@ void Ekf::reset() resetEstimatorAidStatus(_aid_src_gnss_yaw); #endif // CONFIG_EKF2_GNSS_YAW +#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); @@ -180,13 +193,12 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // run a separate filter for terrain estimation runTerrainEstimator(imu_sample_delayed); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, getGyroBias(), getAccelBias(), - _state.quat_nominal, _state.vel, _state.pos); + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); return true; } @@ -221,10 +233,10 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) // Initialise the terrain estimator initHagl(); -#endif // CONFIG_EKF2_RANGE_FINDER +#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); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index db21a6b3f1..2899fca28a 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -49,6 +49,7 @@ #include "bias_estimator.hpp" #include "height_bias_estimator.hpp" #include "position_bias_estimator.hpp" +#include "python/ekf_derivation/generated/state.h" #include #include @@ -59,14 +60,9 @@ enum class Likelihood { LOW, MEDIUM, HIGH }; class Ekf final : public EstimatorInterface { public: - static constexpr uint8_t _k_num_states{24}; ///< number of EKF states - - typedef matrix::Vector Vector24f; - typedef matrix::SquareMatrix SquareMatrix24f; + typedef matrix::Vector VectorState; + typedef matrix::SquareMatrix SquareMatrixState; typedef matrix::SquareMatrix Matrix2f; - template - - using SparseVector24f = matrix::SparseVectorf<24, Idxs...>; Ekf() { @@ -81,6 +77,8 @@ public: // should be called every time new data is pushed into the filter bool update(); + static uint8_t getNumberOfStates() { return State::size; } + void getGpsVelPosInnov(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovVar(float hvel[2], float &vvel, float hpos[2], float &vpos) const; void getGpsVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; @@ -91,27 +89,16 @@ public: void getEvVelPosInnovRatio(float &hvel, float &vvel, float &hpos, float &vpos) const; #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_BAROMETER) + const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } + const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } + void getBaroHgtInnov(float &baro_hgt_innov) const { baro_hgt_innov = _aid_src_baro_hgt.innovation; } void getBaroHgtInnovVar(float &baro_hgt_innov_var) const { baro_hgt_innov_var = _aid_src_baro_hgt.innovation_variance; } void getBaroHgtInnovRatio(float &baro_hgt_innov_ratio) const { baro_hgt_innov_ratio = _aid_src_baro_hgt.test_ratio; } +#endif // CONFIG_EKF2_BAROMETER -#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; } - - void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } - void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } - void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } - - void getHaglInnov(float &hagl_innov) const { hagl_innov = _hagl_innov; } - void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _hagl_innov_var; } - void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _hagl_test_ratio; } - - void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } - void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } - void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } - +#if defined(CONFIG_EKF2_TERRAIN) // terrain estimate bool isTerrainEstimateValid() const; @@ -125,13 +112,64 @@ public: // 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; } + + void getHaglInnov(float &hagl_innov) const { hagl_innov = _aid_src_terrain_range_finder.innovation; } + void getHaglInnovVar(float &hagl_innov_var) const { hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; } + void getHaglInnovRatio(float &hagl_innov_ratio) const { hagl_innov_ratio = _aid_src_terrain_range_finder.test_ratio; } +# 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; } + + void getTerrainFlowInnov(float flow_innov[2]) const + { + flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; + flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; + } + + void getTerrainFlowInnovVar(float flow_innov_var[2]) const + { + flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; + flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; + } + + void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#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; } + + void getRngHgtInnov(float &rng_hgt_innov) const { rng_hgt_innov = _aid_src_rng_hgt.innovation; } + void getRngHgtInnovVar(float &rng_hgt_innov_var) const { rng_hgt_innov_var = _aid_src_rng_hgt.innovation_variance; } + void getRngHgtInnovRatio(float &rng_hgt_innov_ratio) const { rng_hgt_innov_ratio = _aid_src_rng_hgt.test_ratio; } + + void getHaglRateInnov(float &hagl_rate_innov) const { hagl_rate_innov = _rng_consistency_check.getInnov(); } + void getHaglRateInnovVar(float &hagl_rate_innov_var) const { hagl_rate_innov_var = _rng_consistency_check.getInnovVar(); } + void getHaglRateInnovRatio(float &hagl_rate_innov_ratio) const { hagl_rate_innov_ratio = _rng_consistency_check.getSignedTestRatioLpf(); } #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; } - void getFlowInnov(float flow_innov[2]) const; - void getFlowInnovVar(float flow_innov_var[2]) const; + void getFlowInnov(float flow_innov[2]) const + { + flow_innov[0] = _aid_src_optical_flow.innovation[0]; + flow_innov[1] = _aid_src_optical_flow.innovation[1]; + } + + void getFlowInnovVar(float flow_innov_var[2]) const + { + flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; + flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; + } + void getFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_optical_flow.test_ratio[0], _aid_src_optical_flow.test_ratio[1]); } const Vector2f &getFlowVelBody() const { return _flow_vel_body; } @@ -142,12 +180,6 @@ public: const Vector3f getFlowGyro() const { return _flow_sample_delayed.gyro_xyz * (1.f / _flow_sample_delayed.dt); } const Vector3f &getFlowGyroIntegral() const { return _flow_sample_delayed.gyro_xyz; } - - void getTerrainFlowInnov(float flow_innov[2]) const; - void getTerrainFlowInnovVar(float flow_innov_var[2]) const; - void getTerrainFlowInnovRatio(float &flow_innov_ratio) const { flow_innov_ratio = math::max(_aid_src_terrain_optical_flow.test_ratio[0], _aid_src_terrain_optical_flow.test_ratio[1]); } - - const auto &aid_src_terrain_optical_flow() const { return _aid_src_terrain_optical_flow; } #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_AUXVEL) @@ -158,15 +190,16 @@ public: void getHeadingInnov(float &heading_innov) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov = _aid_src_mag_heading.innovation; return; - } - if (_control_status.flags.mag_3D) { + } else if (_control_status.flags.mag_3D) { heading_innov = Vector3f(_aid_src_mag.innovation).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -185,15 +218,16 @@ public: void getHeadingInnovVar(float &heading_innov_var) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov_var = _aid_src_mag_heading.innovation_variance; return; - } - if (_control_status.flags.mag_3D) { + } else if (_control_status.flags.mag_3D) { heading_innov_var = Vector3f(_aid_src_mag.innovation_variance).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -212,15 +246,16 @@ public: void getHeadingInnovRatio(float &heading_innov_ratio) const { +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { heading_innov_ratio = _aid_src_mag_heading.test_ratio; return; - } - if (_control_status.flags.mag_3D) { + } else if (_control_status.flags.mag_3D) { heading_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); return; } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -237,9 +272,11 @@ public: #endif // CONFIG_EKF2_EXTERNAL_VISION } +#if defined(CONFIG_EKF2_MAGNETOMETER) void getMagInnov(float mag_innov[3]) const { memcpy(mag_innov, _aid_src_mag.innovation, sizeof(_aid_src_mag.innovation)); } void getMagInnovVar(float mag_innov_var[3]) const { memcpy(mag_innov_var, _aid_src_mag.innovation_variance, sizeof(_aid_src_mag.innovation_variance)); } void getMagInnovRatio(float &mag_innov_ratio) const { mag_innov_ratio = Vector3f(_aid_src_mag.test_ratio).max(); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) void getDragInnov(float drag_innov[2]) const { _drag_innov.copyTo(drag_innov); } @@ -264,28 +301,28 @@ public: void getGravityInnovRatio(float &grav_innov_ratio) const { grav_innov_ratio = Vector3f(_aid_src_gravity.test_ratio).max(); } // get the state vector at the delayed time horizon - matrix::Vector getStateAtFusionHorizonAsVector() const; + matrix::Vector getStateAtFusionHorizonAsVector() const; // get the wind velocity in m/s const Vector2f &getWindVelocity() const { return _state.wind_vel; }; + Vector2f getWindVelocityVariance() const { return getStateVariance(); } - // get the wind velocity var - Vector2f getWindVelocityVariance() const { return P.slice<2, 2>(22, 22).diag(); } + template + matrix::VectorgetStateVariance() const { return P.slice(S.idx, S.idx).diag(); } // calling getStateCovariance().diag() uses more flash space + + template + matrix::SquareMatrixgetStateCovariance() const { return P.slice(S.idx, S.idx); } // get the full covariance matrix - const matrix::SquareMatrix &covariances() const { return P; } + const matrix::SquareMatrix &covariances() const { return P; } // get the diagonal elements of the covariance matrix - matrix::Vector covariances_diagonal() const { return P.diag(); } + matrix::Vector covariances_diagonal() const { return P.diag(); } - // get the orientation (quaterion) covariances - matrix::SquareMatrix orientation_covariances() const { return P.slice<4, 4>(0, 0); } + matrix::Vector getQuaternionVariance() const { return getStateVariance(); } + Vector3f getVelocityVariance() const { return getStateVariance(); }; + Vector3f getPositionVariance() const { return getStateVariance(); } - // get the linear velocity covariances - matrix::SquareMatrix velocity_covariances() const { return P.slice<3, 3>(4, 4); } - - // get the position covariances - matrix::SquareMatrix position_covariances() const { return P.slice<3, 3>(7, 7); } // ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined bool collect_gps(const gpsMessage &gps) override; @@ -316,10 +353,6 @@ public: void resetGyroBias(); void resetAccelBias(); - Vector3f getVelocityVariance() const { return P.slice<3, 3>(4, 4).diag(); }; - - Vector3f getPositionVariance() const { return P.slice<3, 3>(7, 7).diag(); } - // First argument returns GPS drift metrics in the following array locations // 0 : Horizontal position drift rate (m/s) // 1 : Vertical position drift rate (m/s) @@ -354,35 +387,42 @@ public: bool isYawFinalAlignComplete() const { +#if defined(CONFIG_EKF2_MAGNETOMETER) const bool is_using_mag = (_control_status.flags.mag_3D || _control_status.flags.mag_hdg); const bool is_mag_alignment_in_flight_complete = is_using_mag && _control_status.flags.mag_aligned_in_flight && ((_time_delayed_us - _flt_mag_align_start_time) > (uint64_t)1e6); return _control_status.flags.yaw_align && (is_mag_alignment_in_flight_complete || !is_using_mag); +#else + return _control_status.flags.yaw_align; +#endif } - // gyro bias (states 10, 11, 12) - Vector3f getGyroBias() const { return _state.delta_ang_bias / _dt_ekf_avg; } // get the gyroscope bias in rad/s - Vector3f getGyroBiasVariance() const { return Vector3f{P(10, 10), P(11, 11), P(12, 12)} / sq(_dt_ekf_avg); } // get the gyroscope bias variance in rad/s + // gyro bias + const Vector3f &getGyroBias() const { return _state.gyro_bias; } // get the gyroscope bias in rad/s + Vector3f getGyroBiasVariance() const { return getStateVariance(); } // get the gyroscope bias variance in rad/s float getGyroBiasLimit() const { return _params.gyro_bias_lim; } - // accel bias (states 13, 14, 15) - Vector3f getAccelBias() const { return _state.delta_vel_bias / _dt_ekf_avg; } // get the accelerometer bias in m/s**2 - Vector3f getAccelBiasVariance() const { return Vector3f{P(13, 13), P(14, 14), P(15, 15)} / sq(_dt_ekf_avg); } // get the accelerometer bias variance in m/s**2 + // accel bias + const Vector3f &getAccelBias() const { return _state.accel_bias; } // get the accelerometer bias in m/s**2 + Vector3f getAccelBiasVariance() const { return getStateVariance(); } // get the accelerometer bias variance in m/s**2 float getAccelBiasLimit() const { return _params.acc_bias_lim; } - // mag bias (states 19, 20, 21) +#if defined(CONFIG_EKF2_MAGNETOMETER) + const Vector3f &getMagEarthField() const { return _state.mag_I; } + const Vector3f &getMagBias() const { return _state.mag_B; } Vector3f getMagBiasVariance() const { - if (_control_status.flags.mag_3D) { - return Vector3f{P(19, 19), P(20, 20), P(21, 21)}; + if (_control_status.flags.mag) { + return getStateVariance(); } - return _saved_mag_bf_variance; + return _saved_mag_bf_covmat.diag(); } float getMagBiasLimit() const { return 0.5f; } // 0.5 Gauss +#endif // CONFIG_EKF2_MAGNETOMETER bool accel_bias_inhibited() const { return _accel_bias_inhibit[0] || _accel_bias_inhibit[1] || _accel_bias_inhibit[2]; } bool gyro_bias_inhibited() const { return _gyro_bias_inhibit[0] || _gyro_bias_inhibit[1] || _gyro_bias_inhibit[2]; } @@ -442,6 +482,7 @@ public: // rotate quaternion covariances into variances for an equivalent rotation vector Vector3f calcRotVecVariances() const; + float getYawVar() const; // set minimum continuous period without GPS fail required to mark a healthy GPS status void set_min_required_gps_health_time(uint32_t time_us) { _min_gps_health_time_us = time_us; } @@ -460,7 +501,7 @@ public: bool isYawEmergencyEstimateAvailable() const; uint8_t getHeightSensorRef() const { return _height_sensor_ref; } - const BiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); } + const BiasEstimator::status &getGpsHgtBiasEstimatorStatus() const { return _gps_hgt_b_est.getStatus(); } #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -477,8 +518,6 @@ public: const auto &aid_src_sideslip() const { return _aid_src_sideslip; } #endif // CONFIG_EKF2_SIDESLIP - const auto &aid_src_baro_hgt() const { return _aid_src_baro_hgt; } - const auto &aid_src_fake_hgt() const { return _aid_src_fake_hgt; } const auto &aid_src_fake_pos() const { return _aid_src_fake_pos; } @@ -497,8 +536,10 @@ public: const auto &aid_src_gnss_yaw() const { return _aid_src_gnss_yaw; } #endif // CONFIG_EKF2_GNSS_YAW +#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 const auto &aid_src_gravity() const { return _aid_src_gravity; } @@ -567,54 +608,48 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - // used by magnetometer fusion mode selection + // zero gyro update + Vector3f _zgup_delta_ang{}; + float _zgup_delta_ang_dt{0.f}; + Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) float _yaw_delta_ef{0.0f}; ///< Recent change in yaw angle measured about the earth frame D axis (rad) float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) - bool _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable - bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable - uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected - uint64_t _mag_use_not_inhibit_us{0}; ///< last system time in usec before magnetometer use was inhibited - float _last_static_yaw{NAN}; ///< last yaw angle recorded when on ground motion checks were passing (rad) - bool _mag_yaw_reset_req{false}; ///< true when a reset of the yaw using the magnetometer data has been requested - 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. - bool _synthetic_mag_z_active{false}; ///< true if we are generating synthetic magnetometer Z measurements - - SquareMatrix24f P{}; ///< state covariance matrix - - Vector3f _delta_angle_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta angle bias variance - Vector3f _delta_vel_bias_var_accum{}; ///< kahan summation algorithm accumulator for delta velocity bias variance + SquareMatrixState P{}; ///< state covariance matrix #if defined(CONFIG_EKF2_DRAG_FUSION) Vector2f _drag_innov{}; ///< multirotor drag measurement innovation (m/sec**2) Vector2f _drag_innov_var{}; ///< multirotor drag measurement innovation variance ((m/sec**2)**2) #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_RANGE_FINDER) - estimator_aid_source1d_s _aid_src_rng_hgt{}; - - HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; - - float _hagl_innov{0.0f}; ///< innovation of the last height above terrain measurement (m) - float _hagl_innov_var{0.0f}; ///< innovation variance for the last height above terrain measurement (m**2) - float _hagl_test_ratio{}; // height above terrain measurement innovation consistency check ratio - - uint64_t _time_last_healthy_rng_data{0}; - +#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 - uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator - terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground + 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}; #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_terrain_optical_flow{}; // optical flow processing Vector3f _flow_gyro_bias{}; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) @@ -628,10 +663,8 @@ private: Vector2f _flow_compensated_XY_rad{}; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), 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 - uint64_t _time_last_flow_terrain_fuse{0}; ///< time the last fusion of optical flow measurements for terrain estimation were performed (uSec) #endif // CONFIG_EKF2_OPTICAL_FLOW - estimator_aid_source1d_s _aid_src_baro_hgt{}; #if defined(CONFIG_EKF2_AIRSPEED) estimator_aid_source1d_s _aid_src_airspeed{}; #endif // CONFIG_EKF2_AIRSPEED @@ -654,7 +687,6 @@ private: uint8_t _nb_ev_vel_reset_available{0}; uint8_t _nb_ev_yaw_reset_available{0}; #endif // CONFIG_EKF2_EXTERNAL_VISION - bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) estimator_aid_source1d_s _aid_src_gnss_hgt{}; estimator_aid_source2d_s _aid_src_gnss_pos{}; @@ -665,9 +697,6 @@ private: uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source #endif // CONFIG_EKF2_GNSS_YAW - estimator_aid_source1d_s _aid_src_mag_heading{}; - estimator_aid_source3d_s _aid_src_mag{}; - estimator_aid_source3d_s _aid_src_gravity{}; #if defined(CONFIG_EKF2_AUXVEL) @@ -690,21 +719,49 @@ private: // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; - uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation - uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) 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{}; + // Variables used to perform in flight resets and switch between height sources - AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) + uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation + + HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; + + bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags +#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 _mag_bias_observable{false}; ///< true when there is enough rotation to make magnetometer bias errors observable + bool _yaw_angle_observable{false}; ///< true when there is enough horizontal acceleration to make yaw observable + uint64_t _time_yaw_started{0}; ///< last system time in usec that a yaw rotation manoeuvre was detected + 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) + uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation // Variables used to control activation of post takeoff functionality uint64_t _flt_mag_align_start_time{0}; ///< time that inflight magnetic field alignment started (uSec) uint64_t _time_last_mov_3d_mag_suitable{0}; ///< last system time that sufficient movement to use 3-axis magnetometer fusion was detected (uSec) - Vector3f _saved_mag_bf_variance {}; ///< magnetic field state variances that have been saved for use at the next initialisation (Gauss**2) - Matrix2f _saved_mag_ef_ne_covmat{}; ///< NE magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) - float _saved_mag_ef_d_variance{}; ///< D magnetic field state variance saved for use at the next initialisation (Gauss**2) + uint64_t _time_last_mag_check_failing{0}; + Matrix3f _saved_mag_ef_covmat{}; ///< NED magnetic field state covariance sub-matrix saved for use at the next initialisation (Gauss**2) + Matrix3f _saved_mag_bf_covmat{}; ///< magnetic field state covariance sub-matrix that has been saved for use at the next initialisation (Gauss**2) +#endif // CONFIG_EKF2_MAGNETOMETER gps_check_fail_status_u _gps_check_fail_status{}; @@ -714,11 +771,11 @@ private: Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2) float _accel_magnitude_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec) float _ang_rate_magnitude_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec) - Vector3f _prev_delta_ang_bias_var{}; ///< saved delta angle XYZ bias variances (rad/sec) - Vector3f _prev_dvel_bias_var{}; ///< saved delta velocity XYZ bias variances (m/sec)**2 + + Vector3f _prev_gyro_bias_var{}; ///< saved gyro XYZ bias variances + Vector3f _prev_accel_bias_var{}; ///< saved accel XYZ bias variances // height sensor status - bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent // imu fault status @@ -740,13 +797,16 @@ private: // predict ekf covariance void predictCovariance(const imuSample &imu_delayed); - // ekf sequential fusion of magnetometer measurements - bool fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bool update_all_states = true); + template + void resetStateCovariance(const matrix::SquareMatrix &cov) { + P.uncorrelateCovarianceSetVariance(S.idx, 0.0f); + P.slice(S.idx, S.idx) = cov; + } // update quaternion states and covariances using an innovation, observation variance and Jacobian vector - bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status); - bool fuseYaw(float innovation, float variance, estimator_aid_source1d_s &aid_src_status, const Vector24f &H_YAW); - void computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const; + 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; #if defined(CONFIG_EKF2_GNSS_YAW) void controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing); @@ -760,17 +820,20 @@ private: void updateGpsYaw(const gpsSample &gps_sample); - void startGpsYawFusion(const gpsSample &gps_sample); - #endif // CONFIG_EKF2_GNSS_YAW void stopGpsYawFusion(); +#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); + // 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) // control fusion of air data observations @@ -806,7 +869,7 @@ private: #endif // CONFIG_EKF2_DRAG_FUSION // fuse single velocity and position measurement - bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index); + bool fuseVelPosHeight(const float innov, const float innov_var, const int state_index); void resetVelocityTo(const Vector3f &vel, const Vector3f &new_vel_var); @@ -843,12 +906,7 @@ private: void fuseVelocity(estimator_aid_source2d_s &vel_aid_src); void fuseVelocity(estimator_aid_source3d_s &vel_aid_src); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // range height - void controlRangeHeightFusion(); - bool isConditionalRangeAidSuitable(); - void stopRngHgtFusion(); - +#if defined(CONFIG_EKF2_TERRAIN) // terrain vertical position estimator void initHagl(); void runTerrainEstimator(const imuSample &imu_delayed); @@ -856,16 +914,34 @@ private: float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } + void controlHaglFakeFusion(); + void terrainHandleVerticalPositionReset(float delta_z); + +# 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 fuseHaglRng(); - void startHaglRngFusion(); - void resetHaglRngIfNeeded(); + void updateHaglRng(estimator_aid_source1d_s &aid_src) const; + void fuseHaglRng(estimator_aid_source1d_s &aid_src); void resetHaglRng(); void stopHaglRngFusion(); - float getRngVar(); + float getRngVar() const; +# endif // CONFIG_EKF2_RANGE_FINDER - void controlHaglFakeFusion(); +# 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); +# endif // CONFIG_EKF2_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN + +#if defined(CONFIG_EKF2_RANGE_FINDER) + // range height + void controlRangeHeightFusion(); + bool isConditionalRangeAidSuitable(); + void stopRngHgtFusion(); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -888,68 +964,55 @@ private: void fuseOptFlow(); float predictFlowRange(); Vector2f predictFlowVelBody(); - - // update the terrain vertical position estimate using an optical flow measurement - void controlHaglFlowFusion(); - void startHaglFlowFusion(); - void resetHaglFlow(); - void stopHaglFlowFusion(); - void fuseFlowForTerrain(estimator_aid_source2d_s &flow); #endif // CONFIG_EKF2_OPTICAL_FLOW - // reset the heading and magnetic field states using the declination and magnetometer measurements - // return true if successful - bool resetMagHeading(); - +#if defined(CONFIG_EKF2_MAGNETOMETER) // Return the magnetic declination in radians to be used by the alignment and fusion processing float getMagDeclination(); +#endif // CONFIG_EKF2_MAGNETOMETER - void clearInhibitedStateKalmanGains(Vector24f &K) const + void clearInhibitedStateKalmanGains(VectorState &K) const { - // gyro bias: states 10, 11, 12 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { - K(10 + i) = 0.f; + K(State::gyro_bias.idx + i) = 0.f; } } - // accel bias: states 13, 14, 15 - for (unsigned i = 0; i < 3; i++) { + for (unsigned i = 0; i < State::accel_bias.dof; i++) { if (_accel_bias_inhibit[i]) { - K(13 + i) = 0.f; + K(State::accel_bias.idx + i) = 0.f; } } - // mag I: states 16, 17, 18 - if (!_control_status.flags.mag_3D) { - K(16) = 0.f; - K(17) = 0.f; - K(18) = 0.f; + if (!_control_status.flags.mag) { + for (unsigned i = 0; i < State::mag_I.dof; i++) { + K(State::mag_I.idx + i) = 0.f; + } } - // mag B: states 19, 20, 21 - if (!_control_status.flags.mag_3D) { - K(19) = 0.f; - K(20) = 0.f; - K(21) = 0.f; + if (!_control_status.flags.mag) { + for (unsigned i = 0; i < State::mag_B.dof; i++) { + K(State::mag_B.idx + i) = 0.f; + } } - // wind: states 22, 23 if (!_control_status.flags.wind) { - K(22) = 0.f; - K(23) = 0.f; + for (unsigned i = 0; i < State::wind_vel.dof; i++) { + K(State::wind_vel.idx + i) = 0.f; + } } } - bool measurementUpdate(Vector24f &K, float innovation_variance, float innovation) + bool measurementUpdate(VectorState &K, float innovation_variance, float innovation) { clearInhibitedStateKalmanGains(K); - const Vector24f KS = K * innovation_variance; - SquareMatrix24f KHP; + const VectorState KS = K * innovation_variance; + SquareMatrixState KHP; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned col = 0; col < _k_num_states; col++) { + for (unsigned row = 0; row < State::size; row++) { + for (unsigned col = 0; col < State::size; col++) { // Instad of literally computing KHP, use an equvalent // equation involving less mathematical operations KHP(row, col) = KS(row) * K(col); @@ -973,20 +1036,24 @@ private: // if the covariance correction will result in a negative variance, then // the covariance matrix is unhealthy and must be corrected - bool checkAndFixCovarianceUpdate(const SquareMatrix24f &KHP); + bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); // limit the diagonal of the covariance matrix // force symmetry when the argument is true void fixCovarianceErrors(bool force_symmetry); + void constrainStateVar(const IdxDof &state, float min, float max); + // constrain the ekf states void constrainStates(); // generic function which will perform a fusion step given a kalman gain K // and a scalar innovation value - void fuse(const Vector24f &K, float innovation); + 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; @@ -1000,6 +1067,7 @@ private: #if defined(CONFIG_EKF2_EXTERNAL_VISION) // control fusion of external vision observations void controlExternalVisionFusion(); + 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); @@ -1018,24 +1086,37 @@ private: bool shouldResetGpsFusion() const; bool isYawFailure() const; +#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); - bool magReset(); + bool checkHaglYawResetReq() const; + + void resetMagHeading(const Vector3f &mag); + void resetMagStates(const Vector3f &mag, bool reset_heading = true); bool haglYawResetReq(); - void selectMagAuto(); - void check3DMagFusionSuitability(); void checkYawAngleObservability(); void checkMagBiasObservability(); - bool canUse3DMagFusion() const; + void checkMagHeadingConsistency(); - void checkMagDeclRequired(); - bool shouldInhibitMag() const; - bool magFieldStrengthDisturbed(const Vector3f &mag) const; + bool checkMagField(const Vector3f &mag); static bool isMeasuredMatchingExpected(float measured, float expected, float gate); - void runMagAndMagDeclFusions(const Vector3f &mag); - void run3DMagAndDeclFusions(const Vector3f &mag); + + void stopMagHdgFusion(); + void stopMagFusion(); + + // load and save mag field state covariance data for re-use + void loadMagCovData(); + void saveMagCovData(); + + // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter + // sensor measurement + float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); + +#endif // CONFIG_EKF2_MAGNETOMETER // control fusion of fake position observations to constrain drift void controlFakePosFusion(); @@ -1046,6 +1127,8 @@ private: void stopFakeHgtFusion(); void controlZeroVelocityUpdate(); + void controlZeroGyroUpdate(const imuSample &imu_delayed); + void fuseDeltaAngBias(float innov, float innov_var, int obs_index); void controlZeroInnovationHeadingUpdate(); @@ -1061,57 +1144,34 @@ private: // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(const imuSample &imu_delayed); void checkHeightSensorRefFallback(); - void controlBaroHeightFusion(); void controlGnssHeightFusion(const gpsSample &gps_sample); - void stopMagFusion(); - void stopMag3DFusion(); - void stopMagHdgFusion(); - void startMagHdgFusion(); - void startMag3DFusion(); - +#if defined(CONFIG_EKF2_BAROMETER) + void controlBaroHeightFusion(); void stopBaroHgtFusion(); - void stopGpsHgtFusion(); void updateGroundEffect(); +#endif // CONFIG_EKF2_BAROMETER + + void stopGpsHgtFusion(); // gravity fusion: heuristically enable / disable gravity fusion void controlGravityFusion(const imuSample &imu_delayed); - // initialise the quaternion covariances using rotation vector variances - // do not call before quaternion states are initialised - void initialiseQuatCovariances(Vector3f &rot_vec_var); + void resetQuatCov(const float yaw_noise = NAN); + void resetQuatCov(const Vector3f &euler_noise_ned); - // perform a limited reset of the magnetic field related state covariances - void resetMagRelatedCovariances(); - - void resetQuatCov(); - void zeroQuatCov(); void resetMagCov(); // perform a reset of the wind states and related covariances void resetWind(); void resetWindToZero(); - // Increase the yaw error variance of the quaternions - // Argument is additional yaw variance in rad**2 - void increaseQuatYawErrVariance(float yaw_variance); - - // load and save mag field state covariance data for re-use - void loadMagCovData(); - void saveMagCovData(); - void clearMagCov(); - void zeroMagCov(); - - void resetZDeltaAngBiasCov(); + void resetGyroBiasZCov(); // uncorrelate quaternion states from other states void uncorrelateQuatFromOtherStates(); - // calculate a synthetic value for the magnetometer Z component, given the 3D magnetomter - // sensor measurement - float calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted); - 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); @@ -1128,13 +1188,11 @@ private: } void stopGpsFusion(); - void stopGpsPosFusion(); - void stopGpsVelFusion(); void resetFakePosFusion(); void stopFakePosFusion(); - void setVelPosStatus(const int index, const bool healthy); + void setVelPosStatus(const int state_index, const bool healthy); // reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch // yaw : Euler yaw angle (rad) @@ -1149,12 +1207,13 @@ private: uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; uint8_t _position_sensor_ref{static_cast(PositionSensor::GNSS)}; - HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; #if defined(CONFIG_EKF2_EXTERNAL_VISION) HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; PositionBiasEstimator _ev_pos_b_est{static_cast(PositionSensor::EV), _position_sensor_ref}; + AlphaFilter _ev_q_error_filt{0.001f}; + bool _ev_q_error_initialized{false}; #endif // CONFIG_EKF2_EXTERNAL_VISION // Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator @@ -1179,7 +1238,6 @@ private: status.innovation_variance = 0; status.test_ratio = INFINITY; - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } @@ -1203,7 +1261,6 @@ private: status.test_ratio[i] = INFINITY; } - status.fusion_enabled = false; status.innovation_rejected = true; status.fused = false; } diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9e89016abb..fa27b5620c 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -40,8 +40,11 @@ */ #include "ekf.h" +#include "python/ekf_derivation/generated/quat_var_to_rot_var.h" +#include "python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" #include +#include #include void Ekf::resetHorizontalVelocityToZero() @@ -64,11 +67,11 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f _state.vel.xy() = new_horz_vel; if (PX4_ISFINITE(new_horz_vel_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(4, math::max(sq(0.01f), new_horz_vel_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx, math::max(sq(0.01f), new_horz_vel_var(0))); } if (PX4_ISFINITE(new_horz_vel_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(5, math::max(sq(0.01f), new_horz_vel_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1))); } _output_predictor.resetHorizontalVelocityTo(delta_horz_vel); @@ -94,7 +97,7 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var) _state.vel(2) = new_vert_vel; if (PX4_ISFINITE(new_vert_vel_var)) { - P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var)); + P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var)); } _output_predictor.resetVerticalVelocityTo(delta_vert_vel); @@ -130,11 +133,11 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f _state.pos.xy() = new_horz_pos; if (PX4_ISFINITE(new_horz_pos_var(0))) { - P.uncorrelateCovarianceSetVariance<1>(7, math::max(sq(0.01f), new_horz_pos_var(0))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); } if (PX4_ISFINITE(new_horz_pos_var(1))) { - P.uncorrelateCovarianceSetVariance<1>(8, math::max(sq(0.01f), new_horz_pos_var(1))); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } _output_predictor.resetHorizontalPositionTo(delta_horz_pos); @@ -177,7 +180,7 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation - P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var)); + P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } const float delta_z = new_vert_pos - old_vert_pos; @@ -197,7 +200,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _state_reset_status.reset_count.posD++; +#if defined(CONFIG_EKF2_BAROMETER) _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - delta_z); #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -206,6 +211,10 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v _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 + // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; } @@ -224,20 +233,23 @@ void Ekf::constrainStates() _state.vel = matrix::constrain(_state.vel, -1000.0f, 1000.0f); _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); - const float delta_ang_bias_limit = getGyroBiasLimit() * _dt_ekf_avg; - _state.delta_ang_bias = matrix::constrain(_state.delta_ang_bias, -delta_ang_bias_limit, delta_ang_bias_limit); + const float gyro_bias_limit = getGyroBiasLimit(); + _state.gyro_bias = matrix::constrain(_state.gyro_bias, -gyro_bias_limit, gyro_bias_limit); - const float delta_vel_bias_limit = getAccelBiasLimit() * _dt_ekf_avg; - _state.delta_vel_bias = matrix::constrain(_state.delta_vel_bias, -delta_vel_bias_limit, delta_vel_bias_limit); + const float accel_bias_limit = getAccelBiasLimit(); + _state.accel_bias = matrix::constrain(_state.accel_bias, -accel_bias_limit, accel_bias_limit); _state.mag_I = matrix::constrain(_state.mag_I, -1.0f, 1.0f); +#if defined(CONFIG_EKF2_MAGNETOMETER) _state.mag_B = matrix::constrain(_state.mag_B, -getMagBiasLimit(), getMagBiasLimit()); +#endif // CONFIG_EKF2_MAGNETOMETER + _state.wind_vel = matrix::constrain(_state.wind_vel, -100.0f, 100.0f); } +#if defined(CONFIG_EKF2_BARO_COMPENSATION) float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const { -#if defined(CONFIG_EKF2_BARO_COMPENSATION) 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 @@ -265,11 +277,11 @@ float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) // correct baro measurement using pressure error estimate and assuming sea level gravity return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G); } -#endif // CONFIG_EKF2_BARO_COMPENSATION // 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 @@ -357,44 +369,18 @@ void Ekf::getAuxVelInnovVar(float aux_vel_innov_var[2]) const } #endif // CONFIG_EKF2_AUXVEL -#if defined(CONFIG_EKF2_OPTICAL_FLOW) -void Ekf::getFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_optical_flow.innovation[1]; -} - -void Ekf::getFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_optical_flow.innovation_variance[1]; -} - -void Ekf::getTerrainFlowInnov(float flow_innov[2]) const -{ - flow_innov[0] = _aid_src_terrain_optical_flow.innovation[0]; - flow_innov[1] = _aid_src_terrain_optical_flow.innovation[1]; -} - -void Ekf::getTerrainFlowInnovVar(float flow_innov_var[2]) const -{ - flow_innov_var[0] = _aid_src_terrain_optical_flow.innovation_variance[0]; - flow_innov_var[1] = _aid_src_terrain_optical_flow.innovation_variance[1]; -} -#endif // CONFIG_EKF2_OPTICAL_FLOW - // get the state vector at the delayed time horizon matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const { matrix::Vector state; - state.slice<4, 1>(0, 0) = _state.quat_nominal; - state.slice<3, 1>(4, 0) = _state.vel; - state.slice<3, 1>(7, 0) = _state.pos; - state.slice<3, 1>(10, 0) = _state.delta_ang_bias; - state.slice<3, 1>(13, 0) = _state.delta_vel_bias; - state.slice<3, 1>(16, 0) = _state.mag_I; - state.slice<3, 1>(19, 0) = _state.mag_B; - state.slice<2, 1>(22, 0) = _state.wind_vel; + state.slice(State::quat_nominal.idx, 0) = _state.quat_nominal; + state.slice(State::vel.idx, 0) = _state.vel; + state.slice(State::pos.idx, 0) = _state.pos; + state.slice(State::gyro_bias.idx, 0) = _state.gyro_bias; + state.slice(State::accel_bias.idx, 0) = _state.accel_bias; + state.slice(State::mag_I.idx, 0) = _state.mag_I; + state.slice(State::mag_B.idx, 0) = _state.mag_B; + state.slice(State::wind_vel.idx, 0) = _state.wind_vel; return state; } @@ -430,6 +416,24 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons _pos_ref.initReference(latitude, longitude, _time_delayed_us); _gps_alt_ref = altitude; + 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; + } + + // We don't know the uncertainty of the origin + _gpos_origin_eph = 0.f; + _gpos_origin_epv = 0.f; + + _NED_origin_initialised = true; + // minimum change in position or height that triggers a reset static constexpr float MIN_RESET_DIST_M = 0.01f; @@ -466,7 +470,7 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const // report absolute accuracy taking into account the uncertainty in location of the origin // If not aiding, return 0 for horizontal position estimate as no estimate is available // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8) + sq(_gps_origin_eph)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx) + sq(_gpos_origin_eph)); // If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -484,14 +488,14 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9) + sq(_gps_origin_epv)); + *ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2) + sq(_gpos_origin_epv)); } // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const { // TODO - allow for baro drift in vertical position error - float hpos_err = sqrtf(P(7, 7) + P(8, 8)); + float hpos_err = sqrtf(P.trace<2>(State::pos.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal position error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -509,13 +513,13 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const } *ekf_eph = hpos_err; - *ekf_epv = sqrtf(P(9, 9)); + *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(4, 4) + P(5, 5)); + float hvel_err = sqrtf(P.trace<2>(State::vel.idx)); // If we are dead-reckoning for too long, use the innovations as a conservative alternate measure of the horizontal velocity error // The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors @@ -548,7 +552,7 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const } *ekf_evh = hvel_err; - *ekf_evv = sqrtf(P(6, 6)); + *ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2)); } /* @@ -581,9 +585,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } -#endif // CONFIG_EKF2_RANGE_FINDER -#if defined(CONFIG_EKF2_OPTICAL_FLOW) +# if defined(CONFIG_EKF2_OPTICAL_FLOW) // Keep within flow AND range sensor limits when exclusively using optical flow const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); @@ -601,7 +604,9 @@ 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_OPTICAL_FLOW + +#endif // CONFIG_EKF2_RANGE_FINDER } void Ekf::resetImuBias() @@ -612,25 +617,28 @@ void Ekf::resetImuBias() void Ekf::resetGyroBias() { - // Zero the delta angle and delta velocity bias states - _state.delta_ang_bias.zero(); + // Zero the gyro bias states + _state.gyro_bias.zero(); // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(10, sq(_params.switch_on_gyro_bias * _dt_ekf_avg)); + P.uncorrelateCovarianceSetVariance(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias)); + + // Set previous frame values + _prev_gyro_bias_var = getStateVariance(); } void Ekf::resetAccelBias() { - // Zero the delta angle and delta velocity bias states - _state.delta_vel_bias.zero(); + // Zero the accel bias states + _state.accel_bias.zero(); // Zero the corresponding covariances and set // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(13, sq(_params.switch_on_accel_bias * _dt_ekf_avg)); + P.uncorrelateCovarianceSetVariance(State::accel_bias.idx, sq(_params.switch_on_accel_bias)); // Set previous frame values - _prev_dvel_bias_var = P.slice<3, 3>(13, 13).diag(); + _prev_accel_bias_var = getStateVariance(); } // get EKF innovation consistency check status information comprising of: @@ -647,6 +655,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f // return the largest magnetometer innovation test ratio mag = 0.f; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { mag = math::max(mag, sqrtf(_aid_src_mag_heading.test_ratio)); } @@ -654,6 +663,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f if (_control_status.flags.mag_3D) { mag = math::max(mag, sqrtf(Vector3f(_aid_src_mag.test_ratio).max())); } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_GNSS_YAW) if (_control_status.flags.gps_yaw) { @@ -702,10 +712,12 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f 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); n_hgt_sources++; } +#endif // CONFIG_EKF2_BAROMETER if (_control_status.flags.gps_hgt) { hgt_sum += sqrtf(_aid_src_gnss_hgt.test_ratio); @@ -738,11 +750,23 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f tas = sqrtf(_aid_src_airspeed.test_ratio); #endif // CONFIG_EKF2_AIRSPEED -#if defined(CONFIG_EKF2_RANGE_FINDER) - // return the terrain height innovation test ratio - hagl = sqrtf(_hagl_test_ratio); + 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); + } #endif // CONFIG_EKF2_RANGE_FINDER +# 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])); + } +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + #if defined(CONFIG_EKF2_SIDESLIP) // return the synthetic sideslip innovation test ratio beta = sqrtf(_aid_src_sideslip.test_ratio); @@ -760,15 +784,16 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const 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; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); -#endif // CONFIG_EKF2_RANGE_FINDER +#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; +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_control_status.flags.mag_hdg) { if (_aid_src_mag_heading.test_ratio < 1.f) { mag_innov_good = false; @@ -779,6 +804,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const mag_innov_good = false; } } +#endif // CONFIG_EKF2_MAGNETOMETER 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; @@ -788,25 +814,24 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const *status = soln_status.value; } -void Ekf::fuse(const Vector24f &K, float innovation) +void Ekf::fuse(const VectorState &K, float innovation) { - _state.quat_nominal -= K.slice<4, 1>(0, 0) * innovation; + _state.quat_nominal -= K.slice(State::quat_nominal.idx, 0) * innovation; _state.quat_nominal.normalize(); _R_to_earth = Dcmf(_state.quat_nominal); - _state.vel -= K.slice<3, 1>(4, 0) * innovation; - _state.pos -= K.slice<3, 1>(7, 0) * innovation; - _state.delta_ang_bias -= K.slice<3, 1>(10, 0) * innovation; - _state.delta_vel_bias -= K.slice<3, 1>(13, 0) * innovation; - _state.mag_I -= K.slice<3, 1>(16, 0) * innovation; - _state.mag_B -= K.slice<3, 1>(19, 0) * innovation; - _state.wind_vel -= K.slice<2, 1>(22, 0) * innovation; + _state.vel -= K.slice(State::vel.idx, 0) * innovation; + _state.pos -= K.slice(State::pos.idx, 0) * innovation; + _state.gyro_bias -= K.slice(State::gyro_bias.idx, 0) * innovation; + _state.accel_bias -= K.slice(State::accel_bias.idx, 0) * innovation; + _state.mag_I -= K.slice(State::mag_I.idx, 0) * innovation; + _state.mag_B -= K.slice(State::mag_B.idx, 0) * innovation; + _state.wind_vel -= K.slice(State::wind_vel.idx, 0) * innovation; } void Ekf::uncorrelateQuatFromOtherStates() { - P.slice<_k_num_states - 4, 4>(4, 0) = 0.f; - P.slice<4, _k_num_states - 4>(0, 4) = 0.f; + P.uncorrelateCovarianceBlock(State::quat_nominal.idx); } void Ekf::updateDeadReckoningStatus() @@ -881,159 +906,32 @@ void Ekf::updateVerticalDeadReckoningStatus() // calculate the variances for the rotation vector equivalent Vector3f Ekf::calcRotVecVariances() const { - Vector3f rot_var_vec; - float q0, q1, q2, q3; - - if (_state.quat_nominal(0) >= 0.0f) { - q0 = _state.quat_nominal(0); - q1 = _state.quat_nominal(1); - q2 = _state.quat_nominal(2); - q3 = _state.quat_nominal(3); - - } else { - q0 = -_state.quat_nominal(0); - q1 = -_state.quat_nominal(1); - q2 = -_state.quat_nominal(2); - q3 = -_state.quat_nominal(3); - } - float t2 = q0*q0; - float t3 = acosf(q0); - float t4 = -t2+1.0f; - float t5 = t2-1.0f; - if ((t4 > 1e-9f) && (t5 < -1e-9f)) { - float t6 = 1.0f/t5; - float t7 = q1*t6*2.0f; - float t8 = 1.0f/powf(t4,1.5f); - float t9 = q0*q1*t3*t8*2.0f; - float t10 = t7+t9; - float t11 = 1.0f/sqrtf(t4); - float t12 = q2*t6*2.0f; - float t13 = q0*q2*t3*t8*2.0f; - float t14 = t12+t13; - float t15 = q3*t6*2.0f; - float t16 = q0*q3*t3*t8*2.0f; - float t17 = t15+t16; - rot_var_vec(0) = t10*(P(0,0)*t10+P(1,0)*t3*t11*2.0f)+t3*t11*(P(0,1)*t10+P(1,1)*t3*t11*2.0f)*2.0f; - rot_var_vec(1) = t14*(P(0,0)*t14+P(2,0)*t3*t11*2.0f)+t3*t11*(P(0,2)*t14+P(2,2)*t3*t11*2.0f)*2.0f; - rot_var_vec(2) = t17*(P(0,0)*t17+P(3,0)*t3*t11*2.0f)+t3*t11*(P(0,3)*t17+P(3,3)*t3*t11*2.0f)*2.0f; - } else { - rot_var_vec = 4.0f * P.slice<3,3>(1,1).diag(); - } - - return rot_var_vec; + Vector3f rot_var; + sym::QuatVarToRotVar(getStateAtFusionHorizonAsVector(), P, FLT_EPSILON, &rot_var); + return rot_var; } -// initialise the quaternion covariances using rotation vector variances -// do not call before quaternion states are initialised -void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) +float Ekf::getYawVar() const { - // calculate an equivalent rotation vector from the quaternion - float q0,q1,q2,q3; - if (_state.quat_nominal(0) >= 0.0f) { - q0 = _state.quat_nominal(0); - q1 = _state.quat_nominal(1); - q2 = _state.quat_nominal(2); - q3 = _state.quat_nominal(3); + VectorState H_YAW; + float yaw_var = 0.f; + computeYawInnovVarAndH(0.f, yaw_var, H_YAW); - } else { - q0 = -_state.quat_nominal(0); - q1 = -_state.quat_nominal(1); - q2 = -_state.quat_nominal(2); - q3 = -_state.quat_nominal(3); - } - float delta = 2.0f*acosf(q0); - float scaler = (delta/sinf(delta*0.5f)); - float rotX = scaler*q1; - float rotY = scaler*q2; - float rotZ = scaler*q3; - - // autocode generated using matlab symbolic toolbox - float t2 = rotX*rotX; - float t4 = rotY*rotY; - float t5 = rotZ*rotZ; - float t6 = t2+t4+t5; - if (t6 > 1e-9f) { - float t7 = sqrtf(t6); - float t8 = t7*0.5f; - float t3 = sinf(t8); - float t9 = t3*t3; - float t10 = 1.0f/t6; - float t11 = 1.0f/sqrtf(t6); - float t12 = cosf(t8); - float t13 = 1.0f/powf(t6,1.5f); - float t14 = t3*t11; - float t15 = rotX*rotY*t3*t13; - float t16 = rotX*rotZ*t3*t13; - float t17 = rotY*rotZ*t3*t13; - float t18 = t2*t10*t12*0.5f; - float t27 = t2*t3*t13; - float t19 = t14+t18-t27; - float t23 = rotX*rotY*t10*t12*0.5f; - float t28 = t15-t23; - float t20 = rotY*rot_vec_var(1)*t3*t11*t28*0.5f; - float t25 = rotX*rotZ*t10*t12*0.5f; - float t31 = t16-t25; - float t21 = rotZ*rot_vec_var(2)*t3*t11*t31*0.5f; - float t22 = t20+t21-rotX*rot_vec_var(0)*t3*t11*t19*0.5f; - float t24 = t15-t23; - float t26 = t16-t25; - float t29 = t4*t10*t12*0.5f; - float t34 = t3*t4*t13; - float t30 = t14+t29-t34; - float t32 = t5*t10*t12*0.5f; - float t40 = t3*t5*t13; - float t33 = t14+t32-t40; - float t36 = rotY*rotZ*t10*t12*0.5f; - float t39 = t17-t36; - float t35 = rotZ*rot_vec_var(2)*t3*t11*t39*0.5f; - float t37 = t15-t23; - float t38 = t17-t36; - float t41 = rot_vec_var(0)*(t15-t23)*(t16-t25); - float t42 = t41-rot_vec_var(1)*t30*t39-rot_vec_var(2)*t33*t39; - float t43 = t16-t25; - float t44 = t17-t36; - - // zero all the quaternion covariances - P.uncorrelateCovarianceSetVariance<2>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<2>(2, 0.0f); - - - // Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox - P(0,0) = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f; - P(0,1) = t22; - P(0,2) = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f; - P(0,3) = rotX*rot_vec_var(0)*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rot_vec_var(2)*t3*t11*t33*0.5f; - P(1,0) = t22; - P(1,1) = rot_vec_var(0)*(t19*t19)+rot_vec_var(1)*(t24*t24)+rot_vec_var(2)*(t26*t26); - P(1,2) = rot_vec_var(2)*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(1,3) = rot_vec_var(1)*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(2,0) = t35-rotY*rot_vec_var(1)*t3*t11*t30*0.5f+rotX*rot_vec_var(0)*t3*t11*(t15-t23)*0.5f; - P(2,1) = rot_vec_var(2)*(t16-t25)*(t17-t36)-rot_vec_var(0)*t19*t28-rot_vec_var(1)*t28*t30; - P(2,2) = rot_vec_var(1)*(t30*t30)+rot_vec_var(0)*(t37*t37)+rot_vec_var(2)*(t38*t38); - P(2,3) = t42; - P(3,0) = rotZ*rot_vec_var(2)*t3*t11*t33*(-0.5f)+rotX*rot_vec_var(0)*t3*t11*(t16-t25)*0.5f+rotY*rot_vec_var(1)*t3*t11*(t17-t36)*0.5f; - P(3,1) = rot_vec_var(1)*(t15-t23)*(t17-t36)-rot_vec_var(0)*t19*t31-rot_vec_var(2)*t31*t33; - P(3,2) = t42; - P(3,3) = rot_vec_var(2)*(t33*t33)+rot_vec_var(0)*(t43*t43)+rot_vec_var(1)*(t44*t44); - - } else { - // the equations are badly conditioned so use a small angle approximation - P.uncorrelateCovarianceSetVariance<1>(0, 0.0f); - P.uncorrelateCovarianceSetVariance<3>(1, 0.25f * rot_vec_var); - } + return yaw_var; } +#if defined(CONFIG_EKF2_BAROMETER) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); } else -#endif // CONFIG_EKF2_RANGE_FINDER +#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)) { @@ -1045,83 +943,22 @@ void Ekf::updateGroundEffect() _control_status.flags.gnd_effect = false; } } - -// Increase the yaw error variance of the quaternions -// Argument is additional yaw variance in rad**2 -void Ekf::increaseQuatYawErrVariance(float yaw_variance) -{ - // See DeriveYawResetEquations.m for derivation which produces code fragments in C_code4.txt file - // The auto-code was cleaned up and had terms multiplied by zero removed to give the following: - - // Intermediate variables - float SG[3]; - SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3)); - SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3); - SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3); - - float SQ[4]; - SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1])); - SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2])); - SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2])); - SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1])); - - // Limit yaw variance increase to prevent a badly conditioned covariance matrix - yaw_variance = fminf(yaw_variance, 1.0e-2f); - - // Add covariances for additonal yaw uncertainty to existing covariances. - // This assumes that the additional yaw error is uncorrrelated to existing errors - P(0,0) += yaw_variance*sq(SQ[2]); - P(0,1) += yaw_variance*SQ[1]*SQ[2]; - P(1,1) += yaw_variance*sq(SQ[1]); - P(0,2) += yaw_variance*SQ[0]*SQ[2]; - P(1,2) += yaw_variance*SQ[0]*SQ[1]; - P(2,2) += yaw_variance*sq(SQ[0]); - P(0,3) -= yaw_variance*SQ[2]*SQ[3]; - P(1,3) -= yaw_variance*SQ[1]*SQ[3]; - P(2,3) -= yaw_variance*SQ[0]*SQ[3]; - P(3,3) += yaw_variance*sq(SQ[3]); - P(1,0) += yaw_variance*SQ[1]*SQ[2]; - P(2,0) += yaw_variance*SQ[0]*SQ[2]; - P(2,1) += yaw_variance*SQ[0]*SQ[1]; - P(3,0) -= yaw_variance*SQ[2]*SQ[3]; - P(3,1) -= yaw_variance*SQ[1]*SQ[3]; - P(3,2) -= yaw_variance*SQ[0]*SQ[3]; -} - -// save covariance data for re-use when auto-switching between heading and 3-axis fusion -void Ekf::saveMagCovData() -{ - // save variances for XYZ body axis field - _saved_mag_bf_variance(0) = P(19, 19); - _saved_mag_bf_variance(1) = P(20, 20); - _saved_mag_bf_variance(2) = P(21, 21); - - // save the NE axis covariance sub-matrix - _saved_mag_ef_ne_covmat = P.slice<2, 2>(16, 16); - - // save variance for the D earth axis - _saved_mag_ef_d_variance = P(18, 18); -} - -void Ekf::loadMagCovData() -{ - // re-instate variances for the XYZ body axis field - P(19, 19) = _saved_mag_bf_variance(0); - P(20, 20) = _saved_mag_bf_variance(1); - P(21, 21) = _saved_mag_bf_variance(2); - - // re-instate the NE axis covariance sub-matrix - P.slice<2, 2>(16, 16) = _saved_mag_ef_ne_covmat; - - // re-instate the D earth axis variance - P(18, 18) = _saved_mag_ef_d_variance; -} +#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 + const matrix::SquareMatrix3f rot_cov = diag(calcRotVecVariances()); + Vector3f rot_var_ned_before_reset = matrix::SquareMatrix3f(_R_to_earth * rot_cov * _R_to_earth.T()).diag(); + + // 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)); @@ -1134,14 +971,20 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _state.quat_nominal = quat_after_reset; uncorrelateQuatFromOtherStates(); - // update the yaw angle variance - if (yaw_variance > FLT_EPSILON) { - increaseQuatYawErrVariance(yaw_variance); - } + // 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; @@ -1155,41 +998,29 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) _state_reset_status.reset_count.quat++; _time_last_heading_fuse = _time_delayed_us; - - _last_static_yaw = NAN; } -// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator -// Resets the horizontal velocity and position to the default navigation sensor -// Returns true if the reset was successful bool Ekf::resetYawToEKFGSF() { if (!isYawEmergencyEstimateAvailable()) { return false; } + // don't allow reset if there's just been a yaw reset + const bool yaw_alignment_changed = (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align); + const bool quat_reset = (_state_reset_status.reset_count.quat != _state_reset_count_prev.quat); + + if (yaw_alignment_changed || quat_reset) { + return false; + } + + ECL_INFO("yaw estimator reset heading %.3f -> %.3f rad", + (double)getEulerYaw(_R_to_earth), (double)_yawEstimator.getYaw()); + resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); - // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight - _flt_mag_align_start_time = _time_delayed_us; _control_status.flags.yaw_align = true; - - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - // stop using the magnetometer in the main EKF otherwise it's 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; - - } else if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; - - } -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - if (_control_status.flags.ev_yaw) { - _inhibit_ev_yaw_use = true; - } -#endif // CONFIG_EKF2_EXTERNAL_VISION + _information_events.flags.yaw_aligned_to_imu_gps = true; return true; } @@ -1241,5 +1072,5 @@ void Ekf::resetWindToZero() _state.wind_vel.setZero(); // start with a small initial uncertainty to improve the initial estimate - P.uncorrelateCovarianceSetVariance<2>(22, _params.initial_wind_uncertainty); + P.uncorrelateCovarianceSetVariance(State::wind_vel.idx, _params.initial_wind_uncertainty); } diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 0955251586..04879dfd92 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -47,8 +47,12 @@ EstimatorInterface::~EstimatorInterface() { delete _gps_buffer; +#if defined(CONFIG_EKF2_MAGNETOMETER) delete _mag_buffer; +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_BAROMETER) delete _baro_buffer; +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) delete _range_buffer; #endif // CONFIG_EKF2_RANGE_FINDER @@ -102,6 +106,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample) #endif // CONFIG_EKF2_DRAG_FUSION } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EstimatorInterface::setMagData(const magSample &mag_sample) { if (!_initialised) { @@ -137,6 +142,7 @@ void EstimatorInterface::setMagData(const magSample &mag_sample) 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 void EstimatorInterface::setGpsData(const gpsMessage &gps) { @@ -217,6 +223,7 @@ void EstimatorInterface::setGpsData(const gpsMessage &gps) } } +#if defined(CONFIG_EKF2_BAROMETER) void EstimatorInterface::setBaroData(const baroSample &baro_sample) { if (!_initialised) { @@ -252,6 +259,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) 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 #if defined(CONFIG_EKF2_AIRSPEED) void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) @@ -471,7 +479,7 @@ void EstimatorInterface::setSystemFlagData(const systemFlagUpdate &system_flags) _system_flag_buffer->push(system_flags_new); } else { - ECL_WARN("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); } } @@ -494,6 +502,17 @@ void EstimatorInterface::setDragData(const imuSample &imu) } } + // don't use any accel samples that are clipping + if (imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]) { + // reset accumulators + _drag_sample_count = 0; + _drag_down_sampled.accelXY.zero(); + _drag_down_sampled.time_us = 0; + _drag_sample_time_dt = 0.0f; + + return; + } + _drag_sample_count++; // note acceleration is accumulated as a delta velocity _drag_down_sampled.accelXY(0) += imu.delta_vel(0); @@ -696,13 +715,17 @@ void EstimatorInterface::print_status() printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size()); } +#if defined(CONFIG_EKF2_MAGNETOMETER) if (_mag_buffer) { printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size()); } +#endif // CONFIG_EKF2_MAGNETOMETER +#if defined(CONFIG_EKF2_BAROMETER) if (_baro_buffer) { printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size()); } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) if (_range_buffer) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7126f57c8b..53881bb95e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2020 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -64,7 +64,6 @@ #include "common.h" #include "RingBuffer.h" #include "imu_down_sampler.hpp" -#include "utils.hpp" #include "output_predictor.h" #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -87,11 +86,15 @@ public: void setIMUData(const imuSample &imu_sample); +#if defined(CONFIG_EKF2_MAGNETOMETER) void setMagData(const magSample &mag_sample); +#endif // CONFIG_EKF2_MAGNETOMETER void setGpsData(const gpsMessage &gps); +#if defined(CONFIG_EKF2_BAROMETER) void setBaroData(const baroSample &baro_sample); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_AIRSPEED) void setAirspeedData(const airspeedSample &airspeed_sample); @@ -141,16 +144,34 @@ public: void set_in_air_status(bool in_air) { if (!in_air) { + if (_control_status.flags.in_air) { + ECL_DEBUG("no longer in air"); + } + _time_last_on_ground_us = _time_delayed_us; } else { + if (!_control_status.flags.in_air) { + ECL_DEBUG("in air"); + } + _time_last_in_air = _time_delayed_us; } _control_status.flags.in_air = in_air; } - void set_vehicle_at_rest(bool at_rest) { _control_status.flags.vehicle_at_rest = at_rest; } + void set_vehicle_at_rest(bool at_rest) + { + if (!_control_status.flags.vehicle_at_rest && at_rest) { + ECL_DEBUG("at rest"); + + } else if (_control_status.flags.vehicle_at_rest && !at_rest) { + ECL_DEBUG("no longer at rest"); + } + + _control_status.flags.vehicle_at_rest = at_rest; + } // return true if the attitude is usable bool attitude_valid() const { return _control_status.flags.tilt_align; } @@ -207,19 +228,21 @@ public: int getNumberOfActiveVerticalVelocityAidingSources() const; const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } + float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } Vector3f getVelocity() const { return _output_predictor.getVelocity(); } const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } Vector3f getPosition() const { return _output_predictor.getPosition(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } +#if defined(CONFIG_EKF2_MAGNETOMETER) // Get the value of magnetic declination in degrees to be saved for use at the next startup // Returns true when the declination can be saved // At the next startup, set param.mag_declination_deg to the value saved - bool get_mag_decl_deg(float *val) const + bool get_mag_decl_deg(float &val) const { if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) { - *val = math::degrees(_mag_declination_gps); + val = math::degrees(_mag_declination_gps); return true; } else { @@ -227,6 +250,26 @@ public: } } + bool get_mag_inc_deg(float &val) const + { + if (_NED_origin_initialised) { + val = math::degrees(_mag_inclination_gps); + return true; + + } else { + return false; + } + } + + void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const + { + inc_deg = math::degrees(_mag_inclination); + inc_ref_deg = math::degrees(_mag_inclination_gps); + strength_gs = _mag_strength; + strength_ref_gs = _mag_strength_gps; + } +#endif // CONFIG_EKF2_MAGNETOMETER + // get EKF mode status const filter_control_status_u &control_status() const { return _control_status; } const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; } @@ -339,8 +382,8 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized bool _NED_origin_initialised{false}; - float _gps_origin_eph{0.0f}; // horizontal position uncertainty of the GPS origin - float _gps_origin_epv{0.0f}; // vertical position uncertainty of the GPS origin + float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin + float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin MapProjection _gps_pos_prev{}; // Contains WGS-84 position latitude and longitude of the previous GPS message float _gps_alt_prev{0.0f}; // height from the previous GPS message (m) @@ -375,8 +418,11 @@ protected: RingBuffer _imu_buffer{kBufferLengthDefault}; RingBuffer *_gps_buffer{nullptr}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) RingBuffer *_mag_buffer{nullptr}; - RingBuffer *_baro_buffer{nullptr}; + uint64_t _time_last_mag_buffer_push{0}; +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_AIRSPEED) RingBuffer *_airspeed_buffer{nullptr}; @@ -392,8 +438,11 @@ protected: RingBuffer *_system_flag_buffer{nullptr}; uint64_t _time_last_gps_buffer_push{0}; - uint64_t _time_last_mag_buffer_push{0}; + +#if defined(CONFIG_EKF2_BAROMETER) + RingBuffer *_baro_buffer{nullptr}; uint64_t _time_last_baro_buffer_push{0}; +#endif // CONFIG_EKF2_BAROMETER uint64_t _time_last_gnd_effect_on{0}; @@ -402,10 +451,15 @@ protected: // allocate data buffers and initialize interface variables bool initialise_interface(uint64_t timestamp); + uint64_t _wmm_gps_time_last_checked{0}; // time WMM last checked + uint64_t _wmm_gps_time_last_set{0}; // time WMM last set 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) + float _mag_inclination{NAN}; + float _mag_strength{NAN}; + // this is the current status of the filter control modes filter_control_status_u _control_status{}; diff --git a/src/modules/ekf2/EKF/ev_control.cpp b/src/modules/ekf2/EKF/ev_control.cpp index da27f11924..22ac4ec764 100644 --- a/src/modules/ekf2/EKF/ev_control.cpp +++ b/src/modules/ekf2/EKF/ev_control.cpp @@ -58,6 +58,7 @@ void Ekf::controlExternalVisionFusion() && ((_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); @@ -80,7 +81,26 @@ void Ekf::controlExternalVisionFusion() stopEvYawFusion(); stopEvHgtFusion(); + _ev_q_error_initialized = false; + _warning_events.flags.vision_data_stopped = true; ECL_WARN("vision data stopped"); } } + +void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset) +{ + const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + + if (!q_error.isAllFinite()) { + return; + } + + if (!_ev_q_error_initialized || ev_reset) { + _ev_q_error_filt.reset(q_error); + _ev_q_error_initialized = true; + + } else { + _ev_q_error_filt.update(q_error); + } +} diff --git a/src/modules/ekf2/EKF/ev_height_control.cpp b/src/modules/ekf2/EKF/ev_height_control.cpp index aefd7e1681..4ad4dd7e0f 100644 --- a/src/modules/ekf2/EKF/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/ev_height_control.cpp @@ -56,10 +56,9 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com Matrix3f pos_cov{matrix::diag(ev_sample.position_var)}; // rotate EV to the EKF reference frame unless we're operating entirely in vision frame - // TODO: only necessary if there's a roll/pitch offset between VIO and EKF if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) { - const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); + const Quatf q_error(_ev_q_error_filt.getState()); if (q_error.isAllFinite()) { const Dcmf R_ev_to_ekf(q_error); @@ -95,7 +94,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com if (measurement_valid && quality_sufficient) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) @@ -105,10 +104,7 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com && continuing_conditions_passing; if (_control_status.flags.ev_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { - if (ev_reset) { if (quality_sufficient) { diff --git a/src/modules/ekf2/EKF/ev_pos_control.cpp b/src/modules/ekf2/EKF/ev_pos_control.cpp index 980a488672..e969af287c 100644 --- a/src/modules/ekf2/EKF/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/ev_pos_control.cpp @@ -95,8 +95,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common } else { // rotate EV to the EKF reference frame - const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); - const Dcmf R_ev_to_ekf = Dcmf(q_error); + const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState()); pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth; pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose(); @@ -165,7 +164,7 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common 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(P(7, 7), P(8, 8))); + _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance())); } if (!measurement_valid) { @@ -176,7 +175,6 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common && continuing_conditions_passing; if (_control_status.flags.ev_pos) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { const bool bias_estimator_change = (bias_fusion_was_active != _ev_pos_b_est.fusionActive()); diff --git a/src/modules/ekf2/EKF/ev_vel_control.cpp b/src/modules/ekf2/EKF/ev_vel_control.cpp index 807948ef27..a2431689d9 100644 --- a/src/modules/ekf2/EKF/ev_vel_control.cpp +++ b/src/modules/ekf2/EKF/ev_vel_control.cpp @@ -80,8 +80,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common } else { // rotate EV to the EKF reference frame - const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized()); - const Dcmf R_ev_to_ekf = Dcmf(q_error); + 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(); @@ -139,7 +138,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common && ((Vector3f(aid_src.test_ratio).max() < 0.1f) || !isHorizontalAidingActive()); if (_control_status.flags.ev_vel) { - aid_src.fusion_enabled = true; if (continuing_conditions_passing) { diff --git a/src/modules/ekf2/EKF/ev_yaw_control.cpp b/src/modules/ekf2/EKF/ev_yaw_control.cpp index e7f1d3c643..d7d4eee92a 100644 --- a/src/modules/ekf2/EKF/ev_yaw_control.cpp +++ b/src/modules/ekf2/EKF/ev_yaw_control.cpp @@ -49,10 +49,14 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common 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); + if (ev_reset) { + _control_status.flags.ev_yaw_fault = false; + } + // determine if we should use EV yaw aiding bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::YAW)) && _control_status.flags.tilt_align - && !_inhibit_ev_yaw_use + && !_control_status.flags.ev_yaw_fault && PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); @@ -72,8 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common && isTimedOut(aid_src.time_last_fuse, (uint32_t)1e6); if (_control_status.flags.ev_yaw) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { if (ev_reset) { @@ -92,7 +94,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } } else if (quality_sufficient) { - fuseYaw(aid_src.innovation, aid_src.observation_variance, aid_src); + fuseYaw(aid_src); } else { aid_src.innovation_rejected = true; @@ -154,7 +156,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common } else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) { // turn on fusion of external vision yaw measurements and disable all other heading fusion - stopMagFusion(); stopGpsYawFusion(); stopGpsFusion(); diff --git a/src/modules/ekf2/EKF/fake_height_control.cpp b/src/modules/ekf2/EKF/fake_height_control.cpp index ef8c318081..8b7eba5664 100644 --- a/src/modules/ekf2/EKF/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/fake_height_control.cpp @@ -63,9 +63,9 @@ void Ekf::controlFakeHgtFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate); - - fuseVerticalPosition(aid_src); + if (aid_src.test_ratio < sq(100.0f / innov_gate)) { + fuseVerticalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); diff --git a/src/modules/ekf2/EKF/fake_pos_control.cpp b/src/modules/ekf2/EKF/fake_pos_control.cpp index afe33b5fec..5ff14f8862 100644 --- a/src/modules/ekf2/EKF/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/fake_pos_control.cpp @@ -76,14 +76,16 @@ void Ekf::controlFakePosFusion() if (continuing_conditions_passing) { // always protect against extreme values that could result in a NaN - aid_src.fusion_enabled = (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.test_ratio[0] < sq(100.0f / innov_gate)) + && (aid_src.test_ratio[1] < sq(100.0f / innov_gate)) + ) { + fuseHorizontalPosition(aid_src); + } const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5); if (is_fusion_failing) { + ECL_WARN("fake position fusion failing, resetting"); resetFakePosFusion(); } diff --git a/src/modules/ekf2/EKF/gnss_height_control.cpp b/src/modules/ekf2/EKF/gnss_height_control.cpp index 56b75a054b..100d4150f1 100644 --- a/src/modules/ekf2/EKF/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/gnss_height_control.cpp @@ -82,7 +82,7 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) if (measurement_valid && gps_checks_passing && !gps_checks_failing) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9)); + 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 @@ -98,8 +98,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample) && !gps_checks_failing; if (_control_status.flags.gps_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); diff --git a/src/modules/ekf2/EKF/gps_checks.cpp b/src/modules/ekf2/EKF/gps_checks.cpp index 61d8546329..0b45b1bd78 100644 --- a/src/modules/ekf2/EKF/gps_checks.cpp +++ b/src/modules/ekf2/EKF/gps_checks.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -84,52 +84,53 @@ bool Ekf::collect_gps(const gpsMessage &gps) _NED_origin_initialised = true; - _earth_rate_NED = calcEarthRateNED((float)math::radians(_pos_ref.getProjectionReferenceLat())); - - const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); - - // set the magnetic field data returned by the geo library using the current GPS position - _mag_declination_gps = get_mag_declination_radians(lat, lon); - _mag_inclination_gps = get_mag_inclination_radians(lat, lon); - _mag_strength_gps = get_mag_strength_gauss(lat, lon); - - // request a reset of the yaw using the new declination - if ((_params.mag_fusion_type != MagFuseType::NONE) - && !declination_was_valid) { - _mag_yaw_reset_req = true; - } - // save the horizontal and vertical position uncertainty of the origin - _gps_origin_eph = gps.eph; - _gps_origin_epv = gps.epv; + _gpos_origin_eph = gps.eph; + _gpos_origin_epv = gps.epv; _information_events.flags.gps_checks_passed = true; ECL_INFO("GPS checks passed"); + } - } else if (!_NED_origin_initialised) { - // a rough 2D fix is still sufficient to lookup declination - if ((gps.fix_type >= 2) && (gps.eph < 1000)) { + if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) { + // a rough 2D fix is sufficient to lookup declination + const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.eph < 1000); - const bool declination_was_valid = PX4_ISFINITE(_mag_declination_gps); + 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 * 1.0e-7; const double lon = gps.lon * 1.0e-7; // set the magnetic field data returned by the geo library using the current GPS position - _mag_declination_gps = get_mag_declination_radians(lat, lon); - _mag_inclination_gps = get_mag_inclination_radians(lat, lon); - _mag_strength_gps = get_mag_strength_gauss(lat, lon); + 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); - // request mag yaw reset if there's a mag declination for the first time - if (_params.mag_fusion_type != MagFuseType::NONE) { - if (!declination_was_valid && PX4_ISFINITE(_mag_declination_gps)) { - _mag_yaw_reset_req = true; + 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; } } _earth_rate_NED = calcEarthRateNED((float)math::radians(lat)); } + + _wmm_gps_time_last_checked = _time_delayed_us; } // start collecting GPS if there is a 3D fix and the NED origin has been set diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 9848ad6b0b..4c620de17f 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -41,7 +41,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) { - if (!_gps_buffer || !((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL))) { + if (!_gps_buffer || (_params.gnss_ctrl == 0)) { stopGpsFusion(); return; } @@ -95,7 +95,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) vel_obs_var, // observation variance math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate _aid_src_gnss_vel); - _aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); + const bool gnss_vel_enabled = (_params.gnss_ctrl & GnssCtrl::VEL); // GNSS position const Vector2f position{gps_sample.pos}; @@ -117,37 +117,28 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) pos_obs_var, // observation variance math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate _aid_src_gnss_pos); - _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start - if (_control_status.flags.tilt_align && _NED_origin_initialised - && gps_checks_passing && !gps_checks_failing) { - - if (!_control_status.flags.yaw_align) { - if (_control_status.flags.ev_yaw && !_control_status.flags.yaw_align) { - - // give mag a chance to start and yaw align if currently blocked by EV yaw - const bool mag_enabled = (_params.mag_fusion_type <= MagFuseType::MAG_3D); - const bool mag_available = (_mag_counter != 0); - - if (mag_enabled && mag_available - && !_control_status.flags.mag_field_disturbed - && !_control_status.flags.mag_fault) { - - stopEvYawFusion(); - } - } - } - } -#endif // CONFIG_EKF2_EXTERNAL_VISION + const bool gnss_pos_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - const bool mandatory_conditions_passing = ((_params.gnss_ctrl & GnssCtrl::HPOS) || (_params.gnss_ctrl & GnssCtrl::VEL)) - && _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _NED_origin_initialised; + bool mandatory_conditions_passing = false; + + if ((gnss_pos_enabled || gnss_vel_enabled) + && _control_status.flags.tilt_align + && _NED_origin_initialised + ) { + // if GPS is otherwise ready to go other than yaw align + if (!_control_status.flags.yaw_align && gps_checks_passing && !gps_checks_failing) { + + if (resetYawToEKFGSF()) { + ECL_INFO("GPS yaw aligned using IMU"); + } + } + + if (_control_status.flags.yaw_align) { + mandatory_conditions_passing = true; + } + } const bool continuing_conditions_passing = mandatory_conditions_passing && !gps_checks_failing; const bool starting_conditions_passing = continuing_conditions_passing && gps_checks_passing; @@ -157,8 +148,13 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (continuing_conditions_passing || !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { - fuseVelocity(_aid_src_gnss_vel); - fuseHorizontalPosition(_aid_src_gnss_pos); + if (gnss_vel_enabled) { + fuseVelocity(_aid_src_gnss_vel); + } + + if (gnss_pos_enabled) { + fuseHorizontalPosition(_aid_src_gnss_pos); + } bool do_vel_pos_reset = shouldResetGpsFusion(); @@ -173,6 +169,31 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) */ if (resetYawToEKFGSF()) { ECL_WARN("GPS emergency yaw reset"); + + if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + // 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; + } + +#endif // CONFIG_EKF2_GNSS_YAW + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + + if (_control_status.flags.ev_yaw) { + _control_status.flags.ev_yaw_fault = true; + } + +#endif // CONFIG_EKF2_EXTERNAL_VISION + do_vel_pos_reset = true; } } @@ -180,15 +201,19 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) if (do_vel_pos_reset) { ECL_WARN("GPS fusion timeout, resetting velocity and position"); - // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + // reset velocity + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + if (gnss_pos_enabled) { + // reset position + _information_events.flags.reset_pos_to_gps = true; + resetHorizontalPositionTo(position, pos_obs_var); + _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; + } } } else { @@ -203,16 +228,6 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } else { if (starting_conditions_passing) { -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - // Do not use external vision for yaw if using GPS because yaw needs to be - // defined relative to an NED reference frame - if (_control_status.flags.ev_yaw) { - // Stop the vision for yaw fusion and do not allow it to start again - stopEvYawFusion(); - _inhibit_ev_yaw_use = true; - } -#endif // CONFIG_EKF2_EXTERNAL_VISION - ECL_INFO("starting GPS fusion"); _information_events.flags.starting_gps_fusion = true; @@ -222,34 +237,21 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) || !_control_status_prev.flags.yaw_align ) { // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + if (gnss_vel_enabled) { + _information_events.flags.reset_vel_to_gps = true; + resetVelocityTo(velocity, vel_obs_var); + _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; + } } - // reset position - _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(position, pos_obs_var); - _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; - - _control_status.flags.gps = true; - - } else if (gps_checks_passing && !_control_status.flags.yaw_align && (_params.mag_fusion_type == MagFuseType::NONE)) { - // If no mag is used, align using the yaw estimator (if available) - if (resetYawToEKFGSF()) { - _information_events.flags.yaw_aligned_to_imu_gps = true; - ECL_INFO("GPS yaw aligned using IMU, resetting vel and pos"); - - // reset velocity - _information_events.flags.reset_vel_to_gps = true; - resetVelocityTo(velocity, vel_obs_var); - _aid_src_gnss_vel.time_last_fuse = _time_delayed_us; - + if (gnss_pos_enabled) { // reset position _information_events.flags.reset_pos_to_gps = true; resetHorizontalPositionTo(position, pos_obs_var); _aid_src_gnss_pos.time_last_fuse = _time_delayed_us; } + + _control_status.flags.gps = true; } } @@ -275,7 +277,7 @@ bool Ekf::shouldResetGpsFusion() const * with no aiding we need to do something */ bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max) - && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); + && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max); #if defined(CONFIG_EKF2_OPTICAL_FLOW) @@ -379,9 +381,13 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi } else { if (starting_conditions_passing) { // Try to activate GPS yaw fusion - startGpsYawFusion(gps_sample); + if (resetYawToGps(gps_sample.yaw)) { + 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; - if (_control_status.flags.gps_yaw) { _nb_gps_yaw_reset_available = 1; } } @@ -393,72 +399,42 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } - - // 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 - && !_control_status.flags.gps_yaw - && _control_status_prev.flags.gps_yaw) { - _control_status.flags.yaw_align = false; - } -} - -void Ekf::startGpsYawFusion(const gpsSample &gps_sample) -{ - if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) { - ECL_INFO("starting GPS yaw fusion"); - _control_status.flags.yaw_align = true; - _control_status.flags.mag_dec = false; - - stopMagHdgFusion(); - stopMag3DFusion(); - _control_status.flags.gps_yaw = true; - } } #endif // CONFIG_EKF2_GNSS_YAW void Ekf::stopGpsYawFusion() { #if defined(CONFIG_EKF2_GNSS_YAW) + if (_control_status.flags.gps_yaw) { - ECL_INFO("stopping GPS yaw fusion"); + _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) { - stopGpsPosFusion(); - stopGpsVelFusion(); - - _control_status.flags.gps = false; - } - - if (_control_status.flags.gps_yaw) { - stopGpsYawFusion(); - } - - // We do not need to know the true North anymore - // EV yaw can start again - _inhibit_ev_yaw_use = false; -} - -void Ekf::stopGpsPosFusion() -{ - if (_control_status.flags.gps) { - ECL_INFO("stopping GPS position fusion"); - _control_status.flags.gps = false; - + ECL_INFO("stopping GPS position and velocity fusion"); resetEstimatorAidStatus(_aid_src_gnss_pos); + resetEstimatorAidStatus(_aid_src_gnss_vel); + + _control_status.flags.gps = false; } -} -void Ekf::stopGpsVelFusion() -{ - ECL_INFO("stopping GPS velocity fusion"); - - resetEstimatorAidStatus(_aid_src_gnss_vel); + stopGpsHgtFusion(); + stopGpsYawFusion(); } diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 7ddfb391ee..22bdfb4847 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2018 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -64,7 +64,7 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) float heading_innov_var; { - Vector24f H; + VectorState H; sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } @@ -73,8 +73,6 @@ void Ekf::updateGpsYaw(const gpsSample &gps_sample) gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg); gnss_yaw.innovation_variance = heading_innov_var; - gnss_yaw.fusion_enabled = _control_status.flags.gps_yaw; - gnss_yaw.timestamp_sample = gps_sample.time_us; const float innov_gate = math::max(_params.heading_innov_gate, 1.0f); @@ -91,7 +89,7 @@ void Ekf::fuseGpsYaw() return; } - Vector24f H; + VectorState H; { float heading_pred; @@ -102,8 +100,6 @@ void Ekf::fuseGpsYaw() sym::ComputeGnssYawPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, _gps_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H); } - const SparseVector24f<0,1,2,3> Hfusion(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 @@ -126,12 +122,12 @@ void Ekf::fuseGpsYaw() // 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 - resetZDeltaAngBiasCov(); + resetGyroBiasZCov(); } // calculate the Kalman gains // only calculate gains for states we are using - Vector24f Kfusion = P * Hfusion / gnss_yaw.innovation_variance; + VectorState Kfusion = P * H / gnss_yaw.innovation_variance; const bool is_fused = measurementUpdate(Kfusion, gnss_yaw.innovation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 8a09b72f79..d004a9b3ec 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -60,7 +60,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) // calculate kalman gains and innovation variances Vector3f innovation; // innovation of the last gravity fusion observation (m/s**2) Vector3f innovation_variance; - Vector24f Kx, Ky, Kz; // Kalman gain vectors + VectorState Kx, Ky, Kz; // Kalman gain vectors sym::ComputeGravityInnovVarAndKAndH( getStateAtFusionHorizonAsVector(), P, measurement, measurement_var, FLT_EPSILON, &innovation, &innovation_variance, &Kx, &Ky, &Kz); @@ -80,9 +80,9 @@ void Ekf::controlGravityFusion(const imuSample &imu) float innovation_gate = 1.f; setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate); - _aid_src_gravity.fusion_enabled = _control_status.flags.gravity_vector; + const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; - if (_aid_src_gravity.fusion_enabled && !_aid_src_gravity.innovation_rejected) { + if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { // perform fusion for each axis _aid_src_gravity.fused = measurementUpdate(Kx, innovation_variance(0), innovation(0)) && measurementUpdate(Ky, innovation_variance(1), innovation(1)) diff --git a/src/modules/ekf2/EKF/height_bias_estimator.hpp b/src/modules/ekf2/EKF/height_bias_estimator.hpp index 9082842b9e..1d1b9629f5 100644 --- a/src/modules/ekf2/EKF/height_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/height_bias_estimator.hpp @@ -35,7 +35,8 @@ * @file height_bias_estimator.hpp */ -#pragma once +#ifndef EKF_HEIGHT_BIAS_ESTIMATOR_HPP +#define EKF_HEIGHT_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" @@ -72,3 +73,5 @@ private: bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool }; + +#endif // !EKF_HEIGHT_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index c9cfff5a3b..6f7a3305b2 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -41,10 +41,14 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) { checkVerticalAccelerationHealth(imu_delayed); +#if defined(CONFIG_EKF2_BAROMETER) updateGroundEffect(); controlBaroHeightFusion(); +#endif // CONFIG_EKF2_BAROMETER + controlGnssHeightFusion(_gps_sample_delayed); + #if defined(CONFIG_EKF2_RANGE_FINDER) controlRangeHeightFusion(); #endif // CONFIG_EKF2_RANGE_FINDER @@ -175,9 +179,11 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const bool failed_lim{false}; } 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 (_control_status.flags.gps_hgt) { checks[1] = {ReferenceType::GNSS, _aid_src_gnss_hgt.innovation, _aid_src_gnss_hgt.innovation_variance}; diff --git a/src/modules/ekf2/EKF/imu_down_sampler.hpp b/src/modules/ekf2/EKF/imu_down_sampler.hpp index ec0d8a1008..29732bd584 100644 --- a/src/modules/ekf2/EKF/imu_down_sampler.hpp +++ b/src/modules/ekf2/EKF/imu_down_sampler.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2019-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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/modules/ekf2/EKF/mag_3d_control.cpp b/src/modules/ekf2/EKF/mag_3d_control.cpp new file mode 100644 index 0000000000..738441cf27 --- /dev/null +++ b/src/modules/ekf2/EKF/mag_3d_control.cpp @@ -0,0 +1,233 @@ +/**************************************************************************** + * + * 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); + + // 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)) + && (wmm_updated || checkHaglYawResetReq() || isRecent(_time_last_mov_3d_mag_suitable, (uint64_t)3e6)) + && 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 + && isRecent(aid_src.time_last_fuse, 500'000) + && getMagBiasVariance().longerThan(0.f) && !getMagBiasVariance().longerThan(sq(0.02f)) + && !_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()) { + 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; + + loadMagCovData(); + + // 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() < sq(0.0001f)) + || (getStateVariance().min() < sq(0.0001f)) + ) { + 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; + + saveMagCovData(); + } +} + +void Ekf::saveMagCovData() +{ + // save the NED axis covariance sub-matrix + _saved_mag_ef_covmat = getStateCovariance(); + + // save the XYZ body covariance sub-matrix + _saved_mag_bf_covmat = getStateCovariance(); +} + +void Ekf::loadMagCovData() +{ + // re-instate the NED axis covariance sub-matrix + resetStateCovariance(_saved_mag_ef_covmat); + + // re-instate the XYZ body axis covariance sub-matrix + resetStateCovariance(_saved_mag_bf_covmat); +} diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index 47d892737f..0dacfad3e1 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2019-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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -41,293 +41,185 @@ void Ekf::controlMagFusion() { - bool mag_data_ready = false; - - magSample mag_sample; - - if (_mag_buffer) { - mag_data_ready = _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample); - - if (mag_data_ready) { - - // sensor or calibration has changed, clear any mag bias and reset low pass filter - if (mag_sample.reset || (_mag_counter == 0)) { - // Zero the magnetometer bias states - _state.mag_B.zero(); - - // Zero the corresponding covariances and set - // variances to the values use for initial alignment - P.uncorrelateCovarianceSetVariance<3>(19, sq(_params.mag_noise)); - - // reset any saved covariance data for re-use when auto-switching between heading and 3-axis fusion - _saved_mag_bf_variance.zero(); - - _control_status.flags.mag_fault = false; - - _mag_lpf.reset(mag_sample.mag); - _mag_counter = 1; - - } else { - _mag_lpf.update(mag_sample.mag); - _mag_counter++; - } - - // 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) - && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) - ) { - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); - mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred); - _control_status.flags.synthetic_mag_z = true; - - } else { - _control_status.flags.synthetic_mag_z = false; - } - - _control_status.flags.mag_field_disturbed = magFieldStrengthDisturbed(mag_sample.mag); - - - // compute mag heading innovation (for estimator_aid_src_mag_heading logging) - const Vector3f mag_observation = mag_sample.mag - _state.mag_B; - const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); - const Vector3f mag_earth_pred = R_to_earth * mag_observation; - - resetEstimatorAidStatus(_aid_src_mag_heading); - _aid_src_mag_heading.timestamp_sample = mag_sample.time_us; - _aid_src_mag_heading.observation = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - _aid_src_mag_heading.innovation = wrap_pi(getEulerYaw(_R_to_earth) - _aid_src_mag_heading.observation); - - // compute magnetometer innovations (for estimator_aid_src_mag logging) - // rotate magnetometer earth field state into body frame - const Vector3f mag_I_body = _state.quat_nominal.rotateVectorInverse(_state.mag_I); - const Vector3f mag_innov = mag_I_body - mag_observation; - - resetEstimatorAidStatus(_aid_src_mag); - _aid_src_mag.timestamp_sample = mag_sample.time_us; - mag_observation.copyTo(_aid_src_mag.observation); - mag_innov.copyTo(_aid_src_mag.innovation); - - } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { - // No data anymore. Stop until it comes back. - stopMagFusion(); - } - } - - // If we are on ground, reset the flight alignment flag so that the mag fields will be - // re-initialised next time we achieve flight altitude - if (!_control_status.flags.in_air) { + // 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; } - if (mag_data_ready && !_control_status.flags.tilt_align && !_control_status.flags.yaw_align) { - // calculate the initial magnetic field and yaw alignment - // but do not mark the yaw alignement complete as it needs to be - // reset once the leveling phase is done - if (_params.mag_fusion_type <= MagFuseType::MAG_3D) { - if ((_mag_counter > 1) && isTimedOut(_aid_src_mag_heading.time_last_fuse, (uint64_t)100'000)) { - // rotate the magnetometer measurements into earth frame using a zero yaw angle - // the angle of the projection onto the horizontal gives the yaw angle - const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState(); - const float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); + // check mag state observability + checkYawAngleObservability(); + checkMagBiasObservability(); + checkMagHeadingConsistency(); - const float yaw_prev = getEulerYaw(_R_to_earth); - - if (fabsf(yaw_new - yaw_prev) > math::radians(1.f)) { - - ECL_INFO("mag heading init %.3f -> %.3f rad (declination %.1f)", (double)yaw_prev, (double)yaw_new, (double)getMagDeclination()); - - // update the rotation matrix using the new yaw value - _R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal)); - _state.quat_nominal = _R_to_earth; - - // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); - - // set the earth magnetic field states using the updated rotation - _state.mag_I = _R_to_earth * _mag_lpf.getState(); - _state.mag_B.zero(); - - _aid_src_mag_heading.time_last_fuse = _time_delayed_us; - _time_last_heading_fuse = _time_delayed_us; - - _last_static_yaw = NAN; - } - } - } - - return; + if (_mag_bias_observable || _yaw_angle_observable) { + _time_last_mov_3d_mag_suitable = _time_delayed_us; } - if (_params.mag_fusion_type >= MagFuseType::NONE - || _control_status.flags.mag_fault - || !_control_status.flags.tilt_align) { - + if (_params.mag_fusion_type == MagFuseType::NONE) { stopMagFusion(); + stopMagHdgFusion(); return; } - _mag_yaw_reset_req |= !_control_status.flags.yaw_align; + // 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(); + } - if (mag_data_ready && !_control_status.flags.ev_yaw && !_control_status.flags.gps_yaw) { + magSample mag_sample; - if (shouldInhibitMag()) { - if (uint32_t(_time_delayed_us - _mag_use_not_inhibit_us) > (uint32_t)5e6) { - // If magnetometer use has been inhibited continuously then stop the fusion - stopMagFusion(); - } + if (_mag_buffer && _mag_buffer->pop_first_older_than(_time_delayed_us, &mag_sample)) { - return; + if (mag_sample.reset || (_mag_counter == 0)) { + // sensor or calibration has changed, reset low pass filter (reset handled by controlMag3DFusion/controlMagHeadingFusion) + _control_status.flags.mag_fault = false; + + _state.mag_B.zero(); + resetMagCov(); + + _mag_lpf.reset(mag_sample.mag); + _mag_counter = 1; } else { - _mag_use_not_inhibit_us = _time_delayed_us; + _mag_lpf.update(mag_sample.mag); + _mag_counter++; } - const bool mag_enabled_previously = _control_status_prev.flags.mag_hdg || _control_status_prev.flags.mag_3D; + 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); - // Determine if we should use simple magnetic heading fusion which works better when - // there are large external disturbances or the more accurate 3-axis fusion - switch (_params.mag_fusion_type) { - default: - // FALLTHROUGH - case MagFuseType::AUTO: - selectMagAuto(); - break; + // 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) + && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) + ) { + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) + * Vector3f(_mag_strength_gps, 0, 0); - case MagFuseType::INDOOR: + mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred); - /* fallthrough */ - case MagFuseType::HEADING: - startMagHdgFusion(); - break; + _control_status.flags.synthetic_mag_z = true; - case MagFuseType::MAG_3D: - startMag3DFusion(); - break; + } else { + _control_status.flags.synthetic_mag_z = false; } - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { + controlMag3DFusion(mag_sample, starting_conditions_passing, _aid_src_mag); + controlMagHeadingFusion(mag_sample, starting_conditions_passing, _aid_src_mag_heading); - if (_mag_yaw_reset_req || !_control_status.flags.yaw_align || mag_sample.reset || !mag_enabled_previously || haglYawResetReq()) { - - if (magReset()) { - _mag_yaw_reset_req = false; - - } else { - // mag reset failed, try again next time - _mag_yaw_reset_req = true; - } - } - } - - if (!_control_status.flags.yaw_align) { - // Having the yaw aligned is mandatory to continue - return; - } - - checkMagDeclRequired(); - - runMagAndMagDeclFusions(mag_sample.mag); + } else if (!isNewestSampleRecent(_time_last_mag_buffer_push, 2 * MAG_MAX_INTERVAL)) { + // No data anymore. Stop until it comes back. + stopMagHdgFusion(); + stopMagFusion(); } } -bool Ekf::haglYawResetReq() +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) { // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. -#if defined(CONFIG_EKF2_RANGE_FINDER) static constexpr float mag_anomalies_max_hagl = 1.5f; const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; -#else - return true; -#endif // CONFIG_EKF2_RANGE_FINDER } +#endif // CONFIG_EKF2_TERRAIN return false; } -bool Ekf::magReset() +void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading) { - // prevent a reset being performed more than once on the same frame - if ((_flt_mag_align_start_time == _time_delayed_us) - || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { - return false; - } + // reinit mag states + const Vector3f mag_I_before_reset = _state.mag_I; + const Vector3f mag_B_before_reset = _state.mag_B; - bool has_realigned_yaw = false; + // reset covariances to default + resetMagCov(); - // use yaw estimator if available - if (_control_status.flags.gps && isYawEmergencyEstimateAvailable() - && (_mag_counter > 1) // mag LPF available - ) { + // if world magnetic model (inclination, declination, strength) available then use it to reset mag states + if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { - resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + // use expected earth field to reset states + const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) + * Vector3f(_mag_strength_gps, 0, 0); - _information_events.flags.yaw_aligned_to_imu_gps = true; + // mag_B: reset + if (isYawEmergencyEstimateAvailable()) { + const Dcmf R_to_earth = updateYawInRotMat(_yawEstimator.getYaw(), _R_to_earth); + const Dcmf R_to_body = R_to_earth.transpose(); - // if world magnetic model (inclination, declination, strength) available then use it to reset mag states - if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) { - // use predicted earth field to reset states - const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0); - _state.mag_I = mag_earth_pred; + // 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) { + // mag_B: reset using WMM const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal); - _state.mag_B = _mag_lpf.getState() - (R_to_body * mag_earth_pred); + _state.mag_B = mag - (R_to_body * mag_earth_pred); } else { - // Use the last magnetometer measurements to reset the field states - // calculate initial earth magnetic field states - _state.mag_I = _R_to_earth * _mag_lpf.getState(); _state.mag_B.zero(); } - ECL_DEBUG("resetting mag I: [%.3f, %.3f, %.3f], B: [%.3f, %.3f, %.3f]", - (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), - (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2) - ); + // mag_I: reset, skipped if no change in state and variance good + _state.mag_I = mag_earth_pred; - resetMagCov(); - - has_realigned_yaw = true; - } - - if (!has_realigned_yaw) { - has_realigned_yaw = resetMagHeading(); - } - - if (has_realigned_yaw) { - _control_status.flags.yaw_align = true; - - if (_control_status.flags.in_air) { - _control_status.flags.mag_aligned_in_flight = true; - - // record the time for the magnetic field alignment event - _flt_mag_align_start_time = _time_delayed_us; + if (reset_heading) { + resetMagHeading(mag); } - return true; + // earth field was reset to WMM, skip initial declination fusion + _mag_decl_cov_reset = true; + + } else { + // mag_B: reset + _state.mag_B.zero(); + + // Use the magnetometer measurement to reset the field states + if (reset_heading) { + resetMagHeading(mag); + } + + // mag_I: use the last magnetometer measurements to reset the field states + _state.mag_I = _R_to_earth * mag; } - return false; -} + if (!mag_I_before_reset.longerThan(0.f)) { + ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]", + (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2), + (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2) + ); -void Ekf::selectMagAuto() -{ - check3DMagFusionSuitability(); - canUse3DMagFusion() ? startMag3DFusion() : startMagHdgFusion(); -} + } else { + ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]", + (double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2), + (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2) + ); -void Ekf::check3DMagFusionSuitability() -{ - checkYawAngleObservability(); - checkMagBiasObservability(); + if (mag_B_before_reset.longerThan(0.f) || _state.mag_B.longerThan(0.f)) { + ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]", + (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2), + (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2) + ); + } + } - if (_mag_bias_observable || _yaw_angle_observable) { - _time_last_mov_3d_mag_suitable = _time_delayed_us; + // record the start time for the magnetic field alignment + if (_control_status.flags.in_air) { + _control_status.flags.mag_aligned_in_flight = true; + _flt_mag_align_start_time = _time_delayed_us; } } @@ -366,56 +258,79 @@ void Ekf::checkMagBiasObservability() _time_yaw_started = _time_delayed_us; } -bool Ekf::canUse3DMagFusion() const +void Ekf::checkMagHeadingConsistency() { - // Use of 3D fusion requires an in-air heading alignment but it should not - // be used when the heading and mag biases are not observable for more than 2 seconds - return _control_status.flags.mag_aligned_in_flight - && ((_time_delayed_us - _time_last_mov_3d_mag_suitable) < (uint64_t)2e6); + if (fabsf(_mag_heading_innov_lpf.getState()) < _params.mag_heading_noise) { + if (_yaw_angle_observable) { + // yaw angle must be observable to consider consistency + _control_status.flags.mag_heading_consistent = true; + } + + } else { + _control_status.flags.mag_heading_consistent = false; + } } -void Ekf::checkMagDeclRequired() +bool Ekf::checkMagField(const Vector3f &mag_sample) { - // 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 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_3D && (not_using_ne_aiding || user_selected)); -} + _control_status.flags.mag_field_disturbed = false; -bool Ekf::shouldInhibitMag() const -{ - // If the user has selected auto protection against indoor magnetic field errors, only use the magnetometer - // if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding - // is available, assume that we are operating indoors and the magnetometer should not be used. - // Also inhibit mag fusion when a strong magnetic field interference is detected or the user - // has explicitly stopped magnetometer use. - const bool user_selected = (_params.mag_fusion_type == MagFuseType::INDOOR); + if (_params.mag_check == 0) { + // skip all checks + return true; + } - const bool heading_not_required_for_navigation = !_control_status.flags.gps; - - return (user_selected && heading_not_required_for_navigation) || _control_status.flags.mag_field_disturbed; -} - -bool Ekf::magFieldStrengthDisturbed(const Vector3f &mag_sample) const -{ - if (_params.check_mag_strength - && ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || (_params.mag_fusion_type == MagFuseType::INDOOR && _control_status.flags.gps))) { + bool is_check_failing = false; + _mag_strength = mag_sample.length(); + if (_params.mag_check & static_cast(MagCheckMask::STRENGTH)) { if (PX4_ISFINITE(_mag_strength_gps)) { - constexpr float wmm_gate_size = 0.2f; // +/- Gauss - return !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size); + if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) { + _control_status.flags.mag_field_disturbed = true; + is_check_failing = true; + } + + } else if (_params.mag_check & static_cast(MagCheckMask::FORCE_WMM)) { + is_check_failing = true; } else { constexpr float average_earth_mag_field_strength = 0.45f; // Gauss constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss - return !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size); + + if (!isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size)) { + _control_status.flags.mag_field_disturbed = true; + is_check_failing = true; + } } } - return false; + const Vector3f mag_earth = _R_to_earth * mag_sample; + _mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); + + if (_params.mag_check & static_cast(MagCheckMask::INCLINATION)) { + if (PX4_ISFINITE(_mag_inclination_gps)) { + const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg); + const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps); + + if (fabsf(inc_error_rad) > inc_tol_rad) { + _control_status.flags.mag_field_disturbed = true; + is_check_failing = true; + } + + } else if (_params.mag_check & static_cast(MagCheckMask::FORCE_WMM)) { + is_check_failing = true; + + } else { + // No check possible when the global position is unknown + // TODO: add parameter to remember the inclination between boots + } + } + + if (is_check_failing || (_time_last_mag_check_failing == 0)) { + _time_last_mag_check_failing = _time_delayed_us; + } + + return ((_time_delayed_us - _time_last_mag_check_failing) > (uint64_t)_min_mag_health_time_us); } bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, const float gate) @@ -424,106 +339,41 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected, && (measured <= expected + gate); } -void Ekf::runMagAndMagDeclFusions(const Vector3f &mag) +void Ekf::resetMagHeading(const Vector3f &mag) { - if (_control_status.flags.mag_3D) { - run3DMagAndDeclFusions(mag); + // use mag bias if variance good (unless configured for HEADING only) + Vector3f mag_bias{0.f, 0.f, 0.f}; + const Vector3f mag_bias_var = getMagBiasVariance(); - } else if (_control_status.flags.mag_hdg) { - // Rotate the measurements into earth frame using the zero yaw angle - Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); - - Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B); - - // the angle of the projection onto the horizontal gives the yaw angle - // calculate the yaw innovation and wrap to the interval between +-pi - float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - - float innovation = wrap_pi(getEulerYaw(_R_to_earth) - measured_hdg); - float obs_var = fmaxf(sq(_params.mag_heading_noise), 1.e-4f); - - _aid_src_mag_heading.fusion_enabled = _control_status.flags.mag_hdg; - - fuseYaw(innovation, obs_var, _aid_src_mag_heading); - } -} - -void Ekf::run3DMagAndDeclFusions(const Vector3f &mag) -{ - // sanity check mag_B before they are used to constrain heading drift - const Vector3f mag_bias_var = P.slice<3, 3>(19, 19).diag(); - const bool mag_bias_var_good = (mag_bias_var.min() > 0.f) && (mag_bias_var.max() < sq(0.02f)); - - const bool update_all_states = _control_status.flags.mag_aligned_in_flight && mag_bias_var_good; - - 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, _aid_src_mag, update_all_states); - - } 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. - fuseMag(mag, _aid_src_mag, update_all_states); - - if (_control_status.flags.mag_dec) { - fuseDeclination(0.5f); - } - } -} - -bool Ekf::resetMagHeading() -{ - // prevent a reset being performed more than once on the same frame - if ((_flt_mag_align_start_time == _time_delayed_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { - return false; + if ((mag_bias_var.min() > 0.f) && (mag_bias_var.max() <= sq(_params.mag_noise))) { + mag_bias = _state.mag_B; } - const Vector3f mag_init = _mag_lpf.getState(); + // calculate mag heading + // rotate the magnetometer measurements into earth frame using a zero yaw angle + const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); - const bool mag_available = (_mag_counter > 1) && !magFieldStrengthDisturbed(mag_init); + // the angle of the projection onto the horizontal gives the yaw angle + const Vector3f mag_earth_pred = R_to_earth * (mag - mag_bias); - // low pass filtered mag required - if (!mag_available) { - return false; - } + // calculate the observed yaw angle and yaw variance + const float declination = getMagDeclination(); + float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + declination; + float yaw_new_variance = math::max(sq(_params.mag_heading_noise), sq(0.01f)); - const bool heading_required_for_navigation = _control_status.flags.gps; + ECL_INFO("reset mag heading %.3f -> %.3f rad (bias:[%.3f, %.3f, %.3f], declination:%.1f)", + (double)getEulerYaw(_R_to_earth), (double)yaw_new, + (double)mag_bias(0), (double)mag_bias(1), (double)mag_bias(2), + (double)declination); - if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) || ((_params.mag_fusion_type == MagFuseType::INDOOR) && heading_required_for_navigation)) { + // update quaternion states and corresponding covarainces + resetQuatStateYaw(yaw_new, yaw_new_variance); + _mag_heading_last_declination = declination; - // rotate the magnetometer measurements into earth frame using a zero yaw angle - const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth); + _time_last_heading_fuse = _time_delayed_us; - // the angle of the projection onto the horizontal gives the yaw angle - const Vector3f mag_earth_pred = R_to_earth * mag_init; - - // calculate the observed yaw angle and yaw variance - float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination(); - float yaw_new_variance = sq(fmaxf(_params.mag_heading_noise, 1.e-2f)); - - ECL_INFO("reset mag heading %.3f -> %.3f rad (declination %.1f)", (double)getEulerYaw(_R_to_earth), (double)yaw_new, (double)getMagDeclination()); - - // update quaternion states and corresponding covarainces - resetQuatStateYaw(yaw_new, yaw_new_variance); - - // set the earth magnetic field states using the updated rotation - _state.mag_I = _R_to_earth * mag_init; - - resetMagCov(); - - // record the time for the magnetic field alignment event - _flt_mag_align_start_time = _time_delayed_us; - - return true; - } - - return false; + _mag_heading_innov_lpf.reset(0.f); + _control_status.flags.mag_heading_consistent = true; } float Ekf::getMagDeclination() @@ -535,72 +385,12 @@ float Ekf::getMagDeclination() } else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) { // use parameter value until GPS is available, then use value returned by geo library - if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) { + if (PX4_ISFINITE(_mag_declination_gps)) { return _mag_declination_gps; - - } else { - return math::radians(_params.mag_declination_deg); } - - } else { - // always use the parameter value - return math::radians(_params.mag_declination_deg); } + + // otherwise use the parameter value + return math::radians(_params.mag_declination_deg); } -void Ekf::stopMagFusion() -{ - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - ECL_INFO("stopping all mag fusion"); - stopMag3DFusion(); - stopMagHdgFusion(); - clearMagCov(); - } -} - -void Ekf::stopMag3DFusion() -{ - // save covariance data for re-use if currently doing 3-axis fusion - if (_control_status.flags.mag_3D) { - saveMagCovData(); - - _control_status.flags.mag_3D = false; - _control_status.flags.mag_dec = 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; - } -} - -void Ekf::stopMagHdgFusion() -{ - if (_control_status.flags.mag_hdg) { - _control_status.flags.mag_hdg = false; - - _fault_status.flags.bad_hdg = false; - } -} - -void Ekf::startMagHdgFusion() -{ - if (!_control_status.flags.mag_hdg) { - stopMag3DFusion(); - ECL_INFO("starting mag heading fusion"); - _control_status.flags.mag_hdg = true; - } -} - -void Ekf::startMag3DFusion() -{ - if (!_control_status.flags.mag_3D) { - - stopMagHdgFusion(); - - zeroMagCov(); - loadMagCovData(); - _control_status.flags.mag_3D = true; - } -} diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 098373ef68..f033750d11 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -54,19 +54,17 @@ 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 = sq(fmaxf(_params.mag_noise, 0.0f)); + 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 - const char* numerical_error_covariance_reset_string = "numerical error - covariance reset"; + const char *numerical_error_covariance_reset_string = "numerical error - covariance reset"; Vector3f mag_innov; Vector3f innov_var; // Observation jacobian and Kalman gain vectors - SparseVector24f<0,1,2,3,16,17,18,19,20,21> Hfusion; - Vector24f H; - const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + VectorState H; + const VectorState state_vector = getStateAtFusionHorizonAsVector(); sym::ComputeMagInnovInnovVarAndHx(state_vector, P, mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H); - Hfusion = H; innov_var.copyTo(aid_src_mag.innovation_variance); @@ -75,7 +73,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_x = true; // we need to re-initialise covariances and abort this fusion step - resetMagRelatedCovariances(); + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + ECL_ERR("magX %s", numerical_error_covariance_reset_string); return false; } @@ -88,7 +91,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_y = true; // we need to re-initialise covariances and abort this fusion step - resetMagRelatedCovariances(); + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + ECL_ERR("magY %s", numerical_error_covariance_reset_string); return false; } @@ -100,7 +108,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_z = true; // we need to re-initialise covariances and abort this fusion step - resetMagRelatedCovariances(); + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + ECL_ERR("magZ %s", numerical_error_covariance_reset_string); return false; } @@ -118,8 +131,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo aid_src_mag.innovation[i] = mag_innov(i); } - aid_src_mag.fusion_enabled = _control_status.flags.mag_3D && update_all_states; - // do not use the synthesized measurement for the magnetomter Z component for 3D fusion if (_control_status.flags.synthetic_mag_z) { aid_src_mag.innovation[2] = 0.0f; @@ -149,7 +160,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo } 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); - Hfusion = 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); @@ -159,7 +169,12 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_y = true; // we need to re-initialise covariances and abort this fusion step - resetMagRelatedCovariances(); + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + ECL_ERR("magY %s", numerical_error_covariance_reset_string); return false; } @@ -173,7 +188,6 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo // 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); - Hfusion = 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); @@ -183,13 +197,18 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_z = true; // we need to re-initialise covariances and abort this fusion step - resetMagRelatedCovariances(); + if (update_all_states) { + resetQuatCov(_params.mag_heading_noise); + } + + resetMagCov(); + ECL_ERR("magZ %s", numerical_error_covariance_reset_string); return false; } } - Vector24f Kfusion = P * Hfusion / aid_src_mag.innovation_variance[index]; + VectorState Kfusion = P * H / aid_src_mag.innovation_variance[index]; if (!update_all_states) { for (unsigned row = 0; row <= 15; row++) { @@ -214,7 +233,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo _fault_status.flags.bad_mag_y = !fused[1]; _fault_status.flags.bad_mag_z = !fused[2]; - if (fused[0] && fused[1] && fused[2]) { + if (fused[0] && fused[1] && (fused[2] || _control_status.flags.synthetic_mag_z)) { aid_src_mag.fused = true; aid_src_mag.time_last_fuse = _time_delayed_us; @@ -229,121 +248,20 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo return false; } -// update quaternion states and covariances using the yaw innovation and yaw observation variance -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status) +bool Ekf::fuseDeclination(float decl_sigma) { - Vector24f H_YAW; - computeYawInnovVarAndH(variance, aid_src_status.innovation_variance, H_YAW); - - return fuseYaw(innovation, variance, aid_src_status, H_YAW); -} - -bool Ekf::fuseYaw(const float innovation, const float variance, estimator_aid_source1d_s& aid_src_status, const Vector24f &H_YAW) -{ - aid_src_status.innovation = innovation; - - float heading_innov_var_inv = 0.f; - - // check if the innovation variance calculation is badly conditioned - if (aid_src_status.innovation_variance >= variance) { - // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.flags.bad_hdg = false; - heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; - - } else { - // 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("yaw fusion numerical error - covariance reset"); + if (!_control_status.flags.mag) { return false; } - // calculate the Kalman gains - // only calculate gains for states we are using - Vector24f Kfusion; - - for (uint8_t row = 0; row < _k_num_states; row++) { - for (uint8_t col = 0; col <= 3; col++) { - Kfusion(row) += P(row, col) * H_YAW(col); - } - - Kfusion(row) *= heading_innov_var_inv; - } - - // 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); - - // set the magnetometer unhealthy if the test fails - if (aid_src_status.innovation_rejected) { - _innov_check_fail_status.flags.reject_yaw = true; - - // if we are in air we don't want to fuse the measurement - // 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) - ) { - // 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)); - 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 - // being stuck on a previous bad estimate - resetZDeltaAngBiasCov(); - - } else { - return false; - } - - } else { - _innov_check_fail_status.flags.reject_yaw = false; - } - - if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { - - _time_last_heading_fuse = _time_delayed_us; - - aid_src_status.time_last_fuse = _time_delayed_us; - aid_src_status.fused = true; - - _fault_status.flags.bad_hdg = false; - - return true; - } - - // otherwise - aid_src_status.fused = false; - _fault_status.flags.bad_hdg = true; - return false; -} - -void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vector24f &H_YAW) const -{ - if (shouldUse321RotationSequence(_R_to_earth)) { - sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - - } else { - sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); - } -} - -bool Ekf::fuseDeclination(float decl_sigma) -{ // observation variance (rad**2) const float R_DECL = sq(decl_sigma); - Vector24f H; + VectorState H; float decl_pred; float innovation_variance; + // TODO: review getMagDeclination() usage, use mag_I, _mag_declination_gps, or parameter? sym::ComputeMagDeclinationPredInnovVarAndH(getStateAtFusionHorizonAsVector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance, &H); const float innovation = wrap_pi(decl_pred - getMagDeclination()); @@ -353,10 +271,8 @@ bool Ekf::fuseDeclination(float decl_sigma) return false; } - SparseVector24f<16,17> Hfusion(H); - // Calculate the Kalman gains - Vector24f Kfusion = P * Hfusion / innovation_variance; + VectorState Kfusion = P * H / innovation_variance; const bool is_fused = measurementUpdate(Kfusion, innovation_variance, innovation); @@ -371,26 +287,29 @@ bool Ekf::fuseDeclination(float decl_sigma) 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 - // set to 50% of the horizontal strength from geo tables if location is known - float decl_reference; + float decl_reference = NAN; float h_field_min = 0.001f; - if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) { - // use parameter value until GPS is available, then use value returned by geo library - if (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps)) { - decl_reference = _mag_declination_gps; - h_field_min = fmaxf(h_field_min, 0.5f * _mag_strength_gps * cosf(_mag_inclination_gps)); + 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; - } else { - decl_reference = math::radians(_params.mag_declination_deg); - } + // 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 { - // always use the parameter value + } else if (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL) { + // 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 @@ -404,9 +323,8 @@ void Ekf::limitDeclination() } else { // too small to scale radially so set to expected value - const float mag_declination = getMagDeclination(); - _state.mag_I(0) = 2.0f * h_field_min * cosf(mag_declination); - _state.mag_I(1) = 2.0f * h_field_min * sinf(mag_declination); + _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; @@ -426,6 +344,14 @@ void Ekf::limitDeclination() _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) diff --git a/src/modules/ekf2/EKF/mag_heading_control.cpp b/src/modules/ekf2/EKF/mag_heading_control.cpp new file mode 100644 index 0000000000..612474937e --- /dev/null +++ b/src/modules/ekf2/EKF/mag_heading_control.cpp @@ -0,0 +1,204 @@ +/**************************************************************************** + * + * 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/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index a061e2c7f1..88811ee9c2 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -47,7 +47,6 @@ #include #include "python/ekf_derivation/generated/compute_flow_xy_innov_var_and_hx.h" #include "python/ekf_derivation/generated/compute_flow_y_innov_var_and_h.h" -#include "utils.hpp" void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) { @@ -77,22 +76,30 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; + + const VectorState state_vector = getStateAtFusionHorizonAsVector(); + + 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() { - _aid_src_optical_flow.fusion_enabled = true; - 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 Vector24f state_vector = getStateAtFusionHorizonAsVector(); + const VectorState state_vector = getStateAtFusionHorizonAsVector(); Vector2f innov_var; - Vector24f H; + VectorState H; sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); innov_var.copyTo(_aid_src_optical_flow.innovation_variance); @@ -139,8 +146,7 @@ void Ekf::fuseOptFlow() } } - SparseVector24f<0,1,2,3,4,5,6> Hfusion(H); - Vector24f Kfusion = P * Hfusion / _aid_src_optical_flow.innovation_variance[index]; + VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; if (measurementUpdate(Kfusion, _aid_src_optical_flow.innovation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true; diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index ae77e228cf..a8207db178 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Accumulate autopilot gyro data across the same time interval as the flow sensor const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); - if (_delta_time_of < 0.1f) { + if (_delta_time_of < 0.2f) { _imu_del_ang_of += delta_angle; _delta_time_of += imu_delayed.delta_ang_dt; @@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } if (_flow_data_ready) { - const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); + int32_t min_quality = _params.flow_qual_min; + if (!_control_status.flags.in_air) { + min_quality = _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_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); @@ -204,7 +209,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) if (_time_delayed_us > (_flow_sample_delayed.time_us - uint32_t(1e6f * _flow_sample_delayed.dt) / 2)) { // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data - if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + if (isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)10e6)) { fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } diff --git a/src/modules/ekf2/EKF/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor.cpp index bdb8077ea0..32bf4fe8ad 100644 --- a/src/modules/ekf2/EKF/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor.cpp @@ -237,11 +237,15 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector // rotate the relative velocity into earth frame _vel_imu_rel_body_ned = _R_to_earth_now * vel_imu_rel_body; } + + // update auxiliary yaw estimate + const Vector3f unbiased_delta_angle = delta_angle - delta_angle_bias_scaled; + const float spin_del_ang_D = unbiased_delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); + _unaided_yaw = matrix::wrap_pi(_unaided_yaw + spin_del_ang_D); } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) + 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) { diff --git a/src/modules/ekf2/EKF/output_predictor.h b/src/modules/ekf2/EKF/output_predictor.h index 5eb3b44e2f..ee5cc0f710 100644 --- a/src/modules/ekf2/EKF/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor.h @@ -68,8 +68,7 @@ public: const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); void correctOutputStates(const uint64_t time_delayed_us, - 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::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); @@ -95,6 +94,9 @@ public: const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } + // get a yaw value solely based on bias-removed gyro integration + float getUnaidedYaw() const { return _unaided_yaw; } + // get the velocity of the body frame origin in local NED earth frame matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } @@ -185,6 +187,8 @@ private: matrix::Vector3f _imu_pos_body{}; ///< xyz position of IMU in body frame (m) + float _unaided_yaw{}; + // output complementary filter tuning float _vel_tau{0.25f}; ///< velocity state correction time constant (1/sec) float _pos_tau{0.25f}; ///< position state correction time constant (1/sec) diff --git a/src/modules/ekf2/EKF/position_bias_estimator.hpp b/src/modules/ekf2/EKF/position_bias_estimator.hpp index ddcbe144ce..0e997e2d28 100644 --- a/src/modules/ekf2/EKF/position_bias_estimator.hpp +++ b/src/modules/ekf2/EKF/position_bias_estimator.hpp @@ -35,7 +35,8 @@ * @file position_bias_estimator.hpp */ -#pragma once +#ifndef EKF_POSITION_BIAS_ESTIMATOR_HPP +#define EKF_POSITION_BIAS_ESTIMATOR_HPP #include "bias_estimator.hpp" @@ -111,3 +112,5 @@ private: bool _is_sensor_fusion_active{false}; // TODO: replace by const ref and remove setter when migrating _control_status.flags from union to bool }; + +#endif // !EKF_POSITION_BIAS_ESTIMATOR_HPP diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py index 5568fe27d8..d23b9a33e4 100755 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- """ Copyright (c) 2022 PX4 Development Team @@ -33,6 +33,9 @@ File: derivation.py Description: """ +import symforce +symforce.set_epsilon_to_symbol() + import symforce.symbolic as sf from derivation_utils import * @@ -47,12 +50,12 @@ class State: px = 7 py = 8 pz = 9 - d_ang_bx = 10 - d_ang_by = 11 - d_ang_bz = 12 - d_vel_bx = 13 - d_vel_by = 14 - d_vel_bz = 15 + gyro_bx = 10 + gyro_by = 11 + gyro_bz = 12 + accel_bx = 13 + accel_by = 14 + accel_bz = 15 ix = 16 iy = 17 iz = 18 @@ -79,17 +82,20 @@ def predict_covariance( state: VState, P: MState, d_vel: sf.V3, + d_vel_dt: sf.Scalar, d_vel_var: sf.V3, d_ang: sf.V3, - d_ang_var: sf.Scalar, - dt: sf.Scalar + d_ang_dt: sf.Scalar, + d_ang_var: sf.Scalar ): g = sf.Symbol("g") # does not appear in the jacobians - d_vel_b = sf.V3(state[State.d_vel_bx], state[State.d_vel_by], state[State.d_vel_bz]) + accel_b = sf.V3(state[State.accel_bx], state[State.accel_by], state[State.accel_bz]) + d_vel_b = accel_b * d_vel_dt d_vel_true = d_vel - d_vel_b - d_ang_b = sf.V3(state[State.d_ang_bx], state[State.d_ang_by], state[State.d_ang_bz]) + gyro_b = sf.V3(state[State.gyro_bx], state[State.gyro_by], state[State.gyro_bz]) + d_ang_b = gyro_b * d_ang_dt d_ang_true = d_ang - d_ang_b q = state_to_quat(state) @@ -97,12 +103,12 @@ def predict_covariance( v = sf.V3(state[State.vx], state[State.vy], state[State.vz]) p = sf.V3(state[State.px], state[State.py], state[State.pz]) - q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) - v_new = v + R_to_earth * d_vel_true + sf.V3(0 ,0 ,g) * dt - p_new = p + v * dt + q_new = q * sf.Quaternion(sf.V3(0.5 * d_ang_true[0], 0.5 * d_ang_true[1], 0.5 * d_ang_true[2]), 1) + v_new = v + R_to_earth * d_vel_true + sf.V3(0, 0, g) * d_vel_dt + p_new = p + v * d_vel_dt # Predicted state vector at time t + dt - state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.d_ang_bx:State.n_states])]]) + state_new = VState.block_matrix([[sf.V4(q_new.w, q_new.x, q_new.y, q_new.z)], [v_new], [p_new], [sf.Matrix(state[State.gyro_bx:State.n_states])]]) # State propagation jacobian A = state_new.jacobian(state) @@ -112,8 +118,8 @@ def predict_covariance( var_u = sf.Matrix.diag([d_vel_var[0], d_vel_var[1], d_vel_var[2], d_ang_var, d_ang_var, d_ang_var]) P_new = A * P * A.T + G * var_u * G.T - # Generate the equations for the lower triangular matrix and the diagonal only - # Since the matrix is symmetric, the upper triangle does not need to be derived + # Generate the equations for the upper triangular matrix and the diagonal only + # Since the matrix is symmetric, the lower triangle does not need to be derived # and can simply be copied in the implementation for index in range(State.n_states): for j in range(State.n_states): @@ -413,16 +419,17 @@ def predict_drag( cd: sf.Scalar, cm: sf.Scalar, epsilon: sf.Scalar - ): +) -> (sf.Scalar): R_to_body = state_to_rot3(state).inverse() vel_rel = sf.V3(state[State.vx] - state[State.wx], state[State.vy] - state[State.wy], state[State.vz]) vel_rel_body = R_to_body * vel_rel + vel_rel_body_xy = sf.V2(vel_rel_body[0], vel_rel_body[1]) - bluff_body_drag = -0.5 * rho * cd * sf.V2(vel_rel_body) * vel_rel_body.norm(epsilon=epsilon) - momentum_drag = -cm * sf.V2(vel_rel_body) + bluff_body_drag = -0.5 * rho * cd * vel_rel_body_xy * vel_rel_body.norm(epsilon=epsilon) + momentum_drag = -cm * vel_rel_body_xy return bluff_body_drag + momentum_drag @@ -435,7 +442,7 @@ def compute_drag_x_innov_var_and_k( cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, VState): meas_pred = predict_drag(state, rho, cd, cm, epsilon) Hx = sf.V1(meas_pred[0]).jacobian(state) @@ -455,7 +462,7 @@ def compute_drag_y_innov_var_and_k( cm: sf.Scalar, R: sf.Scalar, epsilon: sf.Scalar -) -> (sf.Scalar, sf.Scalar, VState): +) -> (sf.Scalar, VState): meas_pred = predict_drag(state, rho, cd, cm, epsilon) Hy = sf.V1(meas_pred[1]).jacobian(state) @@ -496,6 +503,38 @@ def compute_gravity_innov_var_and_k_and_h( return (innov, innov_var, K[0], K[1], K[2]) +def quat_var_to_rot_var( + state: VState, + P: MState, + epsilon: sf.Scalar +): + J = sf.V3(state_to_rot3(state).to_tangent(epsilon=epsilon)).jacobian(state) + rot_cov = J * P * J.T + return sf.V3(rot_cov[0, 0], rot_cov[1, 1], rot_cov[2, 2]) + +def rot_var_ned_to_lower_triangular_quat_cov( + state: VState, + rot_var_ned: sf.V3 +): + # This function converts an attitude variance defined by a 3D vector in NED frame + # into a 4x4 covariance matrix representing the uncertainty on each of the 4 quaternion parameters + # Note: the resulting quaternion uncertainty is defined as a perturbation + # at the tip of the quaternion (i.e.:body-frame uncertainty) + q = sf.V4(state[State.qw], state[State.qx], state[State.qy], state[State.qz]) + attitude = state_to_rot3(state) + J = q.jacobian(attitude) + + # Convert uncertainties from NED to body frame + rot_cov_ned = sf.M33.diag(rot_var_ned) + adjoint = attitude.to_rotation_matrix() # the adjoint of SO(3) is simply the rotation matrix itself + rot_cov_body = adjoint.T * rot_cov_ned * adjoint + + # Convert yaw (body) to quaternion parameter uncertainty + q_var = J * rot_cov_body * J.T + + # Generate lower trangle only and copy it to the upper part in implementation (produces less code) + return q_var.lower_triangle() + print("Derive EKF2 equations...") generate_px4_function(compute_airspeed_innov_and_innov_var, output_names=["innov", "innov_var"]) generate_px4_function(compute_airspeed_h_and_k, output_names=["H", "K"]) @@ -517,3 +556,14 @@ generate_px4_function(compute_gnss_yaw_pred_innov_var_and_h, output_names=["meas generate_px4_function(compute_drag_x_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_drag_y_innov_var_and_k, output_names=["innov_var", "K"]) generate_px4_function(compute_gravity_innov_var_and_k_and_h, output_names=["innov", "innov_var", "Kx", "Ky", "Kz"]) +generate_px4_function(quat_var_to_rot_var, output_names=["rot_var"]) +generate_px4_function(rot_var_ned_to_lower_triangular_quat_cov, output_names=["q_cov_lower_triangle"]) + +generate_px4_state({"quat_nominal": sf.V4, + "vel": sf.V3, + "pos": sf.V3, + "gyro_bias": sf.V3, + "accel_bias": sf.V3, + "mag_I": sf.V3, + "mag_B": sf.V3, + "wind_vel": sf.V2}) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py index ca2987e00f..f2b00ac0da 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py +++ b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_utils.py @@ -87,3 +87,32 @@ def generate_python_function(function_name, output_names): metadata = codegen.generate_function( output_dir="generated", skip_directory_nesting=True) + +def generate_px4_state(states): + print("Generate EKF state definition") + filename = "state.h" + f = open(f"./generated/{filename}", "w") + header = ["// --------------------------------------------------\n", + "// This file was autogenerated, do NOT modify by hand\n", + "// --------------------------------------------------\n", + "\n#ifndef EKF_STATE_H", + "\n#define EKF_STATE_H\n\n", + "namespace estimator\n{\n"] + f.writelines(header) + + f.write("struct IdxDof { unsigned idx; unsigned dof; };\n"); + + f.write("namespace State {\n"); + + start_index = 0 + for (state_name, state_type) in states.items(): + tangent_dim = state_type.tangent_dim() + f.write(f"\tstatic constexpr IdxDof {state_name}{{{start_index}, {tangent_dim}}};\n") + start_index += tangent_dim + + f.write(f"\tstatic constexpr uint8_t size{{{start_index}}};\n") + f.write("};\n") # namespace State + f.write("}\n") # namespace estimator + f.write("#endif // !EKF_STATE_H\n") + f.close() + print(f" |- {filename}") diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py b/src/modules/ekf2/EKF/python/ekf_derivation/derivation_yaw_estimator.py old mode 100644 new mode 100755 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 42d82b0e78..661c1f16ea 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 4add44e02c..02546feadd 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h index 1ca82c4021..8d1296a91b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_x_innov_var_and_k.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h index 6eb4b77b42..cb817b6699 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_drag_y_innov_var_and_k.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 964b7751ac..ffd18295c7 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 fa96926e2d..43dba35480 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 286312543b..4ef4c91dea 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 a68c1a5210..cb9e308540 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 3b7c2f98f4..1c63223f5d 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -37,6 +37,9 @@ void ComputeMagInnovInnovVarAndHx(const matrix::Matrix& state, matrix::Matrix* const Hx = nullptr) { // Total ops: 471 + // Unused inputs + (void)epsilon; + // Input arrays // Intermediate terms (49) 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 330532de8a..8eaac986f1 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -32,6 +32,9 @@ void ComputeMagYInnovVarAndH(const matrix::Matrix& state, matrix::Matrix* const H = nullptr) { // Total ops: 160 + // Unused inputs + (void)epsilon; + // Input arrays // Intermediate terms (12) 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 fcff6cc92e..906f28119e 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -32,6 +32,9 @@ void ComputeMagZInnovVarAndH(const matrix::Matrix& state, matrix::Matrix* const H = nullptr) { // Total ops: 160 + // Unused inputs + (void)epsilon; + // Input arrays // Intermediate terms (12) 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 1ffb4ab457..31b31db236 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 2126bf1f23..6c13cf39ac 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h index 45946c4ebc..9a873898fd 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h index 4caba10bd7..99e130f676 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h_alternate.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h index b3aef591f4..e7dad2479b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h index b2bbca9c78..0e588fdecf 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h_alternate.h @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- 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 84526a15d7..ee0ee8e1d7 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 @@ -1,6 +1,6 @@ // ----------------------------------------------------------------------------- // This file was autogenerated by symforce from template: -// backends/cpp/templates/function/FUNCTION.h.jinja +// function/FUNCTION.h.jinja // Do NOT modify by hand. // ----------------------------------------------------------------------------- @@ -19,10 +19,11 @@ namespace sym { * state: Matrix24_1 * P: Matrix24_24 * d_vel: Matrix31 + * d_vel_dt: Scalar * d_vel_var: Matrix31 * d_ang: Matrix31 + * d_ang_dt: Scalar * d_ang_var: Scalar - * dt: Scalar * * Outputs: * P_new: Matrix24_24 @@ -30,291 +31,294 @@ namespace sym { template void PredictCovariance(const matrix::Matrix& state, const matrix::Matrix& P, - const matrix::Matrix& d_vel, + const matrix::Matrix& d_vel, const Scalar d_vel_dt, const matrix::Matrix& d_vel_var, - const matrix::Matrix& d_ang, const Scalar d_ang_var, - const Scalar dt, matrix::Matrix* const P_new = nullptr) { - // Total ops: 2876 + const matrix::Matrix& d_ang, const Scalar d_ang_dt, + const Scalar d_ang_var, + matrix::Matrix* const P_new = nullptr) { + // Total ops: 2882 // Input arrays - // Intermediate terms (171) - const Scalar _tmp0 = Scalar(0.5) * d_ang(2, 0); - const Scalar _tmp1 = Scalar(0.5) * state(12, 0); - const Scalar _tmp2 = -_tmp0 + _tmp1; - const Scalar _tmp3 = Scalar(0.5) * d_ang(1, 0); - const Scalar _tmp4 = Scalar(0.5) * state(11, 0); - const Scalar _tmp5 = -_tmp3 + _tmp4; - const Scalar _tmp6 = Scalar(0.5) * d_ang(0, 0); - const Scalar _tmp7 = Scalar(0.5) * state(10, 0); - const Scalar _tmp8 = -_tmp6 + _tmp7; - const Scalar _tmp9 = Scalar(0.5) * state(3, 0); - const Scalar _tmp10 = Scalar(0.5) * state(2, 0); - const Scalar _tmp11 = Scalar(0.5) * state(1, 0); - const Scalar _tmp12 = P(0, 11) + P(1, 11) * _tmp8 + P(10, 11) * _tmp11 + P(11, 11) * _tmp10 + - P(12, 11) * _tmp9 + P(2, 11) * _tmp5 + P(3, 11) * _tmp2; - const Scalar _tmp13 = P(0, 10) + P(1, 10) * _tmp8 + P(10, 10) * _tmp11 + P(11, 10) * _tmp10 + - P(12, 10) * _tmp9 + P(2, 10) * _tmp5 + P(3, 10) * _tmp2; - const Scalar _tmp14 = P(0, 12) + P(1, 12) * _tmp8 + P(10, 12) * _tmp11 + P(11, 12) * _tmp10 + - P(12, 12) * _tmp9 + P(2, 12) * _tmp5 + P(3, 12) * _tmp2; - const Scalar _tmp15 = P(0, 2) + P(1, 2) * _tmp8 + P(10, 2) * _tmp11 + P(11, 2) * _tmp10 + - P(12, 2) * _tmp9 + P(2, 2) * _tmp5 + P(3, 2) * _tmp2; - const Scalar _tmp16 = Scalar(0.5) * P(11, 3); - const Scalar _tmp17 = P(0, 3) + P(1, 3) * _tmp8 + P(10, 3) * _tmp11 + P(12, 3) * _tmp9 + - P(2, 3) * _tmp5 + P(3, 3) * _tmp2 + _tmp16 * state(2, 0); - const Scalar _tmp18 = Scalar(0.5) * P(11, 1); - const Scalar _tmp19 = P(0, 1) + P(1, 1) * _tmp8 + P(10, 1) * _tmp11 + P(12, 1) * _tmp9 + - P(2, 1) * _tmp5 + P(3, 1) * _tmp2 + _tmp18 * state(2, 0); - const Scalar _tmp20 = std::pow(state(3, 0), Scalar(2)); + // Intermediate terms (173) + const Scalar _tmp0 = Scalar(0.5) * d_ang(1, 0); + const Scalar _tmp1 = Scalar(0.5) * d_ang_dt; + const Scalar _tmp2 = _tmp1 * state(11, 0); + const Scalar _tmp3 = -_tmp0 + _tmp2; + const Scalar _tmp4 = Scalar(0.5) * d_ang(2, 0); + const Scalar _tmp5 = _tmp1 * state(12, 0); + const Scalar _tmp6 = -_tmp4 + _tmp5; + const Scalar _tmp7 = Scalar(0.5) * d_ang(0, 0); + const Scalar _tmp8 = _tmp1 * state(10, 0); + const Scalar _tmp9 = -_tmp7 + _tmp8; + const Scalar _tmp10 = _tmp1 * state(1, 0); + const Scalar _tmp11 = _tmp1 * state(3, 0); + const Scalar _tmp12 = _tmp1 * state(2, 0); + const Scalar _tmp13 = P(0, 10) + P(1, 10) * _tmp9 + P(10, 10) * _tmp10 + P(11, 10) * _tmp12 + + P(12, 10) * _tmp11 + P(2, 10) * _tmp3 + P(3, 10) * _tmp6; + const Scalar _tmp14 = P(0, 11) + P(1, 11) * _tmp9 + P(10, 11) * _tmp10 + P(11, 11) * _tmp12 + + P(12, 11) * _tmp11 + P(2, 11) * _tmp3 + P(3, 11) * _tmp6; + const Scalar _tmp15 = P(0, 12) + P(1, 12) * _tmp9 + P(10, 12) * _tmp10 + P(11, 12) * _tmp12 + + P(12, 12) * _tmp11 + P(2, 12) * _tmp3 + P(3, 12) * _tmp6; + const Scalar _tmp16 = P(0, 2) + P(1, 2) * _tmp9 + P(10, 2) * _tmp10 + P(11, 2) * _tmp12 + + P(12, 2) * _tmp11 + P(2, 2) * _tmp3 + P(3, 2) * _tmp6; + const Scalar _tmp17 = P(0, 3) + P(1, 3) * _tmp9 + P(10, 3) * _tmp10 + P(11, 3) * _tmp12 + + P(12, 3) * _tmp11 + P(2, 3) * _tmp3 + P(3, 3) * _tmp6; + const Scalar _tmp18 = P(11, 1) * _tmp1; + const Scalar _tmp19 = P(0, 1) + P(1, 1) * _tmp9 + P(10, 1) * _tmp10 + P(12, 1) * _tmp11 + + P(2, 1) * _tmp3 + P(3, 1) * _tmp6 + _tmp18 * state(2, 0); + const Scalar _tmp20 = std::pow(state(2, 0), Scalar(2)); const Scalar _tmp21 = Scalar(0.25) * d_ang_var; const Scalar _tmp22 = _tmp20 * _tmp21; - const Scalar _tmp23 = Scalar(0.5) * P(11, 0); - const Scalar _tmp24 = P(0, 0) + P(1, 0) * _tmp8 + P(10, 0) * _tmp11 + P(12, 0) * _tmp9 + - P(2, 0) * _tmp5 + P(3, 0) * _tmp2 + _tmp23 * state(2, 0); - const Scalar _tmp25 = std::pow(state(2, 0), Scalar(2)); - const Scalar _tmp26 = _tmp21 * _tmp25; - const Scalar _tmp27 = std::pow(state(1, 0), Scalar(2)); - const Scalar _tmp28 = _tmp21 * _tmp27; - const Scalar _tmp29 = _tmp26 + _tmp28; - const Scalar _tmp30 = Scalar(0.5) * state(0, 0); - const Scalar _tmp31 = _tmp21 * state(0, 0); - const Scalar _tmp32 = _tmp0 - _tmp1; - const Scalar _tmp33 = _tmp6 - _tmp7; - const Scalar _tmp34 = P(0, 12) * _tmp33 + P(1, 12) - P(10, 12) * _tmp30 + P(11, 12) * _tmp9 - - P(12, 12) * _tmp10 + P(2, 12) * _tmp32 + P(3, 12) * _tmp5; - const Scalar _tmp35 = P(0, 11) * _tmp33 + P(1, 11) - P(10, 11) * _tmp30 + P(11, 11) * _tmp9 - - P(12, 11) * _tmp10 + P(2, 11) * _tmp32 + P(3, 11) * _tmp5; - const Scalar _tmp36 = P(0, 10) * _tmp33 + P(1, 10) - P(10, 10) * _tmp30 + P(11, 10) * _tmp9 - - P(12, 10) * _tmp10 + P(2, 10) * _tmp32 + P(3, 10) * _tmp5; - const Scalar _tmp37 = P(0, 3) * _tmp33 + P(1, 3) - P(10, 3) * _tmp30 + P(11, 3) * _tmp9 - - P(12, 3) * _tmp10 + P(2, 3) * _tmp32 + P(3, 3) * _tmp5; - const Scalar _tmp38 = P(0, 2) * _tmp33 + P(1, 2) - P(10, 2) * _tmp30 + P(11, 2) * _tmp9 - - P(12, 2) * _tmp10 + P(2, 2) * _tmp32 + P(3, 2) * _tmp5; - const Scalar _tmp39 = P(0, 0) * _tmp33 + P(1, 0) - P(10, 0) * _tmp30 - P(12, 0) * _tmp10 + - P(2, 0) * _tmp32 + P(3, 0) * _tmp5 + _tmp23 * state(3, 0); - const Scalar _tmp40 = P(0, 1) * _tmp33 + P(1, 1) - P(10, 1) * _tmp30 - P(12, 1) * _tmp10 + - P(2, 1) * _tmp32 + P(3, 1) * _tmp5 + _tmp18 * state(3, 0); - const Scalar _tmp41 = _tmp21 * std::pow(state(0, 0), Scalar(2)); - const Scalar _tmp42 = _tmp22 + _tmp41; - const Scalar _tmp43 = _tmp3 - _tmp4; - const Scalar _tmp44 = P(0, 12) * _tmp43 + P(1, 12) * _tmp2 - P(10, 12) * _tmp9 - - P(11, 12) * _tmp30 + P(12, 12) * _tmp11 + P(2, 12) + P(3, 12) * _tmp33; - const Scalar _tmp45 = P(0, 11) * _tmp43 + P(1, 11) * _tmp2 - P(10, 11) * _tmp9 - - P(11, 11) * _tmp30 + P(12, 11) * _tmp11 + P(2, 11) + P(3, 11) * _tmp33; - const Scalar _tmp46 = P(0, 10) * _tmp43 + P(1, 10) * _tmp2 - P(10, 10) * _tmp9 - - P(11, 10) * _tmp30 + P(12, 10) * _tmp11 + P(2, 10) + P(3, 10) * _tmp33; - const Scalar _tmp47 = P(0, 0) * _tmp43 + P(1, 0) * _tmp2 - P(10, 0) * _tmp9 + P(12, 0) * _tmp11 + - P(2, 0) + P(3, 0) * _tmp33 - _tmp23 * state(0, 0); - const Scalar _tmp48 = P(0, 1) * _tmp43 + P(1, 1) * _tmp2 - P(10, 1) * _tmp9 + P(12, 1) * _tmp11 + - P(2, 1) + P(3, 1) * _tmp33 - _tmp18 * state(0, 0); - const Scalar _tmp49 = P(0, 3) * _tmp43 + P(1, 3) * _tmp2 - P(10, 3) * _tmp9 + P(12, 3) * _tmp11 + - P(2, 3) + P(3, 3) * _tmp33 - _tmp16 * state(0, 0); - const Scalar _tmp50 = P(0, 2) * _tmp43 + P(1, 2) * _tmp2 - P(10, 2) * _tmp9 - P(11, 2) * _tmp30 + - P(12, 2) * _tmp11 + P(2, 2) + P(3, 2) * _tmp33; - const Scalar _tmp51 = _tmp21 * state(3, 0); - const Scalar _tmp52 = P(0, 12) * _tmp32 + P(1, 12) * _tmp43 + P(10, 12) * _tmp10 - - P(11, 12) * _tmp11 - P(12, 12) * _tmp30 + P(2, 12) * _tmp8 + P(3, 12); - const Scalar _tmp53 = P(0, 11) * _tmp32 + P(1, 11) * _tmp43 + P(10, 11) * _tmp10 - - P(11, 11) * _tmp11 - P(12, 11) * _tmp30 + P(2, 11) * _tmp8 + P(3, 11); - const Scalar _tmp54 = P(0, 10) * _tmp32 + P(1, 10) * _tmp43 + P(10, 10) * _tmp10 - - P(11, 10) * _tmp11 - P(12, 10) * _tmp30 + P(2, 10) * _tmp8 + P(3, 10); - const Scalar _tmp55 = P(0, 2) * _tmp32 + P(1, 2) * _tmp43 + P(10, 2) * _tmp10 - - P(11, 2) * _tmp11 - P(12, 2) * _tmp30 + P(2, 2) * _tmp8 + P(3, 2); - const Scalar _tmp56 = P(0, 0) * _tmp32 + P(1, 0) * _tmp43 + P(10, 0) * _tmp10 - - P(12, 0) * _tmp30 + P(2, 0) * _tmp8 + P(3, 0) - _tmp23 * state(1, 0); - const Scalar _tmp57 = P(0, 1) * _tmp32 + P(1, 1) * _tmp43 + P(10, 1) * _tmp10 - - P(12, 1) * _tmp30 + P(2, 1) * _tmp8 + P(3, 1) - _tmp18 * state(1, 0); - const Scalar _tmp58 = P(0, 3) * _tmp32 + P(1, 3) * _tmp43 + P(10, 3) * _tmp10 - - P(11, 3) * _tmp11 - P(12, 3) * _tmp30 + P(2, 3) * _tmp8 + P(3, 3); - const Scalar _tmp59 = d_vel(1, 0) - state(14, 0); - const Scalar _tmp60 = 2 * state(2, 0); - const Scalar _tmp61 = _tmp59 * _tmp60; - const Scalar _tmp62 = d_vel(2, 0) - state(15, 0); - const Scalar _tmp63 = 2 * state(3, 0); - const Scalar _tmp64 = _tmp62 * _tmp63; - const Scalar _tmp65 = _tmp61 + _tmp64; - const Scalar _tmp66 = _tmp60 * _tmp62; - const Scalar _tmp67 = _tmp59 * _tmp63; - const Scalar _tmp68 = _tmp66 - _tmp67; - const Scalar _tmp69 = 2 * state(1, 0); - const Scalar _tmp70 = _tmp62 * _tmp69; - const Scalar _tmp71 = 2 * state(0, 0); - const Scalar _tmp72 = _tmp59 * _tmp71; - const Scalar _tmp73 = d_vel(0, 0) - state(13, 0); - const Scalar _tmp74 = 4 * state(3, 0); - const Scalar _tmp75 = _tmp70 - _tmp72 - _tmp73 * _tmp74; - const Scalar _tmp76 = _tmp71 * state(2, 0); - const Scalar _tmp77 = _tmp69 * state(3, 0); - const Scalar _tmp78 = _tmp76 + _tmp77; - const Scalar _tmp79 = P(0, 15) + P(1, 15) * _tmp8 + P(10, 15) * _tmp11 + P(11, 15) * _tmp10 + - P(12, 15) * _tmp9 + P(2, 15) * _tmp5 + P(3, 15) * _tmp2; - const Scalar _tmp80 = _tmp71 * state(3, 0); - const Scalar _tmp81 = _tmp69 * state(2, 0); - const Scalar _tmp82 = -_tmp80 + _tmp81; - const Scalar _tmp83 = P(0, 14) + P(1, 14) * _tmp8 + P(10, 14) * _tmp11 + P(11, 14) * _tmp10 + - P(12, 14) * _tmp9 + P(2, 14) * _tmp5 + P(3, 14) * _tmp2; - const Scalar _tmp84 = -2 * _tmp20; - const Scalar _tmp85 = -2 * _tmp25; - const Scalar _tmp86 = _tmp84 + _tmp85 + 1; - const Scalar _tmp87 = P(0, 13) + P(1, 13) * _tmp8 + P(10, 13) * _tmp11 + P(11, 13) * _tmp10 + - P(12, 13) * _tmp9 + P(2, 13) * _tmp5 + P(3, 13) * _tmp2; - const Scalar _tmp88 = _tmp59 * _tmp69; - const Scalar _tmp89 = _tmp62 * _tmp71; - const Scalar _tmp90 = 4 * state(2, 0); - const Scalar _tmp91 = -_tmp73 * _tmp90 + _tmp88 + _tmp89; - const Scalar _tmp92 = Scalar(0.5) * P(11, 4); - const Scalar _tmp93 = P(0, 4) + P(1, 4) * _tmp8 + P(10, 4) * _tmp11 + P(12, 4) * _tmp9 + - P(2, 4) * _tmp5 + P(3, 4) * _tmp2 + _tmp92 * state(2, 0); - const Scalar _tmp94 = P(0, 13) * _tmp33 + P(1, 13) - P(10, 13) * _tmp30 + P(11, 13) * _tmp9 - - P(12, 13) * _tmp10 + P(2, 13) * _tmp32 + P(3, 13) * _tmp5; - const Scalar _tmp95 = P(0, 15) * _tmp33 + P(1, 15) - P(10, 15) * _tmp30 + P(11, 15) * _tmp9 - - P(12, 15) * _tmp10 + P(2, 15) * _tmp32 + P(3, 15) * _tmp5; - const Scalar _tmp96 = P(0, 14) * _tmp33 + P(1, 14) - P(10, 14) * _tmp30 + P(11, 14) * _tmp9 - - P(12, 14) * _tmp10 + P(2, 14) * _tmp32 + P(3, 14) * _tmp5; - const Scalar _tmp97 = Scalar(0.5) * P(10, 4); - const Scalar _tmp98 = P(0, 4) * _tmp33 + P(1, 4) - P(12, 4) * _tmp10 + P(2, 4) * _tmp32 + - P(3, 4) * _tmp5 + _tmp92 * state(3, 0) - _tmp97 * state(0, 0); - const Scalar _tmp99 = P(0, 15) * _tmp43 + P(1, 15) * _tmp2 - P(10, 15) * _tmp9 - - P(11, 15) * _tmp30 + P(12, 15) * _tmp11 + P(2, 15) + P(3, 15) * _tmp33; - const Scalar _tmp100 = P(0, 14) * _tmp43 + P(1, 14) * _tmp2 - P(10, 14) * _tmp9 - - P(11, 14) * _tmp30 + P(12, 14) * _tmp11 + P(2, 14) + P(3, 14) * _tmp33; - const Scalar _tmp101 = P(0, 13) * _tmp43 + P(1, 13) * _tmp2 - P(10, 13) * _tmp9 - - P(11, 13) * _tmp30 + P(12, 13) * _tmp11 + P(2, 13) + P(3, 13) * _tmp33; - const Scalar _tmp102 = P(0, 4) * _tmp43 + P(1, 4) * _tmp2 - P(10, 4) * _tmp9 + P(12, 4) * _tmp11 + - P(2, 4) + P(3, 4) * _tmp33 - _tmp92 * state(0, 0); - const Scalar _tmp103 = P(0, 13) * _tmp32 + P(1, 13) * _tmp43 + P(10, 13) * _tmp10 - - P(11, 13) * _tmp11 - P(12, 13) * _tmp30 + P(2, 13) * _tmp8 + P(3, 13); - const Scalar _tmp104 = P(0, 14) * _tmp32 + P(1, 14) * _tmp43 + P(10, 14) * _tmp10 - - P(11, 14) * _tmp11 - P(12, 14) * _tmp30 + P(2, 14) * _tmp8 + P(3, 14); - const Scalar _tmp105 = P(0, 15) * _tmp32 + P(1, 15) * _tmp43 + P(10, 15) * _tmp10 - - P(11, 15) * _tmp11 - P(12, 15) * _tmp30 + P(2, 15) * _tmp8 + P(3, 15); - const Scalar _tmp106 = P(0, 4) * _tmp32 + P(1, 4) * _tmp43 - P(12, 4) * _tmp30 + P(2, 4) * _tmp8 + - P(3, 4) - _tmp92 * state(1, 0) + _tmp97 * state(2, 0); - const Scalar _tmp107 = P(0, 13) * _tmp68 + P(1, 13) * _tmp65 - P(13, 13) * _tmp86 - - P(14, 13) * _tmp82 - P(15, 13) * _tmp78 + P(2, 13) * _tmp91 + - P(3, 13) * _tmp75 + P(4, 13); - const Scalar _tmp108 = P(0, 14) * _tmp68 + P(1, 14) * _tmp65 - P(13, 14) * _tmp86 - - P(14, 14) * _tmp82 - P(15, 14) * _tmp78 + P(2, 14) * _tmp91 + - P(3, 14) * _tmp75 + P(4, 14); - const Scalar _tmp109 = P(0, 15) * _tmp68 + P(1, 15) * _tmp65 - P(13, 15) * _tmp86 - - P(14, 15) * _tmp82 - P(15, 15) * _tmp78 + P(2, 15) * _tmp91 + - P(3, 15) * _tmp75 + P(4, 15); - const Scalar _tmp110 = P(0, 0) * _tmp68 + P(1, 0) * _tmp65 - P(13, 0) * _tmp86 - - P(14, 0) * _tmp82 - P(15, 0) * _tmp78 + P(2, 0) * _tmp91 + - P(3, 0) * _tmp75 + P(4, 0); - const Scalar _tmp111 = P(0, 1) * _tmp68 + P(1, 1) * _tmp65 - P(13, 1) * _tmp86 - - P(14, 1) * _tmp82 - P(15, 1) * _tmp78 + P(2, 1) * _tmp91 + - P(3, 1) * _tmp75 + P(4, 1); - const Scalar _tmp112 = P(0, 2) * _tmp68 + P(1, 2) * _tmp65 - P(13, 2) * _tmp86 - - P(14, 2) * _tmp82 - P(15, 2) * _tmp78 + P(2, 2) * _tmp91 + - P(3, 2) * _tmp75 + P(4, 2); - const Scalar _tmp113 = P(0, 3) * _tmp68 + P(1, 3) * _tmp65 - P(13, 3) * _tmp86 - - P(14, 3) * _tmp82 - P(15, 3) * _tmp78 + P(2, 3) * _tmp91 + - P(3, 3) * _tmp75 + P(4, 3); - const Scalar _tmp114 = P(0, 4) * _tmp68 + P(1, 4) * _tmp65 - P(13, 4) * _tmp86 - - P(14, 4) * _tmp82 - P(15, 4) * _tmp78 + P(2, 4) * _tmp91 + - P(3, 4) * _tmp75 + P(4, 4); - const Scalar _tmp115 = _tmp69 * _tmp73; - const Scalar _tmp116 = _tmp115 + _tmp64; - const Scalar _tmp117 = _tmp63 * _tmp73; - const Scalar _tmp118 = _tmp117 - _tmp70; - const Scalar _tmp119 = _tmp71 * _tmp73; - const Scalar _tmp120 = _tmp119 - _tmp59 * _tmp74 + _tmp66; - const Scalar _tmp121 = _tmp80 + _tmp81; - const Scalar _tmp122 = _tmp60 * state(3, 0); - const Scalar _tmp123 = _tmp69 * state(0, 0); - const Scalar _tmp124 = _tmp122 - _tmp123; - const Scalar _tmp125 = 1 - 2 * _tmp27; - const Scalar _tmp126 = _tmp125 + _tmp84; - const Scalar _tmp127 = 4 * state(1, 0); - const Scalar _tmp128 = _tmp60 * _tmp73; - const Scalar _tmp129 = -_tmp127 * _tmp59 + _tmp128 - _tmp89; - const Scalar _tmp130 = Scalar(0.5) * P(11, 5); - const Scalar _tmp131 = P(0, 5) + P(1, 5) * _tmp8 + P(10, 5) * _tmp11 + P(12, 5) * _tmp9 + - P(2, 5) * _tmp5 + P(3, 5) * _tmp2 + _tmp130 * state(2, 0); - const Scalar _tmp132 = P(0, 5) * _tmp33 + P(1, 5) - P(10, 5) * _tmp30 - P(12, 5) * _tmp10 + - P(2, 5) * _tmp32 + P(3, 5) * _tmp5 + _tmp130 * state(3, 0); - const Scalar _tmp133 = P(0, 5) * _tmp43 + P(1, 5) * _tmp2 - P(10, 5) * _tmp9 + P(12, 5) * _tmp11 + - P(2, 5) + P(3, 5) * _tmp33 - _tmp130 * state(0, 0); - const Scalar _tmp134 = P(0, 5) * _tmp32 + P(1, 5) * _tmp43 + P(10, 5) * _tmp10 - - P(12, 5) * _tmp30 + P(2, 5) * _tmp8 + P(3, 5) - _tmp130 * state(1, 0); - const Scalar _tmp135 = _tmp86 * d_vel_var(0, 0); - const Scalar _tmp136 = _tmp82 * d_vel_var(1, 0); - const Scalar _tmp137 = _tmp78 * d_vel_var(2, 0); - const Scalar _tmp138 = P(0, 5) * _tmp68 + P(1, 5) * _tmp65 - P(13, 5) * _tmp86 - - P(14, 5) * _tmp82 - P(15, 5) * _tmp78 + P(2, 5) * _tmp91 + - P(3, 5) * _tmp75 + P(4, 5); - const Scalar _tmp139 = P(0, 1) * _tmp118 + P(1, 1) * _tmp129 - P(13, 1) * _tmp121 - - P(14, 1) * _tmp126 - P(15, 1) * _tmp124 + P(2, 1) * _tmp116 + - P(3, 1) * _tmp120 + P(5, 1); - const Scalar _tmp140 = P(0, 3) * _tmp118 + P(1, 3) * _tmp129 - P(13, 3) * _tmp121 - - P(14, 3) * _tmp126 - P(15, 3) * _tmp124 + P(2, 3) * _tmp116 + - P(3, 3) * _tmp120 + P(5, 3); - const Scalar _tmp141 = P(0, 13) * _tmp118 + P(1, 13) * _tmp129 - P(13, 13) * _tmp121 - - P(14, 13) * _tmp126 - P(15, 13) * _tmp124 + P(2, 13) * _tmp116 + - P(3, 13) * _tmp120 + P(5, 13); - const Scalar _tmp142 = P(0, 15) * _tmp118 + P(1, 15) * _tmp129 - P(13, 15) * _tmp121 - - P(14, 15) * _tmp126 - P(15, 15) * _tmp124 + P(2, 15) * _tmp116 + - P(3, 15) * _tmp120 + P(5, 15); - const Scalar _tmp143 = P(0, 14) * _tmp118 + P(1, 14) * _tmp129 - P(13, 14) * _tmp121 - - P(14, 14) * _tmp126 - P(15, 14) * _tmp124 + P(2, 14) * _tmp116 + - P(3, 14) * _tmp120 + P(5, 14); - const Scalar _tmp144 = P(0, 0) * _tmp118 + P(1, 0) * _tmp129 - P(13, 0) * _tmp121 - - P(14, 0) * _tmp126 - P(15, 0) * _tmp124 + P(2, 0) * _tmp116 + - P(3, 0) * _tmp120 + P(5, 0); - const Scalar _tmp145 = P(0, 2) * _tmp118 + P(1, 2) * _tmp129 - P(13, 2) * _tmp121 - - P(14, 2) * _tmp126 - P(15, 2) * _tmp124 + P(2, 2) * _tmp116 + - P(3, 2) * _tmp120 + P(5, 2); - const Scalar _tmp146 = P(0, 5) * _tmp118 + P(1, 5) * _tmp129 - P(13, 5) * _tmp121 - - P(14, 5) * _tmp126 - P(15, 5) * _tmp124 + P(2, 5) * _tmp116 + - P(3, 5) * _tmp120 + P(5, 5); - const Scalar _tmp147 = _tmp115 + _tmp61; - const Scalar _tmp148 = -_tmp128 + _tmp88; - const Scalar _tmp149 = -_tmp119 - _tmp62 * _tmp90 + _tmp67; - const Scalar _tmp150 = _tmp117 - _tmp127 * _tmp62 + _tmp72; - const Scalar _tmp151 = -_tmp76 + _tmp77; - const Scalar _tmp152 = _tmp122 + _tmp123; - const Scalar _tmp153 = _tmp125 + _tmp85; - const Scalar _tmp154 = Scalar(0.5) * P(11, 6); - const Scalar _tmp155 = P(0, 6) + P(1, 6) * _tmp8 + P(10, 6) * _tmp11 + P(12, 6) * _tmp9 + - P(2, 6) * _tmp5 + P(3, 6) * _tmp2 + _tmp154 * state(2, 0); - const Scalar _tmp156 = P(0, 6) * _tmp33 + P(1, 6) - P(10, 6) * _tmp30 - P(12, 6) * _tmp10 + - P(2, 6) * _tmp32 + P(3, 6) * _tmp5 + _tmp154 * state(3, 0); - const Scalar _tmp157 = P(0, 6) * _tmp43 + P(1, 6) * _tmp2 - P(10, 6) * _tmp9 + P(12, 6) * _tmp11 + - P(2, 6) + P(3, 6) * _tmp33 - _tmp154 * state(0, 0); - const Scalar _tmp158 = P(0, 6) * _tmp32 + P(1, 6) * _tmp43 + P(10, 6) * _tmp10 - - P(12, 6) * _tmp30 + P(2, 6) * _tmp8 + P(3, 6) - _tmp154 * state(1, 0); - const Scalar _tmp159 = P(0, 6) * _tmp68 + P(1, 6) * _tmp65 - P(13, 6) * _tmp86 - - P(14, 6) * _tmp82 - P(15, 6) * _tmp78 + P(2, 6) * _tmp91 + - P(3, 6) * _tmp75 + P(4, 6); - const Scalar _tmp160 = P(0, 6) * _tmp118 + P(1, 6) * _tmp129 - P(13, 6) * _tmp121 - - P(14, 6) * _tmp126 - P(15, 6) * _tmp124 + P(2, 6) * _tmp116 + - P(3, 6) * _tmp120 + P(5, 6); - const Scalar _tmp161 = P(0, 13) * _tmp148 + P(1, 13) * _tmp150 - P(13, 13) * _tmp151 - - P(14, 13) * _tmp152 - P(15, 13) * _tmp153 + P(2, 13) * _tmp149 + - P(3, 13) * _tmp147 + P(6, 13); - const Scalar _tmp162 = P(0, 14) * _tmp148 + P(1, 14) * _tmp150 - P(13, 14) * _tmp151 - - P(14, 14) * _tmp152 - P(15, 14) * _tmp153 + P(2, 14) * _tmp149 + - P(3, 14) * _tmp147 + P(6, 14); - const Scalar _tmp163 = P(0, 15) * _tmp148 + P(1, 15) * _tmp150 - P(13, 15) * _tmp151 - - P(14, 15) * _tmp152 - P(15, 15) * _tmp153 + P(2, 15) * _tmp149 + - P(3, 15) * _tmp147 + P(6, 15); - const Scalar _tmp164 = P(0, 6) * _tmp148 + P(1, 6) * _tmp150 - P(13, 6) * _tmp151 - - P(14, 6) * _tmp152 - P(15, 6) * _tmp153 + P(2, 6) * _tmp149 + - P(3, 6) * _tmp147 + P(6, 6); - const Scalar _tmp165 = Scalar(0.5) * P(12, 7); - const Scalar _tmp166 = Scalar(0.5) * P(11, 8); - const Scalar _tmp167 = Scalar(0.5) * P(12, 8); - const Scalar _tmp168 = Scalar(0.5) * P(11, 9); - const Scalar _tmp169 = Scalar(0.5) * P(10, 9); - const Scalar _tmp170 = Scalar(0.5) * P(12, 9); + const Scalar _tmp23 = P(0, 0) + P(1, 0) * _tmp9 + P(10, 0) * _tmp10 + P(11, 0) * _tmp12 + + P(12, 0) * _tmp11 + P(2, 0) * _tmp3 + P(3, 0) * _tmp6; + const Scalar _tmp24 = std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp25 = _tmp21 * _tmp24; + const Scalar _tmp26 = std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp27 = _tmp21 * _tmp26; + const Scalar _tmp28 = _tmp25 + _tmp27; + const Scalar _tmp29 = _tmp1 * state(0, 0); + const Scalar _tmp30 = _tmp21 * state(1, 0); + const Scalar _tmp31 = _tmp7 - _tmp8; + const Scalar _tmp32 = _tmp4 - _tmp5; + const Scalar _tmp33 = P(0, 12) * _tmp31 + P(1, 12) - P(10, 12) * _tmp29 + P(11, 12) * _tmp11 - + P(12, 12) * _tmp12 + P(2, 12) * _tmp32 + P(3, 12) * _tmp3; + const Scalar _tmp34 = P(0, 10) * _tmp31 + P(1, 10) - P(10, 10) * _tmp29 + P(11, 10) * _tmp11 - + P(12, 10) * _tmp12 + P(2, 10) * _tmp32 + P(3, 10) * _tmp3; + const Scalar _tmp35 = P(0, 11) * _tmp31 + P(1, 11) - P(10, 11) * _tmp29 + P(11, 11) * _tmp11 - + P(12, 11) * _tmp12 + P(2, 11) * _tmp32 + P(3, 11) * _tmp3; + const Scalar _tmp36 = P(0, 2) * _tmp31 + P(1, 2) - P(10, 2) * _tmp29 + P(11, 2) * _tmp11 - + P(12, 2) * _tmp12 + P(2, 2) * _tmp32 + P(3, 2) * _tmp3; + const Scalar _tmp37 = P(0, 3) * _tmp31 + P(1, 3) - P(10, 3) * _tmp29 + P(11, 3) * _tmp11 - + P(12, 3) * _tmp12 + P(2, 3) * _tmp32 + P(3, 3) * _tmp3; + const Scalar _tmp38 = P(0, 0) * _tmp31 + P(1, 0) - P(10, 0) * _tmp29 + P(11, 0) * _tmp11 - + P(12, 0) * _tmp12 + P(2, 0) * _tmp32 + P(3, 0) * _tmp3; + const Scalar _tmp39 = P(0, 1) * _tmp31 + P(1, 1) - P(10, 1) * _tmp29 - P(12, 1) * _tmp12 + + P(2, 1) * _tmp32 + P(3, 1) * _tmp3 + _tmp18 * state(3, 0); + const Scalar _tmp40 = _tmp21 * std::pow(state(0, 0), Scalar(2)); + const Scalar _tmp41 = _tmp22 + _tmp40; + const Scalar _tmp42 = _tmp21 * state(0, 0); + const Scalar _tmp43 = _tmp0 - _tmp2; + const Scalar _tmp44 = P(0, 0) * _tmp43 + P(1, 0) * _tmp6 - P(10, 0) * _tmp11 - P(11, 0) * _tmp29 + + P(12, 0) * _tmp10 + P(2, 0) + P(3, 0) * _tmp31; + const Scalar _tmp45 = P(0, 1) * _tmp43 + P(1, 1) * _tmp6 - P(10, 1) * _tmp11 + P(12, 1) * _tmp10 + + P(2, 1) + P(3, 1) * _tmp31 - _tmp18 * state(0, 0); + const Scalar _tmp46 = P(0, 3) * _tmp43 + P(1, 3) * _tmp6 - P(10, 3) * _tmp11 - P(11, 3) * _tmp29 + + P(12, 3) * _tmp10 + P(2, 3) + P(3, 3) * _tmp31; + const Scalar _tmp47 = P(0, 11) * _tmp43 + P(1, 11) * _tmp6 - P(10, 11) * _tmp11 - + P(11, 11) * _tmp29 + P(12, 11) * _tmp10 + P(2, 11) + P(3, 11) * _tmp31; + const Scalar _tmp48 = P(0, 10) * _tmp43 + P(1, 10) * _tmp6 - P(10, 10) * _tmp11 - + P(11, 10) * _tmp29 + P(12, 10) * _tmp10 + P(2, 10) + P(3, 10) * _tmp31; + const Scalar _tmp49 = P(0, 12) * _tmp43 + P(1, 12) * _tmp6 - P(10, 12) * _tmp11 - + P(11, 12) * _tmp29 + P(12, 12) * _tmp10 + P(2, 12) + P(3, 12) * _tmp31; + const Scalar _tmp50 = P(0, 2) * _tmp43 + P(1, 2) * _tmp6 - P(10, 2) * _tmp11 - P(11, 2) * _tmp29 + + P(12, 2) * _tmp10 + P(2, 2) + P(3, 2) * _tmp31; + const Scalar _tmp51 = P(0, 2) * _tmp32 + P(1, 2) * _tmp43 + P(10, 2) * _tmp12 - + P(11, 2) * _tmp10 - P(12, 2) * _tmp29 + P(2, 2) * _tmp9 + P(3, 2); + const Scalar _tmp52 = P(0, 1) * _tmp32 + P(1, 1) * _tmp43 + P(10, 1) * _tmp12 - + P(11, 1) * _tmp10 - P(12, 1) * _tmp29 + P(2, 1) * _tmp9 + P(3, 1); + const Scalar _tmp53 = P(0, 0) * _tmp32 + P(1, 0) * _tmp43 + P(10, 0) * _tmp12 - + P(11, 0) * _tmp10 - P(12, 0) * _tmp29 + P(2, 0) * _tmp9 + P(3, 0); + const Scalar _tmp54 = P(0, 11) * _tmp32 + P(1, 11) * _tmp43 + P(10, 11) * _tmp12 - + P(11, 11) * _tmp10 - P(12, 11) * _tmp29 + P(2, 11) * _tmp9 + P(3, 11); + const Scalar _tmp55 = P(0, 12) * _tmp32 + P(1, 12) * _tmp43 + P(10, 12) * _tmp12 - + P(11, 12) * _tmp10 - P(12, 12) * _tmp29 + P(2, 12) * _tmp9 + P(3, 12); + const Scalar _tmp56 = P(0, 10) * _tmp32 + P(1, 10) * _tmp43 + P(10, 10) * _tmp12 - + P(11, 10) * _tmp10 - P(12, 10) * _tmp29 + P(2, 10) * _tmp9 + P(3, 10); + const Scalar _tmp57 = P(0, 3) * _tmp32 + P(1, 3) * _tmp43 + P(10, 3) * _tmp12 - + P(11, 3) * _tmp10 - P(12, 3) * _tmp29 + P(2, 3) * _tmp9 + P(3, 3); + const Scalar _tmp58 = d_vel(2, 0) - d_vel_dt * state(15, 0); + const Scalar _tmp59 = 2 * state(2, 0); + const Scalar _tmp60 = _tmp58 * _tmp59; + const Scalar _tmp61 = d_vel(1, 0) - d_vel_dt * state(14, 0); + const Scalar _tmp62 = 2 * _tmp61; + const Scalar _tmp63 = _tmp62 * state(3, 0); + const Scalar _tmp64 = _tmp60 - _tmp63; + const Scalar _tmp65 = P(0, 13) + P(1, 13) * _tmp9 + P(10, 13) * _tmp10 + P(11, 13) * _tmp12 + + P(12, 13) * _tmp11 + P(2, 13) * _tmp3 + P(3, 13) * _tmp6; + const Scalar _tmp66 = -2 * _tmp20; + const Scalar _tmp67 = 1 - 2 * _tmp24; + const Scalar _tmp68 = _tmp66 + _tmp67; + const Scalar _tmp69 = _tmp68 * d_vel_dt; + const Scalar _tmp70 = P(0, 15) + P(1, 15) * _tmp9 + P(10, 15) * _tmp10 + P(11, 15) * _tmp12 + + P(12, 15) * _tmp11 + P(2, 15) * _tmp3 + P(3, 15) * _tmp6; + const Scalar _tmp71 = _tmp59 * state(0, 0); + const Scalar _tmp72 = 2 * state(1, 0); + const Scalar _tmp73 = _tmp72 * state(3, 0); + const Scalar _tmp74 = _tmp71 + _tmp73; + const Scalar _tmp75 = _tmp74 * d_vel_dt; + const Scalar _tmp76 = P(0, 14) + P(1, 14) * _tmp9 + P(10, 14) * _tmp10 + P(11, 14) * _tmp12 + + P(12, 14) * _tmp11 + P(2, 14) * _tmp3 + P(3, 14) * _tmp6; + const Scalar _tmp77 = 2 * state(0, 0); + const Scalar _tmp78 = _tmp77 * state(3, 0); + const Scalar _tmp79 = _tmp59 * state(1, 0); + const Scalar _tmp80 = -_tmp78 + _tmp79; + const Scalar _tmp81 = _tmp80 * d_vel_dt; + const Scalar _tmp82 = 2 * _tmp58; + const Scalar _tmp83 = _tmp82 * state(3, 0); + const Scalar _tmp84 = _tmp59 * _tmp61; + const Scalar _tmp85 = _tmp83 + _tmp84; + const Scalar _tmp86 = d_vel(0, 0) - d_vel_dt * state(13, 0); + const Scalar _tmp87 = 4 * _tmp86; + const Scalar _tmp88 = _tmp82 * state(0, 0); + const Scalar _tmp89 = _tmp62 * state(1, 0); + const Scalar _tmp90 = -_tmp87 * state(2, 0) + _tmp88 + _tmp89; + const Scalar _tmp91 = _tmp82 * state(1, 0); + const Scalar _tmp92 = _tmp62 * state(0, 0); + const Scalar _tmp93 = -_tmp87 * state(3, 0) + _tmp91 - _tmp92; + const Scalar _tmp94 = P(11, 4) * _tmp1; + const Scalar _tmp95 = P(0, 4) + P(1, 4) * _tmp9 + P(10, 4) * _tmp10 + P(12, 4) * _tmp11 + + P(2, 4) * _tmp3 + P(3, 4) * _tmp6 + _tmp94 * state(2, 0); + const Scalar _tmp96 = P(0, 13) * _tmp31 + P(1, 13) - P(10, 13) * _tmp29 + P(11, 13) * _tmp11 - + P(12, 13) * _tmp12 + P(2, 13) * _tmp32 + P(3, 13) * _tmp3; + const Scalar _tmp97 = P(0, 14) * _tmp31 + P(1, 14) - P(10, 14) * _tmp29 + P(11, 14) * _tmp11 - + P(12, 14) * _tmp12 + P(2, 14) * _tmp32 + P(3, 14) * _tmp3; + const Scalar _tmp98 = P(0, 15) * _tmp31 + P(1, 15) - P(10, 15) * _tmp29 + P(11, 15) * _tmp11 - + P(12, 15) * _tmp12 + P(2, 15) * _tmp32 + P(3, 15) * _tmp3; + const Scalar _tmp99 = P(0, 4) * _tmp31 + P(1, 4) - P(10, 4) * _tmp29 - P(12, 4) * _tmp12 + + P(2, 4) * _tmp32 + P(3, 4) * _tmp3 + _tmp94 * state(3, 0); + const Scalar _tmp100 = P(0, 13) * _tmp43 + P(1, 13) * _tmp6 - P(10, 13) * _tmp11 - + P(11, 13) * _tmp29 + P(12, 13) * _tmp10 + P(2, 13) + P(3, 13) * _tmp31; + const Scalar _tmp101 = P(0, 14) * _tmp43 + P(1, 14) * _tmp6 - P(10, 14) * _tmp11 - + P(11, 14) * _tmp29 + P(12, 14) * _tmp10 + P(2, 14) + P(3, 14) * _tmp31; + const Scalar _tmp102 = P(0, 15) * _tmp43 + P(1, 15) * _tmp6 - P(10, 15) * _tmp11 - + P(11, 15) * _tmp29 + P(12, 15) * _tmp10 + P(2, 15) + P(3, 15) * _tmp31; + const Scalar _tmp103 = P(0, 4) * _tmp43 + P(1, 4) * _tmp6 - P(10, 4) * _tmp11 + + P(12, 4) * _tmp10 + P(2, 4) + P(3, 4) * _tmp31 - _tmp94 * state(0, 0); + const Scalar _tmp104 = P(0, 13) * _tmp32 + P(1, 13) * _tmp43 + P(10, 13) * _tmp12 - + P(11, 13) * _tmp10 - P(12, 13) * _tmp29 + P(2, 13) * _tmp9 + P(3, 13); + const Scalar _tmp105 = P(0, 14) * _tmp32 + P(1, 14) * _tmp43 + P(10, 14) * _tmp12 - + P(11, 14) * _tmp10 - P(12, 14) * _tmp29 + P(2, 14) * _tmp9 + P(3, 14); + const Scalar _tmp106 = P(0, 15) * _tmp32 + P(1, 15) * _tmp43 + P(10, 15) * _tmp12 - + P(11, 15) * _tmp10 - P(12, 15) * _tmp29 + P(2, 15) * _tmp9 + P(3, 15); + const Scalar _tmp107 = P(0, 4) * _tmp32 + P(1, 4) * _tmp43 + P(10, 4) * _tmp12 - + P(12, 4) * _tmp29 + P(2, 4) * _tmp9 + P(3, 4) - _tmp94 * state(1, 0); + const Scalar _tmp108 = P(0, 13) * _tmp64 + P(1, 13) * _tmp85 - P(13, 13) * _tmp69 - + P(14, 13) * _tmp81 - P(15, 13) * _tmp75 + P(2, 13) * _tmp90 + + P(3, 13) * _tmp93 + P(4, 13); + const Scalar _tmp109 = P(0, 14) * _tmp64 + P(1, 14) * _tmp85 - P(13, 14) * _tmp69 - + P(14, 14) * _tmp81 - P(15, 14) * _tmp75 + P(2, 14) * _tmp90 + + P(3, 14) * _tmp93 + P(4, 14); + const Scalar _tmp110 = P(0, 15) * _tmp64 + P(1, 15) * _tmp85 - P(13, 15) * _tmp69 - + P(14, 15) * _tmp81 - P(15, 15) * _tmp75 + P(2, 15) * _tmp90 + + P(3, 15) * _tmp93 + P(4, 15); + const Scalar _tmp111 = P(0, 1) * _tmp64 + P(1, 1) * _tmp85 - P(13, 1) * _tmp69 - + P(14, 1) * _tmp81 - P(15, 1) * _tmp75 + P(2, 1) * _tmp90 + + P(3, 1) * _tmp93 + P(4, 1); + const Scalar _tmp112 = P(0, 0) * _tmp64 + P(1, 0) * _tmp85 - P(13, 0) * _tmp69 - + P(14, 0) * _tmp81 - P(15, 0) * _tmp75 + P(2, 0) * _tmp90 + + P(3, 0) * _tmp93 + P(4, 0); + const Scalar _tmp113 = P(0, 3) * _tmp64 + P(1, 3) * _tmp85 - P(13, 3) * _tmp69 - + P(14, 3) * _tmp81 - P(15, 3) * _tmp75 + P(2, 3) * _tmp90 + + P(3, 3) * _tmp93 + P(4, 3); + const Scalar _tmp114 = P(0, 2) * _tmp64 + P(1, 2) * _tmp85 - P(13, 2) * _tmp69 - + P(14, 2) * _tmp81 - P(15, 2) * _tmp75 + P(2, 2) * _tmp90 + + P(3, 2) * _tmp93 + P(4, 2); + const Scalar _tmp115 = P(0, 4) * _tmp64 + P(1, 4) * _tmp85 - P(13, 4) * _tmp69 - + P(14, 4) * _tmp81 - P(15, 4) * _tmp75 + P(2, 4) * _tmp90 + + P(3, 4) * _tmp93 + P(4, 4); + const Scalar _tmp116 = _tmp59 * state(3, 0); + const Scalar _tmp117 = _tmp72 * state(0, 0); + const Scalar _tmp118 = _tmp116 - _tmp117; + const Scalar _tmp119 = _tmp118 * d_vel_dt; + const Scalar _tmp120 = -2 * _tmp26; + const Scalar _tmp121 = _tmp120 + _tmp67; + const Scalar _tmp122 = _tmp121 * d_vel_dt; + const Scalar _tmp123 = _tmp78 + _tmp79; + const Scalar _tmp124 = _tmp123 * d_vel_dt; + const Scalar _tmp125 = _tmp59 * _tmp86; + const Scalar _tmp126 = 4 * _tmp61; + const Scalar _tmp127 = _tmp125 - _tmp126 * state(1, 0) - _tmp88; + const Scalar _tmp128 = _tmp77 * _tmp86; + const Scalar _tmp129 = -_tmp126 * state(3, 0) + _tmp128 + _tmp60; + const Scalar _tmp130 = 2 * _tmp86 * state(3, 0); + const Scalar _tmp131 = _tmp130 - _tmp91; + const Scalar _tmp132 = _tmp72 * _tmp86; + const Scalar _tmp133 = _tmp132 + _tmp83; + const Scalar _tmp134 = P(0, 5) + P(1, 5) * _tmp9 + P(10, 5) * _tmp10 + P(11, 5) * _tmp12 + + P(12, 5) * _tmp11 + P(2, 5) * _tmp3 + P(3, 5) * _tmp6; + const Scalar _tmp135 = P(0, 5) * _tmp31 + P(1, 5) - P(10, 5) * _tmp29 + P(11, 5) * _tmp11 - + P(12, 5) * _tmp12 + P(2, 5) * _tmp32 + P(3, 5) * _tmp3; + const Scalar _tmp136 = P(0, 5) * _tmp43 + P(1, 5) * _tmp6 - P(10, 5) * _tmp11 - + P(11, 5) * _tmp29 + P(12, 5) * _tmp10 + P(2, 5) + P(3, 5) * _tmp31; + const Scalar _tmp137 = P(0, 5) * _tmp32 + P(1, 5) * _tmp43 + P(10, 5) * _tmp12 - + P(11, 5) * _tmp10 - P(12, 5) * _tmp29 + P(2, 5) * _tmp9 + P(3, 5); + const Scalar _tmp138 = _tmp123 * d_vel_var(0, 0); + const Scalar _tmp139 = _tmp80 * d_vel_var(1, 0); + const Scalar _tmp140 = P(0, 5) * _tmp64 + P(1, 5) * _tmp85 - P(13, 5) * _tmp69 - + P(14, 5) * _tmp81 - P(15, 5) * _tmp75 + P(2, 5) * _tmp90 + + P(3, 5) * _tmp93 + P(4, 5); + const Scalar _tmp141 = P(0, 13) * _tmp131 + P(1, 13) * _tmp127 - P(13, 13) * _tmp124 - + P(14, 13) * _tmp122 - P(15, 13) * _tmp119 + P(2, 13) * _tmp133 + + P(3, 13) * _tmp129 + P(5, 13); + const Scalar _tmp142 = P(0, 14) * _tmp131 + P(1, 14) * _tmp127 - P(13, 14) * _tmp124 - + P(14, 14) * _tmp122 - P(15, 14) * _tmp119 + P(2, 14) * _tmp133 + + P(3, 14) * _tmp129 + P(5, 14); + const Scalar _tmp143 = P(0, 15) * _tmp131 + P(1, 15) * _tmp127 - P(13, 15) * _tmp124 - + P(14, 15) * _tmp122 - P(15, 15) * _tmp119 + P(2, 15) * _tmp133 + + P(3, 15) * _tmp129 + P(5, 15); + const Scalar _tmp144 = P(0, 2) * _tmp131 + P(1, 2) * _tmp127 - P(13, 2) * _tmp124 - + P(14, 2) * _tmp122 - P(15, 2) * _tmp119 + P(2, 2) * _tmp133 + + P(3, 2) * _tmp129 + P(5, 2); + const Scalar _tmp145 = P(0, 0) * _tmp131 + P(1, 0) * _tmp127 - P(13, 0) * _tmp124 - + P(14, 0) * _tmp122 - P(15, 0) * _tmp119 + P(2, 0) * _tmp133 + + P(3, 0) * _tmp129 + P(5, 0); + const Scalar _tmp146 = P(0, 3) * _tmp131 + P(1, 3) * _tmp127 - P(13, 3) * _tmp124 - + P(14, 3) * _tmp122 - P(15, 3) * _tmp119 + P(2, 3) * _tmp133 + + P(3, 3) * _tmp129 + P(5, 3); + const Scalar _tmp147 = P(0, 1) * _tmp131 + P(1, 1) * _tmp127 - P(13, 1) * _tmp124 - + P(14, 1) * _tmp122 - P(15, 1) * _tmp119 + P(2, 1) * _tmp133 + + P(3, 1) * _tmp129 + P(5, 1); + const Scalar _tmp148 = P(0, 5) * _tmp131 + P(1, 5) * _tmp127 - P(13, 5) * _tmp124 - + P(14, 5) * _tmp122 - P(15, 5) * _tmp119 + P(2, 5) * _tmp133 + + P(3, 5) * _tmp129 + P(5, 5); + const Scalar _tmp149 = _tmp116 + _tmp117; + const Scalar _tmp150 = _tmp149 * d_vel_dt; + const Scalar _tmp151 = _tmp120 + _tmp66 + 1; + const Scalar _tmp152 = _tmp151 * d_vel_dt; + const Scalar _tmp153 = -_tmp71 + _tmp73; + const Scalar _tmp154 = _tmp153 * d_vel_dt; + const Scalar _tmp155 = 4 * _tmp58; + const Scalar _tmp156 = _tmp130 - _tmp155 * state(1, 0) + _tmp92; + const Scalar _tmp157 = -_tmp128 - _tmp155 * state(2, 0) + _tmp63; + const Scalar _tmp158 = -_tmp125 + _tmp89; + const Scalar _tmp159 = _tmp132 + _tmp84; + const Scalar _tmp160 = P(11, 6) * _tmp1; + const Scalar _tmp161 = P(0, 6) + P(1, 6) * _tmp9 + P(10, 6) * _tmp10 + P(12, 6) * _tmp11 + + P(2, 6) * _tmp3 + P(3, 6) * _tmp6 + _tmp160 * state(2, 0); + const Scalar _tmp162 = P(0, 6) * _tmp31 + P(1, 6) - P(10, 6) * _tmp29 - P(12, 6) * _tmp12 + + P(2, 6) * _tmp32 + P(3, 6) * _tmp3 + _tmp160 * state(3, 0); + const Scalar _tmp163 = P(0, 6) * _tmp43 + P(1, 6) * _tmp6 - P(10, 6) * _tmp11 + + P(12, 6) * _tmp10 + P(2, 6) + P(3, 6) * _tmp31 - _tmp160 * state(0, 0); + const Scalar _tmp164 = P(0, 6) * _tmp32 + P(1, 6) * _tmp43 + P(10, 6) * _tmp12 - + P(11, 6) * _tmp10 - P(12, 6) * _tmp29 + P(2, 6) * _tmp9 + P(3, 6); + const Scalar _tmp165 = _tmp151 * d_vel_var(2, 0); + const Scalar _tmp166 = P(0, 6) * _tmp64 + P(1, 6) * _tmp85 - P(13, 6) * _tmp69 - + P(14, 6) * _tmp81 - P(15, 6) * _tmp75 + P(2, 6) * _tmp90 + + P(3, 6) * _tmp93 + P(4, 6); + const Scalar _tmp167 = P(0, 6) * _tmp131 + P(1, 6) * _tmp127 - P(13, 6) * _tmp124 - + P(14, 6) * _tmp122 - P(15, 6) * _tmp119 + P(2, 6) * _tmp133 + + P(3, 6) * _tmp129 + P(5, 6); + const Scalar _tmp168 = P(0, 13) * _tmp158 + P(1, 13) * _tmp156 - P(13, 13) * _tmp154 - + P(14, 13) * _tmp150 - P(15, 13) * _tmp152 + P(2, 13) * _tmp157 + + P(3, 13) * _tmp159 + P(6, 13); + const Scalar _tmp169 = P(0, 14) * _tmp158 + P(1, 14) * _tmp156 - P(13, 14) * _tmp154 - + P(14, 14) * _tmp150 - P(15, 14) * _tmp152 + P(2, 14) * _tmp157 + + P(3, 14) * _tmp159 + P(6, 14); + const Scalar _tmp170 = P(0, 15) * _tmp158 + P(1, 15) * _tmp156 - P(13, 15) * _tmp154 - + P(14, 15) * _tmp150 - P(15, 15) * _tmp152 + P(2, 15) * _tmp157 + + P(3, 15) * _tmp159 + P(6, 15); + const Scalar _tmp171 = P(0, 6) * _tmp158 + P(1, 6) * _tmp156 - P(13, 6) * _tmp154 - + P(14, 6) * _tmp150 - P(15, 6) * _tmp152 + P(2, 6) * _tmp157 + + P(3, 6) * _tmp159 + P(6, 6); + const Scalar _tmp172 = P(11, 8) * _tmp1; // Output terms (1) if (P_new != nullptr) { matrix::Matrix& _p_new = (*P_new); - _p_new(0, 0) = _tmp10 * _tmp12 + _tmp11 * _tmp13 + _tmp14 * _tmp9 + _tmp15 * _tmp5 + - _tmp17 * _tmp2 + _tmp19 * _tmp8 + _tmp22 + _tmp24 + _tmp29; + _p_new(0, 0) = _tmp10 * _tmp13 + _tmp11 * _tmp15 + _tmp12 * _tmp14 + _tmp16 * _tmp3 + + _tmp17 * _tmp6 + _tmp19 * _tmp9 + _tmp22 + _tmp23 + _tmp28; _p_new(1, 0) = 0; _p_new(2, 0) = 0; _p_new(3, 0) = 0; @@ -338,10 +342,10 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 0) = 0; _p_new(22, 0) = 0; _p_new(23, 0) = 0; - _p_new(0, 1) = -_tmp10 * _tmp14 + _tmp12 * _tmp9 - _tmp13 * _tmp30 + _tmp15 * _tmp32 + - _tmp17 * _tmp5 + _tmp19 + _tmp24 * _tmp33 - _tmp31 * state(1, 0); - _p_new(1, 1) = -_tmp10 * _tmp34 + _tmp26 - _tmp30 * _tmp36 + _tmp32 * _tmp38 + _tmp33 * _tmp39 + - _tmp35 * _tmp9 + _tmp37 * _tmp5 + _tmp40 + _tmp42; + _p_new(0, 1) = _tmp11 * _tmp14 - _tmp12 * _tmp15 - _tmp13 * _tmp29 + _tmp16 * _tmp32 + + _tmp17 * _tmp3 + _tmp19 + _tmp23 * _tmp31 - _tmp30 * state(0, 0); + _p_new(1, 1) = _tmp11 * _tmp35 - _tmp12 * _tmp33 + _tmp25 - _tmp29 * _tmp34 + _tmp3 * _tmp37 + + _tmp31 * _tmp38 + _tmp32 * _tmp36 + _tmp39 + _tmp41; _p_new(2, 1) = 0; _p_new(3, 1) = 0; _p_new(4, 1) = 0; @@ -364,12 +368,12 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 1) = 0; _p_new(22, 1) = 0; _p_new(23, 1) = 0; - _p_new(0, 2) = _tmp11 * _tmp14 - _tmp12 * _tmp30 - _tmp13 * _tmp9 + _tmp15 + _tmp17 * _tmp33 + - _tmp19 * _tmp2 + _tmp24 * _tmp43 - _tmp31 * state(2, 0); - _p_new(1, 2) = _tmp11 * _tmp34 + _tmp2 * _tmp40 - _tmp21 * state(1, 0) * state(2, 0) - - _tmp30 * _tmp35 + _tmp33 * _tmp37 - _tmp36 * _tmp9 + _tmp38 + _tmp39 * _tmp43; - _p_new(2, 2) = _tmp11 * _tmp44 + _tmp2 * _tmp48 + _tmp28 - _tmp30 * _tmp45 + _tmp33 * _tmp49 + - _tmp42 + _tmp43 * _tmp47 - _tmp46 * _tmp9 + _tmp50; + _p_new(0, 2) = _tmp10 * _tmp15 - _tmp11 * _tmp13 - _tmp14 * _tmp29 + _tmp16 + _tmp17 * _tmp31 + + _tmp19 * _tmp6 + _tmp23 * _tmp43 - _tmp42 * state(2, 0); + _p_new(1, 2) = _tmp10 * _tmp33 - _tmp11 * _tmp34 - _tmp29 * _tmp35 - _tmp30 * state(2, 0) + + _tmp31 * _tmp37 + _tmp36 + _tmp38 * _tmp43 + _tmp39 * _tmp6; + _p_new(2, 2) = _tmp10 * _tmp49 - _tmp11 * _tmp48 + _tmp28 - _tmp29 * _tmp47 + _tmp31 * _tmp46 + + _tmp40 + _tmp43 * _tmp44 + _tmp45 * _tmp6 + _tmp50; _p_new(3, 2) = 0; _p_new(4, 2) = 0; _p_new(5, 2) = 0; @@ -391,14 +395,14 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 2) = 0; _p_new(22, 2) = 0; _p_new(23, 2) = 0; - _p_new(0, 3) = _tmp10 * _tmp13 - _tmp11 * _tmp12 - _tmp14 * _tmp30 + _tmp15 * _tmp8 + _tmp17 + - _tmp19 * _tmp43 + _tmp24 * _tmp32 - _tmp51 * state(0, 0); - _p_new(1, 3) = _tmp10 * _tmp36 - _tmp11 * _tmp35 - _tmp30 * _tmp34 + _tmp32 * _tmp39 + _tmp37 + - _tmp38 * _tmp8 + _tmp40 * _tmp43 - _tmp51 * state(1, 0); - _p_new(2, 3) = _tmp10 * _tmp46 - _tmp11 * _tmp45 - _tmp30 * _tmp44 + _tmp32 * _tmp47 + - _tmp43 * _tmp48 + _tmp49 + _tmp50 * _tmp8 - _tmp51 * state(2, 0); - _p_new(3, 3) = _tmp10 * _tmp54 - _tmp11 * _tmp53 + _tmp29 - _tmp30 * _tmp52 + _tmp32 * _tmp56 + - _tmp41 + _tmp43 * _tmp57 + _tmp55 * _tmp8 + _tmp58; + _p_new(0, 3) = -_tmp10 * _tmp14 + _tmp12 * _tmp13 - _tmp15 * _tmp29 + _tmp16 * _tmp9 + _tmp17 + + _tmp19 * _tmp43 + _tmp23 * _tmp32 - _tmp42 * state(3, 0); + _p_new(1, 3) = -_tmp10 * _tmp35 + _tmp12 * _tmp34 - _tmp29 * _tmp33 - _tmp30 * state(3, 0) + + _tmp32 * _tmp38 + _tmp36 * _tmp9 + _tmp37 + _tmp39 * _tmp43; + _p_new(2, 3) = -_tmp10 * _tmp47 + _tmp12 * _tmp48 - _tmp21 * state(2, 0) * state(3, 0) - + _tmp29 * _tmp49 + _tmp32 * _tmp44 + _tmp43 * _tmp45 + _tmp46 + _tmp50 * _tmp9; + _p_new(3, 3) = -_tmp10 * _tmp54 + _tmp12 * _tmp56 + _tmp27 - _tmp29 * _tmp55 + _tmp32 * _tmp53 + + _tmp41 + _tmp43 * _tmp52 + _tmp51 * _tmp9 + _tmp57; _p_new(4, 3) = 0; _p_new(5, 3) = 0; _p_new(6, 3) = 0; @@ -419,19 +423,19 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 3) = 0; _p_new(22, 3) = 0; _p_new(23, 3) = 0; - _p_new(0, 4) = _tmp15 * _tmp91 + _tmp17 * _tmp75 + _tmp19 * _tmp65 + _tmp24 * _tmp68 - - _tmp78 * _tmp79 - _tmp82 * _tmp83 - _tmp86 * _tmp87 + _tmp93; - _p_new(1, 4) = _tmp37 * _tmp75 + _tmp38 * _tmp91 + _tmp39 * _tmp68 + _tmp40 * _tmp65 - - _tmp78 * _tmp95 - _tmp82 * _tmp96 - _tmp86 * _tmp94 + _tmp98; - _p_new(2, 4) = -_tmp100 * _tmp82 - _tmp101 * _tmp86 + _tmp102 + _tmp47 * _tmp68 + - _tmp48 * _tmp65 + _tmp49 * _tmp75 + _tmp50 * _tmp91 - _tmp78 * _tmp99; - _p_new(3, 4) = -_tmp103 * _tmp86 - _tmp104 * _tmp82 - _tmp105 * _tmp78 + _tmp106 + - _tmp55 * _tmp91 + _tmp56 * _tmp68 + _tmp57 * _tmp65 + _tmp58 * _tmp75; - _p_new(4, 4) = -_tmp107 * _tmp86 - _tmp108 * _tmp82 - _tmp109 * _tmp78 + _tmp110 * _tmp68 + - _tmp111 * _tmp65 + _tmp112 * _tmp91 + _tmp113 * _tmp75 + _tmp114 + - std::pow(_tmp78, Scalar(2)) * d_vel_var(2, 0) + - std::pow(_tmp82, Scalar(2)) * d_vel_var(1, 0) + - std::pow(_tmp86, Scalar(2)) * d_vel_var(0, 0); + _p_new(0, 4) = _tmp16 * _tmp90 + _tmp17 * _tmp93 + _tmp19 * _tmp85 + _tmp23 * _tmp64 - + _tmp65 * _tmp69 - _tmp70 * _tmp75 - _tmp76 * _tmp81 + _tmp95; + _p_new(1, 4) = _tmp36 * _tmp90 + _tmp37 * _tmp93 + _tmp38 * _tmp64 + _tmp39 * _tmp85 - + _tmp69 * _tmp96 - _tmp75 * _tmp98 - _tmp81 * _tmp97 + _tmp99; + _p_new(2, 4) = -_tmp100 * _tmp69 - _tmp101 * _tmp81 - _tmp102 * _tmp75 + _tmp103 + + _tmp44 * _tmp64 + _tmp45 * _tmp85 + _tmp46 * _tmp93 + _tmp50 * _tmp90; + _p_new(3, 4) = -_tmp104 * _tmp69 - _tmp105 * _tmp81 - _tmp106 * _tmp75 + _tmp107 + + _tmp51 * _tmp90 + _tmp52 * _tmp85 + _tmp53 * _tmp64 + _tmp57 * _tmp93; + _p_new(4, 4) = -_tmp108 * _tmp69 - _tmp109 * _tmp81 - _tmp110 * _tmp75 + _tmp111 * _tmp85 + + _tmp112 * _tmp64 + _tmp113 * _tmp93 + _tmp114 * _tmp90 + _tmp115 + + std::pow(_tmp68, Scalar(2)) * d_vel_var(0, 0) + + std::pow(_tmp74, Scalar(2)) * d_vel_var(2, 0) + + std::pow(_tmp80, Scalar(2)) * d_vel_var(1, 0); _p_new(5, 4) = 0; _p_new(6, 4) = 0; _p_new(7, 4) = 0; @@ -451,22 +455,23 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 4) = 0; _p_new(22, 4) = 0; _p_new(23, 4) = 0; - _p_new(0, 5) = _tmp116 * _tmp15 + _tmp118 * _tmp24 + _tmp120 * _tmp17 - _tmp121 * _tmp87 - - _tmp124 * _tmp79 - _tmp126 * _tmp83 + _tmp129 * _tmp19 + _tmp131; - _p_new(1, 5) = _tmp116 * _tmp38 + _tmp118 * _tmp39 + _tmp120 * _tmp37 - _tmp121 * _tmp94 - - _tmp124 * _tmp95 - _tmp126 * _tmp96 + _tmp129 * _tmp40 + _tmp132; - _p_new(2, 5) = -_tmp100 * _tmp126 - _tmp101 * _tmp121 + _tmp116 * _tmp50 + _tmp118 * _tmp47 + - _tmp120 * _tmp49 - _tmp124 * _tmp99 + _tmp129 * _tmp48 + _tmp133; - _p_new(3, 5) = -_tmp103 * _tmp121 - _tmp104 * _tmp126 - _tmp105 * _tmp124 + _tmp116 * _tmp55 + - _tmp118 * _tmp56 + _tmp120 * _tmp58 + _tmp129 * _tmp57 + _tmp134; - _p_new(4, 5) = -_tmp107 * _tmp121 - _tmp108 * _tmp126 - _tmp109 * _tmp124 + _tmp110 * _tmp118 + - _tmp111 * _tmp129 + _tmp112 * _tmp116 + _tmp113 * _tmp120 + _tmp121 * _tmp135 + - _tmp124 * _tmp137 + _tmp126 * _tmp136 + _tmp138; - _p_new(5, 5) = _tmp116 * _tmp145 + _tmp118 * _tmp144 + _tmp120 * _tmp140 + - std::pow(_tmp121, Scalar(2)) * d_vel_var(0, 0) - _tmp121 * _tmp141 + - std::pow(_tmp124, Scalar(2)) * d_vel_var(2, 0) - _tmp124 * _tmp142 + - std::pow(_tmp126, Scalar(2)) * d_vel_var(1, 0) - _tmp126 * _tmp143 + - _tmp129 * _tmp139 + _tmp146; + _p_new(0, 5) = -_tmp119 * _tmp70 - _tmp122 * _tmp76 - _tmp124 * _tmp65 + _tmp127 * _tmp19 + + _tmp129 * _tmp17 + _tmp131 * _tmp23 + _tmp133 * _tmp16 + _tmp134; + _p_new(1, 5) = -_tmp119 * _tmp98 - _tmp122 * _tmp97 - _tmp124 * _tmp96 + _tmp127 * _tmp39 + + _tmp129 * _tmp37 + _tmp131 * _tmp38 + _tmp133 * _tmp36 + _tmp135; + _p_new(2, 5) = -_tmp100 * _tmp124 - _tmp101 * _tmp122 - _tmp102 * _tmp119 + _tmp127 * _tmp45 + + _tmp129 * _tmp46 + _tmp131 * _tmp44 + _tmp133 * _tmp50 + _tmp136; + _p_new(3, 5) = -_tmp104 * _tmp124 - _tmp105 * _tmp122 - _tmp106 * _tmp119 + _tmp127 * _tmp52 + + _tmp129 * _tmp57 + _tmp131 * _tmp53 + _tmp133 * _tmp51 + _tmp137; + _p_new(4, 5) = -_tmp108 * _tmp124 - _tmp109 * _tmp122 - _tmp110 * _tmp119 + _tmp111 * _tmp127 + + _tmp112 * _tmp131 + _tmp113 * _tmp129 + _tmp114 * _tmp133 + + _tmp118 * _tmp74 * d_vel_var(2, 0) + _tmp121 * _tmp139 + _tmp138 * _tmp68 + + _tmp140; + _p_new(5, 5) = std::pow(_tmp118, Scalar(2)) * d_vel_var(2, 0) - _tmp119 * _tmp143 + + std::pow(_tmp121, Scalar(2)) * d_vel_var(1, 0) - _tmp122 * _tmp142 + + std::pow(_tmp123, Scalar(2)) * d_vel_var(0, 0) - _tmp124 * _tmp141 + + _tmp127 * _tmp147 + _tmp129 * _tmp146 + _tmp131 * _tmp145 + _tmp133 * _tmp144 + + _tmp148; _p_new(6, 5) = 0; _p_new(7, 5) = 0; _p_new(8, 5) = 0; @@ -485,33 +490,33 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 5) = 0; _p_new(22, 5) = 0; _p_new(23, 5) = 0; - _p_new(0, 6) = _tmp147 * _tmp17 + _tmp148 * _tmp24 + _tmp149 * _tmp15 + _tmp150 * _tmp19 - - _tmp151 * _tmp87 - _tmp152 * _tmp83 - _tmp153 * _tmp79 + _tmp155; - _p_new(1, 6) = _tmp147 * _tmp37 + _tmp148 * _tmp39 + _tmp149 * _tmp38 + _tmp150 * _tmp40 - - _tmp151 * _tmp94 - _tmp152 * _tmp96 - _tmp153 * _tmp95 + _tmp156; - _p_new(2, 6) = -_tmp100 * _tmp152 - _tmp101 * _tmp151 + _tmp147 * _tmp49 + _tmp148 * _tmp47 + - _tmp149 * _tmp50 + _tmp150 * _tmp48 - _tmp153 * _tmp99 + _tmp157; - _p_new(3, 6) = -_tmp103 * _tmp151 - _tmp104 * _tmp152 - _tmp105 * _tmp153 + _tmp147 * _tmp58 + - _tmp148 * _tmp56 + _tmp149 * _tmp55 + _tmp150 * _tmp57 + _tmp158; - _p_new(4, 6) = -_tmp107 * _tmp151 - _tmp108 * _tmp152 - _tmp109 * _tmp153 + _tmp110 * _tmp148 + - _tmp111 * _tmp150 + _tmp112 * _tmp149 + _tmp113 * _tmp147 + _tmp135 * _tmp151 + - _tmp136 * _tmp152 + _tmp137 * _tmp153 + _tmp159; - _p_new(5, 6) = _tmp121 * _tmp151 * d_vel_var(0, 0) + _tmp124 * _tmp153 * d_vel_var(2, 0) + - _tmp126 * _tmp152 * d_vel_var(1, 0) + _tmp139 * _tmp150 + _tmp140 * _tmp147 - - _tmp141 * _tmp151 - _tmp142 * _tmp153 - _tmp143 * _tmp152 + _tmp144 * _tmp148 + - _tmp145 * _tmp149 + _tmp160; + _p_new(0, 6) = -_tmp150 * _tmp76 - _tmp152 * _tmp70 - _tmp154 * _tmp65 + _tmp156 * _tmp19 + + _tmp157 * _tmp16 + _tmp158 * _tmp23 + _tmp159 * _tmp17 + _tmp161; + _p_new(1, 6) = -_tmp150 * _tmp97 - _tmp152 * _tmp98 - _tmp154 * _tmp96 + _tmp156 * _tmp39 + + _tmp157 * _tmp36 + _tmp158 * _tmp38 + _tmp159 * _tmp37 + _tmp162; + _p_new(2, 6) = -_tmp100 * _tmp154 - _tmp101 * _tmp150 - _tmp102 * _tmp152 + _tmp156 * _tmp45 + + _tmp157 * _tmp50 + _tmp158 * _tmp44 + _tmp159 * _tmp46 + _tmp163; + _p_new(3, 6) = -_tmp104 * _tmp154 - _tmp105 * _tmp150 - _tmp106 * _tmp152 + _tmp156 * _tmp52 + + _tmp157 * _tmp51 + _tmp158 * _tmp53 + _tmp159 * _tmp57 + _tmp164; + _p_new(4, 6) = -_tmp108 * _tmp154 - _tmp109 * _tmp150 - _tmp110 * _tmp152 + _tmp111 * _tmp156 + + _tmp112 * _tmp158 + _tmp113 * _tmp159 + _tmp114 * _tmp157 + _tmp139 * _tmp149 + + _tmp153 * _tmp68 * d_vel_var(0, 0) + _tmp165 * _tmp74 + _tmp166; + _p_new(5, 6) = _tmp118 * _tmp165 + _tmp121 * _tmp149 * d_vel_var(1, 0) + _tmp138 * _tmp153 - + _tmp141 * _tmp154 - _tmp142 * _tmp150 - _tmp143 * _tmp152 + _tmp144 * _tmp157 + + _tmp145 * _tmp158 + _tmp146 * _tmp159 + _tmp147 * _tmp156 + _tmp167; _p_new(6, 6) = - _tmp147 * (P(0, 3) * _tmp148 + P(1, 3) * _tmp150 - P(13, 3) * _tmp151 - P(14, 3) * _tmp152 - - P(15, 3) * _tmp153 + P(2, 3) * _tmp149 + P(3, 3) * _tmp147 + P(6, 3)) + - _tmp148 * (P(0, 0) * _tmp148 + P(1, 0) * _tmp150 - P(13, 0) * _tmp151 - P(14, 0) * _tmp152 - - P(15, 0) * _tmp153 + P(2, 0) * _tmp149 + P(3, 0) * _tmp147 + P(6, 0)) + - _tmp149 * (P(0, 2) * _tmp148 + P(1, 2) * _tmp150 - P(13, 2) * _tmp151 - P(14, 2) * _tmp152 - - P(15, 2) * _tmp153 + P(2, 2) * _tmp149 + P(3, 2) * _tmp147 + P(6, 2)) + - _tmp150 * (P(0, 1) * _tmp148 + P(1, 1) * _tmp150 - P(13, 1) * _tmp151 - P(14, 1) * _tmp152 - - P(15, 1) * _tmp153 + P(2, 1) * _tmp149 + P(3, 1) * _tmp147 + P(6, 1)) + - std::pow(_tmp151, Scalar(2)) * d_vel_var(0, 0) - _tmp151 * _tmp161 + - std::pow(_tmp152, Scalar(2)) * d_vel_var(1, 0) - _tmp152 * _tmp162 + - std::pow(_tmp153, Scalar(2)) * d_vel_var(2, 0) - _tmp153 * _tmp163 + _tmp164; + std::pow(_tmp149, Scalar(2)) * d_vel_var(1, 0) - _tmp150 * _tmp169 + + std::pow(_tmp151, Scalar(2)) * d_vel_var(2, 0) - _tmp152 * _tmp170 + + std::pow(_tmp153, Scalar(2)) * d_vel_var(0, 0) - _tmp154 * _tmp168 + + _tmp156 * (P(0, 1) * _tmp158 + P(1, 1) * _tmp156 - P(13, 1) * _tmp154 - P(14, 1) * _tmp150 - + P(15, 1) * _tmp152 + P(2, 1) * _tmp157 + P(3, 1) * _tmp159 + P(6, 1)) + + _tmp157 * (P(0, 2) * _tmp158 + P(1, 2) * _tmp156 - P(13, 2) * _tmp154 - P(14, 2) * _tmp150 - + P(15, 2) * _tmp152 + P(2, 2) * _tmp157 + P(3, 2) * _tmp159 + P(6, 2)) + + _tmp158 * (P(0, 0) * _tmp158 + P(1, 0) * _tmp156 - P(13, 0) * _tmp154 - P(14, 0) * _tmp150 - + P(15, 0) * _tmp152 + P(2, 0) * _tmp157 + P(3, 0) * _tmp159 + P(6, 0)) + + _tmp159 * (P(0, 3) * _tmp158 + P(1, 3) * _tmp156 - P(13, 3) * _tmp154 - P(14, 3) * _tmp150 - + P(15, 3) * _tmp152 + P(2, 3) * _tmp157 + P(3, 3) * _tmp159 + P(6, 3)) + + _tmp171; _p_new(7, 6) = 0; _p_new(8, 6) = 0; _p_new(9, 6) = 0; @@ -529,27 +534,28 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 6) = 0; _p_new(22, 6) = 0; _p_new(23, 6) = 0; - _p_new(0, 7) = P(0, 7) + P(1, 7) * _tmp8 + P(10, 7) * _tmp11 + P(11, 7) * _tmp10 + - P(2, 7) * _tmp5 + P(3, 7) * _tmp2 + _tmp165 * state(3, 0) + _tmp93 * dt; - _p_new(1, 7) = P(0, 7) * _tmp33 + P(1, 7) - P(10, 7) * _tmp30 + P(11, 7) * _tmp9 + - P(2, 7) * _tmp32 + P(3, 7) * _tmp5 - _tmp165 * state(2, 0) + _tmp98 * dt; - _p_new(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp2 - P(10, 7) * _tmp9 - P(11, 7) * _tmp30 + - P(2, 7) + P(3, 7) * _tmp33 + _tmp102 * dt + _tmp165 * state(1, 0); - _p_new(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp10 - P(11, 7) * _tmp11 + - P(2, 7) * _tmp8 + P(3, 7) + _tmp106 * dt - _tmp165 * state(0, 0); - _p_new(4, 7) = P(0, 7) * _tmp68 + P(1, 7) * _tmp65 - P(13, 7) * _tmp86 - P(14, 7) * _tmp82 - - P(15, 7) * _tmp78 + P(2, 7) * _tmp91 + P(3, 7) * _tmp75 + P(4, 7) + _tmp114 * dt; - _p_new(5, 7) = - P(0, 7) * _tmp118 + P(1, 7) * _tmp129 - P(13, 7) * _tmp121 - P(14, 7) * _tmp126 - - P(15, 7) * _tmp124 + P(2, 7) * _tmp116 + P(3, 7) * _tmp120 + P(5, 7) + - dt * (P(0, 4) * _tmp118 + P(1, 4) * _tmp129 - P(13, 4) * _tmp121 - P(14, 4) * _tmp126 - - P(15, 4) * _tmp124 + P(2, 4) * _tmp116 + P(3, 4) * _tmp120 + P(5, 4)); - _p_new(6, 7) = - P(0, 7) * _tmp148 + P(1, 7) * _tmp150 - P(13, 7) * _tmp151 - P(14, 7) * _tmp152 - - P(15, 7) * _tmp153 + P(2, 7) * _tmp149 + P(3, 7) * _tmp147 + P(6, 7) + - dt * (P(0, 4) * _tmp148 + P(1, 4) * _tmp150 - P(13, 4) * _tmp151 - P(14, 4) * _tmp152 - - P(15, 4) * _tmp153 + P(2, 4) * _tmp149 + P(3, 4) * _tmp147 + P(6, 4)); - _p_new(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(7, 4)); + _p_new(0, 7) = P(0, 7) + P(1, 7) * _tmp9 + P(10, 7) * _tmp10 + P(11, 7) * _tmp12 + + P(12, 7) * _tmp11 + P(2, 7) * _tmp3 + P(3, 7) * _tmp6 + _tmp95 * d_vel_dt; + _p_new(1, 7) = P(0, 7) * _tmp31 + P(1, 7) - P(10, 7) * _tmp29 + P(11, 7) * _tmp11 - + P(12, 7) * _tmp12 + P(2, 7) * _tmp32 + P(3, 7) * _tmp3 + _tmp99 * d_vel_dt; + _p_new(2, 7) = P(0, 7) * _tmp43 + P(1, 7) * _tmp6 - P(10, 7) * _tmp11 - P(11, 7) * _tmp29 + + P(12, 7) * _tmp10 + P(2, 7) + P(3, 7) * _tmp31 + _tmp103 * d_vel_dt; + _p_new(3, 7) = P(0, 7) * _tmp32 + P(1, 7) * _tmp43 + P(10, 7) * _tmp12 - P(11, 7) * _tmp10 - + P(12, 7) * _tmp29 + P(2, 7) * _tmp9 + P(3, 7) + _tmp107 * d_vel_dt; + _p_new(4, 7) = P(0, 7) * _tmp64 + P(1, 7) * _tmp85 - P(13, 7) * _tmp69 - P(14, 7) * _tmp81 - + P(15, 7) * _tmp75 + P(2, 7) * _tmp90 + P(3, 7) * _tmp93 + P(4, 7) + + _tmp115 * d_vel_dt; + _p_new(5, 7) = P(0, 7) * _tmp131 + P(1, 7) * _tmp127 - P(13, 7) * _tmp124 - P(14, 7) * _tmp122 - + P(15, 7) * _tmp119 + P(2, 7) * _tmp133 + P(3, 7) * _tmp129 + P(5, 7) + + d_vel_dt * (P(0, 4) * _tmp131 + P(1, 4) * _tmp127 - P(13, 4) * _tmp124 - + P(14, 4) * _tmp122 - P(15, 4) * _tmp119 + P(2, 4) * _tmp133 + + P(3, 4) * _tmp129 + P(5, 4)); + _p_new(6, 7) = P(0, 7) * _tmp158 + P(1, 7) * _tmp156 - P(13, 7) * _tmp154 - P(14, 7) * _tmp150 - + P(15, 7) * _tmp152 + P(2, 7) * _tmp157 + P(3, 7) * _tmp159 + P(6, 7) + + d_vel_dt * (P(0, 4) * _tmp158 + P(1, 4) * _tmp156 - P(13, 4) * _tmp154 - + P(14, 4) * _tmp150 - P(15, 4) * _tmp152 + P(2, 4) * _tmp157 + + P(3, 4) * _tmp159 + P(6, 4)); + _p_new(7, 7) = P(4, 7) * d_vel_dt + P(7, 7) + d_vel_dt * (P(4, 4) * d_vel_dt + P(7, 4)); _p_new(8, 7) = 0; _p_new(9, 7) = 0; _p_new(10, 7) = 0; @@ -566,26 +572,27 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 7) = 0; _p_new(22, 7) = 0; _p_new(23, 7) = 0; - _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp8 + P(10, 8) * _tmp11 + P(12, 8) * _tmp9 + - P(2, 8) * _tmp5 + P(3, 8) * _tmp2 + _tmp131 * dt + _tmp166 * state(2, 0); - _p_new(1, 8) = P(0, 8) * _tmp33 + P(1, 8) - P(10, 8) * _tmp30 + P(2, 8) * _tmp32 + - P(3, 8) * _tmp5 + _tmp132 * dt + _tmp166 * state(3, 0) - _tmp167 * state(2, 0); - _p_new(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp2 - P(10, 8) * _tmp9 + P(12, 8) * _tmp11 + - P(2, 8) + P(3, 8) * _tmp33 + _tmp133 * dt - _tmp166 * state(0, 0); - _p_new(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp10 + P(2, 8) * _tmp8 + - P(3, 8) + _tmp134 * dt - _tmp166 * state(1, 0) - _tmp167 * state(0, 0); - _p_new(4, 8) = P(0, 8) * _tmp68 + P(1, 8) * _tmp65 - P(13, 8) * _tmp86 - P(14, 8) * _tmp82 - - P(15, 8) * _tmp78 + P(2, 8) * _tmp91 + P(3, 8) * _tmp75 + P(4, 8) + _tmp138 * dt; - _p_new(5, 8) = P(0, 8) * _tmp118 + P(1, 8) * _tmp129 - P(13, 8) * _tmp121 - P(14, 8) * _tmp126 - - P(15, 8) * _tmp124 + P(2, 8) * _tmp116 + P(3, 8) * _tmp120 + P(5, 8) + - _tmp146 * dt; - _p_new(6, 8) = - P(0, 8) * _tmp148 + P(1, 8) * _tmp150 - P(13, 8) * _tmp151 - P(14, 8) * _tmp152 - - P(15, 8) * _tmp153 + P(2, 8) * _tmp149 + P(3, 8) * _tmp147 + P(6, 8) + - dt * (P(0, 5) * _tmp148 + P(1, 5) * _tmp150 - P(13, 5) * _tmp151 - P(14, 5) * _tmp152 - - P(15, 5) * _tmp153 + P(2, 5) * _tmp149 + P(3, 5) * _tmp147 + P(6, 5)); - _p_new(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(7, 5)); - _p_new(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(8, 5)); + _p_new(0, 8) = P(0, 8) + P(1, 8) * _tmp9 + P(10, 8) * _tmp10 + P(12, 8) * _tmp11 + + P(2, 8) * _tmp3 + P(3, 8) * _tmp6 + _tmp134 * d_vel_dt + _tmp172 * state(2, 0); + _p_new(1, 8) = P(0, 8) * _tmp31 + P(1, 8) - P(10, 8) * _tmp29 - P(12, 8) * _tmp12 + + P(2, 8) * _tmp32 + P(3, 8) * _tmp3 + _tmp135 * d_vel_dt + _tmp172 * state(3, 0); + _p_new(2, 8) = P(0, 8) * _tmp43 + P(1, 8) * _tmp6 - P(10, 8) * _tmp11 + P(12, 8) * _tmp10 + + P(2, 8) + P(3, 8) * _tmp31 + _tmp136 * d_vel_dt - _tmp172 * state(0, 0); + _p_new(3, 8) = P(0, 8) * _tmp32 + P(1, 8) * _tmp43 + P(10, 8) * _tmp12 - P(12, 8) * _tmp29 + + P(2, 8) * _tmp9 + P(3, 8) + _tmp137 * d_vel_dt - _tmp172 * state(1, 0); + _p_new(4, 8) = P(0, 8) * _tmp64 + P(1, 8) * _tmp85 - P(13, 8) * _tmp69 - P(14, 8) * _tmp81 - + P(15, 8) * _tmp75 + P(2, 8) * _tmp90 + P(3, 8) * _tmp93 + P(4, 8) + + _tmp140 * d_vel_dt; + _p_new(5, 8) = P(0, 8) * _tmp131 + P(1, 8) * _tmp127 - P(13, 8) * _tmp124 - P(14, 8) * _tmp122 - + P(15, 8) * _tmp119 + P(2, 8) * _tmp133 + P(3, 8) * _tmp129 + P(5, 8) + + _tmp148 * d_vel_dt; + _p_new(6, 8) = P(0, 8) * _tmp158 + P(1, 8) * _tmp156 - P(13, 8) * _tmp154 - P(14, 8) * _tmp150 - + P(15, 8) * _tmp152 + P(2, 8) * _tmp157 + P(3, 8) * _tmp159 + P(6, 8) + + d_vel_dt * (P(0, 5) * _tmp158 + P(1, 5) * _tmp156 - P(13, 5) * _tmp154 - + P(14, 5) * _tmp150 - P(15, 5) * _tmp152 + P(2, 5) * _tmp157 + + P(3, 5) * _tmp159 + P(6, 5)); + _p_new(7, 8) = P(4, 8) * d_vel_dt + P(7, 8) + d_vel_dt * (P(4, 5) * d_vel_dt + P(7, 5)); + _p_new(8, 8) = P(5, 8) * d_vel_dt + P(8, 8) + d_vel_dt * (P(5, 5) * d_vel_dt + P(8, 5)); _p_new(9, 8) = 0; _p_new(10, 8) = 0; _p_new(11, 8) = 0; @@ -601,25 +608,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 8) = 0; _p_new(22, 8) = 0; _p_new(23, 8) = 0; - _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp8 + P(10, 9) * _tmp11 + P(12, 9) * _tmp9 + - P(2, 9) * _tmp5 + P(3, 9) * _tmp2 + _tmp155 * dt + _tmp168 * state(2, 0); - _p_new(1, 9) = P(0, 9) * _tmp33 + P(1, 9) + P(11, 9) * _tmp9 + P(2, 9) * _tmp32 + - P(3, 9) * _tmp5 + _tmp156 * dt - _tmp169 * state(0, 0) - _tmp170 * state(2, 0); - _p_new(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp2 - P(10, 9) * _tmp9 + P(12, 9) * _tmp11 + - P(2, 9) + P(3, 9) * _tmp33 + _tmp157 * dt - _tmp168 * state(0, 0); - _p_new(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 - P(11, 9) * _tmp11 + P(2, 9) * _tmp8 + - P(3, 9) + _tmp158 * dt + _tmp169 * state(2, 0) - _tmp170 * state(0, 0); - _p_new(4, 9) = P(0, 9) * _tmp68 + P(1, 9) * _tmp65 - P(13, 9) * _tmp86 - P(14, 9) * _tmp82 - - P(15, 9) * _tmp78 + P(2, 9) * _tmp91 + P(3, 9) * _tmp75 + P(4, 9) + _tmp159 * dt; - _p_new(5, 9) = P(0, 9) * _tmp118 + P(1, 9) * _tmp129 - P(13, 9) * _tmp121 - P(14, 9) * _tmp126 - - P(15, 9) * _tmp124 + P(2, 9) * _tmp116 + P(3, 9) * _tmp120 + P(5, 9) + - _tmp160 * dt; - _p_new(6, 9) = P(0, 9) * _tmp148 + P(1, 9) * _tmp150 - P(13, 9) * _tmp151 - P(14, 9) * _tmp152 - - P(15, 9) * _tmp153 + P(2, 9) * _tmp149 + P(3, 9) * _tmp147 + P(6, 9) + - _tmp164 * dt; - _p_new(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(7, 6)); - _p_new(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(8, 6)); - _p_new(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(9, 6)); + _p_new(0, 9) = P(0, 9) + P(1, 9) * _tmp9 + P(10, 9) * _tmp10 + P(11, 9) * _tmp12 + + P(12, 9) * _tmp11 + P(2, 9) * _tmp3 + P(3, 9) * _tmp6 + _tmp161 * d_vel_dt; + _p_new(1, 9) = P(0, 9) * _tmp31 + P(1, 9) - P(10, 9) * _tmp29 + P(11, 9) * _tmp11 - + P(12, 9) * _tmp12 + P(2, 9) * _tmp32 + P(3, 9) * _tmp3 + _tmp162 * d_vel_dt; + _p_new(2, 9) = P(0, 9) * _tmp43 + P(1, 9) * _tmp6 - P(10, 9) * _tmp11 - P(11, 9) * _tmp29 + + P(12, 9) * _tmp10 + P(2, 9) + P(3, 9) * _tmp31 + _tmp163 * d_vel_dt; + _p_new(3, 9) = P(0, 9) * _tmp32 + P(1, 9) * _tmp43 + P(10, 9) * _tmp12 - P(11, 9) * _tmp10 - + P(12, 9) * _tmp29 + P(2, 9) * _tmp9 + P(3, 9) + _tmp164 * d_vel_dt; + _p_new(4, 9) = P(0, 9) * _tmp64 + P(1, 9) * _tmp85 - P(13, 9) * _tmp69 - P(14, 9) * _tmp81 - + P(15, 9) * _tmp75 + P(2, 9) * _tmp90 + P(3, 9) * _tmp93 + P(4, 9) + + _tmp166 * d_vel_dt; + _p_new(5, 9) = P(0, 9) * _tmp131 + P(1, 9) * _tmp127 - P(13, 9) * _tmp124 - P(14, 9) * _tmp122 - + P(15, 9) * _tmp119 + P(2, 9) * _tmp133 + P(3, 9) * _tmp129 + P(5, 9) + + _tmp167 * d_vel_dt; + _p_new(6, 9) = P(0, 9) * _tmp158 + P(1, 9) * _tmp156 - P(13, 9) * _tmp154 - P(14, 9) * _tmp150 - + P(15, 9) * _tmp152 + P(2, 9) * _tmp157 + P(3, 9) * _tmp159 + P(6, 9) + + _tmp171 * d_vel_dt; + _p_new(7, 9) = P(4, 9) * d_vel_dt + P(7, 9) + d_vel_dt * (P(4, 6) * d_vel_dt + P(7, 6)); + _p_new(8, 9) = P(5, 9) * d_vel_dt + P(8, 9) + d_vel_dt * (P(5, 6) * d_vel_dt + P(8, 6)); + _p_new(9, 9) = P(6, 9) * d_vel_dt + P(9, 9) + d_vel_dt * (P(6, 6) * d_vel_dt + P(9, 6)); _p_new(10, 9) = 0; _p_new(11, 9) = 0; _p_new(12, 9) = 0; @@ -635,21 +643,21 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(22, 9) = 0; _p_new(23, 9) = 0; _p_new(0, 10) = _tmp13; - _p_new(1, 10) = _tmp36; - _p_new(2, 10) = _tmp46; - _p_new(3, 10) = _tmp54; - _p_new(4, 10) = P(0, 10) * _tmp68 + P(1, 10) * _tmp65 - P(13, 10) * _tmp86 - - P(14, 10) * _tmp82 - P(15, 10) * _tmp78 + P(2, 10) * _tmp91 + - P(3, 10) * _tmp75 + P(4, 10); - _p_new(5, 10) = P(0, 10) * _tmp118 + P(1, 10) * _tmp129 - P(13, 10) * _tmp121 - - P(14, 10) * _tmp126 - P(15, 10) * _tmp124 + P(2, 10) * _tmp116 + - P(3, 10) * _tmp120 + P(5, 10); - _p_new(6, 10) = P(0, 10) * _tmp148 + P(1, 10) * _tmp150 - P(13, 10) * _tmp151 - - P(14, 10) * _tmp152 - P(15, 10) * _tmp153 + P(2, 10) * _tmp149 + - P(3, 10) * _tmp147 + P(6, 10); - _p_new(7, 10) = P(4, 10) * dt + P(7, 10); - _p_new(8, 10) = P(5, 10) * dt + P(8, 10); - _p_new(9, 10) = P(6, 10) * dt + P(9, 10); + _p_new(1, 10) = _tmp34; + _p_new(2, 10) = _tmp48; + _p_new(3, 10) = _tmp56; + _p_new(4, 10) = P(0, 10) * _tmp64 + P(1, 10) * _tmp85 - P(13, 10) * _tmp69 - + P(14, 10) * _tmp81 - P(15, 10) * _tmp75 + P(2, 10) * _tmp90 + + P(3, 10) * _tmp93 + P(4, 10); + _p_new(5, 10) = P(0, 10) * _tmp131 + P(1, 10) * _tmp127 - P(13, 10) * _tmp124 - + P(14, 10) * _tmp122 - P(15, 10) * _tmp119 + P(2, 10) * _tmp133 + + P(3, 10) * _tmp129 + P(5, 10); + _p_new(6, 10) = P(0, 10) * _tmp158 + P(1, 10) * _tmp156 - P(13, 10) * _tmp154 - + P(14, 10) * _tmp150 - P(15, 10) * _tmp152 + P(2, 10) * _tmp157 + + P(3, 10) * _tmp159 + P(6, 10); + _p_new(7, 10) = P(4, 10) * d_vel_dt + P(7, 10); + _p_new(8, 10) = P(5, 10) * d_vel_dt + P(8, 10); + _p_new(9, 10) = P(6, 10) * d_vel_dt + P(9, 10); _p_new(10, 10) = P(10, 10); _p_new(11, 10) = 0; _p_new(12, 10) = 0; @@ -664,22 +672,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 10) = 0; _p_new(22, 10) = 0; _p_new(23, 10) = 0; - _p_new(0, 11) = _tmp12; + _p_new(0, 11) = _tmp14; _p_new(1, 11) = _tmp35; - _p_new(2, 11) = _tmp45; - _p_new(3, 11) = _tmp53; - _p_new(4, 11) = P(0, 11) * _tmp68 + P(1, 11) * _tmp65 - P(13, 11) * _tmp86 - - P(14, 11) * _tmp82 - P(15, 11) * _tmp78 + P(2, 11) * _tmp91 + - P(3, 11) * _tmp75 + P(4, 11); - _p_new(5, 11) = P(0, 11) * _tmp118 + P(1, 11) * _tmp129 - P(13, 11) * _tmp121 - - P(14, 11) * _tmp126 - P(15, 11) * _tmp124 + P(2, 11) * _tmp116 + - P(3, 11) * _tmp120 + P(5, 11); - _p_new(6, 11) = P(0, 11) * _tmp148 + P(1, 11) * _tmp150 - P(13, 11) * _tmp151 - - P(14, 11) * _tmp152 - P(15, 11) * _tmp153 + P(2, 11) * _tmp149 + - P(3, 11) * _tmp147 + P(6, 11); - _p_new(7, 11) = P(4, 11) * dt + P(7, 11); - _p_new(8, 11) = P(5, 11) * dt + P(8, 11); - _p_new(9, 11) = P(6, 11) * dt + P(9, 11); + _p_new(2, 11) = _tmp47; + _p_new(3, 11) = _tmp54; + _p_new(4, 11) = P(0, 11) * _tmp64 + P(1, 11) * _tmp85 - P(13, 11) * _tmp69 - + P(14, 11) * _tmp81 - P(15, 11) * _tmp75 + P(2, 11) * _tmp90 + + P(3, 11) * _tmp93 + P(4, 11); + _p_new(5, 11) = P(0, 11) * _tmp131 + P(1, 11) * _tmp127 - P(13, 11) * _tmp124 - + P(14, 11) * _tmp122 - P(15, 11) * _tmp119 + P(2, 11) * _tmp133 + + P(3, 11) * _tmp129 + P(5, 11); + _p_new(6, 11) = P(0, 11) * _tmp158 + P(1, 11) * _tmp156 - P(13, 11) * _tmp154 - + P(14, 11) * _tmp150 - P(15, 11) * _tmp152 + P(2, 11) * _tmp157 + + P(3, 11) * _tmp159 + P(6, 11); + _p_new(7, 11) = P(4, 11) * d_vel_dt + P(7, 11); + _p_new(8, 11) = P(5, 11) * d_vel_dt + P(8, 11); + _p_new(9, 11) = P(6, 11) * d_vel_dt + P(9, 11); _p_new(10, 11) = P(10, 11); _p_new(11, 11) = P(11, 11); _p_new(12, 11) = 0; @@ -694,22 +702,22 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 11) = 0; _p_new(22, 11) = 0; _p_new(23, 11) = 0; - _p_new(0, 12) = _tmp14; - _p_new(1, 12) = _tmp34; - _p_new(2, 12) = _tmp44; - _p_new(3, 12) = _tmp52; - _p_new(4, 12) = P(0, 12) * _tmp68 + P(1, 12) * _tmp65 - P(13, 12) * _tmp86 - - P(14, 12) * _tmp82 - P(15, 12) * _tmp78 + P(2, 12) * _tmp91 + - P(3, 12) * _tmp75 + P(4, 12); - _p_new(5, 12) = P(0, 12) * _tmp118 + P(1, 12) * _tmp129 - P(13, 12) * _tmp121 - - P(14, 12) * _tmp126 - P(15, 12) * _tmp124 + P(2, 12) * _tmp116 + - P(3, 12) * _tmp120 + P(5, 12); - _p_new(6, 12) = P(0, 12) * _tmp148 + P(1, 12) * _tmp150 - P(13, 12) * _tmp151 - - P(14, 12) * _tmp152 - P(15, 12) * _tmp153 + P(2, 12) * _tmp149 + - P(3, 12) * _tmp147 + P(6, 12); - _p_new(7, 12) = P(4, 12) * dt + P(7, 12); - _p_new(8, 12) = P(5, 12) * dt + P(8, 12); - _p_new(9, 12) = P(6, 12) * dt + P(9, 12); + _p_new(0, 12) = _tmp15; + _p_new(1, 12) = _tmp33; + _p_new(2, 12) = _tmp49; + _p_new(3, 12) = _tmp55; + _p_new(4, 12) = P(0, 12) * _tmp64 + P(1, 12) * _tmp85 - P(13, 12) * _tmp69 - + P(14, 12) * _tmp81 - P(15, 12) * _tmp75 + P(2, 12) * _tmp90 + + P(3, 12) * _tmp93 + P(4, 12); + _p_new(5, 12) = P(0, 12) * _tmp131 + P(1, 12) * _tmp127 - P(13, 12) * _tmp124 - + P(14, 12) * _tmp122 - P(15, 12) * _tmp119 + P(2, 12) * _tmp133 + + P(3, 12) * _tmp129 + P(5, 12); + _p_new(6, 12) = P(0, 12) * _tmp158 + P(1, 12) * _tmp156 - P(13, 12) * _tmp154 - + P(14, 12) * _tmp150 - P(15, 12) * _tmp152 + P(2, 12) * _tmp157 + + P(3, 12) * _tmp159 + P(6, 12); + _p_new(7, 12) = P(4, 12) * d_vel_dt + P(7, 12); + _p_new(8, 12) = P(5, 12) * d_vel_dt + P(8, 12); + _p_new(9, 12) = P(6, 12) * d_vel_dt + P(9, 12); _p_new(10, 12) = P(10, 12); _p_new(11, 12) = P(11, 12); _p_new(12, 12) = P(12, 12); @@ -724,16 +732,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 12) = 0; _p_new(22, 12) = 0; _p_new(23, 12) = 0; - _p_new(0, 13) = _tmp87; - _p_new(1, 13) = _tmp94; - _p_new(2, 13) = _tmp101; - _p_new(3, 13) = _tmp103; - _p_new(4, 13) = _tmp107; + _p_new(0, 13) = _tmp65; + _p_new(1, 13) = _tmp96; + _p_new(2, 13) = _tmp100; + _p_new(3, 13) = _tmp104; + _p_new(4, 13) = _tmp108; _p_new(5, 13) = _tmp141; - _p_new(6, 13) = _tmp161; - _p_new(7, 13) = P(4, 13) * dt + P(7, 13); - _p_new(8, 13) = P(5, 13) * dt + P(8, 13); - _p_new(9, 13) = P(6, 13) * dt + P(9, 13); + _p_new(6, 13) = _tmp168; + _p_new(7, 13) = P(4, 13) * d_vel_dt + P(7, 13); + _p_new(8, 13) = P(5, 13) * d_vel_dt + P(8, 13); + _p_new(9, 13) = P(6, 13) * d_vel_dt + P(9, 13); _p_new(10, 13) = P(10, 13); _p_new(11, 13) = P(11, 13); _p_new(12, 13) = P(12, 13); @@ -748,16 +756,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 13) = 0; _p_new(22, 13) = 0; _p_new(23, 13) = 0; - _p_new(0, 14) = _tmp83; - _p_new(1, 14) = _tmp96; - _p_new(2, 14) = _tmp100; - _p_new(3, 14) = _tmp104; - _p_new(4, 14) = _tmp108; - _p_new(5, 14) = _tmp143; - _p_new(6, 14) = _tmp162; - _p_new(7, 14) = P(4, 14) * dt + P(7, 14); - _p_new(8, 14) = P(5, 14) * dt + P(8, 14); - _p_new(9, 14) = P(6, 14) * dt + P(9, 14); + _p_new(0, 14) = _tmp76; + _p_new(1, 14) = _tmp97; + _p_new(2, 14) = _tmp101; + _p_new(3, 14) = _tmp105; + _p_new(4, 14) = _tmp109; + _p_new(5, 14) = _tmp142; + _p_new(6, 14) = _tmp169; + _p_new(7, 14) = P(4, 14) * d_vel_dt + P(7, 14); + _p_new(8, 14) = P(5, 14) * d_vel_dt + P(8, 14); + _p_new(9, 14) = P(6, 14) * d_vel_dt + P(9, 14); _p_new(10, 14) = P(10, 14); _p_new(11, 14) = P(11, 14); _p_new(12, 14) = P(12, 14); @@ -772,16 +780,16 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 14) = 0; _p_new(22, 14) = 0; _p_new(23, 14) = 0; - _p_new(0, 15) = _tmp79; - _p_new(1, 15) = _tmp95; - _p_new(2, 15) = _tmp99; - _p_new(3, 15) = _tmp105; - _p_new(4, 15) = _tmp109; - _p_new(5, 15) = _tmp142; - _p_new(6, 15) = _tmp163; - _p_new(7, 15) = P(4, 15) * dt + P(7, 15); - _p_new(8, 15) = P(5, 15) * dt + P(8, 15); - _p_new(9, 15) = P(6, 15) * dt + P(9, 15); + _p_new(0, 15) = _tmp70; + _p_new(1, 15) = _tmp98; + _p_new(2, 15) = _tmp102; + _p_new(3, 15) = _tmp106; + _p_new(4, 15) = _tmp110; + _p_new(5, 15) = _tmp143; + _p_new(6, 15) = _tmp170; + _p_new(7, 15) = P(4, 15) * d_vel_dt + P(7, 15); + _p_new(8, 15) = P(5, 15) * d_vel_dt + P(8, 15); + _p_new(9, 15) = P(6, 15) * d_vel_dt + P(9, 15); _p_new(10, 15) = P(10, 15); _p_new(11, 15) = P(11, 15); _p_new(12, 15) = P(12, 15); @@ -796,26 +804,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 15) = 0; _p_new(22, 15) = 0; _p_new(23, 15) = 0; - _p_new(0, 16) = P(0, 16) + P(1, 16) * _tmp8 + P(10, 16) * _tmp11 + P(11, 16) * _tmp10 + - P(12, 16) * _tmp9 + P(2, 16) * _tmp5 + P(3, 16) * _tmp2; - _p_new(1, 16) = P(0, 16) * _tmp33 + P(1, 16) - P(10, 16) * _tmp30 + P(11, 16) * _tmp9 - - P(12, 16) * _tmp10 + P(2, 16) * _tmp32 + P(3, 16) * _tmp5; - _p_new(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp2 - P(10, 16) * _tmp9 - P(11, 16) * _tmp30 + - P(12, 16) * _tmp11 + P(2, 16) + P(3, 16) * _tmp33; - _p_new(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp10 - - P(11, 16) * _tmp11 - P(12, 16) * _tmp30 + P(2, 16) * _tmp8 + P(3, 16); - _p_new(4, 16) = P(0, 16) * _tmp68 + P(1, 16) * _tmp65 - P(13, 16) * _tmp86 - - P(14, 16) * _tmp82 - P(15, 16) * _tmp78 + P(2, 16) * _tmp91 + - P(3, 16) * _tmp75 + P(4, 16); - _p_new(5, 16) = P(0, 16) * _tmp118 + P(1, 16) * _tmp129 - P(13, 16) * _tmp121 - - P(14, 16) * _tmp126 - P(15, 16) * _tmp124 + P(2, 16) * _tmp116 + - P(3, 16) * _tmp120 + P(5, 16); - _p_new(6, 16) = P(0, 16) * _tmp148 + P(1, 16) * _tmp150 - P(13, 16) * _tmp151 - - P(14, 16) * _tmp152 - P(15, 16) * _tmp153 + P(2, 16) * _tmp149 + - P(3, 16) * _tmp147 + P(6, 16); - _p_new(7, 16) = P(4, 16) * dt + P(7, 16); - _p_new(8, 16) = P(5, 16) * dt + P(8, 16); - _p_new(9, 16) = P(6, 16) * dt + P(9, 16); + _p_new(0, 16) = P(0, 16) + P(1, 16) * _tmp9 + P(10, 16) * _tmp10 + P(11, 16) * _tmp12 + + P(12, 16) * _tmp11 + P(2, 16) * _tmp3 + P(3, 16) * _tmp6; + _p_new(1, 16) = P(0, 16) * _tmp31 + P(1, 16) - P(10, 16) * _tmp29 + P(11, 16) * _tmp11 - + P(12, 16) * _tmp12 + P(2, 16) * _tmp32 + P(3, 16) * _tmp3; + _p_new(2, 16) = P(0, 16) * _tmp43 + P(1, 16) * _tmp6 - P(10, 16) * _tmp11 - P(11, 16) * _tmp29 + + P(12, 16) * _tmp10 + P(2, 16) + P(3, 16) * _tmp31; + _p_new(3, 16) = P(0, 16) * _tmp32 + P(1, 16) * _tmp43 + P(10, 16) * _tmp12 - + P(11, 16) * _tmp10 - P(12, 16) * _tmp29 + P(2, 16) * _tmp9 + P(3, 16); + _p_new(4, 16) = P(0, 16) * _tmp64 + P(1, 16) * _tmp85 - P(13, 16) * _tmp69 - + P(14, 16) * _tmp81 - P(15, 16) * _tmp75 + P(2, 16) * _tmp90 + + P(3, 16) * _tmp93 + P(4, 16); + _p_new(5, 16) = P(0, 16) * _tmp131 + P(1, 16) * _tmp127 - P(13, 16) * _tmp124 - + P(14, 16) * _tmp122 - P(15, 16) * _tmp119 + P(2, 16) * _tmp133 + + P(3, 16) * _tmp129 + P(5, 16); + _p_new(6, 16) = P(0, 16) * _tmp158 + P(1, 16) * _tmp156 - P(13, 16) * _tmp154 - + P(14, 16) * _tmp150 - P(15, 16) * _tmp152 + P(2, 16) * _tmp157 + + P(3, 16) * _tmp159 + P(6, 16); + _p_new(7, 16) = P(4, 16) * d_vel_dt + P(7, 16); + _p_new(8, 16) = P(5, 16) * d_vel_dt + P(8, 16); + _p_new(9, 16) = P(6, 16) * d_vel_dt + P(9, 16); _p_new(10, 16) = P(10, 16); _p_new(11, 16) = P(11, 16); _p_new(12, 16) = P(12, 16); @@ -830,26 +838,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 16) = 0; _p_new(22, 16) = 0; _p_new(23, 16) = 0; - _p_new(0, 17) = P(0, 17) + P(1, 17) * _tmp8 + P(10, 17) * _tmp11 + P(11, 17) * _tmp10 + - P(12, 17) * _tmp9 + P(2, 17) * _tmp5 + P(3, 17) * _tmp2; - _p_new(1, 17) = P(0, 17) * _tmp33 + P(1, 17) - P(10, 17) * _tmp30 + P(11, 17) * _tmp9 - - P(12, 17) * _tmp10 + P(2, 17) * _tmp32 + P(3, 17) * _tmp5; - _p_new(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp2 - P(10, 17) * _tmp9 - P(11, 17) * _tmp30 + - P(12, 17) * _tmp11 + P(2, 17) + P(3, 17) * _tmp33; - _p_new(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp10 - - P(11, 17) * _tmp11 - P(12, 17) * _tmp30 + P(2, 17) * _tmp8 + P(3, 17); - _p_new(4, 17) = P(0, 17) * _tmp68 + P(1, 17) * _tmp65 - P(13, 17) * _tmp86 - - P(14, 17) * _tmp82 - P(15, 17) * _tmp78 + P(2, 17) * _tmp91 + - P(3, 17) * _tmp75 + P(4, 17); - _p_new(5, 17) = P(0, 17) * _tmp118 + P(1, 17) * _tmp129 - P(13, 17) * _tmp121 - - P(14, 17) * _tmp126 - P(15, 17) * _tmp124 + P(2, 17) * _tmp116 + - P(3, 17) * _tmp120 + P(5, 17); - _p_new(6, 17) = P(0, 17) * _tmp148 + P(1, 17) * _tmp150 - P(13, 17) * _tmp151 - - P(14, 17) * _tmp152 - P(15, 17) * _tmp153 + P(2, 17) * _tmp149 + - P(3, 17) * _tmp147 + P(6, 17); - _p_new(7, 17) = P(4, 17) * dt + P(7, 17); - _p_new(8, 17) = P(5, 17) * dt + P(8, 17); - _p_new(9, 17) = P(6, 17) * dt + P(9, 17); + _p_new(0, 17) = P(0, 17) + P(1, 17) * _tmp9 + P(10, 17) * _tmp10 + P(11, 17) * _tmp12 + + P(12, 17) * _tmp11 + P(2, 17) * _tmp3 + P(3, 17) * _tmp6; + _p_new(1, 17) = P(0, 17) * _tmp31 + P(1, 17) - P(10, 17) * _tmp29 + P(11, 17) * _tmp11 - + P(12, 17) * _tmp12 + P(2, 17) * _tmp32 + P(3, 17) * _tmp3; + _p_new(2, 17) = P(0, 17) * _tmp43 + P(1, 17) * _tmp6 - P(10, 17) * _tmp11 - P(11, 17) * _tmp29 + + P(12, 17) * _tmp10 + P(2, 17) + P(3, 17) * _tmp31; + _p_new(3, 17) = P(0, 17) * _tmp32 + P(1, 17) * _tmp43 + P(10, 17) * _tmp12 - + P(11, 17) * _tmp10 - P(12, 17) * _tmp29 + P(2, 17) * _tmp9 + P(3, 17); + _p_new(4, 17) = P(0, 17) * _tmp64 + P(1, 17) * _tmp85 - P(13, 17) * _tmp69 - + P(14, 17) * _tmp81 - P(15, 17) * _tmp75 + P(2, 17) * _tmp90 + + P(3, 17) * _tmp93 + P(4, 17); + _p_new(5, 17) = P(0, 17) * _tmp131 + P(1, 17) * _tmp127 - P(13, 17) * _tmp124 - + P(14, 17) * _tmp122 - P(15, 17) * _tmp119 + P(2, 17) * _tmp133 + + P(3, 17) * _tmp129 + P(5, 17); + _p_new(6, 17) = P(0, 17) * _tmp158 + P(1, 17) * _tmp156 - P(13, 17) * _tmp154 - + P(14, 17) * _tmp150 - P(15, 17) * _tmp152 + P(2, 17) * _tmp157 + + P(3, 17) * _tmp159 + P(6, 17); + _p_new(7, 17) = P(4, 17) * d_vel_dt + P(7, 17); + _p_new(8, 17) = P(5, 17) * d_vel_dt + P(8, 17); + _p_new(9, 17) = P(6, 17) * d_vel_dt + P(9, 17); _p_new(10, 17) = P(10, 17); _p_new(11, 17) = P(11, 17); _p_new(12, 17) = P(12, 17); @@ -864,26 +872,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 17) = 0; _p_new(22, 17) = 0; _p_new(23, 17) = 0; - _p_new(0, 18) = P(0, 18) + P(1, 18) * _tmp8 + P(10, 18) * _tmp11 + P(11, 18) * _tmp10 + - P(12, 18) * _tmp9 + P(2, 18) * _tmp5 + P(3, 18) * _tmp2; - _p_new(1, 18) = P(0, 18) * _tmp33 + P(1, 18) - P(10, 18) * _tmp30 + P(11, 18) * _tmp9 - - P(12, 18) * _tmp10 + P(2, 18) * _tmp32 + P(3, 18) * _tmp5; - _p_new(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp2 - P(10, 18) * _tmp9 - P(11, 18) * _tmp30 + - P(12, 18) * _tmp11 + P(2, 18) + P(3, 18) * _tmp33; - _p_new(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp10 - - P(11, 18) * _tmp11 - P(12, 18) * _tmp30 + P(2, 18) * _tmp8 + P(3, 18); - _p_new(4, 18) = P(0, 18) * _tmp68 + P(1, 18) * _tmp65 - P(13, 18) * _tmp86 - - P(14, 18) * _tmp82 - P(15, 18) * _tmp78 + P(2, 18) * _tmp91 + - P(3, 18) * _tmp75 + P(4, 18); - _p_new(5, 18) = P(0, 18) * _tmp118 + P(1, 18) * _tmp129 - P(13, 18) * _tmp121 - - P(14, 18) * _tmp126 - P(15, 18) * _tmp124 + P(2, 18) * _tmp116 + - P(3, 18) * _tmp120 + P(5, 18); - _p_new(6, 18) = P(0, 18) * _tmp148 + P(1, 18) * _tmp150 - P(13, 18) * _tmp151 - - P(14, 18) * _tmp152 - P(15, 18) * _tmp153 + P(2, 18) * _tmp149 + - P(3, 18) * _tmp147 + P(6, 18); - _p_new(7, 18) = P(4, 18) * dt + P(7, 18); - _p_new(8, 18) = P(5, 18) * dt + P(8, 18); - _p_new(9, 18) = P(6, 18) * dt + P(9, 18); + _p_new(0, 18) = P(0, 18) + P(1, 18) * _tmp9 + P(10, 18) * _tmp10 + P(11, 18) * _tmp12 + + P(12, 18) * _tmp11 + P(2, 18) * _tmp3 + P(3, 18) * _tmp6; + _p_new(1, 18) = P(0, 18) * _tmp31 + P(1, 18) - P(10, 18) * _tmp29 + P(11, 18) * _tmp11 - + P(12, 18) * _tmp12 + P(2, 18) * _tmp32 + P(3, 18) * _tmp3; + _p_new(2, 18) = P(0, 18) * _tmp43 + P(1, 18) * _tmp6 - P(10, 18) * _tmp11 - P(11, 18) * _tmp29 + + P(12, 18) * _tmp10 + P(2, 18) + P(3, 18) * _tmp31; + _p_new(3, 18) = P(0, 18) * _tmp32 + P(1, 18) * _tmp43 + P(10, 18) * _tmp12 - + P(11, 18) * _tmp10 - P(12, 18) * _tmp29 + P(2, 18) * _tmp9 + P(3, 18); + _p_new(4, 18) = P(0, 18) * _tmp64 + P(1, 18) * _tmp85 - P(13, 18) * _tmp69 - + P(14, 18) * _tmp81 - P(15, 18) * _tmp75 + P(2, 18) * _tmp90 + + P(3, 18) * _tmp93 + P(4, 18); + _p_new(5, 18) = P(0, 18) * _tmp131 + P(1, 18) * _tmp127 - P(13, 18) * _tmp124 - + P(14, 18) * _tmp122 - P(15, 18) * _tmp119 + P(2, 18) * _tmp133 + + P(3, 18) * _tmp129 + P(5, 18); + _p_new(6, 18) = P(0, 18) * _tmp158 + P(1, 18) * _tmp156 - P(13, 18) * _tmp154 - + P(14, 18) * _tmp150 - P(15, 18) * _tmp152 + P(2, 18) * _tmp157 + + P(3, 18) * _tmp159 + P(6, 18); + _p_new(7, 18) = P(4, 18) * d_vel_dt + P(7, 18); + _p_new(8, 18) = P(5, 18) * d_vel_dt + P(8, 18); + _p_new(9, 18) = P(6, 18) * d_vel_dt + P(9, 18); _p_new(10, 18) = P(10, 18); _p_new(11, 18) = P(11, 18); _p_new(12, 18) = P(12, 18); @@ -898,26 +906,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 18) = 0; _p_new(22, 18) = 0; _p_new(23, 18) = 0; - _p_new(0, 19) = P(0, 19) + P(1, 19) * _tmp8 + P(10, 19) * _tmp11 + P(11, 19) * _tmp10 + - P(12, 19) * _tmp9 + P(2, 19) * _tmp5 + P(3, 19) * _tmp2; - _p_new(1, 19) = P(0, 19) * _tmp33 + P(1, 19) - P(10, 19) * _tmp30 + P(11, 19) * _tmp9 - - P(12, 19) * _tmp10 + P(2, 19) * _tmp32 + P(3, 19) * _tmp5; - _p_new(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp2 - P(10, 19) * _tmp9 - P(11, 19) * _tmp30 + - P(12, 19) * _tmp11 + P(2, 19) + P(3, 19) * _tmp33; - _p_new(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp10 - - P(11, 19) * _tmp11 - P(12, 19) * _tmp30 + P(2, 19) * _tmp8 + P(3, 19); - _p_new(4, 19) = P(0, 19) * _tmp68 + P(1, 19) * _tmp65 - P(13, 19) * _tmp86 - - P(14, 19) * _tmp82 - P(15, 19) * _tmp78 + P(2, 19) * _tmp91 + - P(3, 19) * _tmp75 + P(4, 19); - _p_new(5, 19) = P(0, 19) * _tmp118 + P(1, 19) * _tmp129 - P(13, 19) * _tmp121 - - P(14, 19) * _tmp126 - P(15, 19) * _tmp124 + P(2, 19) * _tmp116 + - P(3, 19) * _tmp120 + P(5, 19); - _p_new(6, 19) = P(0, 19) * _tmp148 + P(1, 19) * _tmp150 - P(13, 19) * _tmp151 - - P(14, 19) * _tmp152 - P(15, 19) * _tmp153 + P(2, 19) * _tmp149 + - P(3, 19) * _tmp147 + P(6, 19); - _p_new(7, 19) = P(4, 19) * dt + P(7, 19); - _p_new(8, 19) = P(5, 19) * dt + P(8, 19); - _p_new(9, 19) = P(6, 19) * dt + P(9, 19); + _p_new(0, 19) = P(0, 19) + P(1, 19) * _tmp9 + P(10, 19) * _tmp10 + P(11, 19) * _tmp12 + + P(12, 19) * _tmp11 + P(2, 19) * _tmp3 + P(3, 19) * _tmp6; + _p_new(1, 19) = P(0, 19) * _tmp31 + P(1, 19) - P(10, 19) * _tmp29 + P(11, 19) * _tmp11 - + P(12, 19) * _tmp12 + P(2, 19) * _tmp32 + P(3, 19) * _tmp3; + _p_new(2, 19) = P(0, 19) * _tmp43 + P(1, 19) * _tmp6 - P(10, 19) * _tmp11 - P(11, 19) * _tmp29 + + P(12, 19) * _tmp10 + P(2, 19) + P(3, 19) * _tmp31; + _p_new(3, 19) = P(0, 19) * _tmp32 + P(1, 19) * _tmp43 + P(10, 19) * _tmp12 - + P(11, 19) * _tmp10 - P(12, 19) * _tmp29 + P(2, 19) * _tmp9 + P(3, 19); + _p_new(4, 19) = P(0, 19) * _tmp64 + P(1, 19) * _tmp85 - P(13, 19) * _tmp69 - + P(14, 19) * _tmp81 - P(15, 19) * _tmp75 + P(2, 19) * _tmp90 + + P(3, 19) * _tmp93 + P(4, 19); + _p_new(5, 19) = P(0, 19) * _tmp131 + P(1, 19) * _tmp127 - P(13, 19) * _tmp124 - + P(14, 19) * _tmp122 - P(15, 19) * _tmp119 + P(2, 19) * _tmp133 + + P(3, 19) * _tmp129 + P(5, 19); + _p_new(6, 19) = P(0, 19) * _tmp158 + P(1, 19) * _tmp156 - P(13, 19) * _tmp154 - + P(14, 19) * _tmp150 - P(15, 19) * _tmp152 + P(2, 19) * _tmp157 + + P(3, 19) * _tmp159 + P(6, 19); + _p_new(7, 19) = P(4, 19) * d_vel_dt + P(7, 19); + _p_new(8, 19) = P(5, 19) * d_vel_dt + P(8, 19); + _p_new(9, 19) = P(6, 19) * d_vel_dt + P(9, 19); _p_new(10, 19) = P(10, 19); _p_new(11, 19) = P(11, 19); _p_new(12, 19) = P(12, 19); @@ -932,26 +940,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 19) = 0; _p_new(22, 19) = 0; _p_new(23, 19) = 0; - _p_new(0, 20) = P(0, 20) + P(1, 20) * _tmp8 + P(10, 20) * _tmp11 + P(11, 20) * _tmp10 + - P(12, 20) * _tmp9 + P(2, 20) * _tmp5 + P(3, 20) * _tmp2; - _p_new(1, 20) = P(0, 20) * _tmp33 + P(1, 20) - P(10, 20) * _tmp30 + P(11, 20) * _tmp9 - - P(12, 20) * _tmp10 + P(2, 20) * _tmp32 + P(3, 20) * _tmp5; - _p_new(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp2 - P(10, 20) * _tmp9 - P(11, 20) * _tmp30 + - P(12, 20) * _tmp11 + P(2, 20) + P(3, 20) * _tmp33; - _p_new(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp10 - - P(11, 20) * _tmp11 - P(12, 20) * _tmp30 + P(2, 20) * _tmp8 + P(3, 20); - _p_new(4, 20) = P(0, 20) * _tmp68 + P(1, 20) * _tmp65 - P(13, 20) * _tmp86 - - P(14, 20) * _tmp82 - P(15, 20) * _tmp78 + P(2, 20) * _tmp91 + - P(3, 20) * _tmp75 + P(4, 20); - _p_new(5, 20) = P(0, 20) * _tmp118 + P(1, 20) * _tmp129 - P(13, 20) * _tmp121 - - P(14, 20) * _tmp126 - P(15, 20) * _tmp124 + P(2, 20) * _tmp116 + - P(3, 20) * _tmp120 + P(5, 20); - _p_new(6, 20) = P(0, 20) * _tmp148 + P(1, 20) * _tmp150 - P(13, 20) * _tmp151 - - P(14, 20) * _tmp152 - P(15, 20) * _tmp153 + P(2, 20) * _tmp149 + - P(3, 20) * _tmp147 + P(6, 20); - _p_new(7, 20) = P(4, 20) * dt + P(7, 20); - _p_new(8, 20) = P(5, 20) * dt + P(8, 20); - _p_new(9, 20) = P(6, 20) * dt + P(9, 20); + _p_new(0, 20) = P(0, 20) + P(1, 20) * _tmp9 + P(10, 20) * _tmp10 + P(11, 20) * _tmp12 + + P(12, 20) * _tmp11 + P(2, 20) * _tmp3 + P(3, 20) * _tmp6; + _p_new(1, 20) = P(0, 20) * _tmp31 + P(1, 20) - P(10, 20) * _tmp29 + P(11, 20) * _tmp11 - + P(12, 20) * _tmp12 + P(2, 20) * _tmp32 + P(3, 20) * _tmp3; + _p_new(2, 20) = P(0, 20) * _tmp43 + P(1, 20) * _tmp6 - P(10, 20) * _tmp11 - P(11, 20) * _tmp29 + + P(12, 20) * _tmp10 + P(2, 20) + P(3, 20) * _tmp31; + _p_new(3, 20) = P(0, 20) * _tmp32 + P(1, 20) * _tmp43 + P(10, 20) * _tmp12 - + P(11, 20) * _tmp10 - P(12, 20) * _tmp29 + P(2, 20) * _tmp9 + P(3, 20); + _p_new(4, 20) = P(0, 20) * _tmp64 + P(1, 20) * _tmp85 - P(13, 20) * _tmp69 - + P(14, 20) * _tmp81 - P(15, 20) * _tmp75 + P(2, 20) * _tmp90 + + P(3, 20) * _tmp93 + P(4, 20); + _p_new(5, 20) = P(0, 20) * _tmp131 + P(1, 20) * _tmp127 - P(13, 20) * _tmp124 - + P(14, 20) * _tmp122 - P(15, 20) * _tmp119 + P(2, 20) * _tmp133 + + P(3, 20) * _tmp129 + P(5, 20); + _p_new(6, 20) = P(0, 20) * _tmp158 + P(1, 20) * _tmp156 - P(13, 20) * _tmp154 - + P(14, 20) * _tmp150 - P(15, 20) * _tmp152 + P(2, 20) * _tmp157 + + P(3, 20) * _tmp159 + P(6, 20); + _p_new(7, 20) = P(4, 20) * d_vel_dt + P(7, 20); + _p_new(8, 20) = P(5, 20) * d_vel_dt + P(8, 20); + _p_new(9, 20) = P(6, 20) * d_vel_dt + P(9, 20); _p_new(10, 20) = P(10, 20); _p_new(11, 20) = P(11, 20); _p_new(12, 20) = P(12, 20); @@ -966,26 +974,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 20) = 0; _p_new(22, 20) = 0; _p_new(23, 20) = 0; - _p_new(0, 21) = P(0, 21) + P(1, 21) * _tmp8 + P(10, 21) * _tmp11 + P(11, 21) * _tmp10 + - P(12, 21) * _tmp9 + P(2, 21) * _tmp5 + P(3, 21) * _tmp2; - _p_new(1, 21) = P(0, 21) * _tmp33 + P(1, 21) - P(10, 21) * _tmp30 + P(11, 21) * _tmp9 - - P(12, 21) * _tmp10 + P(2, 21) * _tmp32 + P(3, 21) * _tmp5; - _p_new(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp2 - P(10, 21) * _tmp9 - P(11, 21) * _tmp30 + - P(12, 21) * _tmp11 + P(2, 21) + P(3, 21) * _tmp33; - _p_new(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp10 - - P(11, 21) * _tmp11 - P(12, 21) * _tmp30 + P(2, 21) * _tmp8 + P(3, 21); - _p_new(4, 21) = P(0, 21) * _tmp68 + P(1, 21) * _tmp65 - P(13, 21) * _tmp86 - - P(14, 21) * _tmp82 - P(15, 21) * _tmp78 + P(2, 21) * _tmp91 + - P(3, 21) * _tmp75 + P(4, 21); - _p_new(5, 21) = P(0, 21) * _tmp118 + P(1, 21) * _tmp129 - P(13, 21) * _tmp121 - - P(14, 21) * _tmp126 - P(15, 21) * _tmp124 + P(2, 21) * _tmp116 + - P(3, 21) * _tmp120 + P(5, 21); - _p_new(6, 21) = P(0, 21) * _tmp148 + P(1, 21) * _tmp150 - P(13, 21) * _tmp151 - - P(14, 21) * _tmp152 - P(15, 21) * _tmp153 + P(2, 21) * _tmp149 + - P(3, 21) * _tmp147 + P(6, 21); - _p_new(7, 21) = P(4, 21) * dt + P(7, 21); - _p_new(8, 21) = P(5, 21) * dt + P(8, 21); - _p_new(9, 21) = P(6, 21) * dt + P(9, 21); + _p_new(0, 21) = P(0, 21) + P(1, 21) * _tmp9 + P(10, 21) * _tmp10 + P(11, 21) * _tmp12 + + P(12, 21) * _tmp11 + P(2, 21) * _tmp3 + P(3, 21) * _tmp6; + _p_new(1, 21) = P(0, 21) * _tmp31 + P(1, 21) - P(10, 21) * _tmp29 + P(11, 21) * _tmp11 - + P(12, 21) * _tmp12 + P(2, 21) * _tmp32 + P(3, 21) * _tmp3; + _p_new(2, 21) = P(0, 21) * _tmp43 + P(1, 21) * _tmp6 - P(10, 21) * _tmp11 - P(11, 21) * _tmp29 + + P(12, 21) * _tmp10 + P(2, 21) + P(3, 21) * _tmp31; + _p_new(3, 21) = P(0, 21) * _tmp32 + P(1, 21) * _tmp43 + P(10, 21) * _tmp12 - + P(11, 21) * _tmp10 - P(12, 21) * _tmp29 + P(2, 21) * _tmp9 + P(3, 21); + _p_new(4, 21) = P(0, 21) * _tmp64 + P(1, 21) * _tmp85 - P(13, 21) * _tmp69 - + P(14, 21) * _tmp81 - P(15, 21) * _tmp75 + P(2, 21) * _tmp90 + + P(3, 21) * _tmp93 + P(4, 21); + _p_new(5, 21) = P(0, 21) * _tmp131 + P(1, 21) * _tmp127 - P(13, 21) * _tmp124 - + P(14, 21) * _tmp122 - P(15, 21) * _tmp119 + P(2, 21) * _tmp133 + + P(3, 21) * _tmp129 + P(5, 21); + _p_new(6, 21) = P(0, 21) * _tmp158 + P(1, 21) * _tmp156 - P(13, 21) * _tmp154 - + P(14, 21) * _tmp150 - P(15, 21) * _tmp152 + P(2, 21) * _tmp157 + + P(3, 21) * _tmp159 + P(6, 21); + _p_new(7, 21) = P(4, 21) * d_vel_dt + P(7, 21); + _p_new(8, 21) = P(5, 21) * d_vel_dt + P(8, 21); + _p_new(9, 21) = P(6, 21) * d_vel_dt + P(9, 21); _p_new(10, 21) = P(10, 21); _p_new(11, 21) = P(11, 21); _p_new(12, 21) = P(12, 21); @@ -1000,26 +1008,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 21) = P(21, 21); _p_new(22, 21) = 0; _p_new(23, 21) = 0; - _p_new(0, 22) = P(0, 22) + P(1, 22) * _tmp8 + P(10, 22) * _tmp11 + P(11, 22) * _tmp10 + - P(12, 22) * _tmp9 + P(2, 22) * _tmp5 + P(3, 22) * _tmp2; - _p_new(1, 22) = P(0, 22) * _tmp33 + P(1, 22) - P(10, 22) * _tmp30 + P(11, 22) * _tmp9 - - P(12, 22) * _tmp10 + P(2, 22) * _tmp32 + P(3, 22) * _tmp5; - _p_new(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp2 - P(10, 22) * _tmp9 - P(11, 22) * _tmp30 + - P(12, 22) * _tmp11 + P(2, 22) + P(3, 22) * _tmp33; - _p_new(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp10 - - P(11, 22) * _tmp11 - P(12, 22) * _tmp30 + P(2, 22) * _tmp8 + P(3, 22); - _p_new(4, 22) = P(0, 22) * _tmp68 + P(1, 22) * _tmp65 - P(13, 22) * _tmp86 - - P(14, 22) * _tmp82 - P(15, 22) * _tmp78 + P(2, 22) * _tmp91 + - P(3, 22) * _tmp75 + P(4, 22); - _p_new(5, 22) = P(0, 22) * _tmp118 + P(1, 22) * _tmp129 - P(13, 22) * _tmp121 - - P(14, 22) * _tmp126 - P(15, 22) * _tmp124 + P(2, 22) * _tmp116 + - P(3, 22) * _tmp120 + P(5, 22); - _p_new(6, 22) = P(0, 22) * _tmp148 + P(1, 22) * _tmp150 - P(13, 22) * _tmp151 - - P(14, 22) * _tmp152 - P(15, 22) * _tmp153 + P(2, 22) * _tmp149 + - P(3, 22) * _tmp147 + P(6, 22); - _p_new(7, 22) = P(4, 22) * dt + P(7, 22); - _p_new(8, 22) = P(5, 22) * dt + P(8, 22); - _p_new(9, 22) = P(6, 22) * dt + P(9, 22); + _p_new(0, 22) = P(0, 22) + P(1, 22) * _tmp9 + P(10, 22) * _tmp10 + P(11, 22) * _tmp12 + + P(12, 22) * _tmp11 + P(2, 22) * _tmp3 + P(3, 22) * _tmp6; + _p_new(1, 22) = P(0, 22) * _tmp31 + P(1, 22) - P(10, 22) * _tmp29 + P(11, 22) * _tmp11 - + P(12, 22) * _tmp12 + P(2, 22) * _tmp32 + P(3, 22) * _tmp3; + _p_new(2, 22) = P(0, 22) * _tmp43 + P(1, 22) * _tmp6 - P(10, 22) * _tmp11 - P(11, 22) * _tmp29 + + P(12, 22) * _tmp10 + P(2, 22) + P(3, 22) * _tmp31; + _p_new(3, 22) = P(0, 22) * _tmp32 + P(1, 22) * _tmp43 + P(10, 22) * _tmp12 - + P(11, 22) * _tmp10 - P(12, 22) * _tmp29 + P(2, 22) * _tmp9 + P(3, 22); + _p_new(4, 22) = P(0, 22) * _tmp64 + P(1, 22) * _tmp85 - P(13, 22) * _tmp69 - + P(14, 22) * _tmp81 - P(15, 22) * _tmp75 + P(2, 22) * _tmp90 + + P(3, 22) * _tmp93 + P(4, 22); + _p_new(5, 22) = P(0, 22) * _tmp131 + P(1, 22) * _tmp127 - P(13, 22) * _tmp124 - + P(14, 22) * _tmp122 - P(15, 22) * _tmp119 + P(2, 22) * _tmp133 + + P(3, 22) * _tmp129 + P(5, 22); + _p_new(6, 22) = P(0, 22) * _tmp158 + P(1, 22) * _tmp156 - P(13, 22) * _tmp154 - + P(14, 22) * _tmp150 - P(15, 22) * _tmp152 + P(2, 22) * _tmp157 + + P(3, 22) * _tmp159 + P(6, 22); + _p_new(7, 22) = P(4, 22) * d_vel_dt + P(7, 22); + _p_new(8, 22) = P(5, 22) * d_vel_dt + P(8, 22); + _p_new(9, 22) = P(6, 22) * d_vel_dt + P(9, 22); _p_new(10, 22) = P(10, 22); _p_new(11, 22) = P(11, 22); _p_new(12, 22) = P(12, 22); @@ -1034,26 +1042,26 @@ void PredictCovariance(const matrix::Matrix& state, _p_new(21, 22) = P(21, 22); _p_new(22, 22) = P(22, 22); _p_new(23, 22) = 0; - _p_new(0, 23) = P(0, 23) + P(1, 23) * _tmp8 + P(10, 23) * _tmp11 + P(11, 23) * _tmp10 + - P(12, 23) * _tmp9 + P(2, 23) * _tmp5 + P(3, 23) * _tmp2; - _p_new(1, 23) = P(0, 23) * _tmp33 + P(1, 23) - P(10, 23) * _tmp30 + P(11, 23) * _tmp9 - - P(12, 23) * _tmp10 + P(2, 23) * _tmp32 + P(3, 23) * _tmp5; - _p_new(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp2 - P(10, 23) * _tmp9 - P(11, 23) * _tmp30 + - P(12, 23) * _tmp11 + P(2, 23) + P(3, 23) * _tmp33; - _p_new(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp10 - - P(11, 23) * _tmp11 - P(12, 23) * _tmp30 + P(2, 23) * _tmp8 + P(3, 23); - _p_new(4, 23) = P(0, 23) * _tmp68 + P(1, 23) * _tmp65 - P(13, 23) * _tmp86 - - P(14, 23) * _tmp82 - P(15, 23) * _tmp78 + P(2, 23) * _tmp91 + - P(3, 23) * _tmp75 + P(4, 23); - _p_new(5, 23) = P(0, 23) * _tmp118 + P(1, 23) * _tmp129 - P(13, 23) * _tmp121 - - P(14, 23) * _tmp126 - P(15, 23) * _tmp124 + P(2, 23) * _tmp116 + - P(3, 23) * _tmp120 + P(5, 23); - _p_new(6, 23) = P(0, 23) * _tmp148 + P(1, 23) * _tmp150 - P(13, 23) * _tmp151 - - P(14, 23) * _tmp152 - P(15, 23) * _tmp153 + P(2, 23) * _tmp149 + - P(3, 23) * _tmp147 + P(6, 23); - _p_new(7, 23) = P(4, 23) * dt + P(7, 23); - _p_new(8, 23) = P(5, 23) * dt + P(8, 23); - _p_new(9, 23) = P(6, 23) * dt + P(9, 23); + _p_new(0, 23) = P(0, 23) + P(1, 23) * _tmp9 + P(10, 23) * _tmp10 + P(11, 23) * _tmp12 + + P(12, 23) * _tmp11 + P(2, 23) * _tmp3 + P(3, 23) * _tmp6; + _p_new(1, 23) = P(0, 23) * _tmp31 + P(1, 23) - P(10, 23) * _tmp29 + P(11, 23) * _tmp11 - + P(12, 23) * _tmp12 + P(2, 23) * _tmp32 + P(3, 23) * _tmp3; + _p_new(2, 23) = P(0, 23) * _tmp43 + P(1, 23) * _tmp6 - P(10, 23) * _tmp11 - P(11, 23) * _tmp29 + + P(12, 23) * _tmp10 + P(2, 23) + P(3, 23) * _tmp31; + _p_new(3, 23) = P(0, 23) * _tmp32 + P(1, 23) * _tmp43 + P(10, 23) * _tmp12 - + P(11, 23) * _tmp10 - P(12, 23) * _tmp29 + P(2, 23) * _tmp9 + P(3, 23); + _p_new(4, 23) = P(0, 23) * _tmp64 + P(1, 23) * _tmp85 - P(13, 23) * _tmp69 - + P(14, 23) * _tmp81 - P(15, 23) * _tmp75 + P(2, 23) * _tmp90 + + P(3, 23) * _tmp93 + P(4, 23); + _p_new(5, 23) = P(0, 23) * _tmp131 + P(1, 23) * _tmp127 - P(13, 23) * _tmp124 - + P(14, 23) * _tmp122 - P(15, 23) * _tmp119 + P(2, 23) * _tmp133 + + P(3, 23) * _tmp129 + P(5, 23); + _p_new(6, 23) = P(0, 23) * _tmp158 + P(1, 23) * _tmp156 - P(13, 23) * _tmp154 - + P(14, 23) * _tmp150 - P(15, 23) * _tmp152 + P(2, 23) * _tmp157 + + P(3, 23) * _tmp159 + P(6, 23); + _p_new(7, 23) = P(4, 23) * d_vel_dt + P(7, 23); + _p_new(8, 23) = P(5, 23) * d_vel_dt + P(8, 23); + _p_new(9, 23) = P(6, 23) * d_vel_dt + P(9, 23); _p_new(10, 23) = P(10, 23); _p_new(11, 23) = P(11, 23); _p_new(12, 23) = P(12, 23); diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h new file mode 100644 index 0000000000..a7a1831f71 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/quat_var_to_rot_var.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: quat_var_to_rot_var + * + * Args: + * state: Matrix24_1 + * P: Matrix24_24 + * epsilon: Scalar + * + * Outputs: + * rot_var: Matrix31 + */ +template +void QuatVarToRotVar(const matrix::Matrix& state, + const matrix::Matrix& P, const Scalar epsilon, + matrix::Matrix* const rot_var = nullptr) { + // Total ops: 61 + + // Input arrays + + // Intermediate terms (17) + const Scalar _tmp0 = std::fabs(state(0, 0)); + const Scalar _tmp1 = 1 - epsilon; + const Scalar _tmp2 = math::min(_tmp0, _tmp1); + const Scalar _tmp3 = 1 - std::pow(_tmp2, Scalar(2)); + const Scalar _tmp4 = (((state(0, 0)) > 0) - ((state(0, 0)) < 0)); + const Scalar _tmp5 = 2 * math::min(0, _tmp4) + 1; + const Scalar _tmp6 = _tmp5 * std::acos(_tmp2); + const Scalar _tmp7 = 2 * _tmp6 / std::sqrt(_tmp3); + const Scalar _tmp8 = _tmp4 * ((((-_tmp0 + _tmp1) > 0) - ((-_tmp0 + _tmp1) < 0)) + 1); + const Scalar _tmp9 = _tmp8 * state(1, 0); + const Scalar _tmp10 = _tmp2 * _tmp6 / (_tmp3 * std::sqrt(_tmp3)); + const Scalar _tmp11 = _tmp5 / _tmp3; + const Scalar _tmp12 = _tmp10 * _tmp9 - _tmp11 * _tmp9; + const Scalar _tmp13 = _tmp10 * _tmp8; + const Scalar _tmp14 = _tmp11 * _tmp8; + const Scalar _tmp15 = _tmp13 * state(2, 0) - _tmp14 * state(2, 0); + const Scalar _tmp16 = _tmp13 * state(3, 0) - _tmp14 * state(3, 0); + + // Output terms (1) + if (rot_var != nullptr) { + matrix::Matrix& _rot_var = (*rot_var); + + _rot_var(0, 0) = _tmp12 * (P(0, 0) * _tmp12 + P(1, 0) * _tmp7) + + _tmp7 * (P(0, 1) * _tmp12 + P(1, 1) * _tmp7); + _rot_var(1, 0) = _tmp15 * (P(0, 0) * _tmp15 + P(2, 0) * _tmp7) + + _tmp7 * (P(0, 2) * _tmp15 + P(2, 2) * _tmp7); + _rot_var(2, 0) = _tmp16 * (P(0, 0) * _tmp16 + P(3, 0) * _tmp7) + + _tmp7 * (P(0, 3) * _tmp16 + P(3, 3) * _tmp7); + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h new file mode 100644 index 0000000000..8b2166f223 --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h @@ -0,0 +1,123 @@ +// ----------------------------------------------------------------------------- +// 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: rot_var_ned_to_lower_triangular_quat_cov + * + * Args: + * state: Matrix24_1 + * rot_var_ned: Matrix31 + * + * Outputs: + * q_cov_lower_triangle: Matrix44 + */ +template +void RotVarNedToLowerTriangularQuatCov( + const matrix::Matrix& state, const matrix::Matrix& rot_var_ned, + matrix::Matrix* const q_cov_lower_triangle = nullptr) { + // Total ops: 185 + + // Input arrays + + // Intermediate terms (54) + const Scalar _tmp0 = 2 * state(0, 0) * state(2, 0); + const Scalar _tmp1 = 2 * state(3, 0); + const Scalar _tmp2 = _tmp1 * state(1, 0); + const Scalar _tmp3 = _tmp0 + _tmp2; + const Scalar _tmp4 = _tmp1 * state(0, 0); + const Scalar _tmp5 = 2 * state(1, 0); + const Scalar _tmp6 = _tmp5 * state(2, 0); + const Scalar _tmp7 = -_tmp4 + _tmp6; + const Scalar _tmp8 = -2 * std::pow(state(3, 0), Scalar(2)); + const Scalar _tmp9 = -2 * std::pow(state(1, 0), Scalar(2)); + const Scalar _tmp10 = _tmp8 + _tmp9 + 1; + const Scalar _tmp11 = _tmp1 * state(2, 0); + const Scalar _tmp12 = _tmp5 * state(0, 0); + const Scalar _tmp13 = _tmp11 - _tmp12; + const Scalar _tmp14 = _tmp13 * rot_var_ned(1, 0); + const Scalar _tmp15 = 1 - 2 * std::pow(state(2, 0), Scalar(2)); + const Scalar _tmp16 = _tmp15 + _tmp9; + const Scalar _tmp17 = _tmp11 + _tmp12; + const Scalar _tmp18 = _tmp17 * rot_var_ned(2, 0); + const Scalar _tmp19 = _tmp10 * _tmp14 + _tmp16 * _tmp18 + _tmp3 * _tmp7 * rot_var_ned(0, 0); + const Scalar _tmp20 = (Scalar(1) / Scalar(2)) * state(3, 0); + const Scalar _tmp21 = -_tmp19 * _tmp20; + const Scalar _tmp22 = std::pow(_tmp10, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp17, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp7, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp23 = (Scalar(1) / Scalar(2)) * state(2, 0); + const Scalar _tmp24 = _tmp15 + _tmp8; + const Scalar _tmp25 = _tmp24 * rot_var_ned(0, 0); + const Scalar _tmp26 = _tmp4 + _tmp6; + const Scalar _tmp27 = -_tmp0 + _tmp2; + const Scalar _tmp28 = (Scalar(1) / Scalar(2)) * _tmp10 * _tmp26 * rot_var_ned(1, 0) + + (Scalar(1) / Scalar(2)) * _tmp18 * _tmp27 + + (Scalar(1) / Scalar(2)) * _tmp25 * _tmp7; + const Scalar _tmp29 = _tmp28 * state(1, 0); + const Scalar _tmp30 = std::pow(_tmp24, Scalar(2)) * rot_var_ned(0, 0) + + std::pow(_tmp26, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp27, Scalar(2)) * rot_var_ned(2, 0); + const Scalar _tmp31 = (Scalar(1) / Scalar(2)) * state(1, 0); + const Scalar _tmp32 = _tmp14 * _tmp26 + _tmp16 * _tmp27 * rot_var_ned(2, 0) + _tmp25 * _tmp3; + const Scalar _tmp33 = _tmp20 * _tmp32; + const Scalar _tmp34 = -_tmp28 * state(2, 0); + const Scalar _tmp35 = _tmp19 * _tmp23; + const Scalar _tmp36 = std::pow(_tmp13, Scalar(2)) * rot_var_ned(1, 0) + + std::pow(_tmp16, Scalar(2)) * rot_var_ned(2, 0) + + std::pow(_tmp3, Scalar(2)) * rot_var_ned(0, 0); + const Scalar _tmp37 = -_tmp31 * _tmp32; + const Scalar _tmp38 = _tmp28 * state(0, 0); + const Scalar _tmp39 = -_tmp20 * _tmp22 + _tmp35 + _tmp38; + const Scalar _tmp40 = (Scalar(1) / Scalar(2)) * state(0, 0); + const Scalar _tmp41 = _tmp23 * _tmp32; + const Scalar _tmp42 = _tmp28 * state(3, 0); + const Scalar _tmp43 = _tmp30 * _tmp40 + _tmp41 - _tmp42; + const Scalar _tmp44 = _tmp32 * _tmp40; + const Scalar _tmp45 = _tmp21 + _tmp23 * _tmp36 + _tmp44; + const Scalar _tmp46 = _tmp19 * _tmp31; + const Scalar _tmp47 = _tmp22 * _tmp40 + _tmp42 - _tmp46; + const Scalar _tmp48 = _tmp20 * _tmp30 + _tmp37 + _tmp38; + const Scalar _tmp49 = _tmp19 * _tmp40; + const Scalar _tmp50 = -_tmp31 * _tmp36 + _tmp33 + _tmp49; + const Scalar _tmp51 = _tmp22 * _tmp31 + _tmp34 + _tmp49; + const Scalar _tmp52 = -_tmp23 * _tmp30 + _tmp29 + _tmp44; + const Scalar _tmp53 = _tmp36 * _tmp40 - _tmp41 + _tmp46; + + // Output terms (1) + if (q_cov_lower_triangle != nullptr) { + matrix::Matrix& _q_cov_lower_triangle = (*q_cov_lower_triangle); + + _q_cov_lower_triangle(0, 0) = -_tmp20 * (-_tmp20 * _tmp36 - _tmp35 + _tmp37) - + _tmp23 * (_tmp21 - _tmp22 * _tmp23 - _tmp29) - + _tmp31 * (-_tmp30 * _tmp31 - _tmp33 + _tmp34); + _q_cov_lower_triangle(1, 0) = -_tmp20 * _tmp45 - _tmp23 * _tmp39 - _tmp31 * _tmp43; + _q_cov_lower_triangle(2, 0) = -_tmp20 * _tmp50 - _tmp23 * _tmp47 - _tmp31 * _tmp48; + _q_cov_lower_triangle(3, 0) = -_tmp20 * _tmp53 - _tmp23 * _tmp51 - _tmp31 * _tmp52; + _q_cov_lower_triangle(0, 1) = 0; + _q_cov_lower_triangle(1, 1) = -_tmp20 * _tmp39 + _tmp23 * _tmp45 + _tmp40 * _tmp43; + _q_cov_lower_triangle(2, 1) = -_tmp20 * _tmp47 + _tmp23 * _tmp50 + _tmp40 * _tmp48; + _q_cov_lower_triangle(3, 1) = -_tmp20 * _tmp51 + _tmp23 * _tmp53 + _tmp40 * _tmp52; + _q_cov_lower_triangle(0, 2) = 0; + _q_cov_lower_triangle(1, 2) = 0; + _q_cov_lower_triangle(2, 2) = _tmp20 * _tmp48 - _tmp31 * _tmp50 + _tmp40 * _tmp47; + _q_cov_lower_triangle(3, 2) = _tmp20 * _tmp52 - _tmp31 * _tmp53 + _tmp40 * _tmp51; + _q_cov_lower_triangle(0, 3) = 0; + _q_cov_lower_triangle(1, 3) = 0; + _q_cov_lower_triangle(2, 3) = 0; + _q_cov_lower_triangle(3, 3) = -_tmp23 * _tmp52 + _tmp31 * _tmp51 + _tmp40 * _tmp53; + } +} // NOLINT(readability/fn_size) + +// NOLINTNEXTLINE(readability/fn_size) +} // namespace sym diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h new file mode 100644 index 0000000000..964fbcd71f --- /dev/null +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/state.h @@ -0,0 +1,23 @@ +// -------------------------------------------------- +// This file was autogenerated, do NOT modify by hand +// -------------------------------------------------- + +#ifndef EKF_STATE_H +#define EKF_STATE_H + +namespace estimator +{ +struct IdxDof { unsigned idx; unsigned dof; }; +namespace State { + static constexpr IdxDof quat_nominal{0, 4}; + static constexpr IdxDof vel{4, 3}; + static constexpr IdxDof pos{7, 3}; + static constexpr IdxDof gyro_bias{10, 3}; + static constexpr IdxDof accel_bias{13, 3}; + static constexpr IdxDof mag_I{16, 3}; + static constexpr IdxDof mag_B{19, 3}; + static constexpr IdxDof wind_vel{22, 2}; + static constexpr uint8_t size{24}; +}; +} +#endif // !EKF_STATE_H diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h index abd6238760..19d384712b 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_compute_measurement_update.h @@ -58,43 +58,43 @@ void YawEstComputeMeasurementUpdate(const matrix::Matrix& P, const // Output terms (4) if (S_inv != nullptr) { - matrix::Matrix& _S_inv = (*S_inv); + matrix::Matrix& _s_inv = (*S_inv); - _S_inv(0, 0) = _tmp4; - _S_inv(1, 0) = -_tmp5; - _S_inv(0, 1) = -_tmp6; - _S_inv(1, 1) = _tmp7; + _s_inv(0, 0) = _tmp4; + _s_inv(1, 0) = -_tmp5; + _s_inv(0, 1) = -_tmp6; + _s_inv(1, 1) = _tmp7; } if (S_det_inv != nullptr) { - Scalar& _S_det_inv = (*S_det_inv); + Scalar& _s_det_inv = (*S_det_inv); - _S_det_inv = _tmp3; + _s_det_inv = _tmp3; } if (K != nullptr) { - matrix::Matrix& _K = (*K); + matrix::Matrix& _k = (*K); - _K(0, 0) = _tmp9; - _K(1, 0) = _tmp10; - _K(2, 0) = _tmp11; - _K(0, 1) = _tmp12; - _K(1, 1) = _tmp13; - _K(2, 1) = _tmp14; + _k(0, 0) = _tmp9; + _k(1, 0) = _tmp10; + _k(2, 0) = _tmp11; + _k(0, 1) = _tmp12; + _k(1, 1) = _tmp13; + _k(2, 1) = _tmp14; } if (P_new != nullptr) { - matrix::Matrix& _P_new = (*P_new); + matrix::Matrix& _p_new = (*P_new); - _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); + _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); } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h index b2c4a3665c..352f9d75b0 100644 --- a/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h +++ b/src/modules/ekf2/EKF/python/ekf_derivation/generated/yaw_est_predict_covariance.h @@ -47,17 +47,17 @@ void YawEstPredictCovariance(const matrix::Matrix& state, // Output terms (1) if (P_new != nullptr) { - matrix::Matrix& _P_new = (*P_new); + matrix::Matrix& _p_new = (*P_new); - _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; - _P_new(1, 2) = _tmp6; - _P_new(2, 2) = P(2, 2) + d_ang_var; + _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; + _p_new(1, 2) = _tmp6; + _p_new(2, 2) = P(2, 2) + d_ang_var; } } // NOLINT(readability/fn_size) diff --git a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp index 0d8daee7de..453da560b4 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.cpp @@ -37,8 +37,12 @@ #include "range_finder_consistency_check.hpp" -void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, 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; + } + const float dt = static_cast(time_us - _time_last_update_us) * 1e-6f; if ((_time_last_update_us == 0) @@ -68,12 +72,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us) { + if (fabsf(vz) < _min_vz_for_valid_consistency) { + // We can only check consistency if there is vertical motion + return; + } + if (fabsf(_signed_test_ratio_lpf.getState()) >= 1.f) { - _is_kinematically_consistent = false; - _time_last_inconsistent_us = time_us; + if ((time_us - _time_last_horizontal_motion) > _signed_test_ratio_tau) { + _is_kinematically_consistent = false; + _time_last_inconsistent_us = time_us; + } } else { - if (fabsf(vz) > _min_vz_for_valid_consistency && _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/range_finder_consistency_check.hpp index 1498a4aa6c..d031e12d97 100644 --- a/src/modules/ekf2/EKF/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/range_finder_consistency_check.hpp @@ -37,7 +37,8 @@ * using the estimated velocity as a reference in order to detect sensor faults */ -#pragma once +#ifndef EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP +#define EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP #include @@ -47,7 +48,7 @@ public: RangeFinderConsistencyCheck() = default; ~RangeFinderConsistencyCheck() = default; - void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us); + void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us); void setGate(float gate) { _gate = gate; } @@ -71,9 +72,12 @@ private: bool _is_kinematically_consistent{true}; uint64_t _time_last_inconsistent_us{}; + uint64_t _time_last_horizontal_motion{}; static constexpr float _signed_test_ratio_tau = 2.f; static constexpr float _min_vz_for_valid_consistency = .5f; static constexpr uint64_t _consistency_hyst_time_us = 1e6; }; + +#endif // !EKF_RANGE_FINDER_CONSISTENCY_CHECK_HPP diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 88a9cb7197..177b841393 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -62,15 +62,15 @@ void Ekf::controlRangeHeightFusion() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - // Run the kinematic consistency check when not moving horizontally - if (_control_status.flags.in_air && !_control_status.flags.fixed_wing - && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) { + 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(6, 6), _time_delayed_us); + _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 { @@ -117,7 +117,7 @@ void Ekf::controlRangeHeightFusion() 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(9, 9)); + 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 @@ -131,8 +131,6 @@ void Ekf::controlRangeHeightFusion() && _range_sensor.isRegularlySendingData(); if (_control_status.flags.rng_hgt) { - aid_src.fusion_enabled = true; - if (continuing_conditions_passing) { fuseVerticalPosition(aid_src); @@ -204,6 +202,7 @@ void Ekf::controlRangeHeightFusion() bool Ekf::isConditionalRangeAidSuitable() { +#if defined(CONFIG_EKF2_TERRAIN) if (_control_status.flags.in_air && _range_sensor.isHealthy() && isTerrainEstimateValid()) { @@ -212,7 +211,10 @@ bool Ekf::isConditionalRangeAidSuitable() 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 = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)); + 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); @@ -234,6 +236,7 @@ bool Ekf::isConditionalRangeAidSuitable() return is_in_range && is_hagl_stable && is_below_max_speed; } +#endif // CONFIG_EKF2_TERRAIN return false; } diff --git a/src/modules/ekf2/EKF/sensor_range_finder.cpp b/src/modules/ekf2/EKF/sensor_range_finder.cpp index 94d3b891ea..54bd752598 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.cpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/modules/ekf2/EKF/sensor_range_finder.hpp b/src/modules/ekf2/EKF/sensor_range_finder.hpp index 94ed284613..8523ae3306 100644 --- a/src/modules/ekf2/EKF/sensor_range_finder.hpp +++ b/src/modules/ekf2/EKF/sensor_range_finder.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 Estimation and Control Library (ECL). All rights reserved. + * Copyright (c) 2020-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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 47063d5f0f..3ccdf2a7b6 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -94,8 +94,6 @@ void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const sideslip.innovation = innov; sideslip.innovation_variance = innov_var; - sideslip.fusion_enabled = _control_status.flags.fuse_aspd; - sideslip.timestamp_sample = _time_delayed_us; const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f); @@ -135,15 +133,15 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) _fault_status.flags.bad_sideslip = false; - Vector24f H; // Observation jacobian - Vector24f K; // Kalman gain vector + VectorState H; // Observation jacobian + VectorState K; // Kalman gain vector sym::ComputeSideslipHAndK(getStateAtFusionHorizonAsVector(), P, sideslip.innovation_variance, FLT_EPSILON, &H, &K); if (update_wind_only) { - for (unsigned row = 0; row <= 21; row++) { - K(row) = 0.f; - } + const Vector2f K_wind = K.slice(State::wind_vel.idx, 0); + K.setZero(); + K.slice(State::wind_vel.idx, 0) = K_wind; } const bool is_fused = measurementUpdate(K, sideslip.innovation_variance, sideslip.innovation); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index a44e6cf3a3..62cf8367c3 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -49,7 +49,9 @@ void Ekf::initHagl() stopHaglFlowFusion(); #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_RANGE_FINDER) stopHaglRngFusion(); +#endif // CONFIG_EKF2_RANGE_FINDER // assume a ground clearance _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; @@ -68,10 +70,14 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) 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) @@ -95,6 +101,7 @@ void Ekf::predictHagl(const imuSample &imu_delayed) _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) @@ -104,20 +111,29 @@ void Ekf::controlHaglRngFusion() return; } + if (_range_sensor.isDataReady()) { + updateHaglRng(_aid_src_terrain_range_finder); + } + if (_range_sensor.isDataHealthy()) { - const bool continuing_conditions_passing = _control_status.flags.in_air && _rng_consistency_check.isKinematicallyConsistent(); - //const bool continuing_conditions_passing = _control_status.flags.in_air && !_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); + + const bool continuing_conditions_passing = _control_status.flags.in_air + //&& !_control_status.flags.rng_hgt // TODO: should not be fused when using range height + && _rng_consistency_check.isKinematicallyConsistent(); + + 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(); + 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(_time_last_hagl_fuse, timeout); + 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) { @@ -143,7 +159,16 @@ void Ekf::controlHaglRngFusion() } else { if (starting_conditions_passing) { - startHaglRngFusion(); + _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(); + } } } @@ -153,42 +178,9 @@ void Ekf::controlHaglRngFusion() } } -void Ekf::startHaglRngFusion() +float Ekf::getRngVar() const { - _hagl_sensor_status.flags.range_finder = true; - resetHaglRngIfNeeded(); -} - -void Ekf::resetHaglRngIfNeeded() -{ - if (_hagl_sensor_status.flags.flow) { - const float meas_hagl = _range_sensor.getDistBottom(); - const float pred_hagl = _terrain_vpos - _state.pos(2); - const float hagl_innov = pred_hagl - meas_hagl; - const float obs_variance = getRngVar(); - - const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); - - const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); - const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var); - - // Reset the state to the measurement only if the test ratio is large, - // otherwise let it converge through the fusion - if (hagl_test_ratio > 0.2f) { - resetHaglRng(); - - } else { - fuseHaglRng(); - } - - } else { - resetHaglRng(); - } -} - -float Ekf::getRngVar() -{ - return fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) + 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()); } @@ -198,26 +190,25 @@ void Ekf::resetHaglRng() _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); _terrain_var = getRngVar(); _terrain_vpos_reset_counter++; - _time_last_hagl_fuse = _time_delayed_us; - _time_last_healthy_rng_data = 0; + + _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); - _hagl_innov = 0.f; - _hagl_innov_var = 0.f; - _hagl_test_ratio = 0.f; _innov_check_fail_status.flags.reject_hagl = false; _hagl_sensor_status.flags.range_finder = false; } - - _time_last_healthy_rng_data = 0; } -void Ekf::fuseHaglRng() +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(); @@ -226,33 +217,55 @@ void Ekf::fuseHaglRng() const float pred_hagl = _terrain_vpos - _state.pos(2); // calculate the innovation - _hagl_innov = pred_hagl - meas_hagl; + 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 - _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + 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 gate_size = fmaxf(_params.range_innov_gate, 1.0f); - _hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); + const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f); - if (_hagl_test_ratio <= 1.0f) { + + aid_src.timestamp_sample = _time_delayed_us; // TODO + + aid_src.observation = pred_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 / _hagl_innov_var; + const float gain = _terrain_var / aid_src.innovation_variance; + // correct the state - _terrain_vpos -= gain * _hagl_innov; + _terrain_vpos -= gain * aid_src.innovation; + // correct the variance _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion event - _time_last_hagl_fuse = _time_delayed_us; _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() @@ -266,10 +279,10 @@ void Ekf::controlHaglFlowFusion() 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 - && isTimedOut(_time_last_hagl_fuse, 5e6f); // TODO: check for range_finder hagl aiding instead? - /* && !_hagl_sensor_status.flags.range_finder; */ + && !_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) { @@ -277,10 +290,9 @@ void Ekf::controlHaglFlowFusion() // TODO: wait until the midpoint of the flow sample has fallen behind the fusion time horizon fuseFlowForTerrain(_aid_src_terrain_optical_flow); - _flow_data_ready = false; // TODO: do something when failing continuously the innovation check - /* const bool is_fusion_failing = isTimedOut(_time_last_flow_terrain_fuse, _params.reset_timeout_max); */ + /* const bool is_fusion_failing = isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.reset_timeout_max); */ /* if (is_fusion_failing) { */ /* resetHaglFlow(); */ @@ -292,28 +304,25 @@ void Ekf::controlHaglFlowFusion() } else { if (starting_conditions_passing) { - startHaglFlowFusion(); + _hagl_sensor_status.flags.flow = true; + // TODO: do a reset instead of trying to fuse the data? + fuseFlowForTerrain(_aid_src_terrain_optical_flow); } } - } else if (_hagl_sensor_status.flags.flow - && (_time_delayed_us > _flow_sample_delayed.time_us + (uint64_t)5e6)) { + _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::startHaglFlowFusion() -{ - _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; -} - 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); } @@ -325,11 +334,13 @@ void Ekf::resetHaglFlow() _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.fusion_enabled = true; + flow.fused = true; const float R_LOS = flow.observation_variance[0]; @@ -397,9 +408,8 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) _fault_status.flags.bad_optflow_X = false; _fault_status.flags.bad_optflow_Y = false; - _time_last_flow_terrain_fuse = _time_delayed_us; - //_aid_src_optical_flow.time_last_fuse = _time_delayed_us; // TODO: separate aid source status for OF terrain? - _aid_src_optical_flow.fused = true; + flow.time_last_fuse = _time_delayed_us; + flow.fused = true; } #endif // CONFIG_EKF2_OPTICAL_FLOW @@ -415,18 +425,28 @@ void Ekf::controlHaglFakeFusion() bool Ekf::isTerrainEstimateValid() const { +#if defined(CONFIG_EKF2_RANGE_FINDER) + // we have been fusing range finder measurements in the last 5 seconds - if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.range_finder && isRecent(_aid_src_terrain_range_finder.time_last_fuse, (uint64_t)5e6)) { return 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(_time_last_flow_terrain_fuse, (uint64_t)5e6)) { + if (_hagl_sensor_status.flags.flow && isRecent(_aid_src_terrain_optical_flow.time_last_fuse, (uint64_t)5e6)) { return true; } + #endif // CONFIG_EKF2_OPTICAL_FLOW return false; } + +void Ekf::terrainHandleVerticalPositionReset(const float delta_z) { + _terrain_vpos += delta_z; +} diff --git a/src/modules/ekf2/EKF/vel_pos_fusion.cpp b/src/modules/ekf2/EKF/vel_pos_fusion.cpp index 17a57518b6..e615dc2d20 100644 --- a/src/modules/ekf2/EKF/vel_pos_fusion.cpp +++ b/src/modules/ekf2/EKF/vel_pos_fusion.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -53,7 +53,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[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); @@ -71,7 +72,8 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[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); @@ -96,7 +98,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa 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(9, 9) + aid_src.observation_variance; + aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance; setEstimatorAidStatusTestRatio(aid_src, innov_gate); @@ -121,7 +123,8 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i]; aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i)); - aid_src.innovation_variance[i] = P(7 + i, 7 + i) + aid_src.observation_variance[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); @@ -131,10 +134,10 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -147,11 +150,11 @@ void Ekf::fuseVelocity(estimator_aid_source2d_s &aid_src) void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) { - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { + if (!aid_src.innovation_rejected) { // vx, vy, vz - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 0) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 1) - && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], 2) + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::vel.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::vel.idx + 1) + && fuseVelPosHeight(aid_src.innovation[2], aid_src.innovation_variance[2], State::vel.idx + 2) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -165,9 +168,9 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src) void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) { // x & y - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], 3) - && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], 4) + if (!aid_src.innovation_rejected) { + if (fuseVelPosHeight(aid_src.innovation[0], aid_src.innovation_variance[0], State::pos.idx) + && fuseVelPosHeight(aid_src.innovation[1], aid_src.innovation_variance[1], State::pos.idx + 1) ) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; @@ -181,8 +184,8 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src) void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) { // z - if (aid_src.fusion_enabled && !aid_src.innovation_rejected) { - if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, 5)) { + if (!aid_src.innovation_rejected) { + if (fuseVelPosHeight(aid_src.innovation, aid_src.innovation_variance, State::pos.idx + 2)) { aid_src.fused = true; aid_src.time_last_fuse = _time_delayed_us; } @@ -190,29 +193,28 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) } // Helper function that fuses a single velocity or position measurement -bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) +bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int state_index) { - Vector24f Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. - const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state + VectorState Kfusion; // Kalman gain vector for any single observation - sequential fusion is used. // calculate kalman gain K = PHS, where S = 1/innovation variance - for (int row = 0; row < _k_num_states; row++) { + for (int row = 0; row < State::size; row++) { Kfusion(row) = P(row, state_index) / innov_var; } clearInhibitedStateKalmanGains(Kfusion); - SquareMatrix24f KHP; + SquareMatrixState KHP; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < _k_num_states; column++) { + for (unsigned row = 0; row < State::size; row++) { + for (unsigned column = 0; column < State::size; column++) { KHP(row, column) = Kfusion(row) * P(state_index, column); } } const bool healthy = checkAndFixCovarianceUpdate(KHP); - setVelPosStatus(obs_index, healthy); + setVelPosStatus(state_index, healthy); if (healthy) { // apply the covariance corrections @@ -229,10 +231,10 @@ bool Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o return false; } -void Ekf::setVelPosStatus(const int index, const bool healthy) +void Ekf::setVelPosStatus(const int state_index, const bool healthy) { - switch (index) { - case 0: + switch (state_index) { + case State::vel.idx: if (healthy) { _fault_status.flags.bad_vel_N = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -243,7 +245,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 1: + case State::vel.idx + 1: if (healthy) { _fault_status.flags.bad_vel_E = false; _time_last_hor_vel_fuse = _time_delayed_us; @@ -254,7 +256,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 2: + case State::vel.idx + 2: if (healthy) { _fault_status.flags.bad_vel_D = false; _time_last_ver_vel_fuse = _time_delayed_us; @@ -265,7 +267,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 3: + case State::pos.idx: if (healthy) { _fault_status.flags.bad_pos_N = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -276,7 +278,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 4: + case State::pos.idx + 1: if (healthy) { _fault_status.flags.bad_pos_E = false; _time_last_hor_pos_fuse = _time_delayed_us; @@ -287,7 +289,7 @@ void Ekf::setVelPosStatus(const int index, const bool healthy) break; - case 5: + case State::pos.idx + 2: if (healthy) { _fault_status.flags.bad_pos_D = false; _time_last_hgt_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp new file mode 100644 index 0000000000..05615585ce --- /dev/null +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -0,0 +1,144 @@ +/**************************************************************************** + * + * 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 "ekf.h" + +#include "python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" +#include "python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" + +#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; + computeYawInnovVarAndH(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 + _fault_status.flags.bad_hdg = false; + + } else { + // 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("yaw fusion numerical error - covariance reset"); + + return false; + } + + // calculate the Kalman gains + // only calculate gains for states we are using + VectorState Kfusion; + const float heading_innov_var_inv = 1.f / aid_src_status.innovation_variance; + + for (uint8_t row = 0; row < State::size; row++) { + for (uint8_t col = 0; col <= 3; col++) { + Kfusion(row) += P(row, col) * H_YAW(col); + } + + Kfusion(row) *= heading_innov_var_inv; + } + + // set the magnetometer unhealthy if the test fails + if (aid_src_status.innovation_rejected) { + _innov_check_fail_status.flags.reject_yaw = true; + + // if we are in air we don't want to fuse the measurement + // 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) + ) { + // 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)); + 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 + // being stuck on a previous bad estimate + resetGyroBiasZCov(); + + } else { + return false; + } + + } else { + _innov_check_fail_status.flags.reject_yaw = false; + } + + if (measurementUpdate(Kfusion, aid_src_status.innovation_variance, aid_src_status.innovation)) { + + _time_last_heading_fuse = _time_delayed_us; + + aid_src_status.time_last_fuse = _time_delayed_us; + aid_src_status.fused = true; + + _fault_status.flags.bad_hdg = false; + + return true; + + } else { + _fault_status.flags.bad_hdg = true; + } + + // otherwise + aid_src_status.fused = false; + return false; +} + +void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const +{ + if (shouldUse321RotationSequence(_R_to_earth)) { + sym::ComputeYaw321InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + + } else { + sym::ComputeYaw312InnovVarAndH(getStateAtFusionHorizonAsVector(), P, variance, FLT_EPSILON, &innovation_variance, &H_YAW); + } +} diff --git a/src/modules/ekf2/EKF/zero_gyro_update.cpp b/src/modules/ekf2/EKF/zero_gyro_update.cpp new file mode 100644 index 0000000000..571bde209a --- /dev/null +++ b/src/modules/ekf2/EKF/zero_gyro_update.cpp @@ -0,0 +1,91 @@ +/**************************************************************************** + * + * 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 zero_gyro_update.cpp + * Control function for ekf zero gyro update + */ + +#include "ekf.h" + +void Ekf::controlZeroGyroUpdate(const imuSample &imu_delayed) +{ + if (!(_params.imu_ctrl & static_cast(ImuCtrl::GyroBias))) { + return; + } + + // When at rest, fuse the gyro data as a direct observation of the gyro bias + if (_control_status.flags.vehicle_at_rest) { + // Downsample gyro data to run the fusion at a lower rate + _zgup_delta_ang += imu_delayed.delta_ang; + _zgup_delta_ang_dt += imu_delayed.delta_ang_dt; + + static constexpr float zgup_dt = 0.2f; + const bool zero_gyro_update_data_ready = _zgup_delta_ang_dt >= zgup_dt; + + if (zero_gyro_update_data_ready) { + Vector3f gyro_bias = _zgup_delta_ang / _zgup_delta_ang_dt; + Vector3f innovation = _state.gyro_bias - gyro_bias; + + const float obs_var = sq(math::constrain(_params.gyro_noise, 0.f, 1.f)); + + const Vector3f innov_var = getGyroBiasVariance() + obs_var; + + for (int i = 0; i < 3; i++) { + fuseDeltaAngBias(innovation(i), innov_var(i), i); + } + + // Reset the integrators + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; + } + + } else if (_control_status_prev.flags.vehicle_at_rest) { + // Reset the integrators + _zgup_delta_ang.setZero(); + _zgup_delta_ang_dt = 0.f; + } +} + +void Ekf::fuseDeltaAngBias(const float innov, const float innov_var, const int obs_index) +{ + VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. + const unsigned state_index = obs_index + State::gyro_bias.idx; + + // calculate kalman gain K = PHS, where S = 1/innovation variance + for (int row = 0; row < State::size; row++) { + K(row) = P(row, state_index) / innov_var; + } + + measurementUpdate(K, innov_var, innov); +} diff --git a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp index 1a12ad1e6e..1bae77c7c8 100644 --- a/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp +++ b/src/modules/ekf2/EKF/zero_innovation_heading_update.cpp @@ -43,52 +43,26 @@ 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; - if (!_control_status.flags.tilt_align) { - // fuse zero heading innovation during the leveling fine alignment step to keep the yaw variance low - float innovation = 0.f; - float obs_var = _control_status.flags.vehicle_at_rest ? 0.001f : 0.1f; - estimator_aid_source1d_s unused; - fuseYaw(innovation, obs_var, unused); - _time_last_heading_fuse = 0; - _last_static_yaw = NAN; + // fuse zero innovation at a limited rate if the yaw variance is too large + if(!yaw_aiding + && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { - } else if (_control_status.flags.vehicle_at_rest) { - // When at rest or no source of yaw aiding is active yaw fusion is run selectively to enable yaw gyro - // bias learning when stationary on ground and to prevent uncontrolled yaw variance growth - const float euler_yaw = getEulerYaw(_R_to_earth); + // Use an observation variance larger than usual but small enough + // to constrain the yaw variance just below the threshold + const float obs_var = _control_status.flags.tilt_align ? 0.25f : 0.001f; - if (PX4_ISFINITE(_last_static_yaw)) { - // fuse last static yaw at a limited rate (every 200 milliseconds) - if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { - float innovation = wrap_pi(euler_yaw - _last_static_yaw); - float obs_var = 0.01f; - estimator_aid_source1d_s unused; - fuseYaw(innovation, obs_var, unused); - } + estimator_aid_source1d_s aid_src_status{}; + aid_src_status.observation = getEulerYaw(_state.quat_nominal); + aid_src_status.observation_variance = obs_var; + aid_src_status.innovation = 0.f; - } else { - // record static yaw when transitioning to at rest - _last_static_yaw = euler_yaw; + VectorState H_YAW; + + 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)) { + // The yaw variance is too large, fuse fake measurement + fuseYaw(aid_src_status, H_YAW); } - - } else { - // vehicle moving and tilt alignment completed - - // fuse zero innovation at a limited rate if the yaw variance is too large - if (!yaw_aiding && isTimedOut(_time_last_heading_fuse, (uint64_t)200'000)) { - float obs_var = 0.25f; - estimator_aid_source1d_s aid_src_status; - Vector24f H_YAW; - - computeYawInnovVarAndH(obs_var, aid_src_status.innovation_variance, H_YAW); - - if ((aid_src_status.innovation_variance - obs_var) > sq(_params.mag_heading_noise)) { - // The yaw variance is too large, fuse fake measurement - float innovation = 0.f; - fuseYaw(innovation, obs_var, aid_src_status, H_YAW); - } - } - - _last_static_yaw = NAN; } } diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp index 0897262ea3..f78dc28cc2 100644 --- a/src/modules/ekf2/EKF/zero_velocity_update.cpp +++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp @@ -46,7 +46,7 @@ void Ekf::controlZeroVelocityUpdate() if (zero_velocity_update_data_ready) { const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest && _control_status_prev.flags.vehicle_at_rest - && !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift + && (!isVerticalVelocityAidingActive() || !_control_status.flags.tilt_align); // otherwise the filter is "too rigid" to follow a position drift if (continuing_conditions_passing) { Vector3f vel_obs{0, 0, 0}; @@ -55,14 +55,11 @@ void Ekf::controlZeroVelocityUpdate() // Set a low variance initially for faster leveling and higher // later to let the states follow the measurements const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f); - Vector3f innov_var{ - P(4, 4) + obs_var, - P(5, 5) + obs_var, - P(6, 6) + obs_var}; + Vector3f innov_var = getVelocityVariance() + obs_var; - fuseVelPosHeight(innovation(0), innov_var(0), 0); - fuseVelPosHeight(innovation(1), innov_var(1), 1); - fuseVelPosHeight(innovation(2), innov_var(2), 2); + for (unsigned i = 0; i < 3; i++) { + fuseVelPosHeight(innovation(i), innov_var(i), State::vel.idx + i); + } _time_last_zero_velocity_fuse = _time_delayed_us; } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a9fad97550..426707abb9 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -60,8 +60,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _params(_ekf.getParamHandle()), _param_ekf2_predict_us(_params->filter_update_interval_us), _param_ekf2_imu_ctrl(_params->imu_ctrl), - _param_ekf2_mag_delay(_params->mag_delay_ms), - _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_gps_delay(_params->gps_delay_ms), #if defined(CONFIG_EKF2_AUXVEL) _param_ekf2_avel_delay(_params->auxvel_delay_ms), @@ -70,16 +68,26 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_acc_noise(_params->accel_noise), _param_ekf2_gyr_b_noise(_params->gyro_bias_p_noise), _param_ekf2_acc_b_noise(_params->accel_bias_p_noise), - _param_ekf2_mag_e_noise(_params->mage_p_noise), - _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_wind_nsd(_params->wind_vel_nsd), _param_ekf2_gps_v_noise(_params->gps_vel_noise), _param_ekf2_gps_p_noise(_params->gps_pos_noise), _param_ekf2_noaid_noise(_params->pos_noaid_noise), +#if defined(CONFIG_EKF2_BAROMETER) + _param_ekf2_baro_ctrl(_params->baro_ctrl), + _param_ekf2_baro_delay(_params->baro_delay_ms), _param_ekf2_baro_noise(_params->baro_noise), _param_ekf2_baro_gate(_params->baro_innov_gate), _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone), _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt), +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + _param_ekf2_aspd_max(_params->max_correction_airspeed), + _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), + _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), + _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp), + _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), + _param_ekf2_pcoef_z(_params->static_pressure_coef_z), +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), #if defined(CONFIG_EKF2_AIRSPEED) @@ -93,6 +101,10 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_beta_noise(_params->beta_noise), _param_ekf2_fuse_beta(_params->beta_fusion_enabled), #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_MAGNETOMETER) + _param_ekf2_mag_delay(_params->mag_delay_ms), + _param_ekf2_mag_e_noise(_params->mage_p_noise), + _param_ekf2_mag_b_noise(_params->magb_p_noise), _param_ekf2_head_noise(_params->mag_heading_noise), _param_ekf2_mag_noise(_params->mag_noise), _param_ekf2_mag_decl(_params->mag_declination_deg), @@ -102,6 +114,11 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_mag_type(_params->mag_fusion_type), _param_ekf2_mag_acclim(_params->mag_acc_gate), _param_ekf2_mag_yawlim(_params->mag_yaw_rate_gate), + _param_ekf2_mag_check(_params->mag_check), + _param_ekf2_mag_chk_str(_params->mag_check_strength_tolerance_gs), + _param_ekf2_mag_chk_inc(_params->mag_check_inclination_tolerance_deg), + _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), +#endif // CONFIG_EKF2_MAGNETOMETER _param_ekf2_gps_check(_params->gps_check_mask), _param_ekf2_req_eph(_params->req_hacc), _param_ekf2_req_epv(_params->req_vacc), @@ -111,16 +128,22 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_req_hdrift(_params->req_hdrift), _param_ekf2_req_vdrift(_params->req_vdrift), _param_ekf2_hgt_ref(_params->height_sensor_ref), - _param_ekf2_baro_ctrl(_params->baro_ctrl), _param_ekf2_gps_ctrl(_params->gnss_ctrl), _param_ekf2_noaid_tout(_params->valid_timeout_max), +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + _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 #if defined(CONFIG_EKF2_RANGE_FINDER) _param_ekf2_rng_ctrl(_params->rng_ctrl), _param_ekf2_rng_delay(_params->range_delay_ms), _param_ekf2_rng_noise(_params->range_noise), _param_ekf2_rng_sfe(_params->range_noise_scaler), _param_ekf2_rng_gate(_params->range_innov_gate), - _param_ekf2_min_rng(_params->rng_gnd_clearance), _param_ekf2_rng_pitch(_params->rng_sens_pitch), _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), @@ -130,9 +153,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_rng_pos_x(_params->rng_pos_body(0)), _param_ekf2_rng_pos_y(_params->rng_pos_body(1)), _param_ekf2_rng_pos_z(_params->rng_pos_body(2)), - _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_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) _param_ekf2_ev_delay(_params->ev_delay_ms), @@ -154,6 +174,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), _param_ekf2_of_qmin(_params->flow_qual_min), + _param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd), _param_ekf2_of_gate(_params->flow_innov_gate), _param_ekf2_of_pos_x(_params->flow_pos_body(0)), _param_ekf2_of_pos_y(_params->flow_pos_body(1)), @@ -180,16 +201,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_bcoef_y(_params->bcoef_y), _param_ekf2_mcoef(_params->mcoef), #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - _param_ekf2_aspd_max(_params->max_correction_airspeed), - _param_ekf2_pcoef_xp(_params->static_pressure_coef_xp), - _param_ekf2_pcoef_xn(_params->static_pressure_coef_xn), - _param_ekf2_pcoef_yp(_params->static_pressure_coef_yp), - _param_ekf2_pcoef_yn(_params->static_pressure_coef_yn), - _param_ekf2_pcoef_z(_params->static_pressure_coef_z), -#endif // CONFIG_EKF2_BARO_COMPENSATION - _param_ekf2_mag_check(_params->check_mag_strength), - _param_ekf2_synthetic_mag_z(_params->synthesize_mag_z), _param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default) { // advertise expected minimal topic set immediately to ensure logging @@ -222,7 +233,9 @@ EKF2::~EKF2() #if defined(CONFIG_EKF2_AUXVEL) perf_free(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_free(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_free(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_free(_msg_missed_optical_flow_perf); @@ -247,12 +260,16 @@ bool EKF2::multi_init(int imu, int mag) _estimator_status_pub.advertise(); _yaw_est_pub.advertise(); +#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(); } +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_EXTERNAL_VISION) // EV advertise @@ -309,13 +326,31 @@ bool EKF2::multi_init(int imu, int mag) #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(); + } + +#endif // CONFIG_EKF2_MAGNETOMETER + _attitude_pub.advertise(); _local_position_pub.advertise(); _global_position_pub.advertise(); _odometry_pub.advertise(); _wind_pub.advertise(); - bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag); + bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu); + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + if (!_magnetometer_sub.ChangeInstance(mag)) { + changed_instance = false; + } + +#endif // CONFIG_EKF2_MAGNETOMETER const int status_instance = _estimator_states_pub.get_instance(); @@ -357,7 +392,9 @@ int EKF2::print_status() #if defined(CONFIG_EKF2_AUXVEL) perf_print_counter(_msg_missed_landing_target_pose_perf); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_MAGNETOMETER) perf_print_counter(_msg_missed_magnetometer_perf); +#endif // CONFIG_EKF2_MAGNETOMETER perf_print_counter(_msg_missed_odometry_perf); #if defined(CONFIG_EKF2_OPTICAL_FLOW) perf_print_counter(_msg_missed_optical_flow_perf); @@ -409,6 +446,8 @@ 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; @@ -425,6 +464,10 @@ void EKF2::Run() } } +#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; @@ -440,6 +483,8 @@ void EKF2::Run() } } } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (!_callback_registered) { @@ -660,7 +705,9 @@ void EKF2::Run() #if defined(CONFIG_EKF2_AUXVEL) UpdateAuxVelSample(ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) UpdateBaroSample(ekf2_timestamps); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) UpdateExtVisionSample(ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -668,7 +715,9 @@ void EKF2::Run() UpdateFlowSample(ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW UpdateGpsSample(ekf2_timestamps); +#if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagSample(ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) UpdateRangeSample(ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER @@ -686,7 +735,9 @@ void EKF2::Run() PublishWindEstimate(now); // publish status/logging messages +#if defined(CONFIG_EKF2_BAROMETER) PublishBaroBias(now); +#endif // CONFIG_EKF2_BAROMETER PublishGnssHgtBias(now); #if defined(CONFIG_EKF2_RANGE_FINDER) PublishRngHgtBias(now); @@ -709,7 +760,9 @@ void EKF2::Run() UpdateAccelCalibration(now); UpdateGyroCalibration(now); +#if defined(CONFIG_EKF2_MAGNETOMETER) UpdateMagCalibration(now); +#endif // CONFIG_EKF2_MAGNETOMETER PublishSensorBias(now); PublishAidSourceStatus(now); @@ -753,6 +806,8 @@ void EKF2::VerifyParams() "GPS lon/lat is required for altitude fusion", _param_ekf2_gps_ctrl.get()); } +#if defined(CONFIG_EKF2_BAROMETER) + if ((_param_ekf2_hgt_ref.get() == HeightSensor::BARO) && (_param_ekf2_baro_ctrl.get() == 0)) { _param_ekf2_baro_ctrl.set(1); _param_ekf2_baro_ctrl.commit(); @@ -764,6 +819,8 @@ void EKF2::VerifyParams() "Baro enabled by EKF2_HGT_REF", _param_ekf2_baro_ctrl.get()); } +#endif // CONFIG_EKF2_BAROMETER + #if defined(CONFIG_EKF2_RANGE_FINDER) if ((_param_ekf2_hgt_ref.get() == HeightSensor::RANGE) && (_param_ekf2_rng_ctrl.get() == RngCtrl::DISABLED)) { @@ -907,6 +964,27 @@ void EKF2::VerifyParams() } #endif // CONFIG_EKF2_OPTICAL_FLOW + +#if defined(CONFIG_EKF2_MAGNETOMETER) + + // EKF2_MAG_TYPE obsolete options + if ((_param_ekf2_mag_type.get() != MagFuseType::AUTO) + && (_param_ekf2_mag_type.get() != MagFuseType::HEADING) + && (_param_ekf2_mag_type.get() != MagFuseType::NONE) + ) { + + mavlink_log_critical(&_mavlink_log_pub, "EKF2_MAG_TYPE invalid, resetting to default"); + /* EVENT + * @description EKF2_AID_MASK is set to {1:.0}. + */ + events::send(events::ID("ekf2_mag_type_invalid"), events::Log::Warning, + "EKF2_MAG_TYPE invalid, resetting to default", _param_ekf2_mag_type.get()); + + _param_ekf2_mag_type.set(0); + _param_ekf2_mag_type.commit(); + } + +#endif // CONFIG_EKF2_MAGNETOMETER } void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) @@ -919,8 +997,10 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) // sideslip PublishAidSourceStatus(_ekf.aid_src_sideslip(), _status_sideslip_pub_last, _estimator_aid_src_sideslip_pub); #endif // CONFIG_EKF2_SIDESLIP +#if defined(CONFIG_EKF2_BAROMETER) // baro height PublishAidSourceStatus(_ekf.aid_src_baro_hgt(), _status_baro_hgt_pub_last, _estimator_aid_src_baro_hgt_pub); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) // RNG height @@ -947,11 +1027,13 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) PublishAidSourceStatus(_ekf.aid_src_gnss_yaw(), _status_gnss_yaw_pub_last, _estimator_aid_src_gnss_yaw_pub); #endif // CONFIG_EKF2_GNSS_YAW +#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 // gravity PublishAidSourceStatus(_ekf.aid_src_gravity(), _status_gravity_pub_last, _estimator_aid_src_gravity_pub); @@ -964,9 +1046,22 @@ void EKF2::PublishAidSourceStatus(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_OPTICAL_FLOW) // 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_OPTICAL_FLOW + +#endif // CONFIG_EKF2_TERRAIN } void EKF2::PublishAttitude(const hrt_abstime ×tamp) @@ -989,6 +1084,7 @@ void EKF2::PublishAttitude(const hrt_abstime ×tamp) } } +#if defined(CONFIG_EKF2_BAROMETER) void EKF2::PublishBaroBias(const hrt_abstime ×tamp) { if (_ekf.aid_src_baro_hgt().timestamp_sample != 0) { @@ -1002,6 +1098,7 @@ void EKF2::PublishBaroBias(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_BAROMETER void EKF2::PublishGnssHgtBias(const hrt_abstime ×tamp) { @@ -1057,7 +1154,7 @@ void EKF2::PublishEvPosBias(const hrt_abstime ×tamp) if ((bias_vec - _last_ev_bias_published).longerThan(0.01f)) { bias.timestamp_sample = _ekf.aid_src_ev_hgt().timestamp_sample; - bias.timestamp = hrt_absolute_time(); + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_ev_pos_bias_pub.publish(bias); _last_ev_bias_published = Vector3f(bias.bias); @@ -1189,7 +1286,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt = NAN; global_pos.terrain_alt_valid = false; -#if defined(CONFIG_EKF2_RANGE_FINDER) +#if defined(CONFIG_EKF2_TERRAIN) if (_ekf.isTerrainEstimateValid()) { // Terrain altitude in m, WGS84 @@ -1197,7 +1294,7 @@ void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) global_pos.terrain_alt_valid = true; } -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN global_pos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning || _ekf.control_status_flags().wind_dead_reckoning; @@ -1251,7 +1348,9 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ekf.getEvVelPosInnov(innovations.ev_hvel, innovations.ev_vvel, innovations.ev_hpos, innovations.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_BAROMETER) _ekf.getBaroHgtInnov(innovations.baro_vpos); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnov(innovations.rng_vpos); #endif // CONFIG_EKF2_RANGE_FINDER @@ -1260,10 +1359,11 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnov(innovations.flow); - _ekf.getTerrainFlowInnov(innovations.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnov(innovations.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnov(innovations.mag_field); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnov(innovations.drag); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1273,11 +1373,18 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnov(innovations.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnov(innovations.hagl); - _ekf.getHaglRateInnov(innovations.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnov(innovations.gravity); + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + _ekf.getHaglInnov(innovations.hagl); +# endif //CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnov(innovations.terr_flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + // Not yet supported innovations.aux_vvel = NAN; @@ -1297,13 +1404,20 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _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); @@ -1325,19 +1439,23 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], test_ratios.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_BAROMETER) _ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovRatio(test_ratios.rng_vpos); + _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovRatio(test_ratios.flow[0]); - _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovRatio(test_ratios.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnovRatio(test_ratios.mag_field[0]); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovRatio(&test_ratios.drag[0]); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1347,11 +1465,18 @@ void EKF2::PublishInnovationTestRatios(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovRatio(test_ratios.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) - _ekf.getHaglInnovRatio(test_ratios.hagl); - _ekf.getHaglRateInnovRatio(test_ratios.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnovRatio(test_ratios.gravity[0]); + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) + _ekf.getHaglInnovRatio(test_ratios.hagl); +# endif +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnovRatio(test_ratios.terr_flow[0]); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN + // Not yet supported test_ratios.aux_vvel = NAN; @@ -1368,19 +1493,23 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_EXTERNAL_VISION) _ekf.getEvVelPosInnovVar(variances.ev_hvel, variances.ev_vvel, variances.ev_hpos, variances.ev_vpos); #endif // CONFIG_EKF2_EXTERNAL_VISION +#if defined(CONFIG_EKF2_BAROMETER) _ekf.getBaroHgtInnovVar(variances.baro_vpos); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getRngHgtInnovVar(variances.rng_vpos); + _ekf.getHaglRateInnovVar(variances.hagl_rate); #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_AUXVEL) _ekf.getAuxVelInnovVar(variances.aux_hvel); #endif // CONFIG_EKF2_AUXVEL #if defined(CONFIG_EKF2_OPTICAL_FLOW) _ekf.getFlowInnovVar(variances.flow); - _ekf.getTerrainFlowInnovVar(variances.terr_flow); #endif // CONFIG_EKF2_OPTICAL_FLOW _ekf.getHeadingInnovVar(variances.heading); +#if defined(CONFIG_EKF2_MAGNETOMETER) _ekf.getMagInnovVar(variances.mag_field); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_DRAG_FUSION) _ekf.getDragInnovVar(variances.drag); #endif // CONFIG_EKF2_DRAG_FUSION @@ -1390,11 +1519,18 @@ void EKF2::PublishInnovationVariances(const hrt_abstime ×tamp) #if defined(CONFIG_EKF2_SIDESLIP) _ekf.getBetaInnovVar(variances.beta); #endif // CONFIG_EKF2_SIDESLIP -#if defined(CONFIG_EKF2_RANGE_FINDER) + +#if defined(CONFIG_EKF2_TERRAIN) +# if defined(CONFIG_EKF2_RANGE_FINDER) _ekf.getHaglInnovVar(variances.hagl); - _ekf.getHaglRateInnovVar(variances.hagl_rate); -#endif // CONFIG_EKF2_RANGE_FINDER +# endif // CONFIG_EKF2_RANGE_FINDER +# if defined(CONFIG_EKF2_OPTICAL_FLOW) + _ekf.getTerrainFlowInnovVar(variances.terr_flow); +# endif // CONFIG_EKF2_OPTICAL_FLOW +#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_RANGE_FINDER + _ekf.getGravityInnovVar(variances.gravity); + // Not yet supported variances.aux_vvel = NAN; @@ -1458,16 +1594,16 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) _ekf.get_quat_reset(&delta_q_reset(0), &lpos.heading_reset_counter); lpos.heading = Eulerf(_ekf.getQuaternion()).psi(); + lpos.unaided_heading = _ekf.getUnaidedYaw(); lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // Distance to bottom surface (ground) in meters - // constrain the distance to ground to _rng_gnd_clearance - lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); +#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(); -#endif // CONFIG_EKF2_RANGE_FINDER +#endif // CONFIG_EKF2_TERRAIN _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); @@ -1529,10 +1665,10 @@ void EKF2::PublishOdometry(const hrt_abstime ×tamp, const imuSample &imu_sa angular_velocity.copyTo(odom.angular_velocity); // velocity covariances - _ekf.velocity_covariances().diag().copyTo(odom.velocity_variance); + _ekf.getVelocityVariance().copyTo(odom.velocity_variance); // position covariances - _ekf.position_covariances().diag().copyTo(odom.position_variance); + _ekf.getPositionVariance().copyTo(odom.position_variance); // orientation covariance _ekf.calcRotVecVariances().copyTo(odom.orientation_variance); @@ -1553,12 +1689,17 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) // estimator_sensor_bias const Vector3f gyro_bias{_ekf.getGyroBias()}; const Vector3f accel_bias{_ekf.getAccelBias()}; - const Vector3f mag_bias{_ekf.getMagBias()}; + +#if defined(CONFIG_EKF2_MAGNETOMETER) + const Vector3f mag_bias {_ekf.getMagBias()}; +#endif // CONFIG_EKF2_MAGNETOMETER // publish at ~1 Hz, or sooner if there's a change if ((gyro_bias - _last_gyro_bias_published).longerThan(0.001f) || (accel_bias - _last_accel_bias_published).longerThan(0.001f) +#if defined(CONFIG_EKF2_MAGNETOMETER) || (mag_bias - _last_mag_bias_published).longerThan(0.001f) +#endif // CONFIG_EKF2_MAGNETOMETER || (timestamp >= _last_sensor_bias_published + 1_s)) { estimator_sensor_bias_s bias{}; @@ -1566,35 +1707,45 @@ void EKF2::PublishSensorBias(const hrt_abstime ×tamp) // take device ids from sensor_selection_s if not using specific vehicle_imu_s if ((_device_id_gyro != 0) && (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::GyroBias))) { + const Vector3f bias_var{_ekf.getGyroBiasVariance()}; + bias.gyro_device_id = _device_id_gyro; gyro_bias.copyTo(bias.gyro_bias); bias.gyro_bias_limit = _ekf.getGyroBiasLimit(); - _ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance); - bias.gyro_bias_valid = true; // TODO + bias_var.copyTo(bias.gyro_bias_variance); + bias.gyro_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f); bias.gyro_bias_stable = _gyro_cal.cal_available; _last_gyro_bias_published = gyro_bias; } if ((_device_id_accel != 0) && (_param_ekf2_imu_ctrl.get() & static_cast(ImuCtrl::AccelBias))) { + const Vector3f bias_var{_ekf.getAccelBiasVariance()}; + bias.accel_device_id = _device_id_accel; accel_bias.copyTo(bias.accel_bias); bias.accel_bias_limit = _ekf.getAccelBiasLimit(); - _ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance); - bias.accel_bias_valid = true; // TODO + bias_var.copyTo(bias.accel_bias_variance); + bias.accel_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f); bias.accel_bias_stable = _accel_cal.cal_available; _last_accel_bias_published = accel_bias; } +#if defined(CONFIG_EKF2_MAGNETOMETER) + if (_device_id_mag != 0) { + const Vector3f bias_var{_ekf.getMagBiasVariance()}; + bias.mag_device_id = _device_id_mag; mag_bias.copyTo(bias.mag_bias); bias.mag_bias_limit = _ekf.getMagBiasLimit(); - _ekf.getMagBiasVariance().copyTo(bias.mag_bias_variance); - bias.mag_bias_valid = true; // TODO + bias_var.copyTo(bias.mag_bias_variance); + bias.mag_bias_valid = bias_var.longerThan(0.f) && !bias_var.longerThan(0.1f); bias.mag_bias_stable = _mag_cal.cal_available; _last_mag_bias_published = mag_bias; } +#endif // CONFIG_EKF2_MAGNETOMETER + bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_sensor_bias_pub.publish(bias); @@ -1607,7 +1758,7 @@ void EKF2::PublishStates(const hrt_abstime ×tamp) // publish estimator states estimator_states_s states; states.timestamp_sample = _ekf.time_delayed_us(); - states.n_states = Ekf::_k_num_states; + states.n_states = Ekf::getNumberOfStates(); _ekf.getStateAtFusionHorizonAsVector().copyTo(states.states); _ekf.covariances_diagonal().copyTo(states.covariances); states.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); @@ -1657,10 +1808,18 @@ void EKF2::PublishStatus(const hrt_abstime ×tamp) status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.accel_device_id = _device_id_accel; +#if defined(CONFIG_EKF2_BAROMETER) status.baro_device_id = _device_id_baro; +#endif // CONFIG_EKF2_BAROMETER status.gyro_device_id = _device_id_gyro; + +#if defined(CONFIG_EKF2_MAGNETOMETER) status.mag_device_id = _device_id_mag; + _ekf.get_mag_checks(status.mag_inclination_deg, status.mag_inclination_ref_deg, status.mag_strength_gs, + status.mag_strength_ref_gs); +#endif // CONFIG_EKF2_MAGNETOMETER + status.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); _estimator_status_pub.publish(status); } @@ -1731,6 +1890,9 @@ void EKF2::PublishStatusFlags(const hrt_abstime ×tamp) status_flags.cs_fake_pos = _ekf.control_status_flags().fake_pos; status_flags.cs_fake_hgt = _ekf.control_status_flags().fake_hgt; status_flags.cs_gravity_vector = _ekf.control_status_flags().gravity_vector; + status_flags.cs_mag = _ekf.control_status_flags().mag; + 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.fault_status_changes = _filter_fault_status_changes; status_flags.fs_bad_mag_x = _ekf.fault_status_flags().bad_mag_x; @@ -1966,6 +2128,7 @@ void EKF2::UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps) } #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF baro sample @@ -2009,6 +2172,7 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } } +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool EKF2::UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2243,9 +2407,9 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) gpsMessage gps_msg{ .time_usec = vehicle_gps_position.timestamp, - .lat = vehicle_gps_position.lat, - .lon = vehicle_gps_position.lon, - .alt = vehicle_gps_position.alt, + .lat = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)), + .lon = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)), + .alt = static_cast(round(vehicle_gps_position.altitude_msl_m * 1e3)), .yaw = vehicle_gps_position.heading, .yaw_offset = vehicle_gps_position.heading_offset, .yaw_accuracy = vehicle_gps_position.heading_accuracy, @@ -2267,10 +2431,11 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) _ekf.setGpsData(gps_msg); _gps_time_usec = gps_msg.time_usec; - _gps_alttitude_ellipsoid = vehicle_gps_position.alt_ellipsoid; + _gps_alttitude_ellipsoid = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)); } } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) { const unsigned last_generation = _magnetometer_sub.get_last_generation(); @@ -2314,6 +2479,7 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps) (int64_t)ekf2_timestamps.timestamp / 100); } } +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) @@ -2454,10 +2620,12 @@ void EKF2::UpdateCalibration(const hrt_abstime ×tamp, InFlightCalibration & // consider bias estimates stable when all checks pass consistently and bias hasn't changed more than 10% of the limit const float bias_change_limit = 0.1f * bias_limit; - if ((cal.last_us != 0) && !(cal.bias - bias).longerThan(bias_change_limit)) { - cal.total_time_us += timestamp - cal.last_us; + if (!(cal.bias - bias).longerThan(bias_change_limit)) { + if (cal.last_us != 0) { + cal.total_time_us += timestamp - cal.last_us; + } - if (cal.total_time_us > 30_s) { + if (cal.total_time_us > 10_s) { cal.cal_available = true; } @@ -2512,23 +2680,25 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) bias_valid, learning_valid); } +#if defined(CONFIG_EKF2_MAGNETOMETER) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { - const bool bias_valid = (_ekf.control_status_flags().mag_hdg || _ekf.control_status_flags().mag_3D) - && _ekf.control_status_flags().mag_aligned_in_flight - && !_ekf.control_status_flags().mag_fault - && !_ekf.control_status_flags().mag_field_disturbed; + const Vector3f mag_bias = _ekf.getMagBias(); + const Vector3f mag_bias_var = _ekf.getMagBiasVariance(); - const bool learning_valid = bias_valid && _ekf.control_status_flags().mag_3D; + const bool bias_valid = (_ekf.fault_status().value == 0) + && _ekf.control_status_flags().yaw_align + && mag_bias_var.longerThan(0.f) && !mag_bias_var.longerThan(0.02f); - UpdateCalibration(timestamp, _mag_cal, _ekf.getMagBias(), _ekf.getMagBiasVariance(), _ekf.getMagBiasLimit(), - bias_valid, learning_valid); + const bool learning_valid = bias_valid && _ekf.control_status_flags().mag; + + UpdateCalibration(timestamp, _mag_cal, mag_bias, mag_bias_var, _ekf.getMagBiasLimit(), bias_valid, learning_valid); // update stored declination value if (!_mag_decl_saved) { float declination_deg; - if (_ekf.get_mag_decl_deg(&declination_deg)) { + if (_ekf.get_mag_decl_deg(declination_deg)) { _param_ekf2_mag_decl.update(); if (PX4_ISFINITE(declination_deg) && (fabsf(declination_deg - _param_ekf2_mag_decl.get()) > 0.1f)) { @@ -2540,6 +2710,7 @@ void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) } } } +#endif // CONFIG_EKF2_MAGNETOMETER int EKF2::custom_command(int argc, char *argv[]) { @@ -2568,16 +2739,18 @@ int EKF2::task_spawn(int argc, char *argv[]) // ekf selector requires SENS_IMU_MODE = 0 multi_mode = true; - // IMUs (1 - 4 supported) + // IMUs (1 - MAX_NUM_IMUS supported) param_get(param_find("EKF2_MULTI_IMU"), &imu_instances); - if (imu_instances < 1 || imu_instances > 4) { - const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast(1), static_cast(4)); + if (imu_instances < 1 || imu_instances > MAX_NUM_IMUS) { + const int32_t imu_instances_limited = math::constrain(imu_instances, static_cast(1), + static_cast(MAX_NUM_IMUS)); PX4_WARN("EKF2_MULTI_IMU limited %" PRId32 " -> %" PRId32, imu_instances, imu_instances_limited); param_set_no_notification(param_find("EKF2_MULTI_IMU"), &imu_instances_limited); imu_instances = imu_instances_limited; } +#if defined(CONFIG_EKF2_MAGNETOMETER) int32_t sens_mag_mode = 1; const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE"); param_get(param_sens_mag_mode, &sens_mag_mode); @@ -2586,9 +2759,10 @@ int EKF2::task_spawn(int argc, char *argv[]) const param_t param_ekf2_mult_mag = param_find("EKF2_MULTI_MAG"); param_get(param_ekf2_mult_mag, &mag_instances); - // Mags (1 - 4 supported) - if (mag_instances > 4) { - const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast(1), static_cast(4)); + // Mags (1 - MAX_NUM_MAGS supported) + if (mag_instances > MAX_NUM_MAGS) { + const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast(1), + static_cast(MAX_NUM_MAGS)); PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited); param_set_no_notification(param_ekf2_mult_mag, &mag_instances_limited); mag_instances = mag_instances_limited; @@ -2607,6 +2781,8 @@ int EKF2::task_spawn(int argc, char *argv[]) } else { mag_instances = 1; } + +#endif // CONFIG_EKF2_MAGNETOMETER } if (multi_mode && !replay_mode) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a40971e79f..8eea2e9dec 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include #include @@ -85,7 +84,6 @@ #include #include #include -#include #include #include #include @@ -100,6 +98,14 @@ # include #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) +# include +#endif // CONFIG_EKF2_BAROMETER + +#if defined(CONFIG_EKF2_MAGNETOMETER) +# include +#endif // CONFIG_EKF2_MAGNETOMETER + #if defined(CONFIG_EKF2_OPTICAL_FLOW) # include # include @@ -167,7 +173,10 @@ private: void PublishAidSourceStatus(const hrt_abstime ×tamp); void PublishAttitude(const hrt_abstime ×tamp); + +#if defined(CONFIG_EKF2_BAROMETER) void PublishBaroBias(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_BAROMETER void PublishGnssHgtBias(const hrt_abstime ×tamp); #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -204,7 +213,9 @@ private: #if defined(CONFIG_EKF2_AUXVEL) void UpdateAuxVelSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_AUXVEL +#if defined(CONFIG_EKF2_BAROMETER) void UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_BAROMETER #if defined(CONFIG_EKF2_EXTERNAL_VISION) bool UpdateExtVisionSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_EXTERNAL_VISION @@ -212,7 +223,9 @@ private: bool UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_OPTICAL_FLOW void UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps); +#endif // CONFIG_EKF2_MAGNETOMETER #if defined(CONFIG_EKF2_RANGE_FINDER) void UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps); #endif // CONFIG_EKF2_RANGE_FINDER @@ -230,7 +243,9 @@ private: const matrix::Vector3f &bias_variance, float bias_limit, bool bias_valid, bool learning_valid); void UpdateAccelCalibration(const hrt_abstime ×tamp); void UpdateGyroCalibration(const hrt_abstime ×tamp); +#if defined(CONFIG_EKF2_MAGNETOMETER) void UpdateMagCalibration(const hrt_abstime ×tamp); +#endif // CONFIG_EKF2_MAGNETOMETER // publish helper for estimator_aid_source topics template @@ -269,17 +284,11 @@ private: perf_counter_t _ecl_ekf_update_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL update")}; perf_counter_t _ecl_ekf_update_full_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": ECL full update")}; perf_counter_t _msg_missed_imu_perf{perf_alloc(PC_COUNT, MODULE_NAME": IMU message missed")}; - perf_counter_t _msg_missed_air_data_perf{nullptr}; perf_counter_t _msg_missed_gps_perf{nullptr}; - perf_counter_t _msg_missed_magnetometer_perf{nullptr}; perf_counter_t _msg_missed_odometry_perf{nullptr}; - // Used to control saving of mag declination to be used on next startup - bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved - InFlightCalibration _accel_cal{}; InFlightCalibration _gyro_cal{}; - InFlightCalibration _mag_cal{}; uint64_t _gps_time_usec{0}; int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid @@ -287,28 +296,41 @@ private: float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 uint8_t _accel_calibration_count{0}; - uint8_t _baro_calibration_count{0}; uint8_t _gyro_calibration_count{0}; - uint8_t _mag_calibration_count{0}; uint32_t _device_id_accel{0}; - uint32_t _device_id_baro{0}; uint32_t _device_id_gyro{0}; - uint32_t _device_id_mag{0}; Vector3f _last_accel_bias_published{}; Vector3f _last_gyro_bias_published{}; - Vector3f _last_mag_bias_published{}; hrt_abstime _last_sensor_bias_published{0}; hrt_abstime _last_gps_status_published{0}; - hrt_abstime _status_baro_hgt_pub_last{0}; - hrt_abstime _status_rng_hgt_pub_last{0}; - hrt_abstime _status_fake_hgt_pub_last{0}; hrt_abstime _status_fake_pos_pub_last{0}; +#if defined(CONFIG_EKF2_MAGNETOMETER) + uint32_t _device_id_mag {0}; + + perf_counter_t _msg_missed_magnetometer_perf {nullptr}; + + // Used to control saving of mag declination to be used on next startup + bool _mag_decl_saved = false; ///< true when the magnetic declination has been saved + + InFlightCalibration _mag_cal{}; + uint8_t _mag_calibration_count{0}; + 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 + #if defined(CONFIG_EKF2_EXTERNAL_VISION) uORB::PublicationMulti _estimator_aid_src_ev_hgt_pub {ORB_ID(estimator_aid_src_ev_hgt)}; uORB::PublicationMulti _estimator_aid_src_ev_pos_pub{ORB_ID(estimator_aid_src_ev_pos)}; @@ -333,9 +355,6 @@ private: hrt_abstime _status_gnss_yaw_pub_last {0}; #endif // CONFIG_EKF2_GNSS_YAW - hrt_abstime _status_mag_pub_last{0}; - hrt_abstime _status_mag_heading_pub_last{0}; - hrt_abstime _status_gravity_pub_last{0}; #if defined(CONFIG_EKF2_AUXVEL) @@ -347,20 +366,45 @@ private: perf_counter_t _msg_missed_landing_target_pose_perf{nullptr}; #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)}; - uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; - uORB::PublicationMulti _estimator_aid_src_terrain_optical_flow_pub{ORB_ID(estimator_aid_src_terrain_optical_flow)}; + uORB::PublicationMulti _estimator_aid_src_optical_flow_pub{ORB_ID(estimator_aid_src_optical_flow)}; hrt_abstime _status_optical_flow_pub_last{0}; - hrt_abstime _status_terrain_optical_flow_pub_last{0}; + perf_counter_t _msg_missed_optical_flow_perf{nullptr}; #endif // CONFIG_EKF2_OPTICAL_FLOW +#if defined(CONFIG_EKF2_BAROMETER) + perf_counter_t _msg_missed_air_data_perf {nullptr}; + + uint8_t _baro_calibration_count {0}; + uint32_t _device_id_baro{0}; + hrt_abstime _status_baro_hgt_pub_last{0}; + float _last_baro_bias_published{}; + + uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; + + uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; + uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; +#endif // CONFIG_EKF2_BAROMETER + float _last_gnss_hgt_bias_published{}; - float _last_rng_hgt_bias_published{}; #if defined(CONFIG_EKF2_AIRSPEED) uORB::Subscription _airspeed_sub {ORB_ID(airspeed)}; @@ -385,8 +429,6 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; 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)}; @@ -397,6 +439,9 @@ private: 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)}; @@ -423,7 +468,6 @@ private: uint32_t _filter_information_event_changes{0}; uORB::PublicationMulti _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; - uORB::PublicationMulti _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)}; uORB::PublicationMulti _estimator_gnss_hgt_bias_pub{ORB_ID(estimator_gnss_hgt_bias)}; uORB::PublicationMultiData _estimator_event_flags_pub{ORB_ID(estimator_event_flags)}; uORB::PublicationMulti _estimator_gps_status_pub{ORB_ID(estimator_gps_status)}; @@ -436,8 +480,6 @@ private: uORB::PublicationMulti _estimator_status_pub{ORB_ID(estimator_status)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _estimator_aid_src_baro_hgt_pub {ORB_ID(estimator_aid_src_baro_hgt)}; - uORB::PublicationMulti _estimator_aid_src_fake_hgt_pub{ORB_ID(estimator_aid_src_fake_hgt)}; uORB::PublicationMulti _estimator_aid_src_fake_pos_pub{ORB_ID(estimator_aid_src_fake_pos)}; @@ -448,9 +490,6 @@ private: uORB::PublicationMulti _estimator_aid_src_gnss_yaw_pub {ORB_ID(estimator_aid_src_gnss_yaw)}; #endif // CONFIG_EKF2_GNSS_YAW - 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)}; - uORB::PublicationMulti _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; // publications with topic dependent on multi-mode @@ -471,10 +510,6 @@ private: (ParamExtInt) _param_ekf2_predict_us, (ParamExtInt) _param_ekf2_imu_ctrl, - (ParamExtFloat) - _param_ekf2_mag_delay, ///< magnetometer measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_baro_delay, ///< barometer height measurement delay relative to the IMU (mSec) (ParamExtFloat) _param_ekf2_gps_delay, ///< GPS measurement delay relative to the IMU (mSec) @@ -493,10 +528,6 @@ private: _param_ekf2_gyr_b_noise, ///< process noise for IMU rate gyro bias prediction (rad/sec**2) (ParamExtFloat) _param_ekf2_acc_b_noise,///< process noise for IMU accelerometer bias prediction (m/sec**3) - (ParamExtFloat) - _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) @@ -506,14 +537,26 @@ private: _param_ekf2_gps_p_noise, ///< minimum allowed observation noise for gps position fusion (m) (ParamExtFloat) _param_ekf2_noaid_noise, ///< observation noise for non-aiding position fusion (m) - (ParamExtFloat) - _param_ekf2_baro_noise, ///< observation noise for barometric height fusion (m) - (ParamExtFloat) - _param_ekf2_baro_gate, ///< barometric height innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_gnd_eff_dz, ///< barometric deadzone range for negative innovations (m) - (ParamExtFloat) - _param_ekf2_gnd_max_hgt, ///< maximum height above the ground level for expected negative baro innovations (m) + +#if defined(CONFIG_EKF2_BAROMETER) + (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection + (ParamExtFloat) _param_ekf2_baro_delay, + (ParamExtFloat) _param_ekf2_baro_noise, + (ParamExtFloat) _param_ekf2_baro_gate, + (ParamExtFloat) _param_ekf2_gnd_eff_dz, + (ParamExtFloat) _param_ekf2_gnd_max_hgt, + +# if defined(CONFIG_EKF2_BARO_COMPENSATION) + // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth + (ParamExtFloat) _param_ekf2_aspd_max, + (ParamExtFloat) _param_ekf2_pcoef_xp, + (ParamExtFloat) _param_ekf2_pcoef_xn, + (ParamExtFloat) _param_ekf2_pcoef_yp, + (ParamExtFloat) _param_ekf2_pcoef_yn, + (ParamExtFloat) _param_ekf2_pcoef_z, +# endif // CONFIG_EKF2_BARO_COMPENSATION +#endif // CONFIG_EKF2_BAROMETER + (ParamExtFloat) _param_ekf2_gps_p_gate, ///< GPS horizontal position innovation consistency gate size (STD) (ParamExtFloat) @@ -541,25 +584,24 @@ private: _param_ekf2_fuse_beta, ///< Controls synthetic sideslip fusion, 0 disables, 1 enables #endif // CONFIG_EKF2_SIDESLIP - // control of magnetometer fusion - (ParamExtFloat) - _param_ekf2_head_noise, ///< measurement noise used for simple heading fusion (rad) - (ParamExtFloat) - _param_ekf2_mag_noise, ///< measurement noise used for 3-axis magnetoemeter fusion (Gauss) - - (ParamExtFloat) _param_ekf2_mag_decl,///< magnetic declination (degrees) - (ParamExtFloat) - _param_ekf2_hdg_gate,///< heading fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_mag_gate, ///< magnetometer fusion innovation consistency gate size (STD) - (ParamExtInt) - _param_ekf2_decl_type, ///< bitmask used to control the handling of declination data - (ParamExtInt) - _param_ekf2_mag_type, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_acclim, ///< integer used to specify the type of magnetometer fusion used - (ParamExtFloat) - _param_ekf2_mag_yawlim, ///< yaw rate threshold used by mode select logic (rad/sec) +#if defined(CONFIG_EKF2_MAGNETOMETER) + (ParamExtFloat) _param_ekf2_mag_delay, + (ParamExtFloat) _param_ekf2_mag_e_noise, + (ParamExtFloat) _param_ekf2_mag_b_noise, + (ParamExtFloat) _param_ekf2_head_noise, + (ParamExtFloat) _param_ekf2_mag_noise, + (ParamExtFloat) _param_ekf2_mag_decl, + (ParamExtFloat) _param_ekf2_hdg_gate, + (ParamExtFloat) _param_ekf2_mag_gate, + (ParamExtInt) _param_ekf2_decl_type, + (ParamExtInt) _param_ekf2_mag_type, + (ParamExtFloat) _param_ekf2_mag_acclim, + (ParamExtFloat) _param_ekf2_mag_yawlim, + (ParamExtInt) _param_ekf2_mag_check, + (ParamExtFloat) _param_ekf2_mag_chk_str, + (ParamExtFloat) _param_ekf2_mag_chk_inc, + (ParamExtInt) _param_ekf2_synthetic_mag_z, +#endif // CONFIG_EKF2_MAGNETOMETER (ParamExtInt) _param_ekf2_gps_check, ///< bitmask used to control which GPS quality checks are used @@ -577,48 +619,35 @@ private: (ParamInt) _param_ekf2_aid_mask, ///< bitmasked integer that selects which of the GPS and optical flow aiding sources will be used (ParamExtInt) _param_ekf2_hgt_ref, ///< selects the primary source for height data - (ParamExtInt) _param_ekf2_baro_ctrl,///< barometer control selection (ParamExtInt) _param_ekf2_gps_ctrl, ///< GPS control selection (ParamExtInt) _param_ekf2_noaid_tout, ///< maximum lapsed time from last fusion of measurements that constrain drift before the EKF will report the horizontal nav solution invalid (uSec) +#if defined(CONFIG_EKF2_TERRAIN) || defined(CONFIG_EKF2_OPTICAL_FLOW) || defined(CONFIG_EKF2_RANGE_FINDER) + (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 #if defined(CONFIG_EKF2_RANGE_FINDER) // range finder fusion - (ParamExtInt) _param_ekf2_rng_ctrl, ///< range finder control selection - (ParamExtFloat) - _param_ekf2_rng_delay, ///< range finder measurement delay relative to the IMU (mSec) - (ParamExtFloat) - _param_ekf2_rng_noise, ///< observation noise for range finder measurements (m) - (ParamExtFloat) - _param_ekf2_rng_sfe, ///< scale factor from range to range noise (m/m) - (ParamExtFloat) - _param_ekf2_rng_gate, ///< range finder fusion innovation consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_min_rng, ///< minimum valid value for range when on ground (m) - (ParamExtFloat) _param_ekf2_rng_pitch, ///< range sensor pitch offset (rad) - (ParamExtFloat) - _param_ekf2_rng_a_vmax, ///< maximum allowed horizontal velocity for range aid (m/s) - (ParamExtFloat) - _param_ekf2_rng_a_hmax, ///< maximum allowed absolute altitude (AGL) for range aid (m) - (ParamExtFloat) - _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) - (ParamExtFloat) - _param_ekf2_rng_qlty_t, ///< Minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s) - (ParamExtFloat) - _param_ekf2_rng_k_gate, ///< range finder kinematic consistency gate size (STD) - (ParamExtFloat) - _param_ekf2_rng_pos_x, ///< X position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_y, ///< Y position of range finder in body frame (m) - (ParamExtFloat) - _param_ekf2_rng_pos_z, ///< Z position of range finder in body frame (m) - - (ParamExtInt) - _param_ekf2_terr_mask, ///< bitmasked integer that selects which of range finder and optical flow aiding sources will be used for terrain estimation - (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) - (ParamExtFloat) - _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) + (ParamExtInt) _param_ekf2_rng_ctrl, + (ParamExtFloat) _param_ekf2_rng_delay, + (ParamExtFloat) _param_ekf2_rng_noise, + (ParamExtFloat) _param_ekf2_rng_sfe, + (ParamExtFloat) _param_ekf2_rng_gate, + (ParamExtFloat) _param_ekf2_rng_pitch, + (ParamExtFloat) _param_ekf2_rng_a_vmax, + (ParamExtFloat) _param_ekf2_rng_a_hmax, + (ParamExtFloat) _param_ekf2_rng_a_igate, + (ParamExtFloat) _param_ekf2_rng_qlty_t, + (ParamExtFloat) _param_ekf2_rng_k_gate, + (ParamExtFloat) _param_ekf2_rng_pos_x, + (ParamExtFloat) _param_ekf2_rng_pos_y, + (ParamExtFloat) _param_ekf2_rng_pos_z, #endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -662,7 +691,9 @@ private: (ParamExtFloat) _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air + (ParamExtInt) + _param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) (ParamExtFloat) @@ -716,27 +747,7 @@ private: (ParamExtFloat) _param_ekf2_mcoef, ///< propeller momentum drag coefficient (1/s) #endif // CONFIG_EKF2_DRAG_FUSION -#if defined(CONFIG_EKF2_BARO_COMPENSATION) - // Corrections for static pressure position error where Ps_error = Ps_meas - Ps_truth - // Coef = Ps_error / Pdynamic, where Pdynamic = 1/2 * density * TAS**2 - (ParamExtFloat) - _param_ekf2_aspd_max, ///< upper limit on airspeed used for correction (m/s**2) - (ParamExtFloat) - _param_ekf2_pcoef_xp, ///< static pressure position error coefficient along the positive X body axis - (ParamExtFloat) - _param_ekf2_pcoef_xn, ///< static pressure position error coefficient along the negative X body axis - (ParamExtFloat) - _param_ekf2_pcoef_yp, ///< static pressure position error coefficient along the positive Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_yn, ///< static pressure position error coefficient along the negative Y body axis - (ParamExtFloat) - _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis -#endif // CONFIG_EKF2_BARO_COMPENSATION - (ParamFloat) _param_ekf2_req_gps_h, ///< Required GPS health time - (ParamExtInt) _param_ekf2_mag_check, ///< Mag field strength check - (ParamExtInt) - _param_ekf2_synthetic_mag_z, ///< Enables the use of a synthetic value for the Z axis of the magnetometer calculated from the 3D magnetic field vector at the location of the drone. // Used by EKF-GSF experimental yaw estimator (ParamExtFloat) diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index 6d27130363..a89b68b838 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -28,10 +28,18 @@ depends on MODULES_EKF2 ---help--- EKF2 auxiliary velocity fusion support. +menuconfig EKF2_BAROMETER +depends on MODULES_EKF2 + bool "barometer support" + default y + ---help--- + EKF2 barometer support. + menuconfig EKF2_BARO_COMPENSATION depends on MODULES_EKF2 bool "barometer compensation support" default y + depends on EKF2_BAROMETER ---help--- EKF2 pressure compensation support. @@ -56,10 +64,18 @@ depends on MODULES_EKF2 ---help--- EKF2 GNSS yaw fusion support. +menuconfig EKF2_MAGNETOMETER +depends on MODULES_EKF2 + bool "magnetometer support" + default y + ---help--- + EKF2 magnetometer support. + menuconfig EKF2_OPTICAL_FLOW 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. @@ -78,6 +94,14 @@ depends on MODULES_EKF2 ---help--- EKF2 sideslip fusion support. +menuconfig EKF2_TERRAIN +depends on MODULES_EKF2 + bool "terrain estimator support" + default y + depends on EKF2_OPTICAL_FLOW || EKF2_RANGE_FINDER + ---help--- + EKF2 terrain estimator support. + menuconfig USER_EKF2 bool "ekf2 running as userspace module" default n diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index bf7b62f6ac..1ac4b2dd26 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_ #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow); + 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); diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 5e9af2c4a9..a4fe6ab482 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -77,6 +77,7 @@ public: 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; } @@ -179,6 +180,7 @@ private: #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) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cf4a49ff75..e3d5decc1a 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2015-2016 Estimation and Control Library (ECL). All rights reserved. + * 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 @@ -12,7 +12,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name ECL nor the names of its contributors may be + * 3. Neither the name PX4 nor the names of its contributors may be * used to endorse or promote products derived from this software * without specific prior written permission. * @@ -445,7 +445,7 @@ PARAM_DEFINE_FLOAT(EKF2_BETA_GATE, 5.0f); PARAM_DEFINE_FLOAT(EKF2_BETA_NOISE, 0.3f); /** - * Gate size for magnetic heading fusion + * Gate size for heading fusion * * Sets the number of standard deviations used by the innovation consistency test. * @@ -493,16 +493,10 @@ PARAM_DEFINE_INT32(EKF2_DECL_TYPE, 7); * 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 '3-axis' 3-axis field fusion is used at all times. - * If set to 'VTOL custom' the behaviour is the same as 'Automatic', but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. This can be used by VTOL platforms with large magnetic field disturbances to prevent incorrect bias states being learned during forward flight operation which can adversely affect estimation accuracy after transition to hovering flight. - * If set to 'MC custom' the behaviour is the same as 'Automatic, but if there are no earth frame position or velocity observations being used, the magnetometer will not be used. This enables vehicles to operate with no GPS in environments where the magnetic field cannot be used to provide a heading reference. Prior to flight, the yaw angle is assumed to be constant if movement tests indicate that the vehicle is static. This allows the vehicle to be placed on the ground to learn the yaw gyro bias prior to flight. * 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 2 3-axis - * @value 3 VTOL custom - * @value 4 MC custom * @value 5 None * @reboot_required true */ @@ -901,7 +895,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); /** - * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN. + * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN. * * @group EKF2 * @min 0 @@ -909,6 +903,15 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); */ 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 * @@ -1079,11 +1082,10 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); /** * Airspeed fusion threshold. * -* A value of zero will deactivate airspeed fusion. Any other positive -* value will determine the minimum airspeed which will still be fused. Set to about 90% of the vehicles stall speed. -* Both airspeed fusion and sideslip fusion must be active for the EKF to continue navigating after loss of GPS. -* Use EKF2_FUSE_BETA to activate sideslip fusion. -* Note: side slip fusion is currently not supported for tailsitters. +* 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 @@ -1093,11 +1095,11 @@ PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f); PARAM_DEFINE_FLOAT(EKF2_ARSP_THR, 0.0f); /** -* Boolean determining if synthetic sideslip measurements should fused. +* Enable synthetic sideslip fusion. * -* A value of 1 indicates that fusion is active -* Both sideslip fusion and airspeed fusion must be active for the EKF to continue navigating after loss of GPS. -* Use EKF2_ARSP_THR to activate airspeed 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 @@ -1493,17 +1495,52 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_GPS_H, 10.0f); /** * Magnetic field strength test selection * - * When set, the EKF checks the strength of the magnetic field - * to decide whether the magnetometer data is valid. - * If GPS data is received, the magnetic field is compared to a World + * 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 - * @boolean + * @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. * diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index edb4782b68..7d0af8e502 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -37,12 +37,13 @@ add_subdirectory(sensor_simulator) add_subdirectory(test_helper) px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_attitude_variance.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_airspeed_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) 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_covariance_prediction_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) 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_gyroscope.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_opt_flow_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) 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) @@ -57,7 +58,7 @@ px4_add_unit_gtest(SRC test_EKF_mag_declination_generated.cpp LINKLIBS ecl_EKF e 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_sideslip_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) -px4_add_unit_gtest(SRC test_EKF_terrain_estimator.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_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 cbb92b3d2b..08ff126f62 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.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -90000,1,-0.0096,-0.01,-0.02,-0.0004,0.0026,-0.026,-1.4e-05,0.00011,-0.0023,0,0,0,0,0,0,0.19,-4.7e-10,0.4,0,0,0,0,0,5.2e-07,0.0026,0.0026,0.0011,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -190000,1,-0.0096,-0.011,-0.02,0.0002,0.0039,-0.041,6e-06,0.00043,-0.0044,-3e-13,-2.3e-14,5.8e-15,0,0,-2e-11,0.19,-4.7e-10,0.4,0,0,0,0,0,5.8e-07,0.0027,0.0027,0.0008,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -290000,1,-0.0097,-0.011,-0.02,0.0012,0.0063,-0.053,5e-05,0.00036,-0.007,9.1e-12,9.1e-13,-1.7e-13,0,0,-4.8e-09,0.19,-4.7e-10,0.4,0,0,0,0,0,6.8e-07,0.0029,0.0029,0.00067,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -390000,1,-0.0089,-0.012,0.046,0.0026,0.0083,-0.059,0.00024,0.0011,-0.0094,-1.1e-10,2.8e-11,2.8e-12,0,0,-4.5e-08,0.17,0.0017,0.41,0,0,0,0,0,7e-07,0.0031,0.0031,0.00062,25,25,8.1,0.97,0.97,0.32,1e-06,1e-06,8.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -490000,0.87,-0.0023,-0.015,0.5,0.0023,0.0057,-0.06,0.0002,0.00051,-0.011,1.6e-08,-3.8e-09,-3.3e-10,0,0,-1.6e-07,0.14,0.0014,0.42,0,0,0,0,0,2.5e-06,0.0033,0.0033,0.00053,7.8,7.8,5.9,0.34,0.34,0.31,1e-06,1e-06,8e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -590000,0.76,0.00067,-0.015,0.65,0.00071,0.0084,-0.059,0.00037,0.0012,-0.012,1.6e-08,-3.5e-09,-3.2e-10,0,0,-3.4e-07,0.18,0.0018,0.43,0,0,0,0,0,1.4e-05,0.0036,0.0036,0.00045,7.9,7.9,4.2,0.67,0.67,0.32,1e-06,1e-06,7.1e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -690000,0.72,0.0016,-0.016,0.69,-0.00082,0.0075,-0.06,7.2e-05,0.00069,-0.013,5.4e-08,-2.4e-08,-9e-10,0,0,-6.3e-07,0.2,0.002,0.43,0,0,0,0,0,3.7e-05,0.0039,0.0039,0.00039,2.6,2.6,2.8,0.26,0.26,0.29,1e-06,1e-06,6.3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -790000,0.71,0.002,-0.016,0.7,-0.0028,0.0098,-0.063,-9.3e-05,0.0015,-0.014,5.3e-08,-2.3e-08,-8.8e-10,0,0,-9.5e-07,0.2,0.002,0.43,0,0,0,0,0,6.9e-05,0.0042,0.0042,0.00035,2.7,2.7,2,0.42,0.42,0.28,1e-06,1e-06,5.6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -890000,0.71,0.0021,-0.016,0.7,-0.0034,0.011,-0.077,-0.00019,0.0011,-0.021,1.7e-07,-9e-08,-2.5e-09,0,0,-9.5e-07,0.2,0.002,0.43,0,0,0,0,0,0.00011,0.0046,0.0046,0.00032,1.3,1.3,2,0.2,0.2,0.43,1e-06,1e-06,4.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -990000,0.7,0.0025,-0.016,0.71,-0.0038,0.015,-0.092,-0.00057,0.0024,-0.029,1.7e-07,-9e-08,-2.5e-09,0,0,-9.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00014,0.005,0.005,0.0003,1.4,1.4,2,0.3,0.3,0.61,1e-06,1e-06,4.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1090000,0.7,0.0025,-0.017,0.71,-0.0034,0.016,-0.11,-0.00043,0.0019,-0.039,5.5e-07,-3.7e-07,-8.5e-09,0,0,-9.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00018,0.0053,0.0053,0.00029,0.89,0.89,2,0.17,0.17,0.84,9.9e-07,9.9e-07,3.8e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1190000,0.7,0.0027,-0.017,0.71,-0.0037,0.02,-0.12,-0.00079,0.0037,-0.051,5.5e-07,-3.7e-07,-8.5e-09,0,0,-9.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00022,0.0058,0.0058,0.00028,1.1,1.1,2,0.24,0.24,1.1,9.9e-07,9.9e-07,3.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1290000,0.7,0.0028,-0.017,0.71,-0.0034,0.02,-0.14,-0.00055,0.0028,-0.064,1.4e-06,-1.3e-06,-2.5e-08,0,0,-9.6e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00025,0.006,0.006,0.00027,0.84,0.84,2,0.15,0.15,1.4,9.6e-07,9.6e-07,3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1390000,0.7,0.0029,-0.017,0.71,-0.0032,0.026,-0.15,-0.0009,0.0052,-0.078,1.4e-06,-1.3e-06,-2.5e-08,0,0,-9.6e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00028,0.0066,0.0066,0.00026,1.1,1.1,2,0.21,0.21,1.7,9.6e-07,9.6e-07,2.6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1490000,0.7,0.0027,-0.017,0.71,-0.0024,0.025,-0.16,-0.0006,0.0039,-0.093,2.9e-06,-3.4e-06,-5.9e-08,0,0,-9.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.0003,0.0063,0.0063,0.00025,0.9,0.9,2,0.14,0.14,2.1,9e-07,9e-07,2.3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1590000,0.7,0.0029,-0.018,0.71,-0.0029,0.031,-0.18,-0.00087,0.0066,-0.11,2.9e-06,-3.4e-06,-5.9e-08,0,0,-9.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00032,0.0069,0.0069,0.00024,1.2,1.2,2,0.2,0.2,2.6,9e-07,9e-07,2e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1690000,0.7,0.0025,-0.018,0.71,0.00055,0.028,-0.19,-0.00048,0.0047,-0.13,5e-06,-7e-06,-1.1e-07,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00033,0.006,0.006,0.00023,0.97,0.97,2,0.13,0.13,3,8.1e-07,8.1e-07,1.8e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1790000,0.7,0.0027,-0.018,0.71,0.0025,0.036,-0.2,-0.00035,0.0078,-0.15,5e-06,-7e-06,-1.1e-07,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00035,0.0066,0.0066,0.00023,1.3,1.3,2,0.2,0.2,3.5,8.1e-07,8.1e-07,1.6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1890000,0.7,0.003,-0.018,0.72,0.0038,0.044,-0.22,-2.9e-05,0.012,-0.17,5e-06,-7e-06,-1.1e-07,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00035,0.0073,0.0073,0.00022,1.6,1.6,2,0.3,0.3,4.1,8.1e-07,8.1e-07,1.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1990000,0.7,0.0025,-0.018,0.72,0.0056,0.037,-0.23,0.00047,0.0085,-0.19,6.8e-06,-1.2e-05,-1.6e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00036,0.0059,0.0059,0.00021,1.3,1.3,2,0.2,0.2,4.7,6.9e-07,7e-07,1.2e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2090000,0.7,0.0026,-0.018,0.72,0.0083,0.043,-0.24,0.0012,0.012,-0.22,6.8e-06,-1.2e-05,-1.6e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00036,0.0064,0.0064,0.0002,1.6,1.6,2.1,0.3,0.3,5.3,6.9e-07,7e-07,1.1e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2190000,0.7,0.0021,-0.018,0.72,0.0084,0.034,-0.26,0.0012,0.0083,-0.24,8e-06,-1.8e-05,-1.9e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00037,0.0049,0.0049,0.0002,1.2,1.2,2.1,0.19,0.19,6,5.8e-07,5.8e-07,9.9e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2290000,0.7,0.0021,-0.018,0.72,0.011,0.041,-0.27,0.0022,0.012,-0.27,8e-06,-1.8e-05,-1.9e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00037,0.0054,0.0054,0.00019,1.5,1.5,2.1,0.29,0.29,6.7,5.8e-07,5.8e-07,8.8e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2390000,0.7,0.0016,-0.017,0.72,0.011,0.031,-0.29,0.0019,0.0078,-0.3,8.3e-06,-2.4e-05,-2e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00037,0.004,0.004,0.00018,1,1,2.1,0.19,0.19,7.4,4.9e-07,4.9e-07,7.9e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2490000,0.7,0.0017,-0.018,0.72,0.014,0.036,-0.3,0.0031,0.011,-0.32,8.3e-06,-2.4e-05,-2e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00037,0.0044,0.0044,0.00018,1.3,1.3,2.1,0.27,0.27,8.2,4.9e-07,4.9e-07,7.1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2590000,0.7,0.0014,-0.017,0.72,0.012,0.027,-0.31,0.0023,0.0071,-0.36,8e-06,-2.8e-05,-1.9e-07,0,0,-1.2e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00036,0.0033,0.0033,0.00017,0.89,0.89,2.1,0.18,0.18,9.1,4.1e-07,4.1e-07,6.4e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2690000,0.7,0.0014,-0.017,0.72,0.015,0.031,-0.33,0.0037,0.01,-0.39,8e-06,-2.8e-05,-1.9e-07,0,0,-1.2e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00036,0.0036,0.0036,0.00017,1.1,1.1,2.2,0.25,0.25,10,4.1e-07,4.1e-07,5.8e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2790000,0.7,0.0012,-0.017,0.72,0.014,0.024,-0.34,0.0027,0.0064,-0.42,7.2e-06,-3.3e-05,-1.7e-07,0,0,-1.2e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00036,0.0028,0.0028,0.00016,0.77,0.77,2.2,0.16,0.16,11,3.4e-07,3.4e-07,5.3e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2890000,0.7,0.0012,-0.017,0.72,0.016,0.027,-0.35,0.0041,0.0089,-0.46,7.2e-06,-3.3e-05,-1.7e-07,0,0,-1.2e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00035,0.003,0.003,0.00016,0.96,0.96,2.2,0.23,0.23,12,3.4e-07,3.4e-07,4.8e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -2990000,0.7,0.0011,-0.017,0.72,0.014,0.022,-0.36,0.003,0.0058,-0.49,6e-06,-3.6e-05,-1.3e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00035,0.0024,0.0024,0.00015,0.68,0.68,2.2,0.15,0.15,13,2.9e-07,2.9e-07,4.4e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3090000,0.7,0.0011,-0.017,0.72,0.017,0.024,-0.38,0.0045,0.0081,-0.53,6e-06,-3.6e-05,-1.3e-07,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00034,0.0026,0.0026,0.00015,0.84,0.84,2.2,0.22,0.22,14,2.9e-07,2.9e-07,4e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3190000,0.7,0.00089,-0.017,0.72,0.014,0.019,-0.39,0.0032,0.0053,-0.57,4.6e-06,-3.9e-05,-9.3e-08,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00034,0.0021,0.0021,0.00015,0.6,0.6,2.3,0.14,0.14,15,2.5e-07,2.5e-07,3.6e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3290000,0.7,0.00089,-0.017,0.72,0.017,0.023,-0.4,0.0047,0.0074,-0.61,4.6e-06,-3.9e-05,-9.3e-08,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00033,0.0023,0.0023,0.00014,0.74,0.74,2.3,0.2,0.2,16,2.5e-07,2.5e-07,3.3e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3390000,0.7,0.00084,-0.017,0.72,0.015,0.018,-0.42,0.0033,0.0049,-0.65,3.2e-06,-4.2e-05,-5e-08,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00033,0.0018,0.0018,0.00014,0.54,0.54,2.3,0.14,0.14,17,2.1e-07,2.1e-07,3.1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3490000,0.7,0.00085,-0.017,0.72,0.019,0.021,-0.43,0.005,0.0068,-0.69,3.2e-06,-4.2e-05,-5e-08,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00033,0.002,0.002,0.00013,0.67,0.67,2.3,0.19,0.19,19,2.1e-07,2.1e-07,2.8e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3590000,0.7,0.00079,-0.017,0.72,0.017,0.016,-0.44,0.0036,0.0045,-0.73,1.5e-06,-4.4e-05,-3.6e-09,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00032,0.0016,0.0016,0.00013,0.5,0.5,2.4,0.13,0.13,20,1.8e-07,1.8e-07,2.6e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3690000,0.7,0.00079,-0.017,0.72,0.021,0.018,-0.46,0.0055,0.0063,-0.78,1.5e-06,-4.4e-05,-3.6e-09,0,0,-1.1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00032,0.0018,0.0018,0.00013,0.61,0.61,2.4,0.18,0.18,21,1.8e-07,1.8e-07,2.4e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3790000,0.7,0.00073,-0.017,0.72,0.02,0.014,-0.47,0.004,0.0042,-0.83,-2.9e-07,-4.6e-05,4.8e-08,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00031,0.0014,0.0014,0.00012,0.46,0.46,2.4,0.12,0.12,23,1.5e-07,1.5e-07,2.2e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3890000,0.7,0.00077,-0.017,0.72,0.022,0.016,-0.48,0.0061,0.0057,-0.87,-2.9e-07,-4.6e-05,4.8e-08,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.00031,0.0016,0.0016,0.00012,0.56,0.56,2.4,0.17,0.17,24,1.5e-07,1.5e-07,2.1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -3990000,0.7,0.00081,-0.017,0.72,0.025,0.019,-0.5,0.0084,0.0074,-0.92,-2.9e-07,-4.6e-05,4.8e-08,0,0,-1e-06,0.21,0.0021,0.43,0,0,0,0,0,0.0003,0.0017,0.0017,0.00012,0.68,0.68,2.5,0.23,0.23,26,1.5e-07,1.5e-07,1.9e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4090000,0.7,0.00078,-0.016,0.72,0.021,0.016,-0.51,0.0062,0.0053,-0.97,-2.2e-06,-4.8e-05,1e-07,0,0,-9.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.0003,0.0014,0.0014,0.00012,0.52,0.52,2.5,0.16,0.16,27,1.2e-07,1.2e-07,1.8e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4190000,0.7,0.00077,-0.016,0.72,0.025,0.018,-0.53,0.0086,0.007,-1,-2.2e-06,-4.8e-05,1e-07,0,0,-9.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00029,0.0015,0.0015,0.00011,0.63,0.63,2.5,0.22,0.22,29,1.2e-07,1.2e-07,1.7e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4290000,0.7,0.0008,-0.016,0.72,0.019,0.015,-0.54,0.0063,0.005,-1.1,-4.1e-06,-5e-05,1.6e-07,0,0,-9.3e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00029,0.0012,0.0012,0.00011,0.48,0.48,2.6,0.15,0.15,31,1e-07,1e-07,1.6e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4390000,0.7,0.00067,-0.016,0.71,0.021,0.016,-0.55,0.0083,0.0066,-1.1,-4.1e-06,-5e-05,1.6e-07,0,0,-9.3e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00028,0.0014,0.0014,0.00011,0.58,0.58,2.6,0.2,0.2,33,1e-07,1e-07,1.5e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4490000,0.7,0.00072,-0.016,0.71,0.017,0.013,-0.57,0.0058,0.0046,-1.2,-5.9e-06,-5.1e-05,2.1e-07,0,0,-8.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00028,0.0011,0.0011,0.00011,0.44,0.44,2.6,0.14,0.14,34,8.3e-08,8.3e-08,1.4e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4590000,0.7,0.00068,-0.016,0.71,0.019,0.015,-0.58,0.0076,0.006,-1.2,-5.9e-06,-5.1e-05,2.1e-07,0,0,-8.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00028,0.0012,0.0012,0.0001,0.53,0.53,2.7,0.19,0.19,36,8.3e-08,8.3e-08,1.3e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4690000,0.7,0.00069,-0.016,0.71,0.016,0.012,-0.6,0.0054,0.0043,-1.3,-7.4e-06,-5.2e-05,2.5e-07,0,0,-8.4e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00027,0.00095,0.00095,0.0001,0.41,0.41,2.7,0.14,0.14,38,6.7e-08,6.7e-08,1.2e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4790000,0.7,0.00062,-0.016,0.71,0.018,0.014,-0.61,0.0071,0.0056,-1.4,-7.4e-06,-5.2e-05,2.5e-07,0,0,-8.4e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00027,0.001,0.001,0.0001,0.49,0.49,2.7,0.18,0.18,40,6.7e-08,6.7e-08,1.1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4890000,0.7,0.00064,-0.016,0.71,0.016,0.011,-0.63,0.0051,0.0039,-1.4,-8.7e-06,-5.3e-05,2.9e-07,0,0,-7.9e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00026,0.00083,0.00083,9.9e-05,0.37,0.37,2.8,0.13,0.13,42,5.4e-08,5.4e-08,1.1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -4990000,0.7,0.00068,-0.016,0.71,0.017,0.013,-0.64,0.0068,0.0051,-1.5,-8.7e-06,-5.3e-05,2.9e-07,0,0,-7.9e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00026,0.00089,0.00089,9.7e-05,0.44,0.44,2.8,0.17,0.17,44,5.4e-08,5.4e-08,1e-08,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5090000,0.7,0.00068,-0.015,0.71,0.014,0.01,-0.66,0.0049,0.0037,-1.6,-9.9e-06,-5.4e-05,3.2e-07,0,0,-7.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00026,0.00071,0.00071,9.6e-05,0.34,0.34,2.8,0.13,0.13,46,4.4e-08,4.4e-08,9.5e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5190000,0.7,0.00067,-0.015,0.71,0.016,0.011,-0.67,0.0064,0.0047,-1.6,-9.9e-06,-5.4e-05,3.2e-07,0,0,-7.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00025,0.00077,0.00077,9.4e-05,0.4,0.4,2.9,0.16,0.16,49,4.4e-08,4.4e-08,9e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5290000,0.7,0.00074,-0.015,0.71,0.012,0.0072,-0.68,0.0046,0.0033,-1.7,-1.1e-05,-5.5e-05,3.5e-07,0,0,-7e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00025,0.00062,0.00062,9.3e-05,0.31,0.31,2.9,0.12,0.12,51,3.5e-08,3.5e-08,8.5e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5390000,0.7,0.00083,-0.015,0.71,0.014,0.0065,-0.7,0.0058,0.0039,-1.8,-1.1e-05,-5.5e-05,3.5e-07,0,0,-7e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00025,0.00066,0.00066,9.1e-05,0.37,0.37,2.9,0.16,0.16,53,3.5e-08,3.5e-08,8.1e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5490000,0.7,0.0009,-0.015,0.71,0.011,0.0042,-0.71,0.0042,0.0026,-1.8,-1.2e-05,-5.5e-05,3.8e-07,0,0,-6.7e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00024,0.00053,0.00053,8.9e-05,0.28,0.28,3,0.11,0.11,56,2.8e-08,2.8e-08,7.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5590000,0.7,0.00091,-0.015,0.71,0.012,0.0047,-0.73,0.0053,0.003,-1.9,-1.2e-05,-5.5e-05,3.8e-07,0,0,-6.7e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00024,0.00057,0.00057,8.8e-05,0.33,0.33,3,0.15,0.15,58,2.8e-08,2.8e-08,7.2e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5690000,0.7,0.00099,-0.015,0.71,0.0081,0.003,-0.74,0.0037,0.002,-2,-1.3e-05,-5.6e-05,4e-07,0,0,-6.4e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00024,0.00045,0.00045,8.7e-05,0.26,0.26,3.1,0.11,0.11,61,2.3e-08,2.3e-08,6.9e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5790000,0.7,0.001,-0.015,0.71,0.0074,0.0032,-0.75,0.0045,0.0023,-2.1,-1.3e-05,-5.6e-05,4e-07,0,0,-6.4e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00023,0.00049,0.00049,8.5e-05,0.3,0.3,3.1,0.14,0.14,64,2.3e-08,2.3e-08,6.5e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5890000,0.7,0.0011,-0.015,0.71,0.0048,0.0027,0.0028,0.003,0.0016,-3.7e+02,-1.3e-05,-5.6e-05,4.1e-07,0,0,-6.4e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00023,0.00039,0.00039,8.4e-05,0.23,0.23,9.8,0.11,0.11,0.52,1.8e-08,1.8e-08,6.2e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -5990000,0.7,0.0011,-0.015,0.71,0.0037,0.0029,0.015,0.0034,0.0018,-3.7e+02,-1.3e-05,-5.6e-05,4.1e-07,0,0,-7.3e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00023,0.00042,0.00042,8.3e-05,0.27,0.27,8.8,0.13,0.13,0.33,1.8e-08,1.8e-08,5.9e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6090000,0.7,0.0011,-0.015,0.71,0.0029,0.0038,-0.011,0.0037,0.0022,-3.7e+02,-1.3e-05,-5.6e-05,4.1e-07,0,0,-6.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00022,0.00045,0.00045,8.1e-05,0.32,0.32,7,0.17,0.17,0.33,1.8e-08,1.8e-08,5.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6190000,0.7,0.0011,-0.015,0.71,-0.00035,0.0027,-0.005,0.0022,0.0015,-3.7e+02,-1.3e-05,-5.6e-05,4.2e-07,0,0,-8.8e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00022,0.00036,0.00036,8e-05,0.25,0.25,4.9,0.13,0.13,0.32,1.5e-08,1.5e-08,5.4e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6290000,0.7,0.0011,-0.015,0.71,-5.8e-05,0.0039,-0.012,0.0022,0.0019,-3.7e+02,-1.3e-05,-5.6e-05,4.2e-07,0,0,-9.6e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00022,0.00038,0.00038,7.9e-05,0.29,0.29,3.2,0.16,0.16,0.3,1.5e-08,1.5e-08,5.1e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6390000,0.7,0.0012,-0.014,0.71,-0.0018,0.0033,-0.05,0.0012,0.0014,-3.7e+02,-1.4e-05,-5.6e-05,4.3e-07,0,0,-3.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00021,0.00031,0.00031,7.8e-05,0.23,0.23,2.3,0.12,0.12,0.29,1.2e-08,1.2e-08,4.9e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6490000,0.7,0.0012,-0.015,0.71,-0.0017,0.0039,-0.052,0.001,0.0018,-3.7e+02,-1.4e-05,-5.6e-05,4.3e-07,0,0,-8.5e-07,0.21,0.0021,0.43,0,0,0,0,0,0.00021,0.00033,0.00033,7.7e-05,0.26,0.26,1.5,0.15,0.15,0.26,1.2e-08,1.2e-08,4.7e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6590000,0.7,0.0012,-0.015,0.71,-0.0024,0.0029,-0.099,0.00045,0.0014,-3.7e+02,-1.4e-05,-5.7e-05,4.4e-07,0,0,9.7e-07,0.21,0.0021,0.44,0,0,0,0,0,0.00089,0.00026,0.00026,0.00077,0.19,0.19,1.1,0.12,0.12,0.23,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6690000,0.7,0.0012,-0.015,0.71,-0.0016,0.0037,-0.076,0.00024,0.0017,-3.7e+02,-1.4e-05,-5.7e-05,4.2e-07,0,0,-2.2e-06,0.21,0.0021,0.44,0,0,0,0,0,0.0006,0.00026,0.00026,0.0005,0.19,0.19,0.78,0.14,0.14,0.21,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6790000,0.7,0.0012,-0.014,0.71,-0.0026,0.0034,-0.11,3.6e-05,0.0021,-3.7e+02,-1.4e-05,-5.7e-05,4.5e-07,0,0,1e-07,0.21,0.0021,0.44,0,0,0,0,0,0.00047,0.00026,0.00026,0.00038,0.2,0.2,0.6,0.17,0.17,0.2,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6890000,0.7,0.0012,-0.014,0.71,-0.0043,0.0037,-0.12,-0.00033,0.0025,-3.7e+02,-1.4e-05,-5.7e-05,4.8e-07,0,0,-4.1e-07,0.21,0.0021,0.44,0,0,0,0,0,0.00039,0.00026,0.00026,0.0003,0.21,0.21,0.46,0.21,0.21,0.18,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -6990000,0.7,0.0013,-0.014,0.71,-0.0039,0.0044,-0.12,-0.00074,0.0029,-3.7e+02,-1.4e-05,-5.7e-05,3.8e-07,0,0,-3.4e-06,0.21,0.0021,0.44,0,0,0,0,0,0.00033,0.00026,0.00026,0.00025,0.22,0.22,0.36,0.25,0.25,0.16,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -7090000,0.7,0.0013,-0.014,0.71,-0.0041,0.0038,-0.13,-0.0011,0.0033,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-07,0,0,-7e-06,0.21,0.0021,0.44,0,0,0,0,0,0.0003,0.00027,0.00027,0.00021,0.23,0.23,0.29,0.29,0.29,0.16,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -7190000,0.7,0.0013,-0.014,0.71,-0.0054,0.0037,-0.15,-0.0016,0.0036,-3.7e+02,-1.4e-05,-5.7e-05,2.1e-07,0,0,-4.9e-06,0.21,0.0021,0.44,0,0,0,0,0,0.00027,0.00027,0.00027,0.00019,0.25,0.25,0.24,0.34,0.34,0.15,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -7290000,0.7,0.0013,-0.014,0.71,-0.0064,0.0037,-0.15,-0.0022,0.004,-3.7e+02,-1.4e-05,-5.7e-05,2.4e-07,0,0,-1.2e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00024,0.00027,0.00027,0.00016,0.27,0.27,0.2,0.4,0.4,0.14,9.8e-09,9.8e-09,4.6e-09,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -7390000,0.7,0.0013,-0.014,0.71,-0.0061,0.005,-0.16,-0.0028,0.0044,-3.7e+02,-1.4e-05,-5.7e-05,2.9e-07,0,0,-1.3e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00023,0.00028,0.00028,0.00015,0.29,0.29,0.18,0.46,0.46,0.13,9.7e-09,9.7e-09,4.6e-09,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -7490000,0.7,0.0014,-0.014,0.71,-0.0072,0.0052,-0.16,-0.0035,0.0049,-3.7e+02,-1.4e-05,-5.7e-05,3e-07,0,0,-2.1e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00021,0.00028,0.00028,0.00014,0.32,0.32,0.15,0.53,0.53,0.12,9.7e-09,9.7e-09,4.6e-09,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -7590000,0.7,0.0014,-0.014,0.71,-0.0086,0.0062,-0.17,-0.0042,0.0055,-3.7e+02,-1.4e-05,-5.7e-05,4.5e-07,0,0,-3e-05,0.21,0.0021,0.44,0,0,0,0,0,0.0002,0.00028,0.00028,0.00012,0.35,0.35,0.14,0.6,0.6,0.12,9.7e-09,9.7e-09,4.6e-09,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -7690000,0.7,0.0014,-0.014,0.71,-0.01,0.0068,-0.16,-0.0052,0.0061,-3.7e+02,-1.4e-05,-5.7e-05,4.5e-07,0,0,-5e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00019,0.00029,0.00029,0.00012,0.38,0.38,0.13,0.69,0.69,0.11,9.7e-09,9.7e-09,4.6e-09,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -7790000,0.7,0.0015,-0.014,0.71,-0.011,0.0074,-0.16,-0.0062,0.0067,-3.7e+02,-1.4e-05,-5.7e-05,2.1e-07,0,0,-7e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00018,0.0003,0.0003,0.00011,0.41,0.41,0.12,0.78,0.78,0.11,9.7e-09,9.7e-09,4.6e-09,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -7890000,0.7,0.0015,-0.014,0.71,-0.013,0.0091,-0.16,-0.0074,0.0075,-3.7e+02,-1.4e-05,-5.7e-05,3.3e-07,0,0,-9.6e-05,0.21,0.0021,0.44,0,0,0,0,0,0.00017,0.0003,0.0003,0.0001,0.45,0.45,0.11,0.89,0.89,0.1,9.7e-09,9.7e-09,4.5e-09,4e-06,4e-06,3.8e-06,0,0,0,0,0,0,0,0 -7990000,0.7,0.0015,-0.014,0.71,-0.014,0.01,-0.16,-0.0087,0.0084,-3.7e+02,-1.4e-05,-5.7e-05,4.6e-07,0,0,-0.00011,0.21,0.0021,0.44,0,0,0,0,0,0.00017,0.00031,0.00031,9.5e-05,0.49,0.49,0.1,1,1,0.099,9.6e-09,9.6e-09,4.5e-09,4e-06,4e-06,3.8e-06,0,0,0,0,0,0,0,0 -8090000,0.7,0.0015,-0.014,0.71,-0.016,0.011,-0.17,-0.01,0.0094,-3.7e+02,-1.4e-05,-5.7e-05,6.8e-07,0,0,-0.00011,0.21,0.0021,0.44,0,0,0,0,0,0.00016,0.00031,0.00031,9e-05,0.53,0.53,0.1,1.1,1.1,0.097,9.6e-09,9.6e-09,4.5e-09,4e-06,4e-06,3.7e-06,0,0,0,0,0,0,0,0 -8190000,0.7,0.0015,-0.014,0.71,-0.018,0.013,-0.18,-0.012,0.011,-3.7e+02,-1.4e-05,-5.7e-05,4.8e-07,0,0,-0.00013,0.21,0.0021,0.44,0,0,0,0,0,0.00016,0.00032,0.00032,8.6e-05,0.58,0.58,0.099,1.3,1.3,0.094,9.6e-09,9.6e-09,4.5e-09,4e-06,4e-06,3.7e-06,0,0,0,0,0,0,0,0 -8290000,0.7,0.0015,-0.014,0.71,-0.019,0.013,-0.17,-0.014,0.012,-3.7e+02,-1.4e-05,-5.7e-05,3.4e-07,0,0,-0.00017,0.21,0.0021,0.44,0,0,0,0,0,0.00015,0.00033,0.00033,8.2e-05,0.63,0.63,0.097,1.4,1.4,0.091,9.5e-09,9.5e-09,4.5e-09,4e-06,4e-06,3.6e-06,0,0,0,0,0,0,0,0 -8390000,0.7,0.0016,-0.014,0.71,-0.02,0.014,-0.17,-0.016,0.013,-3.7e+02,-1.4e-05,-5.7e-05,5.9e-07,0,0,-0.00021,0.21,0.0021,0.44,0,0,0,0,0,0.00015,0.00034,0.00034,7.9e-05,0.68,0.68,0.097,1.6,1.6,0.091,9.5e-09,9.5e-09,4.5e-09,4e-06,4e-06,3.5e-06,0,0,0,0,0,0,0,0 -8490000,0.7,0.0015,-0.014,0.71,-0.021,0.015,-0.17,-0.018,0.014,-3.7e+02,-1.4e-05,-5.7e-05,4.9e-07,0,0,-0.00025,0.21,0.0021,0.44,0,0,0,0,0,0.00015,0.00034,0.00034,7.6e-05,0.73,0.73,0.096,1.8,1.8,0.089,9.4e-09,9.4e-09,4.5e-09,4e-06,4e-06,3.4e-06,0,0,0,0,0,0,0,0 -8590000,0.7,0.0016,-0.014,0.71,-0.023,0.017,-0.17,-0.02,0.016,-3.7e+02,-1.4e-05,-5.7e-05,2.4e-07,0,0,-0.00029,0.21,0.0021,0.44,0,0,0,0,0,0.00014,0.00035,0.00035,7.3e-05,0.79,0.79,0.095,2,2,0.088,9.4e-09,9.4e-09,4.4e-09,4e-06,4e-06,3.4e-06,0,0,0,0,0,0,0,0 -8690000,0.7,0.0016,-0.014,0.71,-0.026,0.018,-0.16,-0.022,0.017,-3.7e+02,-1.4e-05,-5.7e-05,5.9e-07,0,0,-0.00035,0.21,0.0021,0.44,0,0,0,0,0,0.00014,0.00036,0.00036,7.1e-05,0.84,0.84,0.096,2.2,2.2,0.088,9.3e-09,9.3e-09,4.4e-09,4e-06,4e-06,3.3e-06,0,0,0,0,0,0,0,0 -8790000,0.7,0.0016,-0.014,0.71,-0.027,0.019,-0.15,-0.025,0.018,-3.7e+02,-1.4e-05,-5.7e-05,4.3e-07,0,0,-0.00041,0.21,0.0021,0.44,0,0,0,0,0,0.00014,0.00037,0.00037,6.9e-05,0.91,0.91,0.095,2.5,2.5,0.087,9.3e-09,9.3e-09,4.4e-09,4e-06,4e-06,3.2e-06,0,0,0,0,0,0,0,0 -8890000,0.7,0.0016,-0.014,0.71,-0.029,0.02,-0.15,-0.027,0.02,-3.7e+02,-1.4e-05,-5.7e-05,3.2e-07,0,0,-0.00045,0.21,0.0021,0.44,0,0,0,0,0,0.00014,0.00037,0.00037,6.7e-05,0.96,0.96,0.095,2.7,2.7,0.086,9.2e-09,9.2e-09,4.4e-09,4e-06,4e-06,3e-06,0,0,0,0,0,0,0,0 -8990000,0.7,0.0016,-0.014,0.71,-0.03,0.02,-0.14,-0.031,0.022,-3.7e+02,-1.4e-05,-5.7e-05,4e-08,0,0,-0.00051,0.21,0.0021,0.44,0,0,0,0,0,0.00014,0.00038,0.00038,6.5e-05,1,1,0.096,3,3,0.087,9.2e-09,9.2e-09,4.4e-09,4e-06,4e-06,2.9e-06,0,0,0,0,0,0,0,0 -9090000,0.7,0.0017,-0.014,0.71,-0.032,0.021,-0.14,-0.033,0.023,-3.7e+02,-1.4e-05,-5.7e-05,-1.4e-07,0,0,-0.00053,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00038,0.00038,6.4e-05,1.1,1.1,0.095,3.3,3.3,0.086,9e-09,9e-09,4.3e-09,4e-06,4e-06,2.8e-06,0,0,0,0,0,0,0,0 -9190000,0.7,0.0017,-0.014,0.71,-0.033,0.021,-0.14,-0.036,0.025,-3.7e+02,-1.4e-05,-5.7e-05,5.1e-07,0,0,-0.00057,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00039,0.00039,6.2e-05,1.2,1.2,0.094,3.6,3.6,0.085,9e-09,9e-09,4.3e-09,4e-06,4e-06,2.7e-06,0,0,0,0,0,0,0,0 -9290000,0.7,0.0016,-0.014,0.71,-0.033,0.022,-0.14,-0.039,0.026,-3.7e+02,-1.3e-05,-5.7e-05,5.8e-07,0,0,-0.00061,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00039,0.00039,6.1e-05,1.2,1.2,0.093,3.9,3.9,0.085,8.9e-09,8.9e-09,4.3e-09,4e-06,4e-06,2.5e-06,0,0,0,0,0,0,0,0 -9390000,0.7,0.0016,-0.014,0.71,-0.034,0.024,-0.14,-0.042,0.028,-3.7e+02,-1.3e-05,-5.7e-05,5.8e-07,0,0,-0.00065,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.0004,0.0004,6e-05,1.3,1.3,0.093,4.3,4.3,0.086,8.9e-09,8.9e-09,4.2e-09,4e-06,4e-06,2.4e-06,0,0,0,0,0,0,0,0 -9490000,0.7,0.0016,-0.014,0.71,-0.035,0.023,-0.13,-0.044,0.029,-3.7e+02,-1.3e-05,-5.7e-05,1.1e-06,0,0,-0.00068,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.0004,0.0004,5.9e-05,1.3,1.3,0.091,4.6,4.6,0.085,8.7e-09,8.7e-09,4.2e-09,4e-06,4e-06,2.3e-06,0,0,0,0,0,0,0,0 -9590000,0.7,0.0016,-0.014,0.71,-0.037,0.024,-0.13,-0.048,0.031,-3.7e+02,-1.3e-05,-5.7e-05,1.2e-06,0,0,-0.00072,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00041,0.00041,5.9e-05,1.4,1.4,0.09,5.1,5.1,0.085,8.7e-09,8.7e-09,4.2e-09,4e-06,4e-06,2.1e-06,0,0,0,0,0,0,0,0 -9690000,0.7,0.0017,-0.014,0.71,-0.037,0.026,-0.12,-0.049,0.031,-3.7e+02,-1.3e-05,-5.7e-05,7.2e-07,0,0,-0.00077,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00041,0.00041,5.8e-05,1.4,1.4,0.089,5.3,5.3,0.086,8.4e-09,8.5e-09,4.1e-09,4e-06,4e-06,2e-06,0,0,0,0,0,0,0,0 -9790000,0.7,0.0017,-0.014,0.71,-0.037,0.027,-0.11,-0.053,0.034,-3.7e+02,-1.3e-05,-5.7e-05,1.1e-06,0,0,-0.00082,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00042,0.00042,5.7e-05,1.5,1.5,0.087,5.9,5.9,0.085,8.4e-09,8.5e-09,4.1e-09,4e-06,4e-06,1.9e-06,0,0,0,0,0,0,0,0 -9890000,0.7,0.0016,-0.014,0.71,-0.037,0.027,-0.11,-0.054,0.034,-3.7e+02,-1.3e-05,-5.7e-05,1e-06,0,0,-0.00085,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00041,0.00041,5.7e-05,1.5,1.5,0.084,6.1,6.1,0.085,8.2e-09,8.2e-09,4e-09,4e-06,4e-06,1.8e-06,0,0,0,0,0,0,0,0 -9990000,0.7,0.0017,-0.014,0.71,-0.038,0.028,-0.1,-0.058,0.037,-3.7e+02,-1.3e-05,-5.7e-05,7.9e-07,0,0,-0.00089,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00042,0.00042,5.7e-05,1.6,1.6,0.083,6.7,6.7,0.086,8.2e-09,8.2e-09,4e-09,4e-06,4e-06,1.7e-06,0,0,0,0,0,0,0,0 -10090000,0.7,0.0017,-0.014,0.71,-0.037,0.026,-0.096,-0.058,0.037,-3.7e+02,-1.3e-05,-5.7e-05,9.1e-07,0,0,-0.00091,0.21,0.0021,0.44,0,0,0,0,0,0.00013,0.00041,0.00041,5.6e-05,1.6,1.6,0.08,6.9,6.9,0.085,8e-09,8e-09,4e-09,4e-06,4e-06,1.6e-06,0,0,0,0,0,0,0,0 -10190000,0.7,0.0017,-0.014,0.71,-0.038,0.029,-0.096,-0.061,0.039,-3.7e+02,-1.3e-05,-5.7e-05,1.8e-07,0,0,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00042,0.00042,5.6e-05,1.7,1.7,0.078,7.5,7.6,0.084,8e-09,8e-09,3.9e-09,4e-06,4e-06,1.4e-06,0,0,0,0,0,0,0,0 -10290000,0.7,0.0017,-0.014,0.71,-0.038,0.028,-0.084,-0.066,0.042,-3.7e+02,-1.3e-05,-5.7e-05,-2.8e-07,0,0,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00043,0.00043,5.6e-05,1.8,1.8,0.076,8.3,8.3,0.085,8e-09,8e-09,3.9e-09,4e-06,4e-06,1.4e-06,0,0,0,0,0,0,0,0 -10390000,0.7,0.0016,-0.014,0.71,0.01,-0.02,0.0096,0.0009,-0.0018,-3.7e+02,-1.3e-05,-5.7e-05,-2.5e-07,-6.4e-10,5.1e-10,-0.001,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00044,0.00044,5.5e-05,0.25,0.25,0.56,0.25,0.25,0.078,8e-09,8e-09,3.8e-09,4e-06,4e-06,1.3e-06,0,0,0,0,0,0,0,0 -10490000,0.7,0.0017,-0.014,0.71,0.0097,-0.018,0.023,0.0019,-0.0036,-3.7e+02,-1.3e-05,-5.7e-05,-7.1e-07,-1.7e-08,1.3e-08,-0.001,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00046,0.00046,5.5e-05,0.26,0.26,0.55,0.26,0.26,0.08,8e-09,8e-09,3.8e-09,4e-06,4e-06,1.2e-06,0,0,0,0,0,0,0,0 -10590000,0.7,0.0017,-0.014,0.71,0.0092,-0.0076,0.026,0.0018,-0.00084,-3.7e+02,-1.3e-05,-5.7e-05,-6.7e-07,-2.8e-06,7.3e-08,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00047,0.00047,5.5e-05,0.13,0.13,0.27,0.13,0.13,0.073,7.9e-09,7.9e-09,3.7e-09,4e-06,4e-06,1.2e-06,0,0,0,0,0,0,0,0 -10690000,0.7,0.0018,-0.014,0.71,0.0079,-0.0075,0.03,0.0027,-0.0016,-3.7e+02,-1.3e-05,-5.7e-05,-8.2e-07,-2.8e-06,8.4e-08,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00048,0.00048,5.5e-05,0.14,0.14,0.26,0.14,0.14,0.078,7.9e-09,7.9e-09,3.7e-09,4e-06,4e-06,1.1e-06,0,0,0,0,0,0,0,0 -10790000,0.7,0.0018,-0.014,0.71,0.0072,-0.0048,0.024,0.0028,-0.0008,-3.7e+02,-1.3e-05,-5.7e-05,-7.4e-07,-5e-06,7.1e-07,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00047,0.00047,5.5e-05,0.099,0.099,0.17,0.091,0.091,0.072,7.8e-09,7.8e-09,3.6e-09,4e-06,4e-06,1.1e-06,0,0,0,0,0,0,0,0 -10890000,0.7,0.0017,-0.014,0.71,0.0067,-0.0041,0.02,0.0034,-0.0012,-3.7e+02,-1.3e-05,-5.7e-05,-7.5e-07,-5e-06,7e-07,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00049,0.00049,5.5e-05,0.11,0.11,0.16,0.098,0.098,0.075,7.8e-09,7.8e-09,3.6e-09,4e-06,4e-06,1.1e-06,0,0,0,0,0,0,0,0 -10990000,0.7,0.0017,-0.014,0.71,0.0086,0.00073,0.014,0.0048,-0.0024,-3.7e+02,-1.3e-05,-5.6e-05,-2.6e-07,-1e-05,6.1e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00047,0.00047,5.5e-05,0.09,0.09,0.12,0.073,0.073,0.071,7.5e-09,7.5e-09,3.5e-09,3.9e-06,3.9e-06,1.1e-06,0,0,0,0,0,0,0,0 -11090000,0.7,0.0017,-0.014,0.71,0.0079,0.0025,0.019,0.0056,-0.0023,-3.7e+02,-1.3e-05,-5.6e-05,2.3e-07,-1e-05,6.1e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00048,0.00048,5.5e-05,0.11,0.11,0.11,0.079,0.079,0.074,7.5e-09,7.5e-09,3.4e-09,3.9e-06,3.9e-06,1.1e-06,0,0,0,0,0,0,0,0 -11190000,0.7,0.0017,-0.014,0.71,0.012,0.0053,0.0077,0.0067,-0.0031,-3.7e+02,-1.2e-05,-5.6e-05,-1.3e-07,-1.3e-05,1.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00044,0.00044,5.4e-05,0.088,0.088,0.084,0.063,0.063,0.069,7e-09,7e-09,3.4e-09,3.8e-06,3.8e-06,1e-06,0,0,0,0,0,0,0,0 -11290000,0.7,0.0018,-0.014,0.71,0.012,0.0071,0.0074,0.0079,-0.0024,-3.7e+02,-1.2e-05,-5.6e-05,-7.6e-07,-1.3e-05,1.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00045,0.00045,5.5e-05,0.11,0.11,0.078,0.07,0.07,0.072,7e-09,7e-09,3.3e-09,3.8e-06,3.8e-06,1e-06,0,0,0,0,0,0,0,0 -11390000,0.7,0.0019,-0.014,0.71,0.0073,0.0067,0.0017,0.0057,-0.0022,-3.7e+02,-1.3e-05,-5.6e-05,-1.3e-06,-9.8e-06,5.2e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0004,0.0004,5.4e-05,0.089,0.089,0.063,0.057,0.057,0.068,6.5e-09,6.5e-09,3.3e-09,3.7e-06,3.7e-06,1e-06,0,0,0,0,0,0,0,0 -11490000,0.7,0.0019,-0.014,0.71,0.0051,0.009,0.0025,0.0063,-0.0014,-3.7e+02,-1.3e-05,-5.6e-05,-2.3e-06,-9.9e-06,5.3e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00041,0.00041,5.4e-05,0.11,0.11,0.058,0.065,0.065,0.069,6.5e-09,6.5e-09,3.2e-09,3.7e-06,3.7e-06,1e-06,0,0,0,0,0,0,0,0 -11590000,0.7,0.0018,-0.014,0.71,0.00045,0.0084,-0.0034,0.0047,-0.0015,-3.7e+02,-1.3e-05,-5.7e-05,-2.5e-06,-5.7e-06,2.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00036,0.00036,5.5e-05,0.089,0.089,0.049,0.054,0.054,0.066,6e-09,6e-09,3.2e-09,3.7e-06,3.7e-06,1e-06,0,0,0,0,0,0,0,0 -11690000,0.7,0.0018,-0.014,0.71,-0.002,0.011,-0.0079,0.0046,-0.00059,-3.7e+02,-1.3e-05,-5.6e-05,-2.8e-06,-5.7e-06,2.5e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00037,0.00037,5.4e-05,0.11,0.11,0.046,0.062,0.062,0.066,6e-09,6e-09,3.1e-09,3.7e-06,3.7e-06,1e-06,0,0,0,0,0,0,0,0 -11790000,0.7,0.0019,-0.014,0.71,-0.008,0.011,-0.0098,0.0021,0.00045,-3.7e+02,-1.3e-05,-5.7e-05,-2.9e-06,-5.1e-06,-1.2e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00032,0.00032,5.4e-05,0.087,0.087,0.039,0.052,0.052,0.063,5.5e-09,5.5e-09,3e-09,3.6e-06,3.6e-06,1e-06,0,0,0,0,0,0,0,0 -11890000,0.7,0.0019,-0.014,0.71,-0.0092,0.012,-0.011,0.0013,0.0016,-3.7e+02,-1.3e-05,-5.7e-05,-3.4e-06,-5.1e-06,-1.1e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00033,0.00033,5.4e-05,0.1,0.1,0.037,0.06,0.06,0.063,5.5e-09,5.6e-09,3e-09,3.6e-06,3.6e-06,1e-06,0,0,0,0,0,0,0,0 -11990000,0.7,0.002,-0.014,0.71,-0.011,0.013,-0.016,-5.8e-05,0.0022,-3.7e+02,-1.3e-05,-5.7e-05,-3.3e-06,-3.9e-06,-1.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00029,0.00029,5.4e-05,0.083,0.083,0.033,0.051,0.051,0.061,5.1e-09,5.1e-09,2.9e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12090000,0.7,0.002,-0.014,0.71,-0.013,0.015,-0.022,-0.0013,0.0036,-3.7e+02,-1.3e-05,-5.7e-05,-2.8e-06,-3.7e-06,-1.7e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00029,0.00029,5.4e-05,0.099,0.099,0.031,0.059,0.059,0.061,5.1e-09,5.1e-09,2.9e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12190000,0.7,0.0017,-0.014,0.71,-0.007,0.012,-0.017,0.0015,0.002,-3.7e+02,-1.3e-05,-5.7e-05,-2.7e-06,2.4e-07,5.5e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00026,0.00026,5.4e-05,0.079,0.079,0.028,0.05,0.05,0.059,4.8e-09,4.8e-09,2.8e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12290000,0.7,0.0016,-0.014,0.71,-0.0094,0.014,-0.016,0.00069,0.0033,-3.7e+02,-1.3e-05,-5.7e-05,-2.6e-06,-3.3e-08,5.7e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00027,0.00027,5.4e-05,0.093,0.093,0.028,0.058,0.058,0.059,4.8e-09,4.8e-09,2.8e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12390000,0.7,0.0014,-0.014,0.71,-0.0045,0.011,-0.015,0.0029,0.0018,-3.7e+02,-1.2e-05,-5.8e-05,-2.9e-06,2.9e-06,1.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00024,0.00024,5.4e-05,0.075,0.075,0.026,0.05,0.05,0.057,4.5e-09,4.5e-09,2.7e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12490000,0.7,0.0014,-0.014,0.71,-0.0056,0.013,-0.018,0.0024,0.003,-3.7e+02,-1.2e-05,-5.8e-05,-3.4e-06,2.8e-06,1.1e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00025,0.00025,5.4e-05,0.087,0.087,0.026,0.058,0.058,0.057,4.5e-09,4.5e-09,2.7e-09,3.5e-06,3.5e-06,1e-06,0,0,0,0,0,0,0,0 -12590000,0.7,0.0014,-0.014,0.71,-0.013,0.011,-0.023,-0.0027,0.0016,-3.7e+02,-1.3e-05,-5.8e-05,-3.4e-06,5.6e-06,5.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00022,0.00022,5.4e-05,0.07,0.07,0.025,0.049,0.049,0.055,4.3e-09,4.3e-09,2.6e-09,3.5e-06,3.5e-06,9.9e-07,0,0,0,0,0,0,0,0 -12690000,0.7,0.0015,-0.014,0.71,-0.013,0.012,-0.027,-0.0041,0.0028,-3.7e+02,-1.3e-05,-5.8e-05,-3.7e-06,5.8e-06,5.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00023,0.00023,5.4e-05,0.081,0.081,0.025,0.058,0.058,0.055,4.3e-09,4.3e-09,2.5e-09,3.5e-06,3.5e-06,9.8e-07,0,0,0,0,0,0,0,0 -12790000,0.7,0.0015,-0.014,0.71,-0.019,0.0092,-0.03,-0.0075,0.0015,-3.7e+02,-1.3e-05,-5.8e-05,-3.5e-06,7.4e-06,4e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00021,0.00021,5.4e-05,0.066,0.066,0.024,0.049,0.049,0.053,4e-09,4e-09,2.5e-09,3.5e-06,3.5e-06,9.7e-07,0,0,0,0,0,0,0,0 -12890000,0.7,0.0015,-0.014,0.71,-0.02,0.0092,-0.029,-0.0094,0.0024,-3.7e+02,-1.3e-05,-5.8e-05,-3.4e-06,6.8e-06,4.5e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00022,0.00022,5.4e-05,0.076,0.076,0.025,0.057,0.057,0.054,4e-09,4e-09,2.4e-09,3.5e-06,3.5e-06,9.6e-07,0,0,0,0,0,0,0,0 -12990000,0.7,0.0012,-0.014,0.71,-0.0078,0.0067,-0.03,-0.0011,0.0013,-3.7e+02,-1.2e-05,-5.8e-05,-2.9e-06,6.9e-06,9.6e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0002,0.0002,5.4e-05,0.062,0.062,0.025,0.049,0.049,0.052,3.8e-09,3.9e-09,2.4e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 -13090000,0.7,0.0012,-0.014,0.71,-0.0084,0.0069,-0.03,-0.0019,0.002,-3.7e+02,-1.2e-05,-5.8e-05,-3.4e-06,6.5e-06,9.8e-06,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00021,0.00021,5.4e-05,0.071,0.071,0.025,0.057,0.057,0.052,3.8e-09,3.9e-09,2.3e-09,3.5e-06,3.5e-06,9.4e-07,0,0,0,0,0,0,0,0 -13190000,0.7,0.00095,-0.014,0.71,0.00043,0.0064,-0.027,0.0043,0.0013,-3.7e+02,-1.1e-05,-5.9e-05,-3.4e-06,5.2e-06,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0002,0.0002,5.4e-05,0.058,0.058,0.025,0.049,0.049,0.051,3.7e-09,3.7e-09,2.3e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 -13290000,0.7,0.00096,-0.014,0.71,0.00025,0.0073,-0.024,0.0043,0.002,-3.7e+02,-1.1e-05,-5.9e-05,-2.9e-06,4e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0002,0.0002,5.4e-05,0.066,0.067,0.027,0.057,0.057,0.051,3.7e-09,3.7e-09,2.2e-09,3.5e-06,3.5e-06,9.1e-07,0,0,0,0,0,0,0,0 -13390000,0.7,0.00081,-0.014,0.71,0.001,0.0062,-0.02,0.0032,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,2.7e-06,1.5e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.4e-05,0.055,0.055,0.026,0.048,0.048,0.05,3.5e-09,3.5e-09,2.2e-09,3.5e-06,3.5e-06,8.8e-07,0,0,0,0,0,0,0,0 -13490000,0.7,0.00084,-0.014,0.71,0.00059,0.0062,-0.019,0.0033,0.0018,-3.7e+02,-1.1e-05,-5.9e-05,-2.1e-06,1.9e-06,1.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0002,0.0002,5.4e-05,0.063,0.063,0.028,0.056,0.056,0.05,3.5e-09,3.5e-09,2.1e-09,3.5e-06,3.5e-06,8.7e-07,0,0,0,0,0,0,0,0 -13590000,0.7,0.00079,-0.014,0.71,0.00079,0.0064,-0.021,0.0023,0.0012,-3.7e+02,-1.1e-05,-5.9e-05,-2.4e-06,1.8e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.4e-05,0.052,0.052,0.028,0.048,0.048,0.05,3.3e-09,3.4e-09,2.1e-09,3.5e-06,3.5e-06,8.4e-07,0,0,0,0,0,0,0,0 -13690000,0.7,0.00076,-0.014,0.71,0.0015,0.0082,-0.025,0.0024,0.0019,-3.7e+02,-1.1e-05,-5.9e-05,-1.8e-06,2.1e-06,1.4e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.0002,0.0002,5.3e-05,0.059,0.059,0.029,0.056,0.056,0.05,3.3e-09,3.4e-09,2e-09,3.5e-06,3.5e-06,8.3e-07,0,0,0,0,0,0,0,0 -13790000,0.7,0.00065,-0.014,0.71,0.0021,0.0041,-0.027,0.0034,-0.00048,-3.7e+02,-1.1e-05,-5.9e-05,-1.8e-06,5.9e-07,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.05,0.05,0.029,0.048,0.048,0.049,3.2e-09,3.2e-09,2e-09,3.5e-06,3.5e-06,7.9e-07,0,0,0,0,0,0,0,0 -13890000,0.7,0.00062,-0.014,0.71,0.0026,0.0041,-0.031,0.0037,-9.2e-05,-3.7e+02,-1.1e-05,-5.9e-05,-1.5e-06,9e-07,1.3e-05,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.056,0.056,0.03,0.056,0.056,0.05,3.2e-09,3.2e-09,2e-09,3.5e-06,3.5e-06,7.8e-07,0,0,0,0,0,0,0,0 -13990000,0.7,0.00056,-0.014,0.71,0.0029,0.0016,-0.03,0.0044,-0.0019,-3.7e+02,-1.1e-05,-6e-05,-1.4e-06,-1.4e-06,1.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.048,0.048,0.03,0.048,0.048,0.05,3e-09,3.1e-09,1.9e-09,3.5e-06,3.5e-06,7.4e-07,0,0,0,0,0,0,0,0 -14090000,0.7,0.00054,-0.014,0.71,0.003,0.0018,-0.031,0.0047,-0.0017,-3.7e+02,-1.1e-05,-6e-05,-7.3e-07,-1.2e-06,1.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.054,0.054,0.031,0.055,0.055,0.05,3e-09,3.1e-09,1.9e-09,3.5e-06,3.5e-06,7.3e-07,0,0,0,0,0,0,0,0 -14190000,0.7,0.00044,-0.014,0.71,0.0063,0.0012,-0.033,0.0067,-0.0015,-3.7e+02,-1.1e-05,-6e-05,-4e-07,-1.5e-06,1.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.046,0.046,0.031,0.048,0.048,0.05,2.9e-09,2.9e-09,1.8e-09,3.5e-06,3.5e-06,6.9e-07,0,0,0,0,0,0,0,0 -14290000,0.7,0.00045,-0.014,0.71,0.0071,0.002,-0.032,0.0074,-0.0014,-3.7e+02,-1.1e-05,-6e-05,-1.5e-07,-2.2e-06,1.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.3e-05,0.052,0.052,0.032,0.055,0.055,0.051,2.9e-09,2.9e-09,1.8e-09,3.5e-06,3.5e-06,6.7e-07,0,0,0,0,0,0,0,0 -14390000,0.7,0.00036,-0.014,0.71,0.0089,0.0029,-0.034,0.0087,-0.0012,-3.7e+02,-1e-05,-6e-05,4.8e-07,-2.6e-06,9.4e-06,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.2e-05,0.044,0.044,0.031,0.047,0.047,0.05,2.8e-09,2.8e-09,1.7e-09,3.4e-06,3.4e-06,6.3e-07,0,0,0,0,0,0,0,0 -14490000,0.7,0.00035,-0.014,0.71,0.0089,0.0042,-0.037,0.0096,-0.00085,-3.7e+02,-1e-05,-6e-05,6.7e-07,-2.1e-06,9e-06,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.2e-05,0.05,0.05,0.032,0.055,0.055,0.051,2.8e-09,2.8e-09,1.7e-09,3.4e-06,3.4e-06,6.2e-07,0,0,0,0,0,0,0,0 -14590000,0.7,0.00034,-0.013,0.71,0.0054,0.0026,-0.037,0.006,-0.0023,-3.7e+02,-1.1e-05,-6e-05,6.9e-07,-4.9e-06,1.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.2e-05,0.043,0.043,0.031,0.047,0.047,0.051,2.6e-09,2.6e-09,1.7e-09,3.4e-06,3.4e-06,5.8e-07,0,0,0,0,0,0,0,0 -14690000,0.7,0.0003,-0.013,0.71,0.0069,-0.0003,-0.034,0.0067,-0.0022,-3.7e+02,-1.1e-05,-6e-05,1.1e-06,-5.9e-06,1.4e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.2e-05,0.048,0.048,0.032,0.054,0.054,0.051,2.6e-09,2.6e-09,1.6e-09,3.4e-06,3.4e-06,5.6e-07,0,0,0,0,0,0,0,0 -14790000,0.7,0.00032,-0.013,0.71,0.0037,-0.0019,-0.03,0.0038,-0.0032,-3.7e+02,-1.1e-05,-6e-05,1.3e-06,-9.2e-06,1.9e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.2e-05,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-09,2.5e-09,1.6e-09,3.4e-06,3.4e-06,5.3e-07,0,0,0,0,0,0,0,0 -14890000,0.7,0.00031,-0.013,0.71,0.0053,-0.00088,-0.033,0.0042,-0.0034,-3.7e+02,-1.1e-05,-6e-05,1.6e-06,-9.1e-06,1.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.2e-05,0.047,0.047,0.031,0.054,0.054,0.052,2.5e-09,2.5e-09,1.6e-09,3.4e-06,3.4e-06,5.1e-07,0,0,0,0,0,0,0,0 -14990000,0.7,0.00031,-0.013,0.71,0.004,-0.0011,-0.029,0.0032,-0.0027,-3.7e+02,-1.1e-05,-6e-05,1.5e-06,-9.8e-06,2.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.1e-05,0.041,0.041,0.03,0.047,0.047,0.051,2.3e-09,2.3e-09,1.5e-09,3.4e-06,3.4e-06,4.8e-07,0,0,0,0,0,0,0,0 -15090000,0.7,0.00023,-0.013,0.71,0.0045,-0.0012,-0.032,0.0036,-0.0028,-3.7e+02,-1.1e-05,-6e-05,1.5e-06,-9.2e-06,2e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00019,0.00019,5.1e-05,0.046,0.046,0.031,0.054,0.054,0.052,2.3e-09,2.3e-09,1.5e-09,3.4e-06,3.4e-06,4.6e-07,0,0,0,0,0,0,0,0 -15190000,0.7,0.00025,-0.013,0.71,0.0038,-7.5e-05,-0.029,0.0029,-0.0023,-3.7e+02,-1.1e-05,-6e-05,1.4e-06,-9.5e-06,2.2e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.1e-05,0.04,0.04,0.03,0.047,0.047,0.052,2.2e-09,2.2e-09,1.4e-09,3.4e-06,3.4e-06,4.3e-07,0,0,0,0,0,0,0,0 -15290000,0.7,0.00021,-0.013,0.71,0.0045,0.00017,-0.027,0.0033,-0.0023,-3.7e+02,-1.1e-05,-6e-05,1.7e-06,-1e-05,2.3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5.1e-05,0.045,0.045,0.03,0.053,0.053,0.052,2.2e-09,2.2e-09,1.4e-09,3.4e-06,3.4e-06,4.2e-07,0,0,0,0,0,0,0,0 -15390000,0.7,0.00021,-0.013,0.71,0.0037,0.00044,-0.024,0.00066,-0.0019,-3.7e+02,-1.2e-05,-6e-05,2.3e-06,-1.1e-05,2.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5e-05,0.039,0.039,0.029,0.047,0.047,0.051,2e-09,2e-09,1.4e-09,3.4e-06,3.4e-06,3.9e-07,0,0,0,0,0,0,0,0 -15490000,0.7,0.00023,-0.013,0.71,0.005,0.00013,-0.024,0.0011,-0.0019,-3.7e+02,-1.2e-05,-6e-05,2e-06,-1.1e-05,2.5e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5e-05,0.044,0.044,0.029,0.053,0.053,0.053,2e-09,2e-09,1.4e-09,3.4e-06,3.4e-06,3.7e-07,0,0,0,0,0,0,0,0 -15590000,0.7,0.00025,-0.013,0.71,0.0031,6.4e-05,-0.023,-0.0012,-0.0016,-3.7e+02,-1.2e-05,-6e-05,1.8e-06,-1.1e-05,2.7e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00017,0.00017,5e-05,0.039,0.039,0.028,0.046,0.046,0.052,1.9e-09,1.9e-09,1.3e-09,3.3e-06,3.3e-06,3.5e-07,0,0,0,0,0,0,0,0 -15690000,0.7,0.00025,-0.013,0.71,0.0034,-3.3e-05,-0.023,-0.00087,-0.0015,-3.7e+02,-1.2e-05,-6e-05,1.8e-06,-1.1e-05,2.7e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,5e-05,0.044,0.044,0.028,0.053,0.053,0.052,1.9e-09,1.9e-09,1.3e-09,3.3e-06,3.3e-06,3.3e-07,0,0,0,0,0,0,0,0 -15790000,0.7,0.00021,-0.013,0.71,0.0039,-0.0018,-0.026,-0.00083,-0.0026,-3.7e+02,-1.2e-05,-6.1e-05,1.8e-06,-1.3e-05,2.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00017,0.00017,5e-05,0.038,0.038,0.027,0.046,0.046,0.051,1.7e-09,1.7e-09,1.3e-09,3.3e-06,3.3e-06,3.1e-07,0,0,0,0,0,0,0,0 -15890000,0.7,0.00016,-0.013,0.71,0.0048,-0.0022,-0.024,-0.00037,-0.0029,-3.7e+02,-1.2e-05,-6.1e-05,1.9e-06,-1.3e-05,2.8e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00018,0.00018,4.9e-05,0.043,0.043,0.027,0.053,0.053,0.052,1.7e-09,1.7e-09,1.2e-09,3.3e-06,3.3e-06,3e-07,0,0,0,0,0,0,0,0 -15990000,0.7,0.00011,-0.013,0.71,0.0046,-0.0031,-0.019,-0.0005,-0.0037,-3.7e+02,-1.2e-05,-6.1e-05,2.3e-06,-1.6e-05,3e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00017,0.00017,4.9e-05,0.038,0.038,0.026,0.046,0.046,0.051,1.6e-09,1.6e-09,1.2e-09,3.3e-06,3.3e-06,2.8e-07,0,0,0,0,0,0,0,0 -16090000,0.71,0.00011,-0.013,0.71,0.0064,-0.0033,-0.016,3.9e-05,-0.004,-3.7e+02,-1.2e-05,-6.1e-05,2.9e-06,-1.7e-05,3.1e-05,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00017,0.00017,4.9e-05,0.042,0.042,0.025,0.053,0.053,0.052,1.6e-09,1.6e-09,1.2e-09,3.3e-06,3.3e-06,2.7e-07,0,0,0,0,0,0,0,0 -16190000,0.71,0.00013,-0.013,0.71,0.0064,-0.0026,-0.014,-0.00023,-0.0033,-3.7e+02,-1.2e-05,-6.1e-05,3e-06,-1.6e-05,3.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00012,0.00016,0.00016,4.9e-05,0.037,0.037,0.025,0.046,0.046,0.051,1.5e-09,1.5e-09,1.2e-09,3.3e-06,3.3e-06,2.5e-07,0,0,0,0,0,0,0,0 -16290000,0.71,0.00015,-0.013,0.71,0.008,-0.0033,-0.016,0.0005,-0.0036,-3.7e+02,-1.2e-05,-6.1e-05,3.5e-06,-1.6e-05,3.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00017,0.00017,4.8e-05,0.042,0.042,0.024,0.052,0.052,0.052,1.5e-09,1.5e-09,1.1e-09,3.3e-06,3.3e-06,2.4e-07,0,0,0,0,0,0,0,0 -16390000,0.71,0.00014,-0.013,0.71,0.0068,-0.0036,-0.015,0.00011,-0.0029,-3.7e+02,-1.2e-05,-6.1e-05,3.4e-06,-1.5e-05,3.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00016,0.00016,4.8e-05,0.037,0.037,0.023,0.046,0.046,0.051,1.4e-09,1.4e-09,1.1e-09,3.2e-06,3.2e-06,2.2e-07,0,0,0,0,0,0,0,0 -16490000,0.71,0.00015,-0.013,0.71,0.0061,-0.0031,-0.018,0.00073,-0.0032,-3.7e+02,-1.2e-05,-6.1e-05,3.5e-06,-1.4e-05,3.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00016,0.00016,4.8e-05,0.041,0.041,0.023,0.052,0.052,0.052,1.4e-09,1.4e-09,1.1e-09,3.2e-06,3.2e-06,2.1e-07,0,0,0,0,0,0,0,0 -16590000,0.71,0.00041,-0.013,0.71,0.0025,-0.00049,-0.018,-0.0023,0.00012,-3.7e+02,-1.3e-05,-6e-05,3.6e-06,-7.3e-06,4.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00016,0.00016,4.8e-05,0.036,0.036,0.022,0.046,0.046,0.051,1.2e-09,1.2e-09,1.1e-09,3.2e-06,3.2e-06,2e-07,0,0,0,0,0,0,0,0 -16690000,0.71,0.0004,-0.013,0.71,0.0027,3e-05,-0.015,-0.002,9.7e-05,-3.7e+02,-1.3e-05,-6e-05,3.3e-06,-7.9e-06,4.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00016,0.00016,4.7e-05,0.041,0.041,0.022,0.052,0.052,0.051,1.2e-09,1.2e-09,1e-09,3.2e-06,3.2e-06,1.9e-07,0,0,0,0,0,0,0,0 -16790000,0.71,0.00054,-0.013,0.71,-0.00072,0.0021,-0.014,-0.0044,0.0027,-3.7e+02,-1.3e-05,-6e-05,3.4e-06,-2.1e-06,5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00015,0.00015,4.7e-05,0.036,0.036,0.021,0.046,0.046,0.05,1.1e-09,1.1e-09,1e-09,3.2e-06,3.2e-06,1.8e-07,0,0,0,0,0,0,0,0 -16890000,0.71,0.00056,-0.013,0.71,-0.00095,0.0031,-0.011,-0.0045,0.003,-3.7e+02,-1.3e-05,-6e-05,3.2e-06,-2.4e-06,5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00016,0.00016,4.7e-05,0.04,0.04,0.021,0.052,0.052,0.051,1.1e-09,1.1e-09,9.8e-10,3.2e-06,3.2e-06,1.7e-07,0,0,0,0,0,0,0,0 -16990000,0.71,0.0005,-0.013,0.71,-0.00097,0.00099,-0.01,-0.005,0.0011,-3.7e+02,-1.3e-05,-6e-05,2.9e-06,-6.4e-06,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00015,0.00015,4.7e-05,0.035,0.035,0.02,0.046,0.046,0.05,1e-09,1e-09,9.6e-10,3.2e-06,3.2e-06,1.6e-07,0,0,0,0,0,0,0,0 -17090000,0.71,0.00047,-0.013,0.71,-0.00014,0.002,-0.01,-0.0051,0.0012,-3.7e+02,-1.3e-05,-6e-05,3e-06,-6.4e-06,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00015,0.00015,4.6e-05,0.039,0.039,0.02,0.052,0.052,0.05,1e-09,1e-09,9.4e-10,3.2e-06,3.2e-06,1.6e-07,0,0,0,0,0,0,0,0 -17190000,0.71,0.00046,-0.013,0.71,0.00024,0.0019,-0.011,-0.0054,-0.00032,-3.7e+02,-1.3e-05,-6e-05,3.2e-06,-9.8e-06,5.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00015,0.00015,4.6e-05,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-10,9.4e-10,9.2e-10,3.1e-06,3.1e-06,1.5e-07,0,0,0,0,0,0,0,0 -17290000,0.71,0.00043,-0.013,0.71,0.0024,0.003,-0.0066,-0.0053,-9.5e-05,-3.7e+02,-1.3e-05,-6e-05,3e-06,-1e-05,5.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00015,0.00015,4.6e-05,0.039,0.039,0.019,0.052,0.052,0.049,9.4e-10,9.4e-10,9e-10,3.1e-06,3.1e-06,1.4e-07,0,0,0,0,0,0,0,0 -17390000,0.71,0.0004,-0.013,0.71,0.003,0.0021,-0.0047,-0.0045,-0.0014,-3.7e+02,-1.3e-05,-6e-05,3.3e-06,-1.4e-05,5.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00014,0.00014,4.6e-05,0.034,0.034,0.018,0.046,0.046,0.048,8.5e-10,8.5e-10,8.8e-10,3.1e-06,3.1e-06,1.3e-07,0,0,0,0,0,0,0,0 -17490000,0.71,0.00039,-0.013,0.71,0.0035,0.0017,-0.003,-0.0042,-0.0012,-3.7e+02,-1.3e-05,-6e-05,3.3e-06,-1.4e-05,5.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00014,0.00014,4.6e-05,0.038,0.038,0.018,0.052,0.052,0.049,8.5e-10,8.5e-10,8.6e-10,3.1e-06,3.1e-06,1.3e-07,0,0,0,0,0,0,0,0 -17590000,0.71,0.00031,-0.013,0.71,0.0047,0.00049,0.0025,-0.0035,-0.0024,-3.7e+02,-1.3e-05,-6.1e-05,3.4e-06,-1.7e-05,5.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00014,0.00014,4.5e-05,0.034,0.034,0.017,0.045,0.045,0.048,7.7e-10,7.7e-10,8.5e-10,3.1e-06,3.1e-06,1.2e-07,0,0,0,0,0,0,0,0 -17690000,0.71,0.00028,-0.013,0.71,0.0057,0.0012,0.0019,-0.003,-0.0023,-3.7e+02,-1.3e-05,-6.1e-05,3.5e-06,-1.7e-05,5.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00014,0.00014,4.5e-05,0.037,0.037,0.017,0.052,0.052,0.048,7.7e-10,7.7e-10,8.3e-10,3.1e-06,3.1e-06,1.2e-07,0,0,0,0,0,0,0,0 -17790000,0.71,0.00019,-0.013,0.71,0.0082,0.0009,0.00058,-0.0019,-0.002,-3.7e+02,-1.3e-05,-6.1e-05,4.1e-06,-1.7e-05,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.5e-05,0.033,0.033,0.016,0.045,0.045,0.048,7e-10,7e-10,8.1e-10,3.1e-06,3.1e-06,1.1e-07,0,0,0,0,0,0,0,0 -17890000,0.71,0.00019,-0.013,0.71,0.0097,0.00018,0.00068,-0.00099,-0.0019,-3.7e+02,-1.3e-05,-6.1e-05,4.4e-06,-1.7e-05,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00014,0.00014,4.5e-05,0.037,0.037,0.016,0.051,0.051,0.048,7e-10,7e-10,7.9e-10,3.1e-06,3.1e-06,1.1e-07,0,0,0,0,0,0,0,0 -17990000,0.71,0.00014,-0.013,0.71,0.011,-0.0016,0.0019,-0.00035,-0.0017,-3.7e+02,-1.3e-05,-6.1e-05,4.3e-06,-1.7e-05,5.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.4e-05,0.032,0.032,0.016,0.045,0.045,0.047,6.4e-10,6.4e-10,7.8e-10,3e-06,3e-06,1e-07,0,0,0,0,0,0,0,0 -18090000,0.71,0.00014,-0.013,0.71,0.012,-0.0018,0.0043,0.00083,-0.0019,-3.7e+02,-1.3e-05,-6.1e-05,3.9e-06,-1.7e-05,5.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.4e-05,0.036,0.036,0.016,0.051,0.051,0.047,6.4e-10,6.4e-10,7.6e-10,3e-06,3e-06,9.7e-08,0,0,0,0,0,0,0,0 -18190000,0.71,0.00011,-0.013,0.71,0.013,-0.00075,0.0056,0.0016,-0.0015,-3.7e+02,-1.3e-05,-6e-05,4.2e-06,-1.7e-05,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.4e-05,0.032,0.032,0.015,0.045,0.045,0.047,5.8e-10,5.8e-10,7.4e-10,3e-06,3e-06,9.2e-08,0,0,0,0,0,0,0,0 -18290000,0.71,5e-05,-0.012,0.71,0.013,-0.0013,0.0068,0.0029,-0.0016,-3.7e+02,-1.3e-05,-6e-05,4e-06,-1.7e-05,5.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.4e-05,0.035,0.035,0.015,0.051,0.051,0.046,5.8e-10,5.8e-10,7.3e-10,3e-06,3e-06,8.9e-08,0,0,0,0,0,0,0,0 -18390000,0.71,6.6e-05,-0.013,0.71,0.014,0.00032,0.008,0.0034,-0.0012,-3.7e+02,-1.3e-05,-6e-05,4.3e-06,-1.6e-05,5.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.3e-05,0.031,0.031,0.014,0.045,0.045,0.046,5.2e-10,5.2e-10,7.1e-10,3e-06,3e-06,8.4e-08,0,0,0,0,0,0,0,0 -18490000,0.71,8.1e-05,-0.013,0.71,0.015,0.00077,0.0076,0.0049,-0.0011,-3.7e+02,-1.3e-05,-6e-05,4.4e-06,-1.6e-05,5.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00013,0.00013,4.3e-05,0.034,0.034,0.014,0.051,0.051,0.046,5.2e-10,5.2e-10,7e-10,3e-06,3e-06,8.2e-08,0,0,0,0,0,0,0,0 -18590000,0.71,8.6e-05,-0.012,0.71,0.014,0.00095,0.0058,0.0037,-0.00098,-3.7e+02,-1.3e-05,-6e-05,4.8e-06,-1.6e-05,5.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.3e-05,0.03,0.03,0.014,0.045,0.045,0.045,4.8e-10,4.8e-10,6.9e-10,3e-06,3e-06,7.8e-08,0,0,0,0,0,0,0,0 -18690000,0.71,5.5e-05,-0.012,0.71,0.014,0.00029,0.0039,0.0051,-0.00089,-3.7e+02,-1.3e-05,-6e-05,4.7e-06,-1.6e-05,5.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.3e-05,0.033,0.033,0.013,0.051,0.051,0.045,4.8e-10,4.8e-10,6.7e-10,3e-06,3e-06,7.5e-08,0,0,0,0,0,0,0,0 -18790000,0.71,8.6e-05,-0.012,0.71,0.013,0.00054,0.0035,0.0039,-0.00071,-3.7e+02,-1.4e-05,-6e-05,4.6e-06,-1.6e-05,6.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.3e-05,0.03,0.03,0.013,0.045,0.045,0.045,4.3e-10,4.3e-10,6.6e-10,3e-06,3e-06,7.2e-08,0,0,0,0,0,0,0,0 -18890000,0.71,0.00011,-0.012,0.71,0.013,0.0011,0.0042,0.0052,-0.0006,-3.7e+02,-1.4e-05,-6e-05,4.9e-06,-1.6e-05,6.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.2e-05,0.033,0.033,0.013,0.051,0.051,0.045,4.3e-10,4.3e-10,6.5e-10,3e-06,3e-06,6.9e-08,0,0,0,0,0,0,0,0 -18990000,0.71,9.6e-05,-0.012,0.71,0.014,0.0019,0.0028,0.0065,-0.00051,-3.7e+02,-1.4e-05,-6e-05,5.1e-06,-1.6e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00011,4.2e-05,0.029,0.029,0.012,0.045,0.045,0.044,3.9e-10,3.9e-10,6.3e-10,2.9e-06,2.9e-06,6.6e-08,0,0,0,0,0,0,0,0 -19090000,0.71,8e-05,-0.012,0.71,0.015,0.0025,0.0058,0.008,-0.00026,-3.7e+02,-1.4e-05,-6e-05,5.1e-06,-1.6e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00012,0.00012,4.2e-05,0.032,0.032,0.012,0.051,0.051,0.044,3.9e-10,3.9e-10,6.2e-10,2.9e-06,2.9e-06,6.5e-08,0,0,0,0,0,0,0,0 -19190000,0.71,8.3e-05,-0.012,0.71,0.015,0.0025,0.0059,0.0087,-0.00026,-3.7e+02,-1.4e-05,-6.1e-05,5.2e-06,-1.7e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4.2e-05,0.028,0.028,0.012,0.045,0.045,0.044,3.6e-10,3.6e-10,6.1e-10,2.9e-06,2.9e-06,6.2e-08,0,0,0,0,0,0,0,0 -19290000,0.71,0.00011,-0.012,0.71,0.015,0.0018,0.0086,0.01,-3e-05,-3.7e+02,-1.4e-05,-6e-05,5.1e-06,-1.7e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4.1e-05,0.031,0.031,0.012,0.05,0.05,0.044,3.6e-10,3.6e-10,6e-10,2.9e-06,2.9e-06,6e-08,0,0,0,0,0,0,0,0 -19390000,0.71,0.00012,-0.012,0.71,0.013,0.00079,0.012,0.0082,-9.2e-05,-3.7e+02,-1.4e-05,-6.1e-05,5.2e-06,-1.7e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4.1e-05,0.027,0.027,0.012,0.044,0.044,0.043,3.3e-10,3.3e-10,5.9e-10,2.9e-06,2.9e-06,5.8e-08,0,0,0,0,0,0,0,0 -19490000,0.71,0.00014,-0.012,0.71,0.012,0.0001,0.0088,0.0094,-5.1e-05,-3.7e+02,-1.4e-05,-6.1e-05,5.5e-06,-1.7e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4.1e-05,0.03,0.03,0.011,0.05,0.05,0.043,3.3e-10,3.3e-10,5.7e-10,2.9e-06,2.9e-06,5.6e-08,0,0,0,0,0,0,0,0 -19590000,0.71,0.00019,-0.012,0.71,0.01,-0.00096,0.0081,0.0076,-0.00012,-3.7e+02,-1.4e-05,-6.1e-05,5.9e-06,-1.7e-05,6.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4.1e-05,0.027,0.027,0.011,0.044,0.044,0.042,3e-10,3e-10,5.6e-10,2.9e-06,2.9e-06,5.4e-08,0,0,0,0,0,0,0,0 -19690000,0.71,0.00019,-0.012,0.71,0.01,-0.0031,0.0096,0.0086,-0.00033,-3.7e+02,-1.4e-05,-6.1e-05,5.7e-06,-1.7e-05,6.7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4e-05,0.029,0.029,0.011,0.05,0.05,0.042,3e-10,3e-10,5.5e-10,2.9e-06,2.9e-06,5.2e-08,0,0,0,0,0,0,0,0 -19790000,0.71,0.00026,-0.012,0.71,0.008,-0.004,0.01,0.007,-0.00026,-3.7e+02,-1.4e-05,-6e-05,5.7e-06,-1.7e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4e-05,0.026,0.026,0.011,0.044,0.044,0.042,2.7e-10,2.7e-10,5.4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19890000,0.71,0.00021,-0.012,0.71,0.0068,-0.0043,0.011,0.0077,-0.00069,-3.7e+02,-1.4e-05,-6.1e-05,6.1e-06,-1.7e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.00011,0.00011,4e-05,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-10,2.7e-10,5.3e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19990000,0.71,0.00019,-0.012,0.71,0.0043,-0.005,0.014,0.0063,-0.00059,-3.7e+02,-1.4e-05,-6e-05,6.6e-06,-1.6e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,4e-05,0.026,0.026,0.01,0.044,0.044,0.041,2.5e-10,2.5e-10,5.2e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20090000,0.71,0.00019,-0.012,0.71,0.0041,-0.0069,0.014,0.0067,-0.0012,-3.7e+02,-1.4e-05,-6e-05,7e-06,-1.6e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,4e-05,0.028,0.028,0.01,0.049,0.049,0.042,2.5e-10,2.5e-10,5.1e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20190000,0.71,0.00029,-0.012,0.71,0.0017,-0.0077,0.017,0.0044,-0.00092,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-1.5e-05,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,3.9e-05,0.025,0.025,0.01,0.044,0.044,0.041,2.3e-10,2.3e-10,5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20290000,0.71,0.00025,-0.012,0.71,0.00061,-0.0092,0.015,0.0045,-0.0018,-3.7e+02,-1.4e-05,-6e-05,7.4e-06,-1.5e-05,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,3.9e-05,0.027,0.027,0.0099,0.049,0.049,0.041,2.3e-10,2.3e-10,4.9e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20390000,0.71,0.00027,-0.012,0.71,-0.0019,-0.0098,0.017,0.0026,-0.0014,-3.7e+02,-1.4e-05,-6e-05,7.4e-06,-1.3e-05,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,3.9e-05,0.024,0.024,0.0097,0.044,0.044,0.041,2.1e-10,2.1e-10,4.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20490000,0.71,0.00033,-0.012,0.71,-0.0023,-0.011,0.016,0.0023,-0.0024,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-1.3e-05,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.00011,0.0001,0.0001,3.9e-05,0.026,0.026,0.0096,0.049,0.049,0.041,2.1e-10,2.1e-10,4.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20590000,0.71,0.00035,-0.012,0.71,-0.002,-0.011,0.013,0.002,-0.0019,-3.7e+02,-1.4e-05,-6e-05,7.1e-06,-1.1e-05,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.9e-05,9.9e-05,3.9e-05,0.024,0.024,0.0094,0.044,0.044,0.04,1.9e-10,1.9e-10,4.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20690000,0.71,0.00037,-0.012,0.71,-0.002,-0.012,0.015,0.0018,-0.003,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-1.1e-05,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.9e-05,9.9e-05,3.9e-05,0.026,0.026,0.0093,0.049,0.049,0.04,1.9e-10,1.9e-10,4.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20790000,0.71,0.0004,-0.012,0.71,-0.0031,-0.011,0.015,0.0015,-0.0024,-3.7e+02,-1.4e-05,-6e-05,7.2e-06,-9.4e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.7e-05,9.7e-05,3.8e-05,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-10,1.8e-10,4.5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20890000,0.71,0.00039,-0.012,0.71,-0.0036,-0.013,0.014,0.0012,-0.0036,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-9.5e-06,7.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.8e-05,9.8e-05,3.8e-05,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-10,1.8e-10,4.4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20990000,0.71,0.00039,-0.012,0.71,-0.0038,-0.014,0.015,0.0028,-0.003,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-7.4e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.6e-05,9.6e-05,3.8e-05,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-10,1.7e-10,4.3e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21090000,0.71,0.00039,-0.012,0.71,-0.004,-0.017,0.015,0.0024,-0.0045,-3.7e+02,-1.4e-05,-6e-05,7.6e-06,-7.5e-06,7.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.7e-05,9.6e-05,3.8e-05,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-10,1.7e-10,4.3e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21190000,0.71,0.00042,-0.012,0.71,-0.0032,-0.015,0.014,0.0038,-0.0037,-3.7e+02,-1.4e-05,-6e-05,7.5e-06,-5.2e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.5e-05,9.4e-05,3.8e-05,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-10,1.5e-10,4.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21290000,0.71,0.00046,-0.012,0.71,-0.0038,-0.018,0.016,0.0035,-0.0053,-3.7e+02,-1.4e-05,-6e-05,7.8e-06,-5.1e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.5e-05,9.5e-05,3.7e-05,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-10,1.5e-10,4.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21390000,0.71,0.00051,-0.012,0.71,-0.0046,-0.017,0.016,0.003,-0.0033,-3.7e+02,-1.4e-05,-6e-05,7.6e-06,-1.4e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.4e-05,9.3e-05,3.7e-05,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-10,1.4e-10,4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21490000,0.71,0.00052,-0.012,0.71,-0.0051,-0.018,0.015,0.0024,-0.005,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,-1.5e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.4e-05,9.4e-05,3.7e-05,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-10,1.4e-10,4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21590000,0.71,0.00054,-0.012,0.71,-0.0056,-0.015,0.015,0.002,-0.003,-3.7e+02,-1.4e-05,-6e-05,7.6e-06,2e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.2e-05,9.2e-05,3.7e-05,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-10,1.3e-10,3.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21690000,0.71,0.00055,-0.012,0.71,-0.0055,-0.016,0.017,0.0015,-0.0046,-3.7e+02,-1.4e-05,-6e-05,7.7e-06,1.9e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.3e-05,9.3e-05,3.7e-05,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-10,1.3e-10,3.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21790000,0.71,0.00057,-0.012,0.71,-0.0062,-0.011,0.015,0.00018,-0.00063,-3.7e+02,-1.4e-05,-5.9e-05,7.5e-06,7.2e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.1e-05,9.1e-05,3.6e-05,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-10,1.2e-10,3.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21890000,0.71,0.00057,-0.012,0.71,-0.0061,-0.012,0.016,-0.00043,-0.0018,-3.7e+02,-1.4e-05,-5.9e-05,7.5e-06,7.2e-06,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.2e-05,9.2e-05,3.6e-05,0.022,0.022,0.0082,0.047,0.047,0.038,1.2e-10,1.3e-10,3.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21990000,0.71,0.00062,-0.012,0.71,-0.0066,-0.0089,0.016,-0.0014,0.0016,-3.7e+02,-1.4e-05,-5.9e-05,7.4e-06,1.2e-05,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9e-05,9e-05,3.6e-05,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22090000,0.71,0.00063,-0.012,0.71,-0.007,-0.0081,0.015,-0.002,0.00076,-3.7e+02,-1.4e-05,-5.9e-05,7.4e-06,1.2e-05,7.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9.1e-05,9.1e-05,3.6e-05,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22190000,0.71,0.0006,-0.012,0.71,-0.0068,-0.0072,0.015,-0.0017,0.00069,-3.7e+02,-1.4e-05,-5.9e-05,7.4e-06,1.2e-05,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9e-05,8.9e-05,3.6e-05,0.02,0.02,0.008,0.042,0.042,0.037,1.1e-10,1.1e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22290000,0.71,0.00064,-0.012,0.71,-0.0081,-0.0079,0.015,-0.0024,-6.7e-05,-3.7e+02,-1.4e-05,-5.9e-05,7.2e-06,1.2e-05,7.3e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,9e-05,9e-05,3.5e-05,0.021,0.021,0.008,0.047,0.047,0.037,1.1e-10,1.1e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22390000,0.71,0.00062,-0.012,0.71,-0.0087,-0.0074,0.017,-0.0021,-7.5e-05,-3.7e+02,-1.4e-05,-5.9e-05,7.3e-06,1.3e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.9e-05,8.9e-05,3.5e-05,0.019,0.019,0.0079,0.042,0.042,0.037,1e-10,1e-10,3.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22490000,0.71,0.00062,-0.012,0.71,-0.0094,-0.0073,0.018,-0.003,-0.00083,-3.7e+02,-1.4e-05,-5.9e-05,7.2e-06,1.3e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.9e-05,8.9e-05,3.5e-05,0.021,0.021,0.0079,0.047,0.047,0.037,1e-10,1e-10,3.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22590000,0.71,0.0006,-0.012,0.71,-0.0091,-0.0068,0.017,-0.0033,0.00024,-3.7e+02,-1.4e-05,-5.9e-05,7.2e-06,1.4e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.8e-05,8.8e-05,3.5e-05,0.019,0.019,0.0078,0.042,0.042,0.036,9.7e-11,9.7e-11,3.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22690000,0.71,0.00064,-0.012,0.71,-0.01,-0.0066,0.018,-0.0043,-0.00043,-3.7e+02,-1.4e-05,-5.9e-05,7.3e-06,1.4e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.9e-05,8.8e-05,3.5e-05,0.02,0.02,0.0079,0.046,0.046,0.037,9.7e-11,9.7e-11,3.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22790000,0.71,0.00062,-0.012,0.71,-0.011,-0.0054,0.019,-0.0054,-0.00034,-3.7e+02,-1.4e-05,-5.9e-05,6.9e-06,1.5e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.7e-05,8.7e-05,3.5e-05,0.019,0.019,0.0078,0.042,0.042,0.036,9.2e-11,9.2e-11,3.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22890000,0.71,0.00063,-0.012,0.71,-0.012,-0.005,0.021,-0.0065,-0.00086,-3.7e+02,-1.4e-05,-5.9e-05,6.8e-06,1.5e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.8e-05,8.8e-05,3.4e-05,0.02,0.02,0.0078,0.046,0.046,0.036,9.2e-11,9.2e-11,3.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22990000,0.71,0.00062,-0.012,0.71,-0.012,-0.0055,0.022,-0.0073,-0.00077,-3.7e+02,-1.4e-05,-5.9e-05,6.9e-06,1.6e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.7e-05,8.7e-05,3.4e-05,0.018,0.018,0.0078,0.041,0.041,0.036,8.7e-11,8.7e-11,3.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23090000,0.71,0.00058,-0.012,0.71,-0.013,-0.0055,0.022,-0.0086,-0.0013,-3.7e+02,-1.4e-05,-5.9e-05,6.7e-06,1.6e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.7e-05,8.7e-05,3.4e-05,0.02,0.02,0.0078,0.046,0.046,0.036,8.7e-11,8.7e-11,3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23190000,0.71,0.00065,-0.012,0.71,-0.014,-0.0064,0.024,-0.012,-0.0012,-3.7e+02,-1.4e-05,-5.9e-05,6.6e-06,1.6e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.6e-05,8.6e-05,3.4e-05,0.018,0.018,0.0077,0.041,0.041,0.036,8.2e-11,8.2e-11,3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23290000,0.71,0.00059,-0.012,0.71,-0.015,-0.0077,0.024,-0.013,-0.0019,-3.7e+02,-1.4e-05,-5.9e-05,6.6e-06,1.6e-05,7.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.7e-05,8.6e-05,3.4e-05,0.019,0.019,0.0078,0.046,0.046,0.036,8.2e-11,8.2e-11,3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23390000,0.71,0.00068,-0.012,0.71,-0.016,-0.0079,0.022,-0.016,-0.0017,-3.7e+02,-1.4e-05,-5.9e-05,6.6e-06,1.7e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.6e-05,8.5e-05,3.4e-05,0.018,0.018,0.0077,0.041,0.041,0.036,7.8e-11,7.8e-11,2.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23490000,0.71,0.0031,-0.0096,0.71,-0.023,-0.0088,-0.012,-0.018,-0.0025,-3.7e+02,-1.4e-05,-5.9e-05,6.6e-06,1.7e-05,7.2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,0.0001,8.6e-05,8.6e-05,3.3e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.8e-11,7.8e-11,2.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23590000,0.71,0.0083,-0.0018,0.71,-0.033,-0.0075,-0.044,-0.017,-0.0012,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,1.9e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.5e-05,8.5e-05,3.3e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-11,7.4e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23690000,0.71,0.0079,0.004,0.71,-0.065,-0.016,-0.094,-0.021,-0.0023,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,1.9e-05,7e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.5e-05,8.5e-05,3.3e-05,0.019,0.019,0.0078,0.045,0.045,0.036,7.4e-11,7.4e-11,2.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23790000,0.71,0.005,0.00065,0.71,-0.089,-0.027,-0.15,-0.021,-0.0017,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,2.1e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.5e-05,8.4e-05,3.3e-05,0.017,0.017,0.0077,0.041,0.041,0.035,7.1e-11,7.1e-11,2.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23890000,0.71,0.0024,-0.0054,0.71,-0.11,-0.036,-0.2,-0.03,-0.0049,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,2.1e-05,6.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.5e-05,8.5e-05,3.3e-05,0.019,0.019,0.0078,0.045,0.045,0.035,7.1e-11,7.1e-11,2.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23990000,0.71,0.00096,-0.01,0.71,-0.11,-0.039,-0.25,-0.034,-0.0081,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,2.2e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.4e-05,8.4e-05,3.3e-05,0.017,0.017,0.0077,0.041,0.041,0.035,6.7e-11,6.7e-11,2.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24090000,0.71,0.0023,-0.0088,0.71,-0.11,-0.04,-0.3,-0.045,-0.012,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,2.2e-05,6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.4e-05,8.4e-05,3.2e-05,0.018,0.018,0.0078,0.045,0.045,0.035,6.7e-11,6.7e-11,2.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24190000,0.71,0.0033,-0.0065,0.71,-0.11,-0.041,-0.35,-0.046,-0.014,-3.7e+02,-1.4e-05,-5.9e-05,6.5e-06,2.4e-05,5.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.3e-05,8.3e-05,3.2e-05,0.017,0.017,0.0077,0.04,0.04,0.035,6.4e-11,6.4e-11,2.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24290000,0.71,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.018,-3.7e+02,-1.4e-05,-5.9e-05,6.4e-06,2.4e-05,5.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.9e-05,8.4e-05,8.3e-05,3.2e-05,0.018,0.018,0.0078,0.045,0.045,0.036,6.4e-11,6.4e-11,2.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24390000,0.71,0.0039,-0.0059,0.71,-0.13,-0.052,-0.46,-0.064,-0.03,-3.7e+02,-1.3e-05,-5.9e-05,6.3e-06,2.1e-05,5.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.3e-05,8.3e-05,3.2e-05,0.017,0.017,0.0078,0.04,0.04,0.035,6.2e-11,6.2e-11,2.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24490000,0.71,0.0047,-0.0018,0.71,-0.14,-0.057,-0.51,-0.077,-0.035,-3.7e+02,-1.3e-05,-5.9e-05,6.2e-06,2.1e-05,5.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.3e-05,8.3e-05,3.2e-05,0.018,0.018,0.0078,0.045,0.045,0.035,6.2e-11,6.2e-11,2.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24590000,0.71,0.0052,0.0019,0.71,-0.16,-0.068,-0.56,-0.081,-0.045,-3.7e+02,-1.3e-05,-5.9e-05,6.3e-06,2e-05,4.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.2e-05,8.2e-05,3.2e-05,0.017,0.017,0.0078,0.04,0.04,0.035,5.9e-11,5.9e-11,2.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24690000,0.71,0.0052,0.0028,0.71,-0.18,-0.082,-0.64,-0.098,-0.052,-3.7e+02,-1.3e-05,-5.9e-05,6.4e-06,2e-05,4.4e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.2e-05,8.2e-05,3.2e-05,0.018,0.018,0.0078,0.044,0.044,0.035,5.9e-11,5.9e-11,2.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24790000,0.71,0.0049,0.0014,0.71,-0.2,-0.094,-0.72,-0.1,-0.063,-3.7e+02,-1.3e-05,-5.9e-05,6.3e-06,2.4e-05,3.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.1e-05,8.1e-05,3.1e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.7e-11,5.7e-11,2.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24890000,0.71,0.0067,0.0031,0.71,-0.22,-0.11,-0.75,-0.13,-0.073,-3.7e+02,-1.3e-05,-5.9e-05,6.1e-06,2.4e-05,3.6e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.8e-05,8.2e-05,8.2e-05,3.1e-05,0.018,0.018,0.0078,0.044,0.044,0.035,5.7e-11,5.7e-11,2.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -24990000,0.71,0.0085,0.0047,0.71,-0.24,-0.11,-0.81,-0.13,-0.081,-3.7e+02,-1.3e-05,-5.9e-05,6e-06,3.2e-05,2.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,8.1e-05,8.1e-05,3.1e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.4e-11,5.4e-11,2.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25090000,0.71,0.0088,0.0041,0.71,-0.27,-0.12,-0.85,-0.15,-0.093,-3.7e+02,-1.3e-05,-5.9e-05,5.8e-06,3.3e-05,2e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,8.1e-05,8.1e-05,3.1e-05,0.018,0.017,0.0079,0.044,0.044,0.035,5.5e-11,5.5e-11,2.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25190000,0.71,0.0082,0.0027,0.71,-0.29,-0.14,-0.91,-0.17,-0.12,-3.7e+02,-1.3e-05,-5.9e-05,5.9e-06,2.9e-05,1.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,8e-05,8e-05,3.1e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.2e-11,5.2e-11,2.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25290000,0.71,0.01,0.0095,0.71,-0.32,-0.15,-0.96,-0.2,-0.13,-3.7e+02,-1.3e-05,-5.9e-05,5.9e-06,2.9e-05,1.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,8e-05,8e-05,3.1e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.3e-11,5.3e-11,2.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25390000,0.71,0.011,0.016,0.71,-0.35,-0.17,-1,-0.22,-0.15,-3.7e+02,-1.2e-05,-5.9e-05,5.9e-06,3e-05,-2.6e-07,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,7.9e-05,7.9e-05,3.1e-05,0.016,0.016,0.0078,0.04,0.04,0.035,5.1e-11,5.1e-11,2.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25490000,0.71,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-1.2e-05,-5.9e-05,5.9e-06,3e-05,-1.1e-07,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,8e-05,7.9e-05,3e-05,0.017,0.017,0.0079,0.044,0.044,0.035,5.1e-11,5.1e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25590000,0.71,0.011,0.015,0.71,-0.44,-0.22,-1.1,-0.28,-0.21,-3.7e+02,-1.2e-05,-5.9e-05,5.9e-06,2.7e-05,-9.3e-06,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.7e-05,7.9e-05,7.8e-05,3e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.9e-11,4.9e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25690000,0.71,0.015,0.022,0.71,-0.49,-0.24,-1.2,-0.33,-0.23,-3.7e+02,-1.2e-05,-5.9e-05,5.9e-06,2.7e-05,-9e-06,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.9e-05,7.9e-05,3e-05,0.017,0.017,0.0079,0.044,0.044,0.035,4.9e-11,4.9e-11,2.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25790000,0.71,0.017,0.028,0.71,-0.53,-0.27,-1.2,-0.34,-0.26,-3.7e+02,-1.2e-05,-5.9e-05,6e-06,3.4e-05,-3.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.8e-05,7.8e-05,3e-05,0.016,0.016,0.0079,0.04,0.04,0.035,4.7e-11,4.7e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25890000,0.71,0.017,0.028,0.71,-0.6,-0.3,-1.3,-0.4,-0.29,-3.7e+02,-1.2e-05,-5.9e-05,6.1e-06,3.4e-05,-3.5e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.8e-05,7.8e-05,3e-05,0.017,0.017,0.008,0.044,0.044,0.035,4.7e-11,4.7e-11,2e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -25990000,0.71,0.016,0.025,0.71,-0.66,-0.33,-1.3,-0.44,-0.34,-3.7e+02,-1.1e-05,-5.9e-05,6.1e-06,2.9e-05,-5.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.7e-05,7.7e-05,3e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.6e-11,4.6e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26090000,0.7,0.021,0.035,0.71,-0.72,-0.36,-1.3,-0.51,-0.38,-3.7e+02,-1.1e-05,-5.9e-05,5.9e-06,3e-05,-5.1e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.7e-05,7.7e-05,2.9e-05,0.017,0.017,0.008,0.043,0.043,0.035,4.6e-11,4.6e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26190000,0.7,0.023,0.045,0.71,-0.78,-0.39,-1.3,-0.53,-0.42,-3.7e+02,-1.1e-05,-5.9e-05,5.9e-06,4e-05,-8.9e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.7e-05,7.6e-05,2.9e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.4e-11,4.4e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26290000,0.7,0.024,0.047,0.71,-0.87,-0.44,-1.3,-0.62,-0.46,-3.7e+02,-1.1e-05,-5.9e-05,5.8e-06,4e-05,-8.8e-05,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.7e-05,7.6e-05,2.9e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.5e-11,4.5e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26390000,0.7,0.023,0.043,0.71,-0.95,-0.49,-1.3,-0.68,-0.55,-3.7e+02,-1e-05,-5.9e-05,5.8e-06,2.7e-05,-0.0001,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.6e-05,7.6e-05,7.6e-05,2.9e-05,0.015,0.015,0.0079,0.039,0.039,0.035,4.3e-11,4.3e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26490000,0.7,0.031,0.059,0.71,-1,-0.53,-1.3,-0.78,-0.6,-3.7e+02,-1e-05,-5.9e-05,5.8e-06,2.7e-05,-0.0001,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,7.6e-05,7.6e-05,2.9e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.3e-11,4.3e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26590000,0.7,0.037,0.075,0.71,-1.1,-0.59,-1.3,-0.82,-0.67,-3.7e+02,-9.5e-06,-5.9e-05,5.5e-06,3.6e-05,-0.00014,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,7.5e-05,7.5e-05,2.9e-05,0.015,0.015,0.008,0.039,0.039,0.035,4.2e-11,4.2e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26690000,0.7,0.038,0.078,0.71,-1.3,-0.65,-1.3,-0.94,-0.73,-3.7e+02,-9.5e-06,-5.9e-05,5.6e-06,3.6e-05,-0.00014,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,7.6e-05,7.5e-05,2.9e-05,0.016,0.016,0.008,0.043,0.043,0.035,4.2e-11,4.2e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26790000,0.7,0.036,0.072,0.71,-1.4,-0.73,-1.3,-1,-0.85,-3.7e+02,-9e-06,-6e-05,5.4e-06,1.3e-05,-0.00016,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.5e-05,7.5e-05,7.4e-05,2.9e-05,0.015,0.014,0.008,0.039,0.039,0.035,4.1e-11,4.1e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26890000,0.7,0.045,0.094,0.7,-1.5,-0.79,-1.3,-1.2,-0.93,-3.7e+02,-9e-06,-6e-05,5.5e-06,1.3e-05,-0.00016,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,7.5e-05,7.4e-05,2.9e-05,0.016,0.016,0.0081,0.043,0.043,0.035,4.1e-11,4.1e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26990000,0.7,0.051,0.12,0.7,-1.7,-0.87,-1.3,-1.2,-1,-3.7e+02,-7.9e-06,-6e-05,5.4e-06,2.1e-05,-0.00021,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,7.4e-05,7.3e-05,3e-05,0.014,0.014,0.008,0.039,0.039,0.035,4e-11,4e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27090000,0.7,0.052,0.12,0.7,-1.9,-0.96,-1.3,-1.4,-1.1,-3.7e+02,-7.9e-06,-6e-05,5.4e-06,2.1e-05,-0.00021,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.4e-05,7.5e-05,7.3e-05,3e-05,0.016,0.015,0.0081,0.043,0.043,0.035,4e-11,4e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27190000,0.71,0.048,0.11,0.7,-2.1,-1,-1.2,-1.6,-1.2,-3.7e+02,-7.8e-06,-5.9e-05,5.4e-06,3.1e-05,-0.00021,-0.0013,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,7.4e-05,7.3e-05,3e-05,0.016,0.016,0.0081,0.045,0.045,0.035,4e-11,4e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27290000,0.71,0.043,0.094,0.7,-2.2,-1.1,-1.2,-1.8,-1.3,-3.7e+02,-7.8e-06,-5.9e-05,5.5e-06,3.1e-05,-0.00021,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.3e-05,7.5e-05,7.4e-05,2.9e-05,0.017,0.017,0.0081,0.05,0.049,0.035,4e-11,4e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27390000,0.71,0.036,0.078,0.7,-2.3,-1.1,-1.2,-2,-1.4,-3.7e+02,-7.2e-06,-5.9e-05,5.6e-06,5.1e-05,-0.00022,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,7.4e-05,7.4e-05,2.9e-05,0.017,0.017,0.0081,0.052,0.052,0.035,3.9e-11,3.9e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27490000,0.71,0.031,0.063,0.7,-2.4,-1.2,-1.2,-2.3,-1.5,-3.7e+02,-7.2e-06,-5.9e-05,5.5e-06,5e-05,-0.00022,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,7.5e-05,7.4e-05,2.8e-05,0.018,0.018,0.0082,0.057,0.057,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27590000,0.71,0.026,0.05,0.7,-2.5,-1.2,-1.2,-2.5,-1.6,-3.7e+02,-7.4e-06,-5.9e-05,5.6e-06,4.9e-05,-0.00021,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,7.4e-05,7.4e-05,2.8e-05,0.018,0.018,0.0082,0.06,0.059,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27690000,0.71,0.025,0.048,0.7,-2.5,-1.2,-1.2,-2.8,-1.7,-3.7e+02,-7.4e-06,-5.9e-05,5.5e-06,4.8e-05,-0.0002,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.2e-05,7.5e-05,7.4e-05,2.8e-05,0.019,0.019,0.0082,0.065,0.065,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27790000,0.71,0.026,0.05,0.7,-2.6,-1.2,-1.2,-3,-1.8,-3.7e+02,-7.5e-06,-5.8e-05,5.4e-06,5e-05,-0.00019,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,7.4e-05,7.4e-05,2.8e-05,0.019,0.019,0.0082,0.068,0.067,0.035,3.8e-11,3.8e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27890000,0.71,0.025,0.048,0.7,-2.6,-1.2,-1.2,-3.3,-2,-3.7e+02,-7.5e-06,-5.8e-05,5.3e-06,4.8e-05,-0.00019,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,7.5e-05,7.4e-05,2.8e-05,0.02,0.02,0.0083,0.074,0.074,0.035,3.8e-11,3.8e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27990000,0.71,0.024,0.045,0.7,-2.7,-1.2,-1.2,-3.6,-2.1,-3.7e+02,-7.9e-06,-5.8e-05,5.4e-06,4.3e-05,-0.00017,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9e-05,7.4e-05,7.4e-05,2.8e-05,0.02,0.019,0.0083,0.076,0.076,0.035,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28090000,0.71,0.03,0.058,0.7,-2.7,-1.3,-1.2,-3.9,-2.2,-3.7e+02,-7.9e-06,-5.8e-05,5.2e-06,4.2e-05,-0.00017,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9e-05,7.4e-05,7.4e-05,2.8e-05,0.021,0.021,0.0084,0.083,0.083,0.035,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28190000,0.71,0.035,0.071,0.7,-2.8,-1.3,-0.95,-4.2,-2.3,-3.7e+02,-8.2e-06,-5.8e-05,5.2e-06,3.9e-05,-0.00016,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9e-05,7.4e-05,7.3e-05,2.8e-05,0.02,0.02,0.0084,0.085,0.085,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28290000,0.71,0.028,0.054,0.7,-2.8,-1.3,-0.089,-4.4,-2.4,-3.7e+02,-8.2e-06,-5.8e-05,5e-06,3.7e-05,-0.00015,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,9e-05,7.4e-05,7.4e-05,2.7e-05,0.02,0.02,0.0085,0.092,0.092,0.036,3.8e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28390000,0.71,0.011,0.023,0.7,-2.8,-1.3,0.77,-4.7,-2.6,-3.7e+02,-8.2e-06,-5.8e-05,4.8e-06,3.5e-05,-0.00014,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.5e-05,7.4e-05,2.7e-05,0.021,0.021,0.0086,0.1,0.099,0.036,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28490000,0.71,0.0026,0.0045,0.7,-2.7,-1.3,1.1,-5,-2.7,-3.7e+02,-8.2e-06,-5.8e-05,4.8e-06,3.2e-05,-0.00014,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.5e-05,7.4e-05,2.7e-05,0.022,0.022,0.0088,0.11,0.11,0.036,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28590000,0.71,0.00078,0.00096,0.7,-2.6,-1.3,0.96,-5.3,-2.8,-3.7e+02,-8.2e-06,-5.8e-05,4.7e-06,2.9e-05,-0.00013,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.5e-05,7.5e-05,2.7e-05,0.024,0.024,0.0089,0.12,0.12,0.036,3.8e-11,3.8e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28690000,0.71,6.9e-05,4.3e-05,0.7,-2.6,-1.2,0.96,-5.5,-2.9,-3.7e+02,-8.1e-06,-5.8e-05,4.7e-06,2.5e-05,-0.00012,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.6e-05,7.5e-05,2.6e-05,0.025,0.025,0.0089,0.13,0.13,0.036,3.8e-11,3.8e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28790000,0.71,-0.00021,-0.00019,0.7,-2.5,-1.2,0.97,-5.9,-3,-3.7e+02,-8.8e-06,-5.8e-05,4.6e-06,-5.4e-06,-0.00018,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.6e-05,7.5e-05,2.6e-05,0.024,0.024,0.0089,0.13,0.13,0.036,3.7e-11,3.7e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28890000,0.71,-0.00023,3.1e-05,0.71,-2.5,-1.2,0.96,-6.1,-3.2,-3.7e+02,-8.8e-06,-5.8e-05,4.6e-06,-9.6e-06,-0.00017,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.9e-05,7.6e-05,7.5e-05,2.6e-05,0.025,0.025,0.009,0.14,0.14,0.036,3.7e-11,3.7e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28990000,0.71,-2.3e-05,0.0005,0.71,-2.4,-1.2,0.95,-6.4,-3.3,-3.7e+02,-9.7e-06,-5.8e-05,4.4e-06,-3e-05,-0.00025,-0.0012,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.6e-05,7.5e-05,2.6e-05,0.024,0.024,0.0089,0.14,0.14,0.036,3.7e-11,3.7e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29090000,0.71,0.00013,0.0009,0.71,-2.4,-1.2,0.94,-6.7,-3.4,-3.7e+02,-9.7e-06,-5.8e-05,4.3e-06,-3.4e-05,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.6e-05,7.5e-05,2.6e-05,0.025,0.025,0.009,0.15,0.15,0.036,3.7e-11,3.7e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29190000,0.71,0.00036,0.0013,0.71,-2.4,-1.1,0.93,-7,-3.5,-3.7e+02,-1e-05,-5.8e-05,4.4e-06,-5.3e-05,-0.00027,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.6e-05,7.5e-05,2.6e-05,0.024,0.024,0.009,0.15,0.15,0.036,3.6e-11,3.6e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29290000,0.71,0.00071,0.0022,0.71,-2.3,-1.1,0.96,-7.2,-3.6,-3.7e+02,-1e-05,-5.8e-05,4.2e-06,-5.8e-05,-0.00025,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.6e-05,0.025,0.026,0.0091,0.16,0.16,0.036,3.6e-11,3.6e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29390000,0.71,0.0013,0.0037,0.71,-2.3,-1.1,0.97,-7.5,-3.7,-3.7e+02,-1.1e-05,-5.8e-05,4e-06,-7.3e-05,-0.00029,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.6e-05,0.024,0.024,0.009,0.16,0.16,0.036,3.5e-11,3.5e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29490000,0.71,0.0018,0.0048,0.71,-2.2,-1.1,0.97,-7.7,-3.8,-3.7e+02,-1.1e-05,-5.8e-05,3.9e-06,-7.7e-05,-0.00028,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.6e-05,0.026,0.026,0.0091,0.17,0.17,0.037,3.6e-11,3.6e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29590000,0.71,0.0022,0.0059,0.71,-2.2,-1.1,0.96,-8,-3.9,-3.7e+02,-1.1e-05,-5.7e-05,3.8e-06,-9.9e-05,-0.00028,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.6e-05,0.024,0.025,0.009,0.17,0.17,0.036,3.5e-11,3.5e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29690000,0.71,0.0025,0.0066,0.71,-2.2,-1.1,0.95,-8.2,-4,-3.7e+02,-1.1e-05,-5.7e-05,3.7e-06,-0.0001,-0.00027,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.5e-05,0.026,0.026,0.0091,0.18,0.18,0.036,3.5e-11,3.5e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29790000,0.71,0.0028,0.0071,0.71,-2.1,-1.1,0.93,-8.5,-4.1,-3.7e+02,-1.2e-05,-5.7e-05,3.7e-06,-0.00012,-0.00028,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.8e-05,7.7e-05,7.5e-05,2.5e-05,0.025,0.025,0.009,0.18,0.18,0.037,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29890000,0.71,0.0029,0.0074,0.71,-2.1,-1.1,0.92,-8.7,-4.2,-3.7e+02,-1.2e-05,-5.7e-05,3.5e-06,-0.00013,-0.00026,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.7e-05,7.5e-05,2.5e-05,0.026,0.026,0.0091,0.19,0.19,0.037,3.5e-11,3.4e-11,1.3e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -29990000,0.71,0.003,0.0076,0.71,-2.1,-1.1,0.9,-9,-4.3,-3.7e+02,-1.2e-05,-5.7e-05,3.4e-06,-0.00015,-0.00026,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.7e-05,7.5e-05,2.5e-05,0.025,0.025,0.009,0.19,0.18,0.036,3.4e-11,3.4e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30090000,0.71,0.003,0.0075,0.71,-2.1,-1,0.89,-9.2,-4.4,-3.7e+02,-1.2e-05,-5.7e-05,3.2e-06,-0.00015,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.026,0.026,0.0091,0.2,0.2,0.036,3.4e-11,3.4e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30190000,0.71,0.0031,0.0074,0.71,-2,-1,0.88,-9.4,-4.5,-3.7e+02,-1.2e-05,-5.7e-05,3.2e-06,-0.00016,-0.00025,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.025,0.025,0.009,0.2,0.19,0.037,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30290000,0.71,0.003,0.0072,0.71,-2,-1,0.86,-9.6,-4.6,-3.7e+02,-1.2e-05,-5.7e-05,3.1e-06,-0.00017,-0.00024,-0.0011,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.026,0.026,0.0091,0.21,0.21,0.037,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30390000,0.71,0.003,0.0071,0.71,-2,-1,0.85,-9.9,-4.7,-3.7e+02,-1.3e-05,-5.7e-05,3e-06,-0.00018,-0.00023,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.025,0.025,0.009,0.2,0.2,0.036,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30490000,0.71,0.0029,0.0068,0.7,-2,-1,0.83,-10,-4.8,-3.7e+02,-1.3e-05,-5.7e-05,3e-06,-0.00018,-0.00022,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.026,0.026,0.009,0.22,0.22,0.037,3.3e-11,3.3e-11,1.2e-10,2.7e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30590000,0.71,0.0029,0.0065,0.7,-1.9,-1,0.79,-10,-4.9,-3.7e+02,-1.3e-05,-5.7e-05,3e-06,-0.00019,-0.00022,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.025,0.025,0.0089,0.21,0.21,0.037,3.2e-11,3.2e-11,1.2e-10,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30690000,0.71,0.0027,0.0062,0.7,-1.9,-0.99,0.79,-11,-5,-3.7e+02,-1.3e-05,-5.7e-05,3e-06,-0.00019,-0.00021,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.026,0.026,0.009,0.23,0.23,0.037,3.3e-11,3.3e-11,1.2e-10,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -30790000,0.71,0.0027,0.0059,0.7,-1.9,-0.98,0.78,-11,-5.1,-3.7e+02,-1.3e-05,-5.7e-05,2.8e-06,-0.0002,-0.0002,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.5e-05,0.025,0.025,0.0089,0.22,0.22,0.037,3.2e-11,3.2e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -30890000,0.71,0.0025,0.0054,0.7,-1.9,-0.98,0.76,-11,-5.2,-3.7e+02,-1.3e-05,-5.7e-05,2.8e-06,-0.00021,-0.00019,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.4e-05,0.026,0.026,0.009,0.24,0.24,0.037,3.2e-11,3.2e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -30990000,0.71,0.0025,0.005,0.7,-1.8,-0.96,0.76,-11,-5.3,-3.7e+02,-1.3e-05,-5.7e-05,2.7e-06,-0.00021,-0.00018,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.5e-05,2.4e-05,0.025,0.025,0.0089,0.23,0.23,0.036,3.2e-11,3.2e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31090000,0.71,0.0023,0.0045,0.7,-1.8,-0.96,0.74,-11,-5.4,-3.7e+02,-1.3e-05,-5.7e-05,2.6e-06,-0.00022,-0.00017,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.5e-05,2.4e-05,0.026,0.026,0.0089,0.25,0.25,0.037,3.2e-11,3.2e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31190000,0.71,0.0022,0.0043,0.7,-1.8,-0.95,0.73,-12,-5.5,-3.7e+02,-1.4e-05,-5.7e-05,2.6e-06,-0.00023,-0.00015,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.4e-05,2.4e-05,0.025,0.025,0.0088,0.24,0.24,0.037,3.1e-11,3.1e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31290000,0.71,0.002,0.0037,0.7,-1.8,-0.94,0.73,-12,-5.6,-3.7e+02,-1.4e-05,-5.7e-05,2.6e-06,-0.00024,-0.00013,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.4e-05,2.4e-05,0.026,0.026,0.0089,0.26,0.25,0.037,3.1e-11,3.1e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31390000,0.71,0.0018,0.0032,0.7,-1.8,-0.93,0.73,-12,-5.7,-3.7e+02,-1.4e-05,-5.7e-05,2.5e-06,-0.00024,-0.00012,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.4e-05,2.4e-05,0.025,0.025,0.0088,0.25,0.25,0.036,3.1e-11,3.1e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31490000,0.71,0.0016,0.0026,0.7,-1.7,-0.92,0.72,-12,-5.8,-3.7e+02,-1.4e-05,-5.7e-05,2.4e-06,-0.00025,-0.0001,-0.001,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.9e-05,7.4e-05,2.4e-05,0.026,0.026,0.0088,0.26,0.26,0.037,3.1e-11,3.1e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31590000,0.71,0.0016,0.0023,0.7,-1.7,-0.91,0.72,-12,-5.8,-3.7e+02,-1.4e-05,-5.7e-05,2.4e-06,-0.00026,-8.6e-05,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.4e-05,2.4e-05,0.025,0.025,0.0087,0.26,0.26,0.037,3e-11,3e-11,1.1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31690000,0.71,0.0013,0.0016,0.7,-1.7,-0.9,0.72,-12,-5.9,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00026,-7.4e-05,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.9e-05,7.4e-05,2.4e-05,0.026,0.026,0.0087,0.27,0.27,0.037,3e-11,3.1e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31790000,0.71,0.0011,0.001,0.71,-1.7,-0.89,0.72,-13,-6,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00027,-5.7e-05,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.8e-05,7.4e-05,2.4e-05,0.025,0.025,0.0087,0.27,0.27,0.037,3e-11,3e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31890000,0.71,0.00089,0.00031,0.7,-1.6,-0.88,0.72,-13,-6.1,-3.7e+02,-1.4e-05,-5.7e-05,2.3e-06,-0.00028,-4.4e-05,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,8.6e-05,7.9e-05,7.4e-05,2.4e-05,0.026,0.026,0.0087,0.28,0.28,0.037,3e-11,3e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -31990000,0.71,0.00076,-0.00015,0.71,-1.6,-0.87,0.71,-13,-6.2,-3.7e+02,-1.4e-05,-5.7e-05,2.2e-06,-0.00028,-2.6e-05,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.8e-05,7.3e-05,2.3e-05,0.025,0.025,0.0086,0.28,0.28,0.036,3e-11,3e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -32090000,0.71,0.00047,-0.00087,0.71,-1.6,-0.86,0.72,-13,-6.3,-3.7e+02,-1.4e-05,-5.7e-05,2.1e-06,-0.00029,-1.1e-05,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.9e-05,7.3e-05,2.3e-05,0.026,0.026,0.0087,0.29,0.29,0.037,3e-11,3e-11,1e-10,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -32190000,0.71,0.00026,-0.0017,0.71,-1.5,-0.85,0.72,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,1.9e-06,-0.0003,8.9e-06,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.8e-05,7.3e-05,2.3e-05,0.025,0.025,0.0086,0.29,0.29,0.036,2.9e-11,2.9e-11,9.9e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -32290000,0.71,3e-05,-0.0024,0.71,-1.5,-0.84,0.71,-13,-6.4,-3.7e+02,-1.4e-05,-5.7e-05,1.9e-06,-0.00031,2.4e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.9e-05,7.3e-05,2.3e-05,0.026,0.026,0.0086,0.3,0.3,0.036,2.9e-11,3e-11,9.8e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -32390000,0.71,-0.00015,-0.003,0.71,-1.5,-0.83,0.71,-14,-6.5,-3.7e+02,-1.4e-05,-5.7e-05,1.9e-06,-0.00031,3.3e-05,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.8e-05,7.3e-05,2.3e-05,0.025,0.025,0.0085,0.3,0.3,0.037,2.9e-11,2.9e-11,9.8e-11,2.6e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -32490000,0.71,-0.00027,-0.0033,0.71,-1.4,-0.82,0.72,-14,-6.6,-3.7e+02,-1.4e-05,-5.7e-05,1.9e-06,-0.00031,4.4e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.9e-05,7.3e-05,2.3e-05,0.026,0.026,0.0085,0.31,0.31,0.037,2.9e-11,2.9e-11,9.7e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -32590000,0.71,-0.00027,-0.0035,0.71,-1.4,-0.8,0.71,-14,-6.7,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00032,5.1e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.8e-05,7.2e-05,2.3e-05,0.025,0.025,0.0084,0.31,0.31,0.036,2.9e-11,2.9e-11,9.6e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -32690000,0.71,-0.00031,-0.0036,0.71,-1.4,-0.79,0.71,-14,-6.8,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00032,5.7e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.9e-05,7.2e-05,2.3e-05,0.026,0.026,0.0085,0.32,0.32,0.036,2.9e-11,2.9e-11,9.5e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -32790000,0.71,-0.00027,-0.0035,0.71,-1.4,-0.78,0.71,-14,-6.8,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00032,6.6e-05,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.8e-05,7.2e-05,2.3e-05,0.025,0.025,0.0084,0.32,0.31,0.036,2.8e-11,2.9e-11,9.4e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -32890000,0.71,-0.00018,-0.0035,0.71,-1.3,-0.77,0.71,-14,-6.9,-3.7e+02,-1.5e-05,-5.7e-05,1.6e-06,-0.00033,7.6e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,8.5e-05,7.9e-05,7.2e-05,2.3e-05,0.026,0.026,0.0084,0.33,0.33,0.036,2.9e-11,2.9e-11,9.3e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -32990000,0.71,-5.1e-05,-0.0034,0.71,-1.3,-0.76,0.7,-15,-7,-3.7e+02,-1.5e-05,-5.7e-05,1.7e-06,-0.00033,8.9e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,8.4e-05,7.8e-05,7.2e-05,2.3e-05,0.025,0.024,0.0083,0.32,0.32,0.036,2.8e-11,2.8e-11,9.2e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33090000,0.71,-8.5e-05,-0.0034,0.71,-1.3,-0.76,0.69,-15,-7.1,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00034,9.4e-05,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,8.4e-05,7.8e-05,7.2e-05,2.3e-05,0.026,0.026,0.0084,0.34,0.34,0.036,2.8e-11,2.8e-11,9.2e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33190000,0.7,0.0033,-0.0025,0.71,-1.3,-0.75,0.64,-15,-7.1,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00034,0.0001,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,8.4e-05,7.8e-05,7.2e-05,2.2e-05,0.025,0.024,0.0083,0.33,0.33,0.036,2.8e-11,2.8e-11,9.1e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33290000,0.65,0.015,-0.0016,0.76,-1.3,-0.73,0.62,-15,-7.2,-3.7e+02,-1.5e-05,-5.7e-05,1.8e-06,-0.00034,0.0001,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.8e-05,7.2e-05,2e-05,0.026,0.026,0.0083,0.35,0.35,0.036,2.8e-11,2.8e-11,9e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33390000,0.55,0.013,-0.0018,0.84,-1.3,-0.72,0.81,-15,-7.3,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00034,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,9e-05,7.7e-05,7.3e-05,1.6e-05,0.024,0.024,0.0083,0.34,0.34,0.036,2.8e-11,2.8e-11,8.9e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33490000,0.41,0.0068,0.00062,0.91,-1.2,-0.71,0.83,-15,-7.4,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,9.1e-05,7.6e-05,7.5e-05,1.6e-05,0.026,0.026,0.0081,0.36,0.35,0.036,2.8e-11,2.8e-11,8.8e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33590000,0.25,0.00095,-0.0019,0.97,-1.2,-0.71,0.79,-15,-7.4,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,8.7e-05,7.4e-05,7.6e-05,1.9e-05,0.025,0.025,0.0078,0.35,0.35,0.036,2.7e-11,2.8e-11,8.8e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33690000,0.087,-0.0023,-0.005,1,-1.2,-0.71,0.8,-15,-7.5,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,7.9e-05,7.3e-05,7.8e-05,2.7e-05,0.028,0.028,0.0076,0.36,0.36,0.036,2.8e-11,2.8e-11,8.7e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33790000,-0.082,-0.0039,-0.0068,1,-1.1,-0.69,0.78,-16,-7.6,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,6.7e-05,7e-05,7.7e-05,3.6e-05,0.028,0.028,0.0074,0.36,0.36,0.036,2.7e-11,2.7e-11,8.6e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33890000,-0.25,-0.005,-0.0075,0.97,-1,-0.66,0.77,-16,-7.6,-3.7e+02,-1.5e-05,-5.7e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,5.5e-05,7e-05,7.8e-05,4.7e-05,0.031,0.032,0.0072,0.37,0.37,0.036,2.7e-11,2.7e-11,8.6e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -33990000,-0.39,-0.0032,-0.011,0.92,-0.98,-0.63,0.74,-16,-7.7,-3.7e+02,-1.5e-05,-5.6e-05,1.9e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,4.3e-05,6.7e-05,7.5e-05,5.6e-05,0.031,0.032,0.007,0.37,0.37,0.035,2.7e-11,2.7e-11,8.5e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34090000,-0.5,-0.0022,-0.013,0.87,-0.93,-0.58,0.74,-16,-7.7,-3.7e+02,-1.5e-05,-5.7e-05,2e-06,-0.00035,0.00011,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,3.5e-05,6.7e-05,7.5e-05,6.1e-05,0.035,0.036,0.0069,0.38,0.38,0.036,2.7e-11,2.7e-11,8.5e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34190000,-0.57,-0.0015,-0.011,0.82,-0.91,-0.54,0.74,-16,-7.8,-3.7e+02,-1.5e-05,-5.7e-05,2e-06,-0.00037,0.00013,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,3e-05,6.3e-05,7.1e-05,6.3e-05,0.036,0.037,0.0067,0.38,0.38,0.035,2.7e-11,2.7e-11,8.4e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34290000,-0.61,-0.0024,-0.0086,0.79,-0.86,-0.49,0.74,-16,-7.9,-3.7e+02,-1.5e-05,-5.7e-05,2e-06,-0.00037,0.00013,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,6.3e-05,7.1e-05,6.2e-05,0.041,0.043,0.0066,0.39,0.39,0.035,2.7e-11,2.7e-11,8.4e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34390000,-0.64,-0.0026,-0.006,0.77,-0.85,-0.46,0.73,-16,-7.9,-3.7e+02,-1.6e-05,-5.7e-05,2.1e-06,-0.00042,0.00018,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,5.9e-05,6.5e-05,6.1e-05,0.041,0.042,0.0064,0.39,0.39,0.035,2.6e-11,2.7e-11,8.3e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34490000,-0.65,-0.0035,-0.0039,0.76,-0.8,-0.42,0.73,-16,-8,-3.7e+02,-1.6e-05,-5.7e-05,2.1e-06,-0.00042,0.00018,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,5.9e-05,6.5e-05,6e-05,0.048,0.049,0.0064,0.4,0.4,0.035,2.7e-11,2.7e-11,8.3e-11,2.6e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34590000,-0.66,-0.0029,-0.0028,0.75,-0.8,-0.4,0.73,-17,-8.1,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00052,0.00026,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,5.4e-05,5.9e-05,5.8e-05,0.046,0.048,0.0063,0.4,0.4,0.034,2.6e-11,2.7e-11,8.3e-11,2.5e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34690000,-0.67,-0.0033,-0.002,0.74,-0.74,-0.36,0.73,-17,-8.1,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00052,0.00026,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,5.4e-05,5.9e-05,5.6e-05,0.053,0.055,0.0063,0.41,0.41,0.034,2.6e-11,2.7e-11,8.3e-11,2.5e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34790000,-0.67,-0.0022,-0.0017,0.74,-0.75,-0.35,0.72,-17,-8.2,-3.7e+02,-1.6e-05,-5.7e-05,2.1e-06,-0.00062,0.00036,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.4e-05,4.9e-05,5.3e-05,5.4e-05,0.05,0.052,0.0062,0.41,0.41,0.034,2.6e-11,2.6e-11,8.2e-11,2.5e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34890000,-0.68,-0.0022,-0.0016,0.74,-0.7,-0.32,0.72,-17,-8.2,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00062,0.00036,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.4e-05,4.9e-05,5.4e-05,5.2e-05,0.058,0.06,0.0062,0.42,0.42,0.034,2.6e-11,2.7e-11,8.2e-11,2.5e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34990000,-0.68,-0.0086,-0.0044,0.73,0.31,0.29,-0.13,-17,-8.2,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.4e-05,4.5e-05,4.9e-05,5.1e-05,0.056,0.057,0.0068,0.42,0.42,0.034,2.6e-11,2.7e-11,8.2e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35090000,-0.68,-0.0086,-0.0045,0.73,0.44,0.31,-0.18,-17,-8.2,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,4.5e-05,4.9e-05,4.9e-05,0.061,0.062,0.0068,0.43,0.43,0.034,2.6e-11,2.7e-11,8.2e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35190000,-0.68,-0.0086,-0.0045,0.73,0.46,0.35,-0.18,-17,-8.2,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,4.5e-05,4.9e-05,4.8e-05,0.065,0.068,0.0067,0.44,0.44,0.034,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35290000,-0.68,-0.0087,-0.0046,0.73,0.48,0.38,-0.18,-17,-8.1,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,4.5e-05,4.9e-05,4.6e-05,0.071,0.073,0.0066,0.45,0.45,0.033,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35390000,-0.68,-0.0087,-0.0045,0.73,0.51,0.41,-0.18,-17,-8.1,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,4.5e-05,4.9e-05,4.5e-05,0.076,0.079,0.0066,0.47,0.47,0.034,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35490000,-0.68,-0.0088,-0.0045,0.73,0.53,0.44,-0.18,-17,-8,-3.7e+02,-1.6e-05,-5.7e-05,2.2e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,4.5e-05,4.9e-05,4.4e-05,0.082,0.086,0.0065,0.49,0.49,0.034,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35590000,-0.68,-0.0057,-0.0045,0.73,0.42,0.36,-0.19,-17,-8.1,-3.7e+02,-1.7e-05,-5.7e-05,2.4e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,3.8e-05,4.1e-05,4.2e-05,0.066,0.069,0.0062,0.48,0.48,0.033,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35690000,-0.68,-0.0057,-0.0045,0.73,0.44,0.39,-0.19,-17,-8.1,-3.7e+02,-1.7e-05,-5.7e-05,2.4e-06,-0.00076,0.0005,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,3.8e-05,4.1e-05,4.1e-05,0.072,0.074,0.0061,0.49,0.49,0.033,2.7e-11,2.7e-11,8.1e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35790000,-0.68,-0.0035,-0.0044,0.73,0.36,0.33,-0.19,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,2.5e-06,-0.00078,0.00052,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.5e-05,3.3e-05,3.5e-05,4e-05,0.06,0.062,0.0059,0.48,0.48,0.033,2.7e-11,2.7e-11,8e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35890000,-0.68,-0.0035,-0.0045,0.73,0.38,0.35,-0.19,-17,-8.1,-3.7e+02,-1.7e-05,-5.7e-05,2.6e-06,-0.00078,0.00052,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,3.3e-05,3.5e-05,3.9e-05,0.065,0.067,0.0058,0.5,0.5,0.033,2.7e-11,2.7e-11,8e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -35990000,-0.68,-0.0016,-0.0044,0.73,0.31,0.3,-0.19,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,2.8e-06,-0.00085,0.00058,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,2.9e-05,3.1e-05,3.8e-05,0.056,0.058,0.0057,0.49,0.49,0.033,2.7e-11,2.7e-11,8e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -36090000,-0.68,-0.0017,-0.0044,0.73,0.33,0.32,-0.19,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,2.8e-06,-0.00085,0.00058,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,2.9e-05,3.1e-05,3.7e-05,0.062,0.064,0.0057,0.51,0.51,0.032,2.7e-11,2.7e-11,8e-11,2.4e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -36190000,-0.68,-0.00027,-0.0043,0.73,0.27,0.27,-0.19,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,2.9e-06,-0.00095,0.00067,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,2.6e-05,2.7e-05,3.7e-05,0.054,0.056,0.0055,0.5,0.5,0.032,2.7e-11,2.7e-11,8e-11,2.3e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -36290000,-0.68,-0.00039,-0.0042,0.73,0.28,0.29,-0.18,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,3e-06,-0.00096,0.00067,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,2.6e-05,2.7e-05,3.6e-05,0.06,0.061,0.0056,0.52,0.51,0.032,2.7e-11,2.8e-11,8e-11,2.3e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -36390000,-0.68,0.00068,-0.0041,0.73,0.24,0.25,-0.18,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.1e-06,-0.0011,0.00077,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.6e-05,2.3e-05,2.5e-05,3.5e-05,0.053,0.054,0.0055,0.51,0.51,0.032,2.7e-11,2.8e-11,8e-11,2.3e-06,2.1e-06,5e-08,0,0,0,0,0,0,0,0 -36490000,-0.68,0.00059,-0.0042,0.73,0.24,0.27,-0.18,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.3e-06,-0.0011,0.00077,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.7e-05,2.3e-05,2.5e-05,3.4e-05,0.058,0.06,0.0055,0.52,0.52,0.032,2.8e-11,2.8e-11,8e-11,2.3e-06,2.1e-06,5e-08,0,0,0,0,0,0,0,0 -36590000,-0.68,0.0014,-0.004,0.73,0.2,0.23,-0.18,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.4e-06,-0.0012,0.00087,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.7e-05,2.1e-05,2.3e-05,3.4e-05,0.052,0.053,0.0055,0.52,0.52,0.031,2.8e-11,2.8e-11,7.9e-11,2.2e-06,2.1e-06,5e-08,0,0,0,0,0,0,0,0 -36690000,-0.68,0.0014,-0.004,0.74,0.21,0.24,-0.17,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.4e-06,-0.0012,0.00087,-0.00092,0.21,0.0021,0.44,0,0,0,0,0,2.7e-05,2.1e-05,2.3e-05,3.3e-05,0.058,0.059,0.0055,0.53,0.53,0.031,2.8e-11,2.8e-11,7.9e-11,2.2e-06,2.1e-06,5e-08,0,0,0,0,0,0,0,0 -36790000,-0.68,0.002,-0.0039,0.74,0.17,0.21,-0.17,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.5e-06,-0.0013,0.00096,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,2.7e-05,2e-05,2.1e-05,3.2e-05,0.051,0.053,0.0055,0.53,0.53,0.031,2.8e-11,2.8e-11,7.9e-11,2.2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 -36890000,-0.68,0.0019,-0.0039,0.74,0.18,0.22,-0.16,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.6e-06,-0.0013,0.00097,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,2.7e-05,2e-05,2.1e-05,3.2e-05,0.057,0.058,0.0056,0.54,0.54,0.031,2.8e-11,2.8e-11,7.9e-11,2.2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 -36990000,-0.68,0.0024,-0.0037,0.74,0.15,0.19,-0.16,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.7e-06,-0.0015,0.001,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.9e-05,2e-05,3.1e-05,0.051,0.052,0.0056,0.54,0.54,0.031,2.8e-11,2.8e-11,7.9e-11,2.1e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 -37090000,-0.68,0.0023,-0.0037,0.74,0.15,0.2,-0.15,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.8e-06,-0.0015,0.0011,-0.00093,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.9e-05,2e-05,3.1e-05,0.056,0.057,0.0057,0.55,0.55,0.031,2.8e-11,2.8e-11,7.9e-11,2.1e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 -37190000,-0.68,0.0026,-0.0036,0.74,0.12,0.17,-0.14,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,3.9e-06,-0.0016,0.0011,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.8e-05,1.9e-05,3e-05,0.05,0.051,0.0057,0.55,0.55,0.031,2.8e-11,2.8e-11,7.9e-11,2e-06,1.9e-06,5e-08,0,0,0,0,0,0,0,0 -37290000,-0.68,0.0026,-0.0036,0.74,0.12,0.18,-0.14,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,4e-06,-0.0016,0.0011,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.8e-05,1.9e-05,3e-05,0.055,0.056,0.0058,0.56,0.56,0.031,2.8e-11,2.9e-11,7.9e-11,2e-06,1.9e-06,5e-08,0,0,0,0,0,0,0,0 -37390000,-0.68,0.0028,-0.0035,0.74,0.1,0.15,-0.13,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,4.1e-06,-0.0017,0.0012,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.8e-05,1.8e-05,2.9e-05,0.049,0.05,0.0059,0.56,0.56,0.031,2.8e-11,2.9e-11,7.9e-11,1.9e-06,1.8e-06,5e-08,0,0,0,0,0,0,0,0 -37490000,-0.68,0.0028,-0.0035,0.74,0.099,0.16,-0.13,-17,-8.2,-3.7e+02,-1.7e-05,-5.8e-05,4.2e-06,-0.0017,0.0012,-0.00094,0.21,0.0021,0.44,0,0,0,0,0,2.8e-05,1.8e-05,1.9e-05,2.9e-05,0.054,0.055,0.006,0.57,0.57,0.031,2.9e-11,2.9e-11,7.9e-11,1.9e-06,1.8e-06,5e-08,0,0,0,0,0,0,0,0 -37590000,-0.68,0.003,-0.0034,0.74,0.08,0.13,-0.12,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.2e-06,-0.0018,0.0012,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,2.9e-05,1.7e-05,1.8e-05,2.8e-05,0.048,0.049,0.0061,0.57,0.57,0.031,2.9e-11,2.9e-11,7.9e-11,1.8e-06,1.7e-06,5e-08,0,0,0,0,0,0,0,0 -37690000,-0.68,0.0029,-0.0034,0.74,0.078,0.14,-0.11,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.3e-06,-0.0018,0.0013,-0.00095,0.21,0.0021,0.44,0,0,0,0,0,2.9e-05,1.7e-05,1.8e-05,2.8e-05,0.053,0.054,0.0062,0.58,0.58,0.031,2.9e-11,2.9e-11,7.8e-11,1.8e-06,1.7e-06,5e-08,0,0,0,0,0,0,0,0 -37790000,-0.68,0.003,-0.0034,0.74,0.061,0.12,-0.1,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.4e-06,-0.0019,0.0013,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,2.9e-05,1.7e-05,1.8e-05,2.7e-05,0.047,0.048,0.0063,0.58,0.58,0.03,2.9e-11,2.9e-11,7.8e-11,1.7e-06,1.6e-06,5e-08,0,0,0,0,0,0,0,0 -37890000,-0.68,0.003,-0.0034,0.74,0.058,0.12,-0.093,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.5e-06,-0.0019,0.0013,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,2.9e-05,1.7e-05,1.8e-05,2.7e-05,0.051,0.052,0.0064,0.59,0.59,0.03,2.9e-11,2.9e-11,7.8e-11,1.7e-06,1.6e-06,5e-08,0,0,0,0,0,0,0,0 -37990000,-0.68,0.0031,-0.0034,0.74,0.044,0.11,-0.084,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.6e-06,-0.0019,0.0013,-0.00096,0.21,0.0021,0.44,0,0,0,0,0,2.9e-05,1.7e-05,1.8e-05,2.7e-05,0.046,0.047,0.0065,0.59,0.59,0.031,2.9e-11,2.9e-11,7.8e-11,1.7e-06,1.6e-06,5e-08,0,0,0,0,0,0,0,0 -38090000,-0.68,0.003,-0.0034,0.74,0.04,0.11,-0.074,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.7e-06,-0.0019,0.0013,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,3e-05,1.7e-05,1.8e-05,2.6e-05,0.05,0.051,0.0066,0.6,0.6,0.031,2.9e-11,2.9e-11,7.8e-11,1.7e-06,1.6e-06,5e-08,0,0,0,0,0,0,0,0 -38190000,-0.68,0.0031,-0.0033,0.74,0.027,0.093,-0.066,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.8e-06,-0.002,0.0014,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,3e-05,1.7e-05,1.8e-05,2.6e-05,0.045,0.046,0.0067,0.6,0.6,0.031,2.9e-11,2.9e-11,7.8e-11,1.6e-06,1.5e-06,5e-08,0,0,0,0,0,0,0,0 -38290000,-0.68,0.0031,-0.0033,0.74,0.023,0.094,-0.059,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,4.9e-06,-0.002,0.0014,-0.00097,0.21,0.0021,0.44,0,0,0,0,0,3e-05,1.7e-05,1.8e-05,2.6e-05,0.049,0.049,0.0068,0.61,0.61,0.031,2.9e-11,3e-11,7.8e-11,1.6e-06,1.5e-06,5e-08,0,0,0,0,0,0,0,0 -38390000,-0.68,0.0031,-0.0032,0.74,0.015,0.081,-0.052,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5e-06,-0.002,0.0014,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,3e-05,1.7e-05,1.8e-05,2.5e-05,0.044,0.044,0.0069,0.61,0.61,0.031,2.9e-11,3e-11,7.8e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0 -38490000,-0.68,0.0031,-0.0032,0.74,0.011,0.083,-0.044,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.1e-06,-0.002,0.0014,-0.00098,0.21,0.0021,0.44,0,0,0,0,0,3.1e-05,1.7e-05,1.8e-05,2.5e-05,0.047,0.048,0.007,0.62,0.62,0.031,3e-11,3e-11,7.8e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0 -38590000,-0.68,0.0031,-0.0031,0.74,0.0062,0.071,-0.038,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.2e-06,-0.0021,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,3.1e-05,1.7e-05,1.8e-05,2.5e-05,0.043,0.043,0.0071,0.62,0.62,0.031,3e-11,3e-11,7.8e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0 -38690000,-0.68,0.003,-0.0031,0.74,0.0017,0.071,-0.031,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.3e-06,-0.0021,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,3.1e-05,1.7e-05,1.8e-05,2.4e-05,0.046,0.047,0.0072,0.63,0.63,0.031,3e-11,3e-11,7.8e-11,1.5e-06,1.4e-06,5e-08,0,0,0,0,0,0,0,0 -38790000,-0.68,0.003,-0.0031,0.74,-0.0036,0.059,-0.023,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.4e-06,-0.0021,0.0014,-0.00099,0.21,0.0021,0.44,0,0,0,0,0,3.1e-05,1.7e-05,1.8e-05,2.4e-05,0.042,0.042,0.0073,0.63,0.63,0.031,3e-11,3e-11,7.7e-11,1.4e-06,1.3e-06,5e-08,0,0,0,0,0,0,0,0 -38890000,-0.68,0.0028,-0.0031,0.74,-0.014,0.049,0.48,-17,-8.2,-3.7e+02,-1.7e-05,-5.7e-05,5.5e-06,-0.0021,0.0014,-0.001,0.21,0.0021,0.44,0,0,0,0,0,3.1e-05,1.7e-05,1.8e-05,2.4e-05,0.044,0.045,0.0075,0.64,0.64,0.032,3e-11,3e-11,7.7e-11,1.4e-06,1.3e-06,5e-08,0,0,0,0,0,0,0,0 +10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4.9e-07,0.0025,0.0025,4.3e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +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,5.2e-07,0.0026,0.0026,0.00011,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +190000,1,-0.0094,-0.011,2.9e-05,6.9e-05,0.0039,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.9e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,5.6e-07,0.0027,0.0027,0.00023,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +290000,1,-0.0094,-0.011,6.4e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-2.8e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,6.1e-07,0.0029,0.0029,0.00019,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +390000,1,-0.0095,-0.011,7.2e-05,0.0024,0.0083,-0.059,0.00021,0.0011,-0.0094,-1.1e-08,2.8e-09,7.6e-11,0,0,-4.5e-06,0,0,0,0,0,0,0,0,6.8e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +490000,1,-0.0095,-0.012,2.2e-05,0.0039,0.0048,-0.06,0.00024,0.00048,-0.011,1.6e-06,-3.7e-07,1.4e-08,0,0,-1.6e-05,0,0,0,0,0,0,0,0,7.7e-07,0.0034,0.0033,0.00018,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,0,0,0,0,0,0,0 +590000,1,-0.0095,-0.012,2.9e-05,0.006,0.0073,-0.059,0.00074,0.0011,-0.012,1.6e-06,-3.4e-07,1.4e-08,0,0,-3.4e-05,0,0,0,0,0,0,0,0,8.7e-07,0.0037,0.0037,0.00025,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,0,0,0,0,0,0,0 +690000,1,-0.0097,-0.012,8.9e-05,0.0063,0.0051,-0.06,0.0005,0.00054,-0.013,5.5e-06,-3.2e-06,1.2e-07,0,0,-6.3e-05,0,0,0,0,0,0,0,0,9.9e-07,0.004,0.004,0.00015,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,0,0,0,0,0,0,0 +790000,1,-0.0098,-0.013,0.0001,0.0086,0.0072,-0.063,0.0012,0.0011,-0.014,5.4e-06,-3.1e-06,1.1e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.1e-06,0.0044,0.0044,0.0002,2.8,2.8,2,0.42,0.42,0.28,0.01,0.01,0.0012,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +890000,1,-0.0099,-0.013,0.00012,0.01,0.0059,-0.077,0.00096,0.00071,-0.021,1.6e-05,-1.5e-05,3.4e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.3e-06,0.0048,0.0048,0.00013,1.3,1.3,2,0.2,0.2,0.43,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +990000,1,-0.0099,-0.013,0.00013,0.015,0.0062,-0.092,0.0022,0.0013,-0.029,1.6e-05,-1.5e-05,3.4e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0053,0.0053,0.00016,1.5,1.5,2,0.3,0.3,0.61,0.0099,0.01,0.00068,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1090000,1,-0.01,-0.014,0.00013,0.016,0.0048,-0.11,0.0018,0.00084,-0.039,4.1e-05,-6.2e-05,9.6e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.6e-06,0.0057,0.0057,0.00011,0.92,0.92,2,0.17,0.17,0.84,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1190000,1,-0.01,-0.014,0.0001,0.02,0.005,-0.12,0.0035,0.0013,-0.051,4.1e-05,-6.2e-05,9.6e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0063,0.0063,0.00013,1.1,1.1,2,0.24,0.24,1.1,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1290000,1,-0.01,-0.014,0.00016,0.02,0.0041,-0.14,0.0027,0.00085,-0.064,8.2e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,1.9e-06,0.0064,0.0064,9.9e-05,0.88,0.88,2,0.15,0.15,1.4,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1390000,1,-0.01,-0.014,0.00017,0.026,0.0038,-0.15,0.0051,0.0013,-0.078,8.2e-05,-0.00019,2.3e-06,0,0,-9.6e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0071,0.0071,0.00011,1.1,1.1,2,0.21,0.21,1.7,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1490000,1,-0.01,-0.014,0.00015,0.024,0.0026,-0.16,0.0038,0.00078,-0.093,0.00014,-0.00046,4.8e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.2e-06,0.0067,0.0067,8.8e-05,0.95,0.95,2,0.14,0.14,2.1,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1590000,1,-0.011,-0.014,0.00014,0.031,0.003,-0.18,0.0065,0.0011,-0.11,0.00014,-0.00046,4.8e-06,0,0,-9.8e-05,0,0,0,0,0,0,0,0,2.4e-06,0.0074,0.0074,0.0001,1.3,1.3,2,0.2,0.2,2.6,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1690000,1,-0.011,-0.014,0.0001,0.028,-0.00047,-0.19,0.0045,0.00057,-0.13,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.2e-06,0.0064,0.0064,7.9e-05,1,1,2,0.14,0.14,3,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1790000,1,-0.011,-0.014,7.5e-05,0.035,-0.0024,-0.2,0.0076,0.00044,-0.15,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.4e-06,0.007,0.007,8.9e-05,1.3,1.3,2,0.2,0.2,3.5,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1890000,1,-0.011,-0.015,5.4e-05,0.043,-0.0037,-0.22,0.011,0.00014,-0.17,0.00019,-0.00088,8.2e-06,0,0,-0.0001,0,0,0,0,0,0,0,0,2.7e-06,0.0076,0.0076,0.0001,1.7,1.7,2,0.31,0.31,4.1,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1990000,1,-0.011,-0.014,4.7e-05,0.036,-0.0051,-0.23,0.0082,-0.00038,-0.19,0.0002,-0.0014,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2.2e-06,0.0061,0.0061,8e-05,1.3,1.3,2,0.2,0.2,4.7,0.0066,0.0066,0.00011,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2090000,1,-0.011,-0.014,6.8e-06,0.041,-0.0077,-0.24,0.012,-0.001,-0.22,0.0002,-0.0014,1.3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2.4e-06,0.0066,0.0066,8.9e-05,1.7,1.7,2.1,0.31,0.31,5.3,0.0066,0.0066,0.00011,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2190000,1,-0.011,-0.014,-1.2e-06,0.033,-0.0072,-0.26,0.0079,-0.0011,-0.24,0.00015,-0.002,1.8e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.8e-06,0.005,0.005,7.2e-05,1.2,1.2,2.1,0.2,0.2,6,0.0055,0.0055,8.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2290000,1,-0.011,-0.014,-1.7e-05,0.039,-0.0098,-0.27,0.011,-0.0019,-0.27,0.00015,-0.002,1.8e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,2e-06,0.0054,0.0054,8e-05,1.5,1.5,2.1,0.3,0.3,6.7,0.0055,0.0055,8.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2390000,1,-0.011,-0.013,-1.9e-05,0.03,-0.009,-0.29,0.0074,-0.0016,-0.3,5.8e-05,-0.0025,2.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.5e-06,0.004,0.004,6.6e-05,1,1,2.1,0.19,0.19,7.4,0.0045,0.0045,7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2490000,1,-0.011,-0.013,-4e-05,0.034,-0.011,-0.3,0.011,-0.0025,-0.32,5.8e-05,-0.0025,2.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.6e-06,0.0044,0.0044,7.2e-05,1.3,1.3,2.1,0.28,0.28,8.2,0.0045,0.0045,7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2590000,1,-0.011,-0.013,-4.3e-05,0.026,-0.0093,-0.31,0.0068,-0.0018,-0.36,-5.2e-05,-0.003,2.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.2e-06,0.0032,0.0032,6.1e-05,0.89,0.89,2.1,0.18,0.18,9.1,0.0038,0.0038,5.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2690000,1,-0.011,-0.013,-5e-05,0.03,-0.011,-0.33,0.0096,-0.0029,-0.39,-5.2e-05,-0.003,2.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.3e-06,0.0035,0.0035,6.6e-05,1.1,1.1,2.2,0.25,0.25,10,0.0038,0.0038,5.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2790000,1,-0.011,-0.013,-7.1e-05,0.023,-0.0096,-0.34,0.0061,-0.002,-0.42,-0.00017,-0.0033,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,5.6e-05,0.77,0.77,2.2,0.16,0.16,11,0.0032,0.0032,4.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2890000,1,-0.011,-0.013,-0.00012,0.027,-0.011,-0.35,0.0086,-0.003,-0.46,-0.00017,-0.0033,3e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,6.1e-05,0.95,0.95,2.2,0.23,0.23,12,0.0032,0.0032,4.7e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +2990000,1,-0.011,-0.013,-9.1e-05,0.022,-0.0097,-0.36,0.0056,-0.0021,-0.49,-0.00028,-0.0036,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,8.4e-07,0.0023,0.0023,5.3e-05,0.67,0.67,2.2,0.15,0.15,13,0.0027,0.0027,3.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3090000,1,-0.011,-0.013,-9.2e-05,0.024,-0.011,-0.38,0.0079,-0.0032,-0.53,-0.00028,-0.0036,3.2e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,5.7e-05,0.82,0.82,2.2,0.22,0.22,14,0.0027,0.0027,3.9e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3190000,1,-0.011,-0.013,-0.00016,0.019,-0.0088,-0.39,0.0052,-0.0022,-0.57,-0.00039,-0.0039,3.4e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,4.9e-05,0.59,0.59,2.3,0.14,0.14,15,0.0023,0.0023,3.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3290000,1,-0.011,-0.013,-0.00013,0.023,-0.011,-0.4,0.0073,-0.0031,-0.61,-0.00039,-0.0039,3.4e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,7.9e-07,0.0022,0.0022,5.3e-05,0.73,0.73,2.3,0.2,0.2,16,0.0023,0.0023,3.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3390000,1,-0.011,-0.012,-0.00017,0.018,-0.0094,-0.42,0.0049,-0.0022,-0.65,-0.00049,-0.0041,3.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.2e-07,0.0017,0.0017,4.6e-05,0.53,0.53,2.3,0.14,0.14,17,0.0019,0.0019,2.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3490000,1,-0.011,-0.013,-0.00018,0.021,-0.012,-0.43,0.0068,-0.0032,-0.69,-0.00049,-0.0041,3.6e-05,0,0,-0.00011,0,0,0,0,0,0,0,0,6.8e-07,0.0019,0.0019,5e-05,0.65,0.65,2.3,0.19,0.19,19,0.0019,0.0019,2.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3590000,1,-0.011,-0.012,-0.00017,0.016,-0.011,-0.44,0.0046,-0.0023,-0.73,-0.0006,-0.0044,3.8e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.3e-07,0.0015,0.0015,4.4e-05,0.48,0.48,2.4,0.13,0.13,20,0.0016,0.0016,2.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3690000,1,-0.011,-0.012,-4.8e-05,0.019,-0.014,-0.46,0.0064,-0.0036,-0.78,-0.0006,-0.0044,3.8e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.9e-07,0.0017,0.0017,4.7e-05,0.59,0.59,2.4,0.18,0.18,21,0.0016,0.0016,2.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3790000,1,-0.011,-0.012,-1.3e-05,0.015,-0.014,-0.47,0.0043,-0.0026,-0.83,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,4.7e-07,0.0014,0.0014,4.1e-05,0.44,0.45,2.4,0.12,0.12,23,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +3890000,1,-0.011,-0.012,-5.1e-05,0.017,-0.015,-0.48,0.006,-0.004,-0.87,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.1e-07,0.0015,0.0015,4.4e-05,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,0,0,0,0,0,0,0 +3990000,1,-0.011,-0.012,-5.2e-05,0.02,-0.016,-0.5,0.0078,-0.0056,-0.92,-0.00072,-0.0045,4e-05,0,0,-0.0001,0,0,0,0,0,0,0,0,5.5e-07,0.0016,0.0016,4.7e-05,0.66,0.66,2.5,0.22,0.23,26,0.0014,0.0014,2.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4090000,1,-0.011,-0.012,-6.7e-05,0.017,-0.014,-0.51,0.0056,-0.0042,-0.97,-0.00084,-0.0047,4.2e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.3e-07,0.0013,0.0013,4.2e-05,0.5,0.5,2.5,0.16,0.16,27,0.0012,0.0011,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4190000,1,-0.011,-0.012,-9.6e-05,0.02,-0.017,-0.53,0.0075,-0.0057,-1,-0.00084,-0.0047,4.2e-05,0,0,-9.6e-05,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,4.4e-05,0.6,0.61,2.5,0.21,0.21,29,0.0012,0.0011,1.8e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4290000,1,-0.01,-0.012,-0.00015,0.017,-0.012,-0.54,0.0054,-0.0042,-1.1,-0.00097,-0.0049,4.4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,3.7e-07,0.0012,0.0012,4e-05,0.46,0.46,2.6,0.15,0.15,31,0.00095,0.00095,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4390000,1,-0.01,-0.012,-0.00014,0.018,-0.013,-0.55,0.0072,-0.0055,-1.1,-0.00097,-0.0049,4.4e-05,0,0,-9.1e-05,0,0,0,0,0,0,0,0,4e-07,0.0013,0.0013,4.2e-05,0.56,0.56,2.6,0.2,0.2,33,0.00095,0.00095,1.6e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4490000,1,-0.01,-0.012,-8.5e-05,0.015,-0.01,-0.57,0.0051,-0.0038,-1.2,-0.0011,-0.005,4.6e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.2e-07,0.001,0.001,3.8e-05,0.43,0.43,2.6,0.14,0.14,34,0.00078,0.00078,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4590000,1,-0.01,-0.012,-6.1e-05,0.018,-0.012,-0.58,0.0068,-0.0049,-1.2,-0.0011,-0.005,4.6e-05,0,0,-8.7e-05,0,0,0,0,0,0,0,0,3.4e-07,0.0011,0.0011,4e-05,0.51,0.51,2.7,0.19,0.19,36,0.00078,0.00078,1.4e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4690000,1,-0.01,-0.012,-6.8e-05,0.014,-0.01,-0.6,0.0049,-0.0035,-1.3,-0.0012,-0.0052,4.7e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.7e-07,0.00091,0.00091,3.6e-05,0.39,0.39,2.7,0.14,0.14,38,0.00064,0.00064,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4790000,1,-0.01,-0.012,-8.2e-05,0.016,-0.012,-0.61,0.0064,-0.0045,-1.4,-0.0012,-0.0052,4.7e-05,0,0,-8.2e-05,0,0,0,0,0,0,0,0,2.9e-07,0.00099,0.00099,3.8e-05,0.47,0.47,2.7,0.18,0.18,40,0.00064,0.00064,1.3e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4890000,1,-0.01,-0.012,-0.0001,0.013,-0.01,-0.63,0.0046,-0.0033,-1.4,-0.0013,-0.0053,4.8e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,2.3e-07,0.0008,0.00079,3.5e-05,0.36,0.36,2.8,0.13,0.13,42,0.00052,0.00052,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +4990000,1,-0.01,-0.012,-0.00013,0.015,-0.011,-0.64,0.006,-0.0043,-1.5,-0.0013,-0.0053,4.8e-05,0,0,-7.7e-05,0,0,0,0,0,0,0,0,2.4e-07,0.00086,0.00086,3.7e-05,0.43,0.43,2.8,0.17,0.17,44,0.00052,0.00052,1.1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5090000,1,-0.01,-0.011,-9.4e-05,0.012,-0.0086,-0.66,0.0043,-0.0031,-1.6,-0.0013,-0.0054,4.9e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,1.9e-07,0.00069,0.00069,3.3e-05,0.33,0.33,2.8,0.12,0.12,46,0.00042,0.00042,1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5190000,1,-0.01,-0.011,-7.3e-05,0.013,-0.01,-0.67,0.0056,-0.004,-1.6,-0.0013,-0.0054,4.9e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,2e-07,0.00075,0.00074,3.5e-05,0.39,0.39,2.9,0.16,0.16,49,0.00042,0.00042,1e-05,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5290000,1,-0.01,-0.011,-9.2e-05,0.0093,-0.0074,-0.68,0.0039,-0.0029,-1.7,-0.0014,-0.0055,5e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.6e-07,0.0006,0.0006,3.2e-05,0.3,0.3,2.9,0.12,0.12,51,0.00034,0.00034,9.1e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5390000,1,-0.0099,-0.011,-3.9e-05,0.0089,-0.0083,-0.7,0.0048,-0.0037,-1.8,-0.0014,-0.0055,5e-05,0,0,-6.9e-05,0,0,0,0,0,0,0,0,1.7e-07,0.00064,0.00064,3.4e-05,0.36,0.36,2.9,0.16,0.16,53,0.00034,0.00034,9.1e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5490000,1,-0.0098,-0.011,-3.6e-05,0.0062,-0.0064,-0.71,0.0033,-0.0026,-1.8,-0.0014,-0.0055,5.1e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.3e-07,0.00052,0.00051,3.1e-05,0.28,0.28,3,0.11,0.11,56,0.00028,0.00027,8.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5590000,1,-0.0098,-0.011,-6.1e-05,0.0069,-0.0068,-0.73,0.0039,-0.0032,-1.9,-0.0014,-0.0055,5.1e-05,0,0,-6.6e-05,0,0,0,0,0,0,0,0,1.4e-07,0.00055,0.00055,3.2e-05,0.33,0.33,3,0.15,0.15,58,0.00028,0.00027,8.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5690000,1,-0.0096,-0.011,4.4e-06,0.0049,-0.004,-0.74,0.0027,-0.0022,-2,-0.0015,-0.0056,5.2e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.1e-07,0.00045,0.00044,3e-05,0.25,0.25,3.1,0.11,0.11,61,0.00022,0.00022,7.5e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5790000,1,-0.0096,-0.011,-3.9e-06,0.0053,-0.003,-0.75,0.0031,-0.0026,-2,-0.0015,-0.0056,5.2e-05,0,0,-6.3e-05,0,0,0,0,0,0,0,0,1.2e-07,0.00048,0.00048,3.1e-05,0.3,0.3,3.1,0.14,0.14,64,0.00022,0.00022,7.5e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5890000,1,-0.0095,-0.011,-3.4e-05,0.0045,-0.0011,0.0028,0.0022,-0.0017,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,9e-08,0.00038,0.00038,2.9e-05,0.23,0.23,9.8,0.1,0.1,0.52,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +5990000,1,-0.0095,-0.011,-1.6e-05,0.0049,0.00031,0.015,0.0026,-0.0017,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-7.3e-05,0,0,0,0,0,0,0,0,9.6e-08,0.00041,0.00041,3e-05,0.27,0.27,8.8,0.13,0.13,0.33,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6090000,1,-0.0094,-0.011,-3.6e-05,0.006,0.0015,-0.011,0.0032,-0.0016,-3.7e+02,-0.0015,-0.0056,5.2e-05,0,0,-6.4e-05,0,0,0,0,0,0,0,0,1e-07,0.00044,0.00044,3.1e-05,0.31,0.31,7,0.17,0.17,0.33,0.00018,0.00018,6.9e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6190000,0.71,0.0013,-0.015,0.71,0.0035,0.003,-0.005,0.0023,-0.00066,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-8.8e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.22,0.22,4.9,0.13,0.13,0.32,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6290000,0.71,0.0013,-0.015,0.71,0.0035,0.0043,-0.012,0.0026,-0.00028,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-9.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.22,0.22,3.2,0.16,0.16,0.3,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6390000,0.71,0.0013,-0.014,0.71,0.0024,0.0048,-0.05,0.0029,0.00018,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-3.6e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.23,0.23,2.3,0.19,0.19,0.29,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6490000,0.71,0.0014,-0.014,0.71,0.0022,0.0056,-0.052,0.0031,0.00072,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-8.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00034,0.00034,0.0028,0.24,0.24,1.5,0.23,0.23,0.26,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6590000,0.71,0.0014,-0.014,0.71,0.0017,0.0057,-0.099,0.0034,0.0013,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,9.5e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00034,0.0028,0.25,0.25,1.1,0.28,0.28,0.23,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6690000,0.71,0.0014,-0.014,0.71,0.0021,0.0068,-0.076,0.0036,0.0019,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.00022,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00035,0.0028,0.26,0.26,0.78,0.33,0.33,0.21,0.00015,0.00015,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6790000,0.71,0.0014,-0.014,0.71,0.00081,0.0068,-0.11,0.0037,0.0026,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,8.4e-06,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00035,0.00035,0.0028,0.28,0.28,0.6,0.38,0.38,0.2,0.00015,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6890000,0.71,0.0014,-0.014,0.71,-0.0012,0.0073,-0.12,0.0037,0.0033,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-4.3e-05,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00036,0.00036,0.0028,0.3,0.3,0.46,0.44,0.44,0.18,0.00015,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +6990000,0.71,0.0015,-0.014,0.71,-0.0013,0.0082,-0.12,0.0035,0.0041,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.00034,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00036,0.00036,0.0028,0.32,0.32,0.36,0.51,0.51,0.16,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7090000,0.71,0.0015,-0.014,0.71,-0.0018,0.0079,-0.13,0.0034,0.0049,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.0007,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00037,0.00037,0.0028,0.35,0.35,0.29,0.58,0.58,0.16,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7190000,0.71,0.0015,-0.014,0.71,-0.0035,0.0081,-0.15,0.0031,0.0057,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.00049,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00038,0.00038,0.0028,0.37,0.37,0.24,0.66,0.66,0.15,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7290000,0.71,0.0015,-0.014,0.71,-0.005,0.0084,-0.15,0.0026,0.0065,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.0012,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00039,0.00039,0.0028,0.41,0.41,0.2,0.76,0.76,0.14,0.00014,0.00014,6.3e-06,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +7390000,0.71,0.0015,-0.014,0.71,-0.0051,0.0099,-0.16,0.0021,0.0074,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.0013,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.0004,0.00039,0.0028,0.44,0.44,0.18,0.85,0.85,0.13,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7490000,0.71,0.0016,-0.014,0.71,-0.0067,0.01,-0.16,0.0015,0.0084,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.0021,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.0004,0.0004,0.0028,0.48,0.48,0.15,0.96,0.96,0.12,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7590000,0.71,0.0016,-0.014,0.71,-0.0086,0.012,-0.17,0.00073,0.0093,-3.7e+02,-0.0015,-0.0057,5.2e-05,0,0,-0.003,0.37,0.0037,0.025,0,0,0,0,0,0.0028,0.00041,0.00041,0.0028,0.51,0.52,0.14,1.1,1.1,0.12,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7690000,0.7,0.0017,-0.014,0.71,-0.011,0.012,-0.16,-0.00031,0.01,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.005,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00042,0.00042,0.0022,0.56,0.56,0.13,1.2,1.2,0.11,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7790000,0.7,0.0017,-0.014,0.71,-0.012,0.013,-0.16,-0.0014,0.011,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.007,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00043,0.00043,0.0022,0.6,0.6,0.12,1.3,1.3,0.11,0.00014,0.00014,6.3e-06,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +7890000,0.7,0.0017,-0.014,0.71,-0.015,0.015,-0.16,-0.0028,0.013,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.0096,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00045,0.00045,0.0022,0.66,0.66,0.11,1.5,1.5,0.1,0.00014,0.00014,6.3e-06,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +7990000,0.7,0.0018,-0.014,0.71,-0.017,0.016,-0.16,-0.0043,0.014,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00046,0.00046,0.0022,0.7,0.7,0.1,1.7,1.7,0.099,0.00014,0.00014,6.3e-06,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +8090000,0.7,0.0018,-0.014,0.71,-0.018,0.018,-0.17,-0.0061,0.016,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.011,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00047,0.00047,0.0022,0.76,0.76,0.1,1.9,1.9,0.097,0.00014,0.00014,6.3e-06,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +8190000,0.7,0.0018,-0.014,0.71,-0.021,0.019,-0.18,-0.0081,0.017,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.013,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00049,0.00048,0.0022,0.83,0.83,0.099,2.1,2.1,0.094,0.00014,0.00014,6.3e-06,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +8290000,0.7,0.0018,-0.014,0.71,-0.022,0.02,-0.17,-0.01,0.019,-3.7e+02,-0.0015,-0.0057,5e-05,0,0,-0.017,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0005,0.0005,0.0022,0.88,0.88,0.097,2.3,2.3,0.091,0.00014,0.00014,6.3e-06,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +8390000,0.7,0.0019,-0.014,0.71,-0.025,0.021,-0.17,-0.013,0.021,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.021,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00051,0.00051,0.0022,0.95,0.95,0.097,2.5,2.5,0.091,0.00014,0.00014,6.3e-06,0.04,0.04,0.035,0,0,0,0,0,0,0,0 +8490000,0.7,0.0018,-0.014,0.71,-0.027,0.022,-0.17,-0.015,0.022,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.025,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00052,0.00052,0.0022,1,1,0.096,2.8,2.8,0.089,0.00014,0.00014,6.3e-06,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +8590000,0.7,0.0019,-0.014,0.71,-0.029,0.025,-0.17,-0.018,0.024,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.029,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00054,0.00054,0.0022,1.1,1.1,0.095,3.1,3.1,0.088,0.00014,0.00014,6.3e-06,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +8690000,0.7,0.0019,-0.014,0.71,-0.032,0.025,-0.16,-0.021,0.026,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.035,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00055,0.00055,0.0022,1.1,1.1,0.096,3.3,3.3,0.088,0.00014,0.00014,6.3e-06,0.04,0.04,0.033,0,0,0,0,0,0,0,0 +8790000,0.7,0.0019,-0.014,0.71,-0.034,0.027,-0.15,-0.024,0.028,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.041,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00056,0.00056,0.0022,1.2,1.2,0.095,3.7,3.7,0.087,0.00014,0.00014,6.3e-06,0.04,0.04,0.032,0,0,0,0,0,0,0,0 +8890000,0.7,0.0019,-0.014,0.71,-0.036,0.028,-0.15,-0.027,0.029,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.045,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00057,0.00057,0.0022,1.3,1.3,0.095,3.9,3.9,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.03,0,0,0,0,0,0,0,0 +8990000,0.7,0.002,-0.014,0.71,-0.038,0.028,-0.14,-0.031,0.032,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.051,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00059,0.00059,0.0022,1.4,1.4,0.096,4.3,4.3,0.087,0.00013,0.00013,6.3e-06,0.04,0.04,0.029,0,0,0,0,0,0,0,0 +9090000,0.7,0.002,-0.014,0.71,-0.04,0.029,-0.14,-0.034,0.033,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.053,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0006,0.00059,0.0022,1.4,1.4,0.095,4.6,4.6,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.028,0,0,0,0,0,0,0,0 +9190000,0.7,0.002,-0.014,0.71,-0.042,0.03,-0.14,-0.038,0.036,-3.7e+02,-0.0015,-0.0056,5e-05,0,0,-0.057,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00062,0.00061,0.0022,1.5,1.5,0.094,5.1,5.1,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.027,0,0,0,0,0,0,0,0 +9290000,0.7,0.002,-0.014,0.71,-0.043,0.03,-0.14,-0.041,0.037,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.061,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00062,0.00062,0.0022,1.6,1.6,0.093,5.3,5.3,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.025,0,0,0,0,0,0,0,0 +9390000,0.7,0.002,-0.014,0.71,-0.044,0.033,-0.14,-0.046,0.039,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.065,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00064,0.00064,0.0023,1.7,1.7,0.093,5.8,5.9,0.086,0.00013,0.00013,6.3e-06,0.04,0.04,0.024,0,0,0,0,0,0,0,0 +9490000,0.7,0.002,-0.014,0.71,-0.045,0.032,-0.13,-0.047,0.04,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.068,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00064,0.00064,0.0023,1.7,1.7,0.091,6.1,6.1,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.023,0,0,0,0,0,0,0,0 +9590000,0.7,0.002,-0.014,0.71,-0.048,0.034,-0.13,-0.052,0.043,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.072,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00066,0.00066,0.0023,1.8,1.8,0.09,6.7,6.7,0.085,0.00013,0.00013,6.3e-06,0.04,0.04,0.021,0,0,0,0,0,0,0,0 +9690000,0.7,0.0021,-0.014,0.71,-0.048,0.035,-0.12,-0.054,0.043,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.077,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00065,0.00065,0.0023,1.9,1.9,0.089,6.8,6.8,0.086,0.00012,0.00012,6.3e-06,0.04,0.04,0.02,0,0,0,0,0,0,0,0 +9790000,0.7,0.0021,-0.014,0.71,-0.049,0.037,-0.11,-0.059,0.046,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.082,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,2,2,0.087,7.5,7.5,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.019,0,0,0,0,0,0,0,0 +9890000,0.7,0.002,-0.014,0.71,-0.049,0.037,-0.11,-0.06,0.046,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.085,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,2,2,0.084,7.6,7.6,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.018,0,0,0,0,0,0,0,0 +9990000,0.7,0.0021,-0.014,0.71,-0.051,0.038,-0.1,-0.065,0.05,-3.7e+02,-0.0015,-0.0056,4.9e-05,0,0,-0.089,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00069,0.00069,0.0023,2.1,2.1,0.083,8.4,8.4,0.086,0.00012,0.00012,6.3e-06,0.04,0.04,0.017,0,0,0,0,0,0,0,0 +10090000,0.7,0.0021,-0.014,0.71,-0.049,0.037,-0.096,-0.065,0.049,-3.7e+02,-0.0015,-0.0056,4.8e-05,0,0,-0.091,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00068,0.00068,0.0023,2.1,2.1,0.08,8.4,8.4,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.016,0,0,0,0,0,0,0,0 +10190000,0.7,0.0021,-0.014,0.71,-0.051,0.04,-0.096,-0.07,0.053,-3.7e+02,-0.0015,-0.0056,4.8e-05,0,0,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0007,0.0007,0.0023,2.2,2.2,0.078,9.2,9.2,0.084,0.00012,0.00012,6.3e-06,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +10290000,0.7,0.0021,-0.014,0.71,-0.053,0.04,-0.084,-0.076,0.056,-3.7e+02,-0.0015,-0.0056,4.8e-05,0,0,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00072,0.00072,0.0023,2.4,2.4,0.076,10,10,0.085,0.00012,0.00012,6.3e-06,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +10390000,0.7,0.0021,-0.014,0.71,0.0095,-0.019,0.0096,0.00087,-0.0017,-3.7e+02,-0.0015,-0.0056,4.8e-05,-6.6e-08,4.9e-08,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00075,0.00075,0.0023,0.25,0.25,0.56,0.25,0.25,0.078,0.00012,0.00012,6.3e-06,0.04,0.04,0.013,0,0,0,0,0,0,0,0 +10490000,0.7,0.0021,-0.014,0.71,0.0082,-0.017,0.023,0.0017,-0.0035,-3.7e+02,-0.0015,-0.0056,4.8e-05,-1.7e-06,1.3e-06,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00077,0.00077,0.0023,0.26,0.26,0.55,0.26,0.26,0.08,0.00012,0.00012,6.3e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +10590000,0.7,0.0022,-0.014,0.71,0.0078,-0.0065,0.026,0.0018,-0.00081,-3.7e+02,-0.0015,-0.0056,4.9e-05,-0.00027,3.1e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00078,0.00078,0.0023,0.14,0.14,0.27,0.13,0.13,0.073,0.00012,0.00012,6.3e-06,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +10690000,0.7,0.0023,-0.014,0.71,0.0057,-0.0058,0.03,0.0025,-0.0014,-3.7e+02,-0.0015,-0.0056,4.9e-05,-0.00027,3.2e-05,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0008,0.0008,0.0023,0.15,0.15,0.26,0.14,0.14,0.078,0.00012,0.00012,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10790000,0.7,0.0023,-0.014,0.71,0.0052,-0.003,0.024,0.0027,-0.00072,-3.7e+02,-0.0014,-0.0056,4.8e-05,-0.00042,0.00016,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00077,0.00077,0.0023,0.11,0.11,0.17,0.091,0.091,0.072,0.00011,0.00011,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10890000,0.7,0.0022,-0.014,0.71,0.0039,-0.0017,0.02,0.0031,-0.00093,-3.7e+02,-0.0014,-0.0056,4.8e-05,-0.00042,0.00016,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0008,0.0008,0.0023,0.13,0.13,0.16,0.098,0.098,0.075,0.00011,0.00011,6.3e-06,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +10990000,0.7,0.0022,-0.014,0.71,0.0067,0.0033,0.014,0.0047,-0.0022,-3.7e+02,-0.0014,-0.0055,4.6e-05,-0.00082,0.00076,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00073,0.00073,0.0023,0.1,0.1,0.12,0.073,0.073,0.071,0.0001,0.0001,6.3e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 +11090000,0.7,0.0022,-0.014,0.71,0.0055,0.0059,0.019,0.0053,-0.0018,-3.7e+02,-0.0014,-0.0055,4.6e-05,-0.00082,0.00076,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00075,0.00075,0.0023,0.13,0.13,0.11,0.08,0.08,0.074,0.0001,0.0001,6.3e-06,0.039,0.039,0.011,0,0,0,0,0,0,0,0 +11190000,0.7,0.0021,-0.014,0.71,0.01,0.0083,0.0077,0.0067,-0.0028,-3.7e+02,-0.0013,-0.0055,4.5e-05,-0.00092,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00065,0.00065,0.0023,0.1,0.1,0.084,0.063,0.063,0.069,9.3e-05,9.3e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11290000,0.7,0.0021,-0.014,0.71,0.01,0.011,0.0074,0.0078,-0.0018,-3.7e+02,-0.0013,-0.0055,4.5e-05,-0.00093,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00067,0.00067,0.0023,0.13,0.13,0.078,0.071,0.071,0.072,9.3e-05,9.3e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11390000,0.7,0.0022,-0.014,0.71,0.0053,0.0092,0.0017,0.0055,-0.0019,-3.7e+02,-0.0013,-0.0056,4.6e-05,-0.0005,0.00089,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00055,0.00055,0.0023,0.1,0.1,0.063,0.058,0.058,0.068,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11490000,0.7,0.0022,-0.014,0.71,0.0028,0.012,0.0025,0.0059,-0.00089,-3.7e+02,-0.0013,-0.0056,4.6e-05,-0.00051,0.00089,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00057,0.00057,0.0023,0.13,0.13,0.058,0.066,0.066,0.069,8.2e-05,8.2e-05,6.3e-06,0.038,0.038,0.01,0,0,0,0,0,0,0,0 +11590000,0.7,0.0021,-0.014,0.71,-0.0016,0.01,-0.0034,0.0045,-0.0014,-3.7e+02,-0.0014,-0.0056,4.6e-05,-8.7e-05,0.00077,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00047,0.00047,0.0023,0.1,0.1,0.049,0.054,0.054,0.066,7.2e-05,7.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11690000,0.7,0.0021,-0.014,0.71,-0.0044,0.013,-0.0079,0.0042,-0.00027,-3.7e+02,-0.0014,-0.0056,4.6e-05,-7.8e-05,0.00076,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00048,0.00048,0.0023,0.12,0.12,0.046,0.063,0.063,0.066,7.2e-05,7.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11790000,0.7,0.0021,-0.014,0.71,-0.01,0.013,-0.0098,0.0018,0.00056,-3.7e+02,-0.0014,-0.0056,4.6e-05,2.9e-05,0.00057,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0004,0.0004,0.0023,0.097,0.097,0.039,0.053,0.053,0.063,6.3e-05,6.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11890000,0.7,0.0021,-0.014,0.71,-0.012,0.014,-0.011,0.00076,0.0019,-3.7e+02,-0.0014,-0.0056,4.6e-05,2.4e-05,0.00057,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00041,0.00041,0.0023,0.12,0.12,0.037,0.061,0.061,0.063,6.3e-05,6.3e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +11990000,0.7,0.0021,-0.014,0.71,-0.013,0.014,-0.016,-0.0003,0.0023,-3.7e+02,-0.0014,-0.0057,4.6e-05,0.00016,0.00059,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00034,0.00034,0.0023,0.091,0.091,0.033,0.051,0.051,0.061,5.7e-05,5.7e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12090000,0.7,0.0021,-0.014,0.71,-0.015,0.016,-0.022,-0.0017,0.0038,-3.7e+02,-0.0014,-0.0057,4.6e-05,0.00018,0.00058,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00035,0.00035,0.0023,0.11,0.11,0.031,0.06,0.06,0.061,5.7e-05,5.7e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12190000,0.7,0.0018,-0.014,0.71,-0.008,0.013,-0.017,0.0015,0.0019,-3.7e+02,-0.0013,-0.0057,4.3e-05,0.00043,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0003,0.0003,0.0023,0.085,0.085,0.028,0.051,0.051,0.059,5.2e-05,5.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12290000,0.7,0.0017,-0.014,0.71,-0.011,0.015,-0.016,0.00055,0.0033,-3.7e+02,-0.0013,-0.0057,4.3e-05,0.00041,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00031,0.00031,0.0023,0.1,0.1,0.028,0.06,0.06,0.059,5.2e-05,5.2e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12390000,0.7,0.0014,-0.014,0.71,-0.0051,0.011,-0.015,0.003,0.0016,-3.7e+02,-0.0012,-0.0058,4.1e-05,0.00057,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00027,0.00027,0.0023,0.079,0.079,0.026,0.05,0.05,0.057,4.8e-05,4.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12490000,0.7,0.0014,-0.014,0.71,-0.0063,0.013,-0.018,0.0025,0.0029,-3.7e+02,-0.0012,-0.0058,4.1e-05,0.00057,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00028,0.00028,0.0023,0.093,0.093,0.026,0.059,0.059,0.057,4.8e-05,4.8e-05,6.3e-06,0.037,0.037,0.01,0,0,0,0,0,0,0,0 +12590000,0.7,0.0015,-0.014,0.71,-0.014,0.011,-0.023,-0.0028,0.0014,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.0007,0.0012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00025,0.00025,0.0023,0.074,0.074,0.025,0.05,0.05,0.055,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0099,0,0,0,0,0,0,0,0 +12690000,0.7,0.0015,-0.014,0.71,-0.014,0.012,-0.027,-0.0042,0.0026,-3.7e+02,-0.0013,-0.0058,4.3e-05,0.00073,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00026,0.00026,0.0023,0.085,0.085,0.025,0.058,0.058,0.055,4.5e-05,4.5e-05,6.3e-06,0.037,0.037,0.0098,0,0,0,0,0,0,0,0 +12790000,0.7,0.0016,-0.013,0.71,-0.02,0.0091,-0.03,-0.0076,0.0013,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.00077,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00023,0.00023,0.0023,0.068,0.068,0.024,0.049,0.049,0.053,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0097,0,0,0,0,0,0,0,0 +12890000,0.7,0.0015,-0.013,0.71,-0.021,0.0091,-0.029,-0.0096,0.0022,-3.7e+02,-0.0014,-0.0059,4.3e-05,0.0007,0.0011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00024,0.00024,0.0023,0.079,0.079,0.025,0.058,0.058,0.054,4.2e-05,4.2e-05,6.3e-06,0.037,0.037,0.0096,0,0,0,0,0,0,0,0 +12990000,0.71,0.0012,-0.014,0.71,-0.0084,0.0066,-0.03,-0.001,0.0012,-3.7e+02,-0.0012,-0.0059,4e-05,0.00068,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00022,0.00022,0.0023,0.064,0.064,0.025,0.049,0.049,0.052,4e-05,4e-05,6.3e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 +13090000,0.7,0.0012,-0.014,0.71,-0.0091,0.0067,-0.03,-0.0019,0.0018,-3.7e+02,-0.0012,-0.0059,4e-05,0.00065,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00023,0.00023,0.0023,0.073,0.073,0.025,0.058,0.058,0.052,4e-05,4e-05,6.3e-06,0.037,0.037,0.0094,0,0,0,0,0,0,0,0 +13190000,0.7,0.00093,-0.014,0.71,1.1e-05,0.0061,-0.027,0.0044,0.0012,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.0005,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.06,0.06,0.025,0.049,0.049,0.051,3.8e-05,3.8e-05,6.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 +13290000,0.71,0.00094,-0.014,0.71,-0.00023,0.0069,-0.024,0.0044,0.0018,-3.7e+02,-0.0012,-0.0059,3.8e-05,0.00037,0.0015,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00022,0.00022,0.0023,0.068,0.068,0.027,0.057,0.057,0.051,3.8e-05,3.8e-05,6.3e-06,0.037,0.037,0.0091,0,0,0,0,0,0,0,0 +13390000,0.71,0.00079,-0.014,0.71,0.00059,0.0059,-0.02,0.0033,0.0011,-3.7e+02,-0.0012,-0.0059,3.7e-05,0.0002,0.0015,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.056,0.056,0.026,0.049,0.049,0.05,3.6e-05,3.6e-05,6.3e-06,0.037,0.037,0.0088,0,0,0,0,0,0,0,0 +13490000,0.71,0.00082,-0.014,0.71,9.8e-05,0.0058,-0.019,0.0034,0.0016,-3.7e+02,-0.0012,-0.0059,3.7e-05,0.00011,0.0016,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.064,0.064,0.028,0.057,0.057,0.05,3.6e-05,3.6e-05,6.3e-06,0.037,0.037,0.0087,0,0,0,0,0,0,0,0 +13590000,0.71,0.00076,-0.014,0.71,0.00035,0.0061,-0.021,0.0024,0.001,-3.7e+02,-0.0012,-0.006,3.7e-05,8.6e-05,0.0015,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.053,0.053,0.028,0.049,0.049,0.05,3.4e-05,3.4e-05,6.3e-06,0.036,0.036,0.0084,0,0,0,0,0,0,0,0 +13690000,0.71,0.00074,-0.014,0.71,0.001,0.0078,-0.025,0.0025,0.0017,-3.7e+02,-0.0012,-0.006,3.7e-05,0.00011,0.0014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.06,0.06,0.029,0.056,0.056,0.05,3.4e-05,3.4e-05,6.3e-06,0.036,0.036,0.0083,0,0,0,0,0,0,0,0 +13790000,0.71,0.00062,-0.014,0.71,0.0016,0.0036,-0.027,0.0035,-0.00064,-3.7e+02,-0.0011,-0.006,3.6e-05,-0.0001,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.051,0.051,0.029,0.048,0.048,0.049,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0079,0,0,0,0,0,0,0,0 +13890000,0.71,0.00059,-0.014,0.71,0.0021,0.0035,-0.031,0.0037,-0.0003,-3.7e+02,-0.0011,-0.006,3.6e-05,-7.6e-05,0.0013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00021,0.00021,0.0023,0.057,0.057,0.03,0.056,0.056,0.05,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0078,0,0,0,0,0,0,0,0 +13990000,0.71,0.00052,-0.014,0.71,0.0024,0.001,-0.03,0.0045,-0.002,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00034,0.0012,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.048,0.048,0.03,0.048,0.048,0.05,3.1e-05,3.1e-05,6.3e-06,0.036,0.036,0.0074,0,0,0,0,0,0,0,0 +14090000,0.71,0.0005,-0.014,0.71,0.0025,0.0012,-0.031,0.0047,-0.0019,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00033,0.0012,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.055,0.055,0.031,0.055,0.055,0.05,3.1e-05,3.1e-05,6.3e-06,0.036,0.036,0.0073,0,0,0,0,0,0,0,0 +14190000,0.71,0.0004,-0.014,0.71,0.0058,0.00063,-0.033,0.0068,-0.0017,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00032,0.00089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.046,0.046,0.031,0.048,0.048,0.05,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0069,0,0,0,0,0,0,0,0 +14290000,0.71,0.00041,-0.014,0.71,0.0066,0.0014,-0.032,0.0074,-0.0016,-3.7e+02,-0.0011,-0.006,3.5e-05,-0.00039,0.00094,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.052,0.052,0.032,0.055,0.055,0.051,2.9e-05,2.9e-05,6.3e-06,0.036,0.036,0.0067,0,0,0,0,0,0,0,0 +14390000,0.71,0.00033,-0.014,0.71,0.0084,0.0023,-0.034,0.0088,-0.0013,-3.7e+02,-0.0011,-0.006,3.4e-05,-0.0004,0.00069,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.031,0.048,0.048,0.05,2.8e-05,2.8e-05,6.3e-06,0.036,0.036,0.0063,0,0,0,0,0,0,0,0 +14490000,0.71,0.00031,-0.013,0.71,0.0084,0.0035,-0.037,0.0096,-0.0011,-3.7e+02,-0.0011,-0.006,3.4e-05,-0.00034,0.00064,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.05,0.05,0.032,0.055,0.055,0.051,2.8e-05,2.8e-05,6.3e-06,0.036,0.036,0.0062,0,0,0,0,0,0,0,0 +14590000,0.71,0.0003,-0.013,0.71,0.0049,0.0019,-0.037,0.006,-0.0025,-3.7e+02,-0.0011,-0.0061,3.5e-05,-0.00072,0.0011,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.043,0.043,0.031,0.047,0.047,0.051,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0058,0,0,0,0,0,0,0,0 +14690000,0.71,0.00026,-0.013,0.71,0.0063,-0.001,-0.034,0.0066,-0.0024,-3.7e+02,-0.0011,-0.0061,3.5e-05,-0.00082,0.0011,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.0002,0.0023,0.049,0.049,0.032,0.054,0.054,0.051,2.6e-05,2.6e-05,6.3e-06,0.036,0.036,0.0056,0,0,0,0,0,0,0,0 +14790000,0.71,0.00029,-0.013,0.71,0.0031,-0.0026,-0.03,0.0037,-0.0034,-3.7e+02,-0.0011,-0.0061,3.6e-05,-0.0012,0.0016,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.042,0.042,0.031,0.047,0.047,0.051,2.5e-05,2.5e-05,6.3e-06,0.036,0.036,0.0053,0,0,0,0,0,0,0,0 +14890000,0.71,0.00028,-0.013,0.71,0.0046,-0.0017,-0.033,0.0041,-0.0036,-3.7e+02,-0.0011,-0.0061,3.6e-05,-0.0012,0.0016,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.0002,0.00019,0.0023,0.047,0.047,0.031,0.054,0.054,0.052,2.5e-05,2.5e-05,6.3e-06,0.036,0.036,0.0051,0,0,0,0,0,0,0,0 +14990000,0.71,0.00028,-0.013,0.71,0.0034,-0.0018,-0.029,0.0032,-0.0029,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0013,0.0019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.041,0.041,0.03,0.047,0.047,0.051,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0048,0,0,0,0,0,0,0,0 +15090000,0.71,0.0002,-0.013,0.71,0.0038,-0.002,-0.032,0.0035,-0.0031,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0012,0.0018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.046,0.046,0.031,0.054,0.054,0.052,2.3e-05,2.3e-05,6.3e-06,0.036,0.036,0.0046,0,0,0,0,0,0,0,0 +15190000,0.71,0.00022,-0.013,0.71,0.0032,-0.00079,-0.029,0.0028,-0.0024,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0012,0.002,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.04,0.04,0.03,0.047,0.047,0.052,2.2e-05,2.2e-05,6.3e-06,0.035,0.035,0.0043,0,0,0,0,0,0,0,0 +15290000,0.71,0.00018,-0.013,0.71,0.0038,-0.00063,-0.027,0.0031,-0.0025,-3.7e+02,-0.0012,-0.0061,3.6e-05,-0.0013,0.0021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.03,0.054,0.054,0.052,2.2e-05,2.2e-05,6.3e-06,0.035,0.035,0.0042,0,0,0,0,0,0,0,0 +15390000,0.71,0.00019,-0.013,0.71,0.003,-0.00028,-0.024,0.00057,-0.002,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0014,0.0023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.04,0.04,0.029,0.047,0.047,0.051,2e-05,2e-05,6.3e-06,0.035,0.035,0.0039,0,0,0,0,0,0,0,0 +15490000,0.71,0.0002,-0.013,0.71,0.0043,-0.00067,-0.024,0.00095,-0.0021,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00019,0.0023,0.045,0.045,0.029,0.053,0.053,0.053,2e-05,2e-05,6.3e-06,0.035,0.035,0.0037,0,0,0,0,0,0,0,0 +15590000,0.71,0.00022,-0.013,0.71,0.0024,-0.00066,-0.023,-0.0013,-0.0017,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.039,0.039,0.028,0.046,0.046,0.052,1.9e-05,1.9e-05,6.3e-06,0.035,0.035,0.0035,0,0,0,0,0,0,0,0 +15690000,0.71,0.00022,-0.013,0.71,0.0027,-0.00083,-0.023,-0.0011,-0.0018,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0013,0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00019,0.00018,0.0023,0.044,0.044,0.028,0.053,0.053,0.052,1.9e-05,1.9e-05,6.3e-06,0.035,0.035,0.0033,0,0,0,0,0,0,0,0 +15790000,0.71,0.00018,-0.013,0.71,0.0032,-0.0025,-0.026,-0.00095,-0.0028,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0016,0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.038,0.038,0.027,0.046,0.046,0.051,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.0031,0,0,0,0,0,0,0,0 +15890000,0.71,0.00014,-0.013,0.71,0.0041,-0.003,-0.024,-0.00056,-0.0031,-3.7e+02,-0.0012,-0.0061,3.7e-05,-0.0016,0.0025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.043,0.043,0.027,0.053,0.053,0.052,1.7e-05,1.7e-05,6.3e-06,0.035,0.035,0.003,0,0,0,0,0,0,0,0 +15990000,0.71,8e-05,-0.013,0.71,0.004,-0.0039,-0.019,-0.00063,-0.0039,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0019,0.0027,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.038,0.038,0.026,0.046,0.046,0.051,1.6e-05,1.6e-05,6.3e-06,0.034,0.034,0.0028,0,0,0,0,0,0,0,0 +16090000,0.71,8.2e-05,-0.013,0.71,0.0057,-0.0041,-0.016,-0.00016,-0.0043,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.002,0.0028,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00018,0.00018,0.0023,0.043,0.043,0.025,0.053,0.053,0.052,1.6e-05,1.6e-05,6.3e-06,0.034,0.034,0.0027,0,0,0,0,0,0,0,0 +16190000,0.71,0.00011,-0.013,0.71,0.0057,-0.0033,-0.014,-0.00037,-0.0035,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0019,0.003,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.037,0.037,0.025,0.046,0.046,0.051,1.5e-05,1.5e-05,6.3e-06,0.034,0.034,0.0025,0,0,0,0,0,0,0,0 +16290000,0.71,0.00013,-0.013,0.71,0.0073,-0.0041,-0.016,0.00029,-0.0038,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0018,0.003,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.042,0.042,0.024,0.053,0.053,0.052,1.5e-05,1.5e-05,6.3e-06,0.034,0.034,0.0024,0,0,0,0,0,0,0,0 +16390000,0.71,0.00012,-0.013,0.71,0.0062,-0.0044,-0.015,-4.3e-05,-0.0031,-3.7e+02,-0.0013,-0.0061,3.8e-05,-0.0017,0.0033,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.037,0.037,0.023,0.046,0.046,0.051,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0022,0,0,0,0,0,0,0,0 +16490000,0.71,0.00013,-0.013,0.71,0.0054,-0.0039,-0.018,0.00051,-0.0035,-3.7e+02,-0.0012,-0.0061,3.8e-05,-0.0017,0.0032,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00017,0.0023,0.041,0.041,0.023,0.052,0.052,0.052,1.4e-05,1.4e-05,6.3e-06,0.034,0.034,0.0021,0,0,0,0,0,0,0,0 +16590000,0.71,0.00039,-0.013,0.71,0.0018,-0.0012,-0.018,-0.0024,-8.3e-05,-3.7e+02,-0.0013,-0.006,4e-05,-0.001,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.036,0.036,0.022,0.046,0.046,0.051,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.002,0,0,0,0,0,0,0,0 +16690000,0.71,0.00038,-0.013,0.71,0.002,-0.00073,-0.015,-0.0022,-0.00018,-3.7e+02,-0.0013,-0.006,4e-05,-0.0011,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00017,0.00016,0.0023,0.041,0.041,0.022,0.052,0.052,0.051,1.2e-05,1.2e-05,6.3e-06,0.033,0.033,0.0019,0,0,0,0,0,0,0,0 +16790000,0.71,0.00053,-0.013,0.71,-0.0013,0.0015,-0.014,-0.0046,0.0025,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0005,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.036,0.036,0.021,0.046,0.046,0.05,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0018,0,0,0,0,0,0,0,0 +16890000,0.71,0.00054,-0.013,0.71,-0.0016,0.0023,-0.011,-0.0047,0.0027,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.00054,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00016,0.00016,0.0023,0.04,0.04,0.021,0.052,0.052,0.051,1.1e-05,1.1e-05,6.3e-06,0.033,0.033,0.0017,0,0,0,0,0,0,0,0 +16990000,0.71,0.00048,-0.013,0.71,-0.0016,0.00033,-0.01,-0.0052,0.00085,-3.7e+02,-0.0013,-0.006,4.2e-05,-0.00095,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,0.00015,0.00015,0.0023,0.035,0.035,0.02,0.046,0.046,0.05,1e-05,1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 +17090000,0.71,0.00045,-0.013,0.71,-0.0008,0.0013,-0.01,-0.0053,0.0009,-3.7e+02,-0.0013,-0.006,4.2e-05,-0.00094,0.0051,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00016,0.00016,0.0023,0.039,0.039,0.02,0.052,0.052,0.05,1e-05,1e-05,6.3e-06,0.033,0.033,0.0016,0,0,0,0,0,0,0,0 +17190000,0.71,0.00044,-0.013,0.71,-0.00035,0.0013,-0.011,-0.0056,-0.00053,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0013,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.035,0.035,0.019,0.046,0.046,0.049,9.4e-06,9.4e-06,6.3e-06,0.032,0.032,0.0015,0,0,0,0,0,0,0,0 +17290000,0.71,0.00041,-0.013,0.71,0.0017,0.0023,-0.0066,-0.0056,-0.00037,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0013,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.039,0.039,0.019,0.052,0.052,0.049,9.4e-06,9.4e-06,6.3e-06,0.032,0.032,0.0014,0,0,0,0,0,0,0,0 +17390000,0.71,0.00038,-0.013,0.71,0.0024,0.0015,-0.0047,-0.0046,-0.0016,-3.7e+02,-0.0014,-0.006,4.1e-05,-0.0017,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.034,0.034,0.018,0.046,0.046,0.048,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 +17490000,0.71,0.00037,-0.013,0.71,0.0029,0.0011,-0.003,-0.0044,-0.0015,-3.7e+02,-0.0014,-0.006,4.1e-05,-0.0017,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00015,0.00015,0.0023,0.038,0.038,0.018,0.052,0.052,0.049,8.5e-06,8.5e-06,6.3e-06,0.032,0.032,0.0013,0,0,0,0,0,0,0,0 +17590000,0.71,0.00029,-0.013,0.71,0.0042,-0.0001,0.0025,-0.0037,-0.0026,-3.7e+02,-0.0014,-0.0061,4.1e-05,-0.002,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.034,0.034,0.017,0.045,0.045,0.048,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 +17690000,0.71,0.00026,-0.013,0.71,0.0051,0.0006,0.0019,-0.0032,-0.0026,-3.7e+02,-0.0014,-0.0061,4.1e-05,-0.002,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.037,0.037,0.017,0.052,0.052,0.048,7.7e-06,7.7e-06,6.3e-06,0.032,0.032,0.0012,0,0,0,0,0,0,0,0 +17790000,0.71,0.00017,-0.013,0.71,0.0076,0.00033,0.00059,-0.0021,-0.0022,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0023,0.033,0.033,0.016,0.045,0.045,0.048,7e-06,7e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 +17890000,0.71,0.00018,-0.013,0.71,0.0092,-0.00043,0.00069,-0.0012,-0.0022,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0024,0.037,0.037,0.016,0.052,0.052,0.048,7e-06,7e-06,6.3e-06,0.032,0.032,0.0011,0,0,0,0,0,0,0,0 +17990000,0.71,0.00012,-0.013,0.71,0.011,-0.0022,0.0019,-0.00052,-0.0019,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.0019,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.032,0.032,0.016,0.045,0.045,0.047,6.4e-06,6.4e-06,6.3e-06,0.031,0.031,0.001,0,0,0,0,0,0,0,0 +18090000,0.71,0.00013,-0.013,0.71,0.012,-0.0023,0.0043,0.0006,-0.0021,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.0019,0.0048,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00014,0.00014,0.0024,0.036,0.036,0.016,0.051,0.051,0.047,6.4e-06,6.4e-06,6.3e-06,0.031,0.031,0.00097,0,0,0,0,0,0,0,0 +18190000,0.71,9.6e-05,-0.013,0.71,0.012,-0.0013,0.0056,0.0015,-0.0017,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.0019,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.032,0.032,0.015,0.045,0.045,0.047,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.00092,0,0,0,0,0,0,0,0 +18290000,0.71,3.7e-05,-0.012,0.71,0.012,-0.0018,0.0068,0.0027,-0.0018,-3.7e+02,-0.0013,-0.0061,4.1e-05,-0.0019,0.0049,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.035,0.035,0.015,0.051,0.051,0.046,5.8e-06,5.8e-06,6.3e-06,0.031,0.031,0.00089,0,0,0,0,0,0,0,0 +18390000,0.71,5.4e-05,-0.013,0.71,0.013,-0.00018,0.008,0.0032,-0.0014,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0018,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.031,0.031,0.014,0.045,0.045,0.046,5.2e-06,5.2e-06,6.3e-06,0.031,0.031,0.00084,0,0,0,0,0,0,0,0 +18490000,0.71,7e-05,-0.012,0.71,0.014,0.00024,0.0076,0.0047,-0.0014,-3.7e+02,-0.0013,-0.006,4.1e-05,-0.0018,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.034,0.034,0.014,0.051,0.051,0.046,5.2e-06,5.2e-06,6.3e-06,0.031,0.031,0.00082,0,0,0,0,0,0,0,0 +18590000,0.71,7.7e-05,-0.012,0.71,0.013,0.00048,0.0058,0.0035,-0.0012,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.03,0.03,0.014,0.045,0.045,0.045,4.8e-06,4.8e-06,6.3e-06,0.031,0.031,0.00078,0,0,0,0,0,0,0,0 +18690000,0.71,4.5e-05,-0.012,0.71,0.014,-0.00022,0.0039,0.0049,-0.0011,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00013,0.00013,0.0024,0.033,0.033,0.013,0.051,0.051,0.045,4.8e-06,4.8e-06,6.3e-06,0.031,0.031,0.00075,0,0,0,0,0,0,0,0 +18790000,0.71,7.5e-05,-0.012,0.71,0.012,8.9e-05,0.0035,0.0037,-0.0009,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.03,0.03,0.013,0.045,0.045,0.045,4.3e-06,4.3e-06,6.3e-06,0.031,0.031,0.00072,0,0,0,0,0,0,0,0 +18890000,0.71,0.0001,-0.012,0.71,0.013,0.00059,0.0042,0.005,-0.00083,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0057,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.033,0.033,0.013,0.051,0.051,0.045,4.3e-06,4.3e-06,6.3e-06,0.031,0.031,0.0007,0,0,0,0,0,0,0,0 +18990000,0.71,8.9e-05,-0.012,0.71,0.014,0.0015,0.0028,0.0063,-0.00069,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.029,0.029,0.012,0.045,0.045,0.044,3.9e-06,3.9e-06,6.3e-06,0.031,0.031,0.00066,0,0,0,0,0,0,0,0 +19090000,0.71,7.3e-05,-0.012,0.71,0.015,0.0021,0.0058,0.0078,-0.00049,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0018,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.032,0.032,0.012,0.051,0.051,0.044,3.9e-06,3.9e-06,6.3e-06,0.031,0.031,0.00065,0,0,0,0,0,0,0,0 +19190000,0.71,7.7e-05,-0.012,0.71,0.015,0.0021,0.0059,0.0086,-0.00044,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.028,0.028,0.012,0.045,0.045,0.044,3.6e-06,3.6e-06,6.3e-06,0.03,0.03,0.00062,0,0,0,0,0,0,0,0 +19290000,0.71,9.9e-05,-0.012,0.71,0.015,0.0013,0.0086,0.01,-0.00025,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.0056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00012,0.00012,0.0024,0.031,0.031,0.012,0.05,0.05,0.044,3.6e-06,3.6e-06,6.3e-06,0.03,0.03,0.0006,0,0,0,0,0,0,0,0 +19390000,0.71,0.00011,-0.012,0.71,0.013,0.00041,0.012,0.008,-0.00027,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.012,0.044,0.044,0.043,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.00058,0,0,0,0,0,0,0,0 +19490000,0.71,0.00014,-0.012,0.71,0.012,-0.00031,0.0088,0.0092,-0.00027,-3.7e+02,-0.0014,-0.006,4.2e-05,-0.0019,0.006,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.03,0.03,0.011,0.05,0.05,0.043,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.00056,0,0,0,0,0,0,0,0 +19590000,0.71,0.00019,-0.012,0.71,0.0096,-0.0013,0.0081,0.0075,-0.00029,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.011,0.044,0.044,0.042,3e-06,3e-06,6.3e-06,0.03,0.03,0.00054,0,0,0,0,0,0,0,0 +19690000,0.71,0.00019,-0.012,0.71,0.01,-0.0035,0.0096,0.0084,-0.00054,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.029,0.029,0.011,0.05,0.05,0.042,3e-06,3e-06,6.3e-06,0.03,0.03,0.00052,0,0,0,0,0,0,0,0 +19790000,0.71,0.00025,-0.012,0.71,0.0077,-0.0044,0.01,0.0068,-0.00043,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.026,0.026,0.011,0.044,0.044,0.042,2.7e-06,2.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19890000,0.71,0.0002,-0.012,0.71,0.0065,-0.0047,0.011,0.0075,-0.0009,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0019,0.0065,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.029,0.029,0.011,0.05,0.05,0.042,2.7e-06,2.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19990000,0.71,0.00019,-0.012,0.71,0.004,-0.0054,0.014,0.0061,-0.00075,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0018,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.025,0.026,0.01,0.044,0.044,0.041,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20090000,0.71,0.00019,-0.012,0.71,0.0038,-0.0073,0.014,0.0065,-0.0014,-3.7e+02,-0.0014,-0.006,4.3e-05,-0.0018,0.0067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.028,0.028,0.01,0.049,0.049,0.042,2.5e-06,2.5e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20190000,0.71,0.0003,-0.012,0.71,0.0015,-0.008,0.017,0.0042,-0.0011,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0017,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.0001,0.0024,0.025,0.025,0.01,0.044,0.044,0.041,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20290000,0.71,0.00026,-0.012,0.71,0.00034,-0.0096,0.015,0.0043,-0.002,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0017,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.00011,0.00011,0.0024,0.027,0.027,0.0099,0.049,0.049,0.041,2.3e-06,2.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20390000,0.71,0.00028,-0.012,0.71,-0.0021,-0.01,0.017,0.0024,-0.0015,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0016,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.024,0.024,0.0097,0.044,0.044,0.041,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20490000,0.71,0.00033,-0.012,0.71,-0.0026,-0.011,0.016,0.0022,-0.0026,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0016,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.026,0.026,0.0096,0.049,0.049,0.041,2.1e-06,2.1e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20590000,0.71,0.00035,-0.012,0.71,-0.0022,-0.011,0.013,0.0019,-0.0021,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0014,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.024,0.024,0.0094,0.044,0.044,0.04,1.9e-06,1.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20690000,0.71,0.00038,-0.012,0.71,-0.0022,-0.012,0.015,0.0016,-0.0032,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0014,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.026,0.026,0.0093,0.049,0.049,0.04,1.9e-06,1.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20790000,0.71,0.00041,-0.012,0.71,-0.0033,-0.011,0.015,0.0014,-0.0025,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0012,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.023,0.023,0.0091,0.043,0.043,0.04,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20890000,0.71,0.0004,-0.012,0.71,-0.0038,-0.014,0.014,0.001,-0.0038,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.0012,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,0.0001,0.0024,0.025,0.025,0.0091,0.049,0.049,0.04,1.8e-06,1.8e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +20990000,0.71,0.0004,-0.012,0.71,-0.004,-0.014,0.015,0.0027,-0.0031,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00098,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.9e-05,9.8e-05,0.0024,0.023,0.023,0.0089,0.043,0.043,0.039,1.7e-06,1.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +21090000,0.71,0.0004,-0.012,0.71,-0.0041,-0.017,0.015,0.0022,-0.0047,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00098,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,0.0001,9.9e-05,0.0024,0.025,0.025,0.0089,0.048,0.048,0.039,1.7e-06,1.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +21190000,0.71,0.00044,-0.012,0.71,-0.0034,-0.016,0.014,0.0037,-0.0038,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00074,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.8e-05,9.7e-05,0.0024,0.022,0.022,0.0087,0.043,0.043,0.039,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21290000,0.71,0.00048,-0.012,0.71,-0.0039,-0.018,0.016,0.0034,-0.0055,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00073,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.9e-05,9.8e-05,0.0024,0.024,0.024,0.0086,0.048,0.048,0.039,1.5e-06,1.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21390000,0.71,0.00053,-0.012,0.71,-0.0047,-0.017,0.016,0.0028,-0.0034,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00036,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.6e-05,0.0024,0.022,0.022,0.0085,0.043,0.043,0.039,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21490000,0.71,0.00054,-0.012,0.71,-0.0053,-0.018,0.015,0.0023,-0.0052,-3.7e+02,-0.0014,-0.006,4.4e-05,-0.00036,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.7e-05,9.7e-05,0.0024,0.023,0.023,0.0085,0.048,0.048,0.038,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21590000,0.71,0.00056,-0.012,0.71,-0.0058,-0.016,0.015,0.0019,-0.0032,-3.7e+02,-0.0014,-0.006,4.5e-05,-7.6e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0024,0.021,0.021,0.0083,0.043,0.043,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21690000,0.71,0.00057,-0.012,0.71,-0.0057,-0.017,0.017,0.0013,-0.0048,-3.7e+02,-0.0014,-0.006,4.5e-05,-8.8e-06,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.6e-05,9.5e-05,0.0024,0.023,0.023,0.0084,0.048,0.048,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21790000,0.71,0.00059,-0.012,0.71,-0.0063,-0.011,0.015,8.8e-05,-0.00078,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00052,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0024,0.021,0.021,0.0082,0.042,0.042,0.038,1.2e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21890000,0.71,0.00059,-0.012,0.71,-0.0063,-0.012,0.016,-0.00054,-0.0019,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00051,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,9.5e-05,9.4e-05,0.0024,0.022,0.022,0.0082,0.047,0.047,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21990000,0.71,0.00065,-0.012,0.71,-0.0068,-0.0092,0.016,-0.0015,0.0014,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00095,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.4e-05,9.3e-05,0.0024,0.02,0.02,0.0081,0.042,0.042,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22090000,0.71,0.00066,-0.012,0.71,-0.0071,-0.0083,0.015,-0.0021,0.00059,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.00095,0.0072,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.4e-05,9.3e-05,0.0024,0.022,0.022,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22190000,0.71,0.00063,-0.012,0.71,-0.0069,-0.0074,0.015,-0.0018,0.00056,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.001,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.2e-05,0.0024,0.02,0.02,0.008,0.042,0.042,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22290000,0.71,0.00067,-0.012,0.71,-0.0083,-0.0081,0.015,-0.0025,-0.00023,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.001,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.3e-05,0.0024,0.021,0.021,0.008,0.047,0.047,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22390000,0.71,0.00064,-0.012,0.71,-0.0089,-0.0076,0.017,-0.0022,-0.00021,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0011,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.2e-05,9.1e-05,0.0024,0.019,0.019,0.0079,0.042,0.042,0.037,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22490000,0.71,0.00065,-0.012,0.71,-0.0095,-0.0075,0.018,-0.0031,-0.00099,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0011,0.007,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.3e-05,9.2e-05,0.0025,0.021,0.021,0.0079,0.047,0.047,0.037,1e-06,1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22590000,0.71,0.00063,-0.012,0.71,-0.0092,-0.007,0.017,-0.0034,0.0001,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9.1e-05,0.0025,0.019,0.019,0.0078,0.042,0.042,0.036,9.7e-07,9.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22690000,0.71,0.00067,-0.012,0.71,-0.01,-0.0068,0.018,-0.0044,-0.00059,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.2e-05,9.1e-05,0.0025,0.02,0.02,0.0079,0.046,0.046,0.037,9.7e-07,9.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22790000,0.71,0.00065,-0.012,0.71,-0.011,-0.0056,0.019,-0.0055,-0.00047,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9e-05,0.0025,0.019,0.019,0.0078,0.042,0.042,0.036,9.2e-07,9.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22890000,0.71,0.00066,-0.012,0.71,-0.012,-0.0052,0.021,-0.0066,-0.001,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0013,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9.1e-05,9e-05,0.0025,0.02,0.02,0.0078,0.046,0.046,0.036,9.2e-07,9.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +22990000,0.71,0.00064,-0.012,0.71,-0.012,-0.0057,0.022,-0.0074,-0.0009,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0014,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,8.9e-05,0.0025,0.018,0.018,0.0078,0.041,0.041,0.036,8.7e-07,8.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23090000,0.71,0.00061,-0.012,0.71,-0.013,-0.0057,0.022,-0.0087,-0.0014,-3.7e+02,-0.0014,-0.0059,4.5e-05,0.0014,0.0068,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,9e-05,0.0025,0.02,0.02,0.0078,0.046,0.046,0.036,8.7e-07,8.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23190000,0.71,0.00067,-0.012,0.71,-0.015,-0.0066,0.024,-0.012,-0.0013,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.9e-05,0.0025,0.018,0.018,0.0077,0.041,0.041,0.036,8.2e-07,8.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23290000,0.71,0.00061,-0.012,0.71,-0.015,-0.0079,0.024,-0.013,-0.002,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0014,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,9e-05,8.9e-05,0.0025,0.019,0.019,0.0078,0.046,0.046,0.036,8.2e-07,8.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23390000,0.71,0.00071,-0.012,0.71,-0.016,-0.0081,0.022,-0.016,-0.0018,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0015,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.8e-05,0.0025,0.018,0.018,0.0077,0.041,0.041,0.036,7.8e-07,7.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23490000,0.71,0.0031,-0.0096,0.71,-0.023,-0.009,-0.012,-0.018,-0.0027,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0015,0.0071,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.9e-05,8.8e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.036,7.8e-07,7.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23590000,0.71,0.0083,-0.0017,0.71,-0.034,-0.0078,-0.044,-0.017,-0.0013,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0017,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.8e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,7.4e-07,7.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23690000,0.7,0.0079,0.004,0.71,-0.065,-0.017,-0.094,-0.021,-0.0025,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0017,0.0069,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.8e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.036,7.4e-07,7.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23790000,0.7,0.005,0.00066,0.71,-0.089,-0.028,-0.15,-0.021,-0.0018,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.7e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,7.1e-07,7.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23890000,0.7,0.0024,-0.0054,0.71,-0.11,-0.037,-0.2,-0.03,-0.0051,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0019,0.0063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.8e-05,8.7e-05,0.0025,0.019,0.019,0.0078,0.045,0.045,0.035,7.1e-07,7.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +23990000,0.7,0.00099,-0.01,0.71,-0.11,-0.04,-0.25,-0.034,-0.0083,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0021,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.6e-05,0.0025,0.017,0.017,0.0077,0.041,0.041,0.035,6.8e-07,6.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24090000,0.7,0.0023,-0.0088,0.71,-0.11,-0.04,-0.3,-0.045,-0.012,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0021,0.0059,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.7e-05,8.7e-05,0.0025,0.018,0.019,0.0078,0.045,0.045,0.035,6.8e-07,6.8e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24190000,0.7,0.0033,-0.0065,0.71,-0.11,-0.041,-0.35,-0.046,-0.014,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0024,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.017,0.017,0.0077,0.04,0.04,0.035,6.5e-07,6.4e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24290000,0.7,0.0038,-0.0057,0.71,-0.12,-0.045,-0.41,-0.058,-0.019,-3.7e+02,-0.0014,-0.0059,4.6e-05,0.0024,0.0053,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.018,0.018,0.0078,0.045,0.045,0.036,6.5e-07,6.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24390000,0.7,0.0039,-0.0059,0.71,-0.13,-0.053,-0.46,-0.064,-0.03,-3.7e+02,-0.0014,-0.0059,4.4e-05,0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.5e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,6.2e-07,6.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24490000,0.7,0.0047,-0.0017,0.71,-0.14,-0.058,-0.51,-0.077,-0.036,-3.7e+02,-0.0014,-0.0059,4.4e-05,0.002,0.005,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.6e-05,8.6e-05,0.0025,0.018,0.018,0.0078,0.045,0.045,0.035,6.2e-07,6.2e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24590000,0.7,0.0052,0.0019,0.71,-0.16,-0.069,-0.56,-0.081,-0.045,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.002,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.5e-05,8.5e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,5.9e-07,5.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24690000,0.7,0.0052,0.0028,0.71,-0.18,-0.083,-0.64,-0.098,-0.053,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.002,0.0042,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.5e-05,8.5e-05,0.0025,0.018,0.018,0.0078,0.044,0.044,0.035,5.9e-07,5.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24790000,0.7,0.0049,0.0014,0.71,-0.2,-0.095,-0.72,-0.1,-0.064,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0024,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.017,0.017,0.0078,0.04,0.04,0.035,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24890000,0.7,0.0067,0.0031,0.71,-0.22,-0.11,-0.75,-0.13,-0.074,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0024,0.0034,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.018,0.018,0.0078,0.044,0.044,0.035,5.7e-07,5.7e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +24990000,0.7,0.0085,0.0047,0.71,-0.24,-0.11,-0.81,-0.13,-0.082,-3.7e+02,-0.0013,-0.0059,4.5e-05,0.0033,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.4e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.5e-07,5.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25090000,0.7,0.0088,0.0041,0.71,-0.27,-0.13,-0.85,-0.15,-0.094,-3.7e+02,-0.0013,-0.0059,4.5e-05,0.0033,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.4e-05,8.4e-05,0.0025,0.018,0.019,0.0079,0.044,0.044,0.035,5.5e-07,5.5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25190000,0.7,0.0082,0.0027,0.71,-0.29,-0.14,-0.91,-0.17,-0.12,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0029,0.0014,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.3e-07,5.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25290000,0.7,0.01,0.0095,0.71,-0.32,-0.15,-0.96,-0.2,-0.13,-3.7e+02,-0.0013,-0.0059,4.3e-05,0.0029,0.0013,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.017,0.019,0.0079,0.044,0.044,0.035,5.3e-07,5.3e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25390000,0.7,0.011,0.016,0.71,-0.35,-0.17,-1,-0.22,-0.15,-3.7e+02,-0.0012,-0.0059,4.1e-05,0.0032,-0.00021,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.2e-05,0.0025,0.016,0.017,0.0078,0.04,0.04,0.035,5.1e-07,5.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25490000,0.7,0.012,0.017,0.71,-0.4,-0.19,-1.1,-0.25,-0.17,-3.7e+02,-0.0012,-0.0059,4.1e-05,0.0032,-0.00019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.3e-05,0.0025,0.017,0.019,0.0079,0.044,0.044,0.035,5.1e-07,5.1e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25590000,0.7,0.011,0.015,0.71,-0.44,-0.22,-1.1,-0.28,-0.21,-3.7e+02,-0.0012,-0.0059,4e-05,0.0029,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.1e-05,8.1e-05,0.0024,0.016,0.018,0.0079,0.04,0.04,0.035,4.9e-07,4.9e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25690000,0.7,0.015,0.022,0.71,-0.49,-0.24,-1.2,-0.33,-0.23,-3.7e+02,-0.0012,-0.0059,4e-05,0.0029,-0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.2e-05,0.0024,0.017,0.02,0.0079,0.044,0.044,0.035,4.9e-07,5e-07,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +25790000,0.7,0.017,0.028,0.71,-0.53,-0.27,-1.2,-0.34,-0.26,-3.7e+02,-0.0012,-0.0059,4.2e-05,0.0038,-0.0037,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.2e-05,8.1e-05,0.0024,0.016,0.019,0.0079,0.04,0.04,0.035,4.8e-07,4.8e-07,6.3e-06,0.029,0.028,0.0005,0,0,0,0,0,0,0,0 +25890000,0.7,0.017,0.029,0.71,-0.6,-0.3,-1.3,-0.4,-0.29,-3.7e+02,-0.0012,-0.0059,4.2e-05,0.0037,-0.0037,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0025,8.3e-05,8.2e-05,0.0024,0.017,0.021,0.008,0.044,0.044,0.035,4.8e-07,4.8e-07,6.3e-06,0.029,0.028,0.0005,0,0,0,0,0,0,0,0 +25990000,0.7,0.016,0.025,0.71,-0.66,-0.33,-1.3,-0.44,-0.34,-3.7e+02,-0.0011,-0.0059,4e-05,0.0034,-0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.1e-05,8.1e-05,0.0024,0.016,0.02,0.0079,0.04,0.04,0.035,4.6e-07,4.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26090000,0.7,0.021,0.035,0.71,-0.72,-0.36,-1.3,-0.51,-0.38,-3.7e+02,-0.0011,-0.0059,4e-05,0.0034,-0.0054,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.3e-05,8.1e-05,0.0024,0.017,0.022,0.008,0.044,0.044,0.035,4.6e-07,4.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26190000,0.7,0.023,0.045,0.71,-0.77,-0.4,-1.3,-0.53,-0.42,-3.7e+02,-0.0011,-0.0059,4.3e-05,0.0045,-0.0093,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.4e-05,8.1e-05,0.0023,0.016,0.021,0.0079,0.039,0.04,0.035,4.5e-07,4.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26290000,0.7,0.024,0.047,0.71,-0.87,-0.44,-1.3,-0.62,-0.46,-3.7e+02,-0.0011,-0.0059,4.3e-05,0.0045,-0.0093,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0024,8.5e-05,8.1e-05,0.0023,0.017,0.024,0.008,0.043,0.045,0.035,4.5e-07,4.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26390000,0.7,0.023,0.043,0.71,-0.94,-0.5,-1.3,-0.68,-0.55,-3.7e+02,-0.001,-0.0059,4.2e-05,0.0033,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,8.2e-05,8e-05,0.0022,0.016,0.023,0.0079,0.039,0.04,0.035,4.4e-07,4.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26490000,0.7,0.031,0.059,0.71,-1,-0.54,-1.3,-0.78,-0.6,-3.7e+02,-0.001,-0.0059,4.2e-05,0.0033,-0.011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0023,8.7e-05,8.1e-05,0.0022,0.018,0.026,0.008,0.043,0.045,0.035,4.4e-07,4.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26590000,0.7,0.037,0.075,0.71,-1.1,-0.59,-1.3,-0.82,-0.67,-3.7e+02,-0.00096,-0.0059,5.1e-05,0.0041,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0022,9.2e-05,8.2e-05,0.0021,0.016,0.026,0.008,0.039,0.041,0.035,4.3e-07,4.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26690000,0.7,0.038,0.078,0.71,-1.3,-0.65,-1.3,-0.94,-0.73,-3.7e+02,-0.00096,-0.0059,5.1e-05,0.0041,-0.015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0022,9.3e-05,8.2e-05,0.0021,0.018,0.031,0.008,0.043,0.045,0.035,4.3e-07,4.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26790000,0.7,0.036,0.072,0.71,-1.4,-0.74,-1.3,-1,-0.86,-3.7e+02,-0.00092,-0.006,3.4e-05,0.0021,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.002,8.8e-05,8.1e-05,0.002,0.017,0.03,0.008,0.039,0.041,0.035,4.2e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26890000,0.7,0.044,0.094,0.71,-1.5,-0.8,-1.3,-1.2,-0.93,-3.7e+02,-0.00092,-0.006,3.4e-05,0.002,-0.017,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.002,9.8e-05,8.3e-05,0.002,0.019,0.036,0.0081,0.043,0.046,0.035,4.2e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +26990000,0.7,0.051,0.12,0.7,-1.7,-0.88,-1.3,-1.2,-1,-3.7e+02,-0.0008,-0.006,3.3e-05,0.0029,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0018,0.00011,8.3e-05,0.0018,0.018,0.035,0.008,0.039,0.042,0.035,4.1e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0.0025,0.0023,0.0025,0.0025,0.0025,0.0025,0,0 +27090000,0.7,0.052,0.12,0.7,-1.9,-0.97,-1.3,-1.4,-1.1,-3.7e+02,-0.0008,-0.006,3.3e-05,0.0028,-0.022,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0018,0.00011,8.3e-05,0.0018,0.02,0.043,0.0081,0.043,0.047,0.035,4.1e-07,4.4e-07,6.2e-06,0.028,0.028,0.0005,0.0025,0.0015,0.0025,0.0025,0.0025,0.0025,0,0 +27190000,0.71,0.049,0.11,0.7,-2.1,-1,-1.2,-1.6,-1.2,-3.7e+02,-0.00078,-0.0059,0.00013,0.0027,-0.023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0016,9.7e-05,8.1e-05,0.0016,0.02,0.044,0.0081,0.046,0.05,0.035,4.1e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.0011,0.0025,0.0025,0.0025,0.0025,0,0 +27290000,0.71,0.043,0.094,0.7,-2.2,-1.1,-1.2,-1.8,-1.3,-3.7e+02,-0.00078,-0.0059,0.00013,0.0027,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0016,9e-05,8e-05,0.0016,0.022,0.051,0.0081,0.051,0.057,0.035,4.1e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.0009,0.0025,0.0025,0.0025,0.0025,0,0 +27390000,0.72,0.037,0.077,0.69,-2.4,-1.1,-1.2,-2,-1.4,-3.7e+02,-0.00071,-0.0059,0.00019,0.0035,-0.025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0014,8.2e-05,7.8e-05,0.0014,0.021,0.047,0.0081,0.053,0.059,0.035,4e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.00074,0.0025,0.0025,0.0025,0.0025,0,0 +27490000,0.72,0.031,0.062,0.69,-2.4,-1.1,-1.2,-2.3,-1.5,-3.7e+02,-0.00071,-0.0059,0.00019,0.0034,-0.025,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0014,8e-05,7.8e-05,0.0014,0.023,0.05,0.0082,0.058,0.067,0.035,4e-07,4.4e-07,6.1e-06,0.028,0.028,0.0005,0.0025,0.00063,0.0025,0.0025,0.0025,0.0025,0,0 +27590000,0.72,0.026,0.05,0.69,-2.5,-1.1,-1.2,-2.5,-1.6,-3.7e+02,-0.00072,-0.0059,0.00026,0.0023,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.7e-05,0.0012,0.022,0.044,0.0082,0.061,0.068,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00056,0.0025,0.0025,0.0025,0.0025,0,0 +27690000,0.72,0.026,0.048,0.69,-2.6,-1.2,-1.2,-2.8,-1.7,-3.7e+02,-0.00072,-0.0059,0.00026,0.0022,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.7e-05,0.0012,0.023,0.045,0.0082,0.067,0.077,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00049,0.0025,0.0025,0.0025,0.0025,0,0 +27790000,0.72,0.026,0.05,0.69,-2.6,-1.2,-1.2,-3.1,-1.8,-3.7e+02,-0.00071,-0.0059,0.0003,0.0016,-0.024,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.7e-05,7.7e-05,0.0011,0.022,0.04,0.0082,0.069,0.078,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00044,0.0025,0.0025,0.0025,0.0025,0,0 +27890000,0.72,0.025,0.048,0.69,-2.6,-1.2,-1.2,-3.3,-1.9,-3.7e+02,-0.00071,-0.0059,0.0003,0.0014,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.7e-05,7.7e-05,0.0011,0.023,0.041,0.0083,0.076,0.087,0.035,4e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.0004,0.0025,0.0025,0.0025,0.0025,0,0 +27990000,0.72,0.024,0.044,0.69,-2.7,-1.2,-1.2,-3.6,-2,-3.7e+02,-0.00074,-0.0059,0.00034,0.00057,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.7e-05,7.6e-05,0.001,0.022,0.036,0.0083,0.078,0.087,0.035,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00037,0.0025,0.0025,0.0025,0.0025,0,0 +28090000,0.72,0.03,0.057,0.69,-2.7,-1.2,-1.2,-3.9,-2.1,-3.7e+02,-0.00074,-0.0059,0.00034,0.00043,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.001,7.7e-05,7.7e-05,0.001,0.023,0.037,0.0084,0.085,0.097,0.035,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00034,0.0025,0.0025,0.0025,0.0025,0,0 +28190000,0.72,0.036,0.071,0.68,-2.8,-1.2,-0.95,-4.2,-2.2,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00016,-0.021,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,7.8e-05,7.6e-05,0.00094,0.022,0.034,0.0084,0.087,0.097,0.035,3.9e-07,4.4e-07,5.9e-06,0.028,0.028,0.0005,0.0025,0.00032,0.0025,0.0025,0.0025,0.0025,0,0 +28290000,0.73,0.028,0.054,0.68,-2.8,-1.2,-0.089,-4.5,-2.4,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00034,-0.02,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,7.7e-05,7.6e-05,0.00094,0.022,0.034,0.0086,0.094,0.11,0.036,3.9e-07,4.4e-07,5.9e-06,0.028,0.028,0.0005,0.0025,0.0003,0.0025,0.0025,0.0025,0.0025,0,0 +28390000,0.73,0.011,0.023,0.68,-2.8,-1.2,0.77,-4.8,-2.5,-3.7e+02,-0.00076,-0.0059,0.00036,-0.00065,-0.019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,7.9e-05,7.7e-05,0.00095,0.023,0.035,0.0087,0.1,0.12,0.036,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00028,0.0025,0.0025,0.0025,0.0025,0,0 +28490000,0.73,0.0015,0.0047,0.68,-2.7,-1.2,1.1,-5,-2.6,-3.7e+02,-0.00075,-0.0059,0.00036,-0.00093,-0.019,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00093,8.1e-05,7.7e-05,0.00095,0.024,0.035,0.0088,0.11,0.13,0.036,3.9e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00026,0.0025,0.0025,0.0025,0.0025,0,0 +28590000,0.73,-0.00042,0.0012,0.69,-2.7,-1.2,0.96,-5.3,-2.7,-3.7e+02,-0.00075,-0.0059,0.00036,-0.0013,-0.018,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00094,8.2e-05,7.7e-05,0.00095,0.025,0.034,0.0089,0.12,0.14,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00025,0.0025,0.0025,0.0025,0.0025,0,0 +28690000,0.73,-0.0012,0.00035,0.69,-2.6,-1.2,0.96,-5.6,-2.8,-3.7e+02,-0.00075,-0.0059,0.00036,-0.0018,-0.017,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00094,8.3e-05,7.8e-05,0.00095,0.026,0.034,0.009,0.13,0.15,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00023,0.0025,0.0025,0.0025,0.0025,0,0 +28790000,0.73,-0.0016,0.00014,0.68,-2.6,-1.2,0.97,-5.9,-2.9,-3.7e+02,-0.00081,-0.006,0.00039,-0.0027,-0.023,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00091,8.3e-05,7.8e-05,0.00091,0.025,0.03,0.0089,0.13,0.15,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00022,0.0025,0.0025,0.0025,0.0025,0,0 +28890000,0.73,-0.0016,0.00036,0.69,-2.5,-1.2,0.96,-6.1,-3.1,-3.7e+02,-0.00081,-0.006,0.00039,-0.0032,-0.022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00091,8.3e-05,7.8e-05,0.00091,0.026,0.03,0.009,0.14,0.16,0.036,3.9e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00021,0.0025,0.0025,0.0025,0.0025,0,0 +28990000,0.73,-0.0015,0.00081,0.68,-2.5,-1.1,0.95,-6.5,-3.2,-3.7e+02,-0.00089,-0.006,0.00041,-0.0027,-0.03,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.025,0.027,0.009,0.14,0.16,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.0002,0.0025,0.0025,0.0025,0.0025,0,0 +29090000,0.73,-0.0013,0.0012,0.68,-2.4,-1.1,0.94,-6.7,-3.3,-3.7e+02,-0.00089,-0.006,0.00041,-0.0032,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.3e-05,7.8e-05,0.00089,0.026,0.027,0.009,0.15,0.17,0.036,3.8e-07,4.5e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00019,0.0025,0.0025,0.0025,0.0025,0,0 +29190000,0.73,-0.0011,0.0016,0.68,-2.4,-1.1,0.93,-7,-3.4,-3.7e+02,-0.00093,-0.006,0.00041,-0.0034,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.024,0.025,0.009,0.15,0.17,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 +29290000,0.73,-0.00074,0.0025,0.68,-2.3,-1.1,0.96,-7.3,-3.5,-3.7e+02,-0.00093,-0.006,0.00041,-0.004,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.026,0.026,0.0091,0.16,0.18,0.036,3.8e-07,4.4e-07,6e-06,0.028,0.028,0.0005,0.0025,0.00018,0.0025,0.0025,0.0025,0.0025,0,0 +29390000,0.73,-0.00016,0.004,0.68,-2.3,-1.1,0.97,-7.6,-3.6,-3.7e+02,-0.00099,-0.006,0.00041,-0.0035,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.024,0.025,0.009,0.16,0.18,0.036,3.7e-07,4.4e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00017,0.0025,0.0025,0.0025,0.0025,0,0 +29490000,0.73,0.00037,0.0051,0.68,-2.2,-1.1,0.97,-7.8,-3.7,-3.7e+02,-0.00099,-0.006,0.00041,-0.004,-0.032,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.026,0.026,0.0091,0.17,0.19,0.037,3.7e-07,4.4e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 +29590000,0.73,0.00081,0.0062,0.68,-2.2,-1.1,0.96,-8.1,-3.8,-3.7e+02,-0.001,-0.006,0.0004,-0.0051,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.3e-05,7.8e-05,0.00089,0.025,0.025,0.009,0.17,0.19,0.036,3.7e-07,4.3e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00016,0.0025,0.0025,0.0025,0.0025,0,0 +29690000,0.73,0.0011,0.0068,0.68,-2.2,-1.1,0.95,-8.3,-3.9,-3.7e+02,-0.001,-0.006,0.0004,-0.0056,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.3e-05,7.8e-05,0.0009,0.026,0.026,0.0091,0.18,0.2,0.036,3.7e-07,4.3e-07,6e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 +29790000,0.73,0.0015,0.0073,0.68,-2.2,-1.1,0.94,-8.6,-4,-3.7e+02,-0.0011,-0.006,0.00038,-0.0059,-0.033,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.025,0.025,0.0091,0.18,0.2,0.037,3.6e-07,4.3e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00015,0.0025,0.0025,0.0025,0.0025,0,0 +29890000,0.73,0.0015,0.0075,0.68,-2.1,-1.1,0.92,-8.8,-4.1,-3.7e+02,-0.0011,-0.006,0.00038,-0.0068,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.026,0.027,0.0091,0.19,0.21,0.037,3.6e-07,4.3e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 +29990000,0.73,0.0017,0.0077,0.68,-2.1,-1.1,0.91,-9,-4.2,-3.7e+02,-0.0011,-0.006,0.00035,-0.008,-0.031,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.025,0.026,0.009,0.19,0.21,0.036,3.5e-07,4.2e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00014,0.0025,0.0025,0.0025,0.0025,0,0 +30090000,0.73,0.0017,0.0076,0.68,-2.1,-1.1,0.89,-9.2,-4.3,-3.7e+02,-0.0011,-0.006,0.00035,-0.0086,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0009,8.4e-05,7.8e-05,0.0009,0.026,0.028,0.0091,0.2,0.22,0.036,3.6e-07,4.2e-07,5.9e-06,0.028,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30190000,0.73,0.0018,0.0074,0.68,-2,-1.1,0.88,-9.5,-4.4,-3.7e+02,-0.0011,-0.0059,0.00032,-0.0088,-0.03,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.4e-05,7.8e-05,0.0009,0.025,0.027,0.009,0.2,0.22,0.037,3.5e-07,4.1e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30290000,0.73,0.0017,0.0073,0.68,-2,-1.1,0.87,-9.7,-4.5,-3.7e+02,-0.0011,-0.0059,0.00032,-0.0092,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.4e-05,7.8e-05,0.0009,0.026,0.029,0.0091,0.21,0.23,0.037,3.5e-07,4.1e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00013,0.0025,0.0025,0.0025,0.0025,0,0 +30390000,0.73,0.0018,0.0071,0.68,-2,-1.1,0.85,-9.9,-4.6,-3.7e+02,-0.0012,-0.0059,0.00029,-0.0098,-0.029,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.4e-05,7.8e-05,0.00089,0.025,0.028,0.009,0.21,0.23,0.036,3.4e-07,4e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30490000,0.73,0.0016,0.0068,0.68,-2,-1.1,0.84,-10,-4.7,-3.7e+02,-0.0012,-0.0059,0.00029,-0.01,-0.028,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8.5e-05,7.8e-05,0.0009,0.026,0.03,0.0091,0.22,0.24,0.037,3.5e-07,4e-07,5.9e-06,0.027,0.027,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30590000,0.73,0.0016,0.0065,0.68,-1.9,-1,0.8,-10,-4.8,-3.7e+02,-0.0012,-0.0059,0.00026,-0.01,-0.028,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00088,8.5e-05,7.8e-05,0.00089,0.025,0.028,0.009,0.22,0.24,0.037,3.4e-07,3.9e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00012,0.0025,0.0025,0.0025,0.0025,0,0 +30690000,0.73,0.0015,0.0062,0.68,-1.9,-1,0.79,-11,-4.9,-3.7e+02,-0.0012,-0.0059,0.00026,-0.011,-0.026,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00088,8.5e-05,7.8e-05,0.00089,0.026,0.03,0.009,0.23,0.25,0.037,3.4e-07,3.9e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30790000,0.73,0.0015,0.0058,0.68,-1.9,-1,0.78,-11,-5,-3.7e+02,-0.0012,-0.0059,0.00022,-0.011,-0.026,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.5e-05,7.7e-05,0.00088,0.025,0.029,0.0089,0.23,0.25,0.037,3.3e-07,3.8e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30890000,0.73,0.0013,0.0053,0.68,-1.9,-1,0.77,-11,-5.1,-3.7e+02,-0.0012,-0.0059,0.00022,-0.011,-0.025,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00087,8.5e-05,7.7e-05,0.00088,0.026,0.031,0.009,0.24,0.26,0.037,3.3e-07,3.8e-07,5.9e-06,0.027,0.026,0.0005,0.0025,0.00011,0.0025,0.0025,0.0025,0.0025,0,0 +30990000,0.73,0.0014,0.0048,0.68,-1.8,-1,0.76,-11,-5.2,-3.7e+02,-0.0013,-0.0059,0.00018,-0.011,-0.025,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.5e-05,7.7e-05,0.00086,0.025,0.03,0.0089,0.24,0.26,0.036,3.3e-07,3.7e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31090000,0.73,0.0012,0.0043,0.68,-1.8,-1,0.75,-11,-5.3,-3.7e+02,-0.0013,-0.0059,0.00018,-0.012,-0.023,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00086,8.5e-05,7.7e-05,0.00087,0.027,0.032,0.0089,0.25,0.27,0.037,3.3e-07,3.7e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31190000,0.73,0.0012,0.004,0.68,-1.8,-1,0.74,-12,-5.4,-3.7e+02,-0.0013,-0.0058,0.00015,-0.012,-0.021,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00084,8.5e-05,7.7e-05,0.00085,0.025,0.03,0.0088,0.25,0.27,0.037,3.2e-07,3.6e-07,5.8e-06,0.027,0.026,0.0005,0.0025,0.0001,0.0025,0.0025,0.0025,0.0025,0,0 +31290000,0.73,0.00093,0.0035,0.68,-1.8,-0.99,0.74,-12,-5.5,-3.7e+02,-0.0013,-0.0058,0.00015,-0.013,-0.02,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00084,8.5e-05,7.7e-05,0.00085,0.027,0.033,0.0089,0.26,0.28,0.037,3.2e-07,3.6e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31390000,0.73,0.00091,0.003,0.69,-1.7,-0.98,0.73,-12,-5.6,-3.7e+02,-0.0013,-0.0058,0.00011,-0.013,-0.018,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00082,8.5e-05,7.6e-05,0.00083,0.025,0.031,0.0088,0.25,0.28,0.036,3.2e-07,3.5e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31490000,0.73,0.00068,0.0024,0.69,-1.7,-0.98,0.73,-12,-5.7,-3.7e+02,-0.0013,-0.0058,0.00011,-0.013,-0.017,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00082,8.5e-05,7.7e-05,0.00083,0.027,0.033,0.0088,0.27,0.29,0.037,3.2e-07,3.5e-07,5.8e-06,0.027,0.026,0.0005,0.0025,9.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +31590000,0.73,0.00074,0.002,0.69,-1.7,-0.96,0.72,-12,-5.7,-3.7e+02,-0.0013,-0.0058,7.3e-05,-0.014,-0.015,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.0008,8.5e-05,7.6e-05,0.00081,0.025,0.031,0.0087,0.26,0.29,0.037,3.1e-07,3.4e-07,5.8e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31690000,0.73,0.00047,0.0013,0.69,-1.7,-0.96,0.73,-12,-5.8,-3.7e+02,-0.0013,-0.0058,7.3e-05,-0.014,-0.014,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.0008,8.5e-05,7.6e-05,0.00081,0.027,0.033,0.0087,0.28,0.3,0.037,3.1e-07,3.4e-07,5.8e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31790000,0.73,0.00039,0.00063,0.69,-1.6,-0.94,0.73,-13,-5.9,-3.7e+02,-0.0014,-0.0058,3.4e-05,-0.014,-0.012,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00078,8.5e-05,7.6e-05,0.00079,0.025,0.031,0.0087,0.27,0.3,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31890000,0.73,0.00012,-5.8e-05,0.69,-1.6,-0.93,0.72,-13,-6,-3.7e+02,-0.0014,-0.0058,3.4e-05,-0.015,-0.011,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00078,8.5e-05,7.6e-05,0.00079,0.027,0.034,0.0087,0.29,0.31,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +31990000,0.72,9.7e-05,-0.00058,0.69,-1.6,-0.92,0.71,-13,-6.1,-3.7e+02,-0.0014,-0.0058,-9.2e-07,-0.015,-0.0094,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00076,8.4e-05,7.5e-05,0.00077,0.025,0.032,0.0086,0.28,0.31,0.036,3e-07,3.2e-07,5.7e-06,0.027,0.026,0.0005,0,0,0,0,0,0,0,0 +32090000,0.72,-0.00021,-0.0013,0.69,-1.5,-0.91,0.72,-13,-6.2,-3.7e+02,-0.0014,-0.0058,-1.4e-06,-0.016,-0.0079,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00076,8.5e-05,7.6e-05,0.00077,0.027,0.034,0.0087,0.3,0.32,0.037,3.1e-07,3.3e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32190000,0.72,-0.00032,-0.0021,0.69,-1.5,-0.9,0.72,-13,-6.3,-3.7e+02,-0.0014,-0.0058,-3.5e-05,-0.016,-0.0058,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00074,8.4e-05,7.5e-05,0.00075,0.025,0.032,0.0086,0.29,0.32,0.036,3e-07,3.2e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32290000,0.72,-0.00057,-0.0028,0.69,-1.5,-0.89,0.71,-14,-6.3,-3.7e+02,-0.0014,-0.0058,-3.5e-05,-0.016,-0.0042,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00074,8.5e-05,7.5e-05,0.00075,0.027,0.034,0.0086,0.3,0.33,0.037,3e-07,3.2e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32390000,0.72,-0.00067,-0.0035,0.69,-1.5,-0.87,0.71,-14,-6.4,-3.7e+02,-0.0014,-0.0057,-5.8e-05,-0.016,-0.0034,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00072,8.4e-05,7.5e-05,0.00073,0.025,0.032,0.0085,0.3,0.33,0.037,3e-07,3.1e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32490000,0.72,-0.0008,-0.0038,0.69,-1.4,-0.86,0.72,-14,-6.5,-3.7e+02,-0.0014,-0.0057,-5.9e-05,-0.017,-0.0022,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00072,8.4e-05,7.5e-05,0.00073,0.027,0.034,0.0085,0.31,0.34,0.037,3e-07,3.1e-07,5.7e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32590000,0.72,-0.0007,-0.004,0.69,-1.4,-0.85,0.72,-14,-6.6,-3.7e+02,-0.0014,-0.0057,-8.1e-05,-0.017,-0.0014,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.0007,8.4e-05,7.5e-05,0.00071,0.025,0.032,0.0084,0.31,0.34,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32690000,0.72,-0.00074,-0.0041,0.69,-1.4,-0.84,0.71,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-8.1e-05,-0.017,-0.00081,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.0007,8.4e-05,7.5e-05,0.00071,0.027,0.034,0.0085,0.32,0.35,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32790000,0.72,-0.00061,-0.0041,0.69,-1.3,-0.83,0.71,-14,-6.7,-3.7e+02,-0.0014,-0.0057,-0.0001,-0.017,0.00012,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00068,8.3e-05,7.4e-05,0.00069,0.025,0.032,0.0084,0.32,0.35,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32890000,0.72,-0.00053,-0.004,0.69,-1.3,-0.82,0.71,-14,-6.8,-3.7e+02,-0.0014,-0.0057,-0.0001,-0.017,0.0013,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00068,8.4e-05,7.4e-05,0.00069,0.027,0.033,0.0084,0.33,0.36,0.036,2.9e-07,3e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +32990000,0.72,-0.0003,-0.004,0.69,-1.3,-0.81,0.7,-15,-6.9,-3.7e+02,-0.0014,-0.0057,-0.00012,-0.017,0.0026,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00066,8.3e-05,7.4e-05,0.00067,0.025,0.031,0.0083,0.33,0.36,0.036,2.9e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33090000,0.72,-0.00034,-0.004,0.69,-1.3,-0.8,0.7,-15,-7,-3.7e+02,-0.0014,-0.0057,-0.00012,-0.018,0.0032,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00066,8.3e-05,7.4e-05,0.00067,0.026,0.033,0.0084,0.34,0.37,0.036,2.9e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33190000,0.72,0.0032,-0.0032,0.7,-1.2,-0.78,0.64,-15,-7,-3.7e+02,-0.0015,-0.0057,-0.00014,-0.018,0.0039,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00065,8.2e-05,7.4e-05,0.00064,0.025,0.031,0.0083,0.34,0.37,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0,0,0,0,0,0,0,0 +33290000,0.67,0.015,-0.0025,0.75,-1.2,-0.77,0.62,-15,-7.1,-3.7e+02,-0.0015,-0.0057,-0.00014,-0.018,0.0043,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00075,8.2e-05,7.4e-05,0.00055,0.026,0.033,0.0083,0.35,0.38,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0.0025,9.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33390000,0.56,0.013,-0.0027,0.83,-1.2,-0.75,0.81,-15,-7.2,-3.7e+02,-0.0015,-0.0057,-0.00015,-0.018,0.0053,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00089,8e-05,7.5e-05,0.00038,0.025,0.03,0.0082,0.35,0.38,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33490000,0.43,0.0066,-0.00011,0.9,-1.2,-0.75,0.83,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-0.00015,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0011,7.9e-05,7.7e-05,0.00022,0.026,0.032,0.0081,0.36,0.39,0.036,2.8e-07,2.9e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33590000,0.27,0.0006,-0.0025,0.96,-1.2,-0.74,0.79,-15,-7.3,-3.7e+02,-0.0015,-0.0057,-0.00017,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.7e-05,7.9e-05,8.1e-05,0.026,0.031,0.0078,0.35,0.39,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33690000,0.1,-0.0028,-0.0055,0.99,-1.1,-0.74,0.8,-15,-7.4,-3.7e+02,-0.0015,-0.0057,-0.00017,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.5e-05,8.1e-05,9.7e-06,0.028,0.034,0.0076,0.37,0.4,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33790000,-0.068,-0.0044,-0.0073,1,-1.1,-0.72,0.78,-16,-7.5,-3.7e+02,-0.0015,-0.0057,-0.00019,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.3e-05,8e-05,8.1e-06,0.028,0.034,0.0074,0.36,0.4,0.036,2.7e-07,2.8e-07,5.5e-06,0.027,0.025,0.0005,0.0025,8.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33890000,-0.24,-0.0057,-0.008,0.97,-1,-0.7,0.77,-16,-7.5,-3.7e+02,-0.0015,-0.0057,-0.00019,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0012,7.2e-05,8.2e-05,7.6e-05,0.032,0.039,0.0072,0.38,0.41,0.036,2.8e-07,2.8e-07,5.6e-06,0.027,0.025,0.0005,0.0025,8.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +33990000,-0.38,-0.0039,-0.012,0.92,-0.96,-0.65,0.74,-16,-7.6,-3.7e+02,-0.0015,-0.0057,-0.00022,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.001,6.9e-05,7.8e-05,0.00019,0.032,0.038,0.007,0.37,0.41,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34090000,-0.49,-0.003,-0.013,0.87,-0.91,-0.61,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00022,-0.018,0.0054,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0009,6.9e-05,7.8e-05,0.0003,0.037,0.044,0.0069,0.39,0.42,0.036,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34190000,-0.56,-0.0023,-0.012,0.83,-0.89,-0.56,0.74,-16,-7.7,-3.7e+02,-0.0015,-0.0057,-0.00023,-0.021,0.0076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00079,6.5e-05,7.3e-05,0.00038,0.037,0.043,0.0067,0.38,0.42,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34290000,-0.6,-0.0032,-0.0091,0.8,-0.84,-0.51,0.74,-16,-7.8,-3.7e+02,-0.0015,-0.0057,-0.00023,-0.021,0.0076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00074,6.5e-05,7.2e-05,0.00044,0.043,0.049,0.0066,0.39,0.43,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34390000,-0.63,-0.0033,-0.0067,0.78,-0.83,-0.47,0.74,-16,-7.9,-3.7e+02,-0.0016,-0.0057,-0.00025,-0.026,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00068,6.1e-05,6.6e-05,0.00047,0.042,0.048,0.0065,0.39,0.43,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34490000,-0.65,-0.0042,-0.0045,0.76,-0.77,-0.42,0.74,-16,-7.9,-3.7e+02,-0.0016,-0.0057,-0.00025,-0.026,0.013,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00066,6.1e-05,6.6e-05,0.00049,0.049,0.055,0.0064,0.4,0.44,0.035,2.7e-07,2.7e-07,5.5e-06,0.027,0.025,0.0005,0.0025,7.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34590000,-0.66,-0.0035,-0.0035,0.75,-0.78,-0.4,0.73,-17,-8,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.036,0.022,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00063,5.6e-05,6e-05,0.0005,0.048,0.052,0.0063,0.4,0.44,0.034,2.7e-07,2.7e-07,5.5e-06,0.026,0.024,0.0005,0.0025,7.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34690000,-0.66,-0.0039,-0.0027,0.75,-0.72,-0.36,0.73,-17,-8,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.036,0.022,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00062,5.6e-05,6e-05,0.00051,0.055,0.06,0.0063,0.41,0.45,0.034,2.7e-07,2.7e-07,5.5e-06,0.026,0.024,0.0005,0.0025,7.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34790000,-0.67,-0.0027,-0.0025,0.75,-0.73,-0.35,0.72,-17,-8.1,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.047,0.033,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00061,5.1e-05,5.5e-05,0.0005,0.052,0.056,0.0062,0.41,0.45,0.034,2.6e-07,2.7e-07,5.5e-06,0.025,0.024,0.0005,0.0025,7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34890000,-0.67,-0.0028,-0.0024,0.74,-0.68,-0.31,0.72,-17,-8.1,-3.7e+02,-0.0016,-0.0057,-0.00026,-0.047,0.033,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00061,5.1e-05,5.5e-05,0.00051,0.059,0.064,0.0062,0.42,0.46,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.024,0.0005,0.0025,6.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +34990000,-0.67,-0.009,-0.0051,0.74,0.34,0.28,-0.13,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.06,0.07,0.0068,0.42,0.45,0.034,2.6e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35090000,-0.67,-0.0091,-0.0051,0.74,0.47,0.3,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.065,0.077,0.0068,0.43,0.46,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35190000,-0.67,-0.0091,-0.0052,0.74,0.49,0.33,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.07,0.082,0.0067,0.44,0.47,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35290000,-0.67,-0.0091,-0.0052,0.74,0.51,0.37,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.076,0.087,0.0066,0.46,0.48,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35390000,-0.67,-0.0092,-0.0052,0.74,0.54,0.4,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.7e-05,5e-05,0.00051,0.082,0.094,0.0066,0.47,0.49,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35490000,-0.67,-0.0092,-0.0052,0.74,0.56,0.44,-0.18,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00027,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.0006,4.6e-05,5e-05,0.00052,0.088,0.1,0.0065,0.49,0.51,0.034,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35590000,-0.67,-0.0061,-0.0053,0.74,0.45,0.36,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00025,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00057,3.9e-05,4.3e-05,0.00049,0.07,0.076,0.0062,0.48,0.5,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35690000,-0.67,-0.0061,-0.0053,0.74,0.46,0.39,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00025,-0.062,0.048,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00057,3.9e-05,4.3e-05,0.00049,0.075,0.081,0.0062,0.5,0.52,0.033,2.7e-07,2.7e-07,5.5e-06,0.025,0.023,0.0005,0.0025,6.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35790000,-0.67,-0.0038,-0.0053,0.74,0.38,0.33,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.064,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3.4e-05,3.8e-05,0.00048,0.062,0.066,0.0059,0.49,0.51,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35890000,-0.67,-0.0038,-0.0053,0.74,0.4,0.36,-0.19,-17,-8,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.064,0.05,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3.4e-05,3.8e-05,0.00048,0.067,0.071,0.0059,0.5,0.53,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +35990000,-0.67,-0.0018,-0.0053,0.74,0.33,0.31,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.072,0.056,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,3e-05,3.4e-05,0.00047,0.058,0.06,0.0057,0.5,0.52,0.033,2.7e-07,2.7e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36090000,-0.67,-0.0019,-0.0052,0.74,0.34,0.33,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00023,-0.072,0.057,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,3e-05,3.4e-05,0.00047,0.063,0.066,0.0057,0.51,0.53,0.032,2.7e-07,2.8e-07,5.4e-06,0.025,0.023,0.0005,0.0025,5.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36190000,-0.67,-0.00042,-0.0051,0.74,0.28,0.28,-0.19,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00022,-0.083,0.066,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.7e-05,3.1e-05,0.00047,0.055,0.057,0.0056,0.5,0.53,0.032,2.7e-07,2.8e-07,5.4e-06,0.024,0.023,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36290000,-0.67,-0.00055,-0.0051,0.74,0.29,0.31,-0.18,-17,-8.1,-3.7e+02,-0.0017,-0.0057,-0.00022,-0.083,0.066,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.7e-05,3.2e-05,0.00047,0.061,0.063,0.0056,0.52,0.54,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.023,0.0005,0.0025,5.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36390000,-0.67,0.00057,-0.0049,0.74,0.24,0.26,-0.18,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.096,0.076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00055,2.4e-05,2.9e-05,0.00047,0.054,0.055,0.0055,0.51,0.54,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.022,0.0005,0.0025,5.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36490000,-0.67,0.00047,-0.0049,0.74,0.25,0.28,-0.18,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.097,0.076,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.4e-05,2.9e-05,0.00047,0.059,0.061,0.0055,0.53,0.55,0.032,2.8e-07,2.8e-07,5.4e-06,0.024,0.022,0.0005,0.0025,5.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36590000,-0.67,0.0013,-0.0047,0.74,0.21,0.24,-0.18,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.11,0.086,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.2e-05,2.7e-05,0.00047,0.053,0.054,0.0055,0.52,0.55,0.031,2.8e-07,2.8e-07,5.4e-06,0.023,0.022,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36690000,-0.67,0.0013,-0.0047,0.74,0.21,0.26,-0.17,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.11,0.086,-0.092,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.2e-05,2.7e-05,0.00047,0.058,0.06,0.0055,0.54,0.56,0.031,2.8e-07,2.8e-07,5.4e-06,0.023,0.022,0.0005,0.0025,5.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36790000,-0.67,0.0019,-0.0045,0.74,0.18,0.22,-0.17,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.12,0.095,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.1e-05,2.6e-05,0.00048,0.052,0.053,0.0055,0.53,0.56,0.031,2.8e-07,2.8e-07,5.4e-06,0.022,0.021,0.0005,0.0025,5.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36890000,-0.67,0.0018,-0.0045,0.74,0.18,0.24,-0.16,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00022,-0.12,0.096,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2.1e-05,2.6e-05,0.00048,0.057,0.059,0.0056,0.55,0.57,0.031,2.8e-07,2.8e-07,5.4e-06,0.022,0.021,0.0005,0.0025,5.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +36990000,-0.67,0.0023,-0.0043,0.74,0.15,0.2,-0.16,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.14,0.1,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2e-05,2.5e-05,0.00048,0.051,0.052,0.0056,0.54,0.57,0.031,2.8e-07,2.8e-07,5.4e-06,0.021,0.02,0.0005,0.0025,5.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37090000,-0.67,0.0023,-0.0042,0.74,0.15,0.21,-0.15,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.14,0.1,-0.093,0.37,0.0037,0.025,0,0,0,0,0,0.00056,2e-05,2.5e-05,0.00048,0.056,0.058,0.0057,0.55,0.58,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.02,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37190000,-0.67,0.0026,-0.0041,0.74,0.12,0.18,-0.14,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.15,0.11,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.9e-05,2.4e-05,0.00048,0.05,0.052,0.0057,0.55,0.58,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.019,0.0005,0.0025,5.1e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37290000,-0.67,0.0026,-0.0041,0.74,0.12,0.19,-0.14,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00023,-0.15,0.11,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.9e-05,2.4e-05,0.00048,0.055,0.057,0.0059,0.56,0.59,0.031,2.8e-07,2.9e-07,5.4e-06,0.021,0.019,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37390000,-0.67,0.0028,-0.0039,0.74,0.1,0.16,-0.13,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.16,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00048,0.05,0.051,0.0059,0.56,0.59,0.031,2.9e-07,2.9e-07,5.4e-06,0.02,0.018,0.0005,0.0025,5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37490000,-0.67,0.0028,-0.0038,0.74,0.099,0.17,-0.13,-17,-8.1,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.16,0.12,-0.094,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.054,0.056,0.006,0.57,0.6,0.031,2.9e-07,2.9e-07,5.4e-06,0.02,0.018,0.0005,0.0025,4.9e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37590000,-0.67,0.003,-0.0037,0.74,0.079,0.14,-0.12,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.17,0.12,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.049,0.05,0.0061,0.57,0.6,0.031,2.9e-07,2.9e-07,5.4e-06,0.019,0.018,0.0005,0.0025,4.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37690000,-0.67,0.0029,-0.0037,0.74,0.077,0.15,-0.11,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00024,-0.17,0.12,-0.095,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.3e-05,0.00049,0.053,0.055,0.0062,0.58,0.61,0.031,2.9e-07,2.9e-07,5.4e-06,0.019,0.018,0.0005,0.0025,4.8e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37790000,-0.67,0.003,-0.0036,0.74,0.061,0.13,-0.1,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00025,-0.18,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.2e-05,0.00049,0.048,0.049,0.0063,0.58,0.61,0.03,2.9e-07,2.9e-07,5.4e-06,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37890000,-0.67,0.003,-0.0036,0.74,0.058,0.13,-0.094,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00025,-0.18,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00057,1.8e-05,2.2e-05,0.00049,0.052,0.053,0.0064,0.59,0.62,0.03,2.9e-07,2.9e-07,5.4e-06,0.018,0.017,0.0005,0.0025,4.7e-05,0.0025,0.0025,0.0025,0.0025,0,0 +37990000,-0.67,0.0031,-0.0036,0.74,0.044,0.11,-0.085,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.19,0.13,-0.096,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.7e-05,2.2e-05,0.00049,0.047,0.048,0.0065,0.59,0.62,0.031,2.9e-07,2.9e-07,5.4e-06,0.017,0.016,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38090000,-0.67,0.003,-0.0035,0.74,0.04,0.12,-0.075,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.19,0.13,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.051,0.052,0.0066,0.6,0.63,0.031,2.9e-07,3e-07,5.4e-06,0.017,0.016,0.0005,0.0025,4.6e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38190000,-0.67,0.0031,-0.0034,0.74,0.027,0.1,-0.067,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.2,0.14,-0.097,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.7e-05,2.2e-05,0.00049,0.045,0.047,0.0067,0.6,0.63,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38290000,-0.67,0.0031,-0.0034,0.74,0.023,0.1,-0.06,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00026,-0.2,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.049,0.051,0.0068,0.61,0.64,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38390000,-0.67,0.0031,-0.0033,0.74,0.015,0.087,-0.052,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.2,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.7e-05,2.2e-05,0.00049,0.044,0.046,0.0069,0.61,0.64,0.031,2.9e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.5e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38490000,-0.67,0.0031,-0.0033,0.74,0.012,0.089,-0.045,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.2,0.14,-0.098,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.048,0.049,0.007,0.62,0.65,0.031,3e-07,3e-07,5.4e-06,0.016,0.015,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38590000,-0.67,0.0031,-0.0031,0.74,0.0071,0.076,-0.038,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.043,0.044,0.0071,0.62,0.65,0.031,3e-07,3e-07,5.4e-06,0.015,0.014,0.0005,0.0025,4.4e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38690000,-0.67,0.003,-0.0031,0.74,0.0029,0.076,-0.031,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00027,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.046,0.048,0.0072,0.63,0.66,0.031,3e-07,3e-07,5.4e-06,0.015,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38790000,-0.67,0.003,-0.0031,0.74,-0.0023,0.063,-0.023,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00028,-0.21,0.14,-0.099,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.00049,0.042,0.043,0.0073,0.63,0.66,0.031,3e-07,3e-07,5.4e-06,0.014,0.014,0.0005,0.0025,4.3e-05,0.0025,0.0025,0.0025,0.0025,0,0 +38890000,-0.67,0.0028,-0.0031,0.74,-0.012,0.052,0.48,-17,-8.2,-3.7e+02,-0.0018,-0.0057,-0.00028,-0.21,0.14,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.00058,1.8e-05,2.2e-05,0.0005,0.045,0.046,0.0075,0.64,0.67,0.032,3e-07,3e-07,5.4e-06,0.014,0.014,0.0005,0.0025,4.2e-05,0.0025,0.0025,0.0025,0.0025,0,0 diff --git a/src/modules/ekf2/test/change_indication/iris_gps.csv b/src/modules/ekf2/test/change_indication/iris_gps.csv index 7983ef3d94..85ba22c801 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.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.8e-07,0.0025,0.0025,0.0018,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,1e-06,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -90000,0.98,-0.0095,-0.012,0.18,-5.5e-05,-0.0032,-0.024,-3.6e-06,-0.00014,-0.0021,0,0,0,0,0,0,0.2,-1.4e-09,0.43,0,0,0,0,0,8.9e-07,0.0026,0.0026,0.001,25,25,10,1e+02,1e+02,1e+02,1e-06,1e-06,9.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -190000,0.98,-0.0092,-0.013,0.21,-0.0013,-0.0036,-0.037,-4.4e-05,-0.00046,-0.017,5.2e-12,-4.3e-12,-1.6e-13,0,0,-6.8e-10,0.2,0.011,0.43,0,0,0,0,0,2.8e-06,0.0027,0.0027,0.00076,25,25,10,1e+02,1e+02,1,1e-06,1e-06,9.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -290000,0.98,-0.0092,-0.013,0.21,-0.0016,-0.0053,-0.046,-0.00015,-0.00032,-0.018,4.4e-11,-5.4e-11,-2.7e-12,0,0,-2.9e-08,0.2,0.011,0.43,0,0,0,0,0,6.6e-06,0.0029,0.0029,0.00063,25,25,9.6,0.37,0.37,0.41,1e-06,1e-06,9.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -390000,0.98,-0.0095,-0.013,0.19,-0.00033,-0.0065,-0.063,-0.00029,-0.00087,-0.013,-6.7e-11,-6.3e-11,3e-12,0,0,8.8e-08,0.2,0.002,0.44,0,0,0,0,0,1.1e-05,0.0031,0.0031,0.00058,25,25,8.1,0.97,0.97,0.32,1e-06,1e-06,8.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -490000,0.98,-0.0095,-0.014,0.19,0.0016,-0.0061,-0.069,2.4e-05,-0.00049,-0.011,-1.2e-08,6.5e-09,2.4e-10,0,0,1.6e-07,0.2,0.002,0.44,0,0,0,0,0,1.7e-05,0.0034,0.0033,0.00056,7.8,7.8,5.9,0.34,0.34,0.31,1e-06,1e-06,8.3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -590000,0.98,-0.0095,-0.014,0.19,0.0014,-0.0091,-0.12,0.00018,-0.0013,-0.03,-1.4e-08,6.7e-09,2.7e-10,0,0,6.4e-07,0.2,0.002,0.44,0,0,0,0,0,2.2e-05,0.0037,0.0037,0.00056,7.9,7.9,4.2,0.67,0.67,0.32,1e-06,1e-06,7.6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -690000,0.98,-0.0096,-0.014,0.19,0.0032,-0.0083,-0.05,0.00021,-0.00076,-0.0089,-5.6e-08,1.3e-08,4.4e-10,0,0,-9.5e-07,0.2,0.002,0.44,0,0,0,0,0,2.8e-05,0.004,0.004,0.00056,2.7,2.7,2.8,0.26,0.26,0.29,1e-06,1e-06,6.8e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -790000,0.98,-0.0096,-0.014,0.19,0.0059,-0.009,-0.054,0.0006,-0.0016,-0.011,-5.5e-08,1.3e-08,3.8e-10,0,0,-1.3e-06,0.2,0.002,0.44,0,0,0,0,0,3.2e-05,0.0044,0.0044,0.00056,2.8,2.8,1.9,0.42,0.42,0.27,1e-06,1e-06,6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -890000,0.98,-0.0096,-0.015,0.19,0.006,-0.0068,-0.093,0.00052,-0.00095,-0.031,-2.2e-07,1.8e-09,5.1e-09,0,0,-3.3e-07,0.2,0.002,0.44,0,0,0,0,0,3.6e-05,0.0048,0.0048,0.00056,1.3,1.3,1.3,0.2,0.2,0.25,9.9e-07,1e-06,5.3e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -990000,0.98,-0.0096,-0.015,0.19,0.0091,-0.0071,-0.12,0.0013,-0.0017,-0.046,-2.2e-07,1.4e-09,5.2e-09,0,0,1.3e-07,0.2,0.002,0.44,0,0,0,0,0,3.9e-05,0.0053,0.0053,0.00056,1.5,1.5,0.95,0.3,0.3,0.23,9.9e-07,9.9e-07,4.6e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1090000,0.98,-0.0096,-0.015,0.19,0.015,-0.0083,-0.13,0.0012,-0.0011,-0.063,-6.1e-07,-1.7e-07,2.2e-08,0,0,4.4e-07,0.2,0.002,0.44,0,0,0,0,0,4.1e-05,0.0057,0.0057,0.00055,0.92,0.92,0.69,0.17,0.17,0.2,9.8e-07,9.8e-07,3.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1190000,0.98,-0.0096,-0.015,0.19,0.021,-0.011,-0.11,0.003,-0.0021,-0.047,-5.8e-07,-1.5e-07,2.1e-08,0,0,-4.8e-06,0.2,0.002,0.44,0,0,0,0,0,4.3e-05,0.0063,0.0063,0.00054,1.1,1.1,0.54,0.24,0.24,0.19,9.8e-07,9.8e-07,3.4e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1290000,0.98,-0.0093,-0.016,0.19,0.024,-0.0097,-0.11,0.0027,-0.0016,-0.048,-1.7e-06,-1e-06,7.7e-08,0,0,-7.5e-06,0.2,0.002,0.44,0,0,0,0,0,4.4e-05,0.0064,0.0064,0.00053,0.88,0.88,0.41,0.15,0.15,0.18,9.5e-07,9.5e-07,2.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1390000,0.98,-0.0093,-0.016,0.19,0.033,-0.012,-0.097,0.0056,-0.0027,-0.038,-1.6e-06,-9.6e-07,7.5e-08,0,0,-1.4e-05,0.2,0.002,0.44,0,0,0,0,0,4.4e-05,0.0071,0.0071,0.00052,1.1,1.1,0.33,0.21,0.21,0.16,9.5e-07,9.5e-07,2.5e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1490000,0.98,-0.009,-0.016,0.19,0.03,-0.01,-0.12,0.0044,-0.0018,-0.053,-3.9e-06,-3.4e-06,2e-07,0,0,-1.2e-05,0.2,0.002,0.44,0,0,0,0,0,4.4e-05,0.0067,0.0067,0.0005,0.95,0.95,0.27,0.14,0.14,0.15,8.8e-07,8.8e-07,2.2e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1590000,0.98,-0.0091,-0.016,0.19,0.038,-0.012,-0.13,0.0077,-0.003,-0.063,-3.9e-06,-3.4e-06,2e-07,0,0,-1.4e-05,0.2,0.002,0.44,0,0,0,0,0,4.4e-05,0.0074,0.0074,0.00049,1.3,1.3,0.23,0.2,0.2,0.14,8.8e-07,8.8e-07,1.9e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1690000,0.98,-0.0088,-0.016,0.19,0.033,-0.0083,-0.13,0.0054,-0.0019,-0.068,-7.3e-06,-7.5e-06,3.9e-07,0,0,-1.8e-05,0.2,0.002,0.44,0,0,0,0,0,4.3e-05,0.0064,0.0064,0.00047,1,1,0.19,0.14,0.14,0.13,7.8e-07,7.8e-07,1.7e-07,4e-06,4e-06,4e-06,0,0,0,0,0,0,0,0 -1790000,0.98,-0.009,-0.016,0.19,0.042,-0.0099,-0.13,0.0092,-0.0029,-0.067,-7.2e-06,-7.4e-06,3.8e-07,0,0,-2.8e-05,0.2,0.002,0.44,0,0,0,0,0,4.3e-05,0.007,0.007,0.00045,1.3,1.3,0.16,0.2,0.2,0.12,7.8e-07,7.8e-07,1.4e-07,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -1890000,0.98,-0.0089,-0.016,0.19,0.049,-0.0086,-0.14,0.014,-0.0037,-0.076,-7.2e-06,-7.4e-06,3.8e-07,0,0,-3.2e-05,0.2,0.002,0.44,0,0,0,0,0,4.2e-05,0.0076,0.0076,0.00044,1.7,1.7,0.15,0.31,0.31,0.12,7.8e-07,7.8e-07,1.3e-07,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -1990000,0.98,-0.0085,-0.016,0.19,0.039,-0.0042,-0.14,0.0096,-0.0021,-0.074,-1.1e-05,-1.3e-05,5.8e-07,0,0,-4.6e-05,0.2,0.002,0.44,0,0,0,0,0,4e-05,0.0061,0.0061,0.00042,1.3,1.3,0.13,0.2,0.2,0.11,6.6e-07,6.6e-07,1.1e-07,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -2090000,0.98,-0.0086,-0.016,0.19,0.047,-0.0042,-0.14,0.014,-0.0026,-0.071,-1.1e-05,-1.3e-05,5.7e-07,0,0,-6.5e-05,0.2,0.002,0.44,0,0,0,0,0,4e-05,0.0066,0.0066,0.00041,1.7,1.7,0.12,0.31,0.31,0.11,6.6e-07,6.6e-07,9.9e-08,4e-06,4e-06,3.9e-06,0,0,0,0,0,0,0,0 -2190000,0.98,-0.0082,-0.015,0.19,0.035,-0.0012,-0.14,0.0091,-0.0012,-0.077,-1.4e-05,-1.8e-05,7.3e-07,0,0,-7.5e-05,0.2,0.002,0.44,0,0,0,0,0,3.8e-05,0.005,0.005,0.0004,1.2,1.2,0.11,0.2,0.2,0.11,5.5e-07,5.5e-07,8.7e-08,4e-06,4e-06,3.8e-06,0,0,0,0,0,0,0,0 -2290000,0.98,-0.0082,-0.016,0.19,0.04,7.1e-05,-0.14,0.013,-0.0012,-0.075,-1.4e-05,-1.8e-05,7.3e-07,0,0,-9.8e-05,0.2,0.002,0.44,0,0,0,0,0,3.7e-05,0.0054,0.0054,0.00038,1.5,1.5,0.11,0.29,0.3,0.1,5.5e-07,5.5e-07,7.8e-08,4e-06,4e-06,3.8e-06,0,0,0,0,0,0,0,0 -2390000,0.98,-0.008,-0.015,0.19,0.031,0.0011,-0.14,0.0082,-0.00046,-0.072,-1.7e-05,-2.3e-05,8.3e-07,0,0,-0.00012,0.2,0.002,0.44,0,0,0,0,0,3.6e-05,0.004,0.004,0.00037,1,1,0.1,0.19,0.19,0.098,4.5e-07,4.5e-07,7e-08,4e-06,4e-06,3.8e-06,0,0,0,0,0,0,0,0 -2490000,0.98,-0.0079,-0.015,0.19,0.033,0.0031,-0.14,0.011,-0.00027,-0.079,-1.7e-05,-2.3e-05,8.3e-07,0,0,-0.00013,0.2,0.002,0.44,0,0,0,0,0,3.5e-05,0.0044,0.0044,0.00036,1.3,1.3,0.1,0.28,0.28,0.097,4.5e-07,4.5e-07,6.2e-08,4e-06,4e-06,3.7e-06,0,0,0,0,0,0,0,0 -2590000,0.98,-0.0078,-0.015,0.19,0.023,0.0022,-0.15,0.0069,8.2e-05,-0.085,-1.8e-05,-2.7e-05,9e-07,0,0,-0.00015,0.2,0.002,0.44,0,0,0,0,0,3.3e-05,0.0032,0.0032,0.00035,0.89,0.89,0.099,0.18,0.18,0.094,3.8e-07,3.8e-07,5.6e-08,4e-06,4e-06,3.6e-06,0,0,0,0,0,0,0,0 -2690000,0.98,-0.0078,-0.015,0.19,0.027,0.0043,-0.15,0.0095,0.00041,-0.084,-1.8e-05,-2.7e-05,8.9e-07,0,0,-0.00018,0.2,0.002,0.44,0,0,0,0,0,3.3e-05,0.0035,0.0035,0.00034,1.1,1.1,0.097,0.25,0.25,0.091,3.8e-07,3.8e-07,5.1e-08,4e-06,4e-06,3.6e-06,0,0,0,0,0,0,0,0 -2790000,0.98,-0.0077,-0.015,0.19,0.021,0.0046,-0.14,0.006,0.00051,-0.081,-1.9e-05,-3e-05,9.2e-07,0,0,-0.00022,0.2,0.002,0.44,0,0,0,0,0,3.2e-05,0.0027,0.0027,0.00033,0.77,0.77,0.096,0.16,0.16,0.089,3.2e-07,3.2e-07,4.6e-08,4e-06,4e-06,3.5e-06,0,0,0,0,0,0,0,0 -2890000,0.98,-0.0077,-0.015,0.19,0.025,0.0043,-0.14,0.0084,0.00092,-0.082,-1.9e-05,-3e-05,9.2e-07,0,0,-0.00025,0.2,0.002,0.44,0,0,0,0,0,3.1e-05,0.0029,0.0029,0.00032,0.95,0.95,0.096,0.23,0.23,0.089,3.2e-07,3.2e-07,4.2e-08,4e-06,4e-06,3.4e-06,0,0,0,0,0,0,0,0 -2990000,0.98,-0.0076,-0.015,0.19,0.019,0.0033,-0.15,0.0055,0.00069,-0.086,-2e-05,-3.3e-05,9.4e-07,0,0,-0.00028,0.2,0.002,0.44,0,0,0,0,0,3e-05,0.0023,0.0023,0.00031,0.67,0.67,0.095,0.15,0.15,0.088,2.7e-07,2.7e-07,3.8e-08,4e-06,4e-06,3.3e-06,0,0,0,0,0,0,0,0 -3090000,0.98,-0.0076,-0.015,0.19,0.026,0.0024,-0.15,0.0077,0.00092,-0.088,-2e-05,-3.3e-05,9.4e-07,0,0,-0.00031,0.2,0.002,0.44,0,0,0,0,0,2.9e-05,0.0025,0.0025,0.0003,0.82,0.82,0.095,0.22,0.22,0.086,2.7e-07,2.7e-07,3.5e-08,4e-06,4e-06,3.2e-06,0,0,0,0,0,0,0,0 -3190000,0.98,-0.0076,-0.015,0.19,0.021,0.0011,-0.15,0.0052,0.00052,-0.098,-2e-05,-3.6e-05,9.5e-07,0,0,-0.00033,0.2,0.002,0.44,0,0,0,0,0,2.8e-05,0.002,0.002,0.00029,0.59,0.59,0.096,0.14,0.14,0.087,2.3e-07,2.3e-07,3.2e-08,4e-06,4e-06,3.1e-06,0,0,0,0,0,0,0,0 -3290000,0.98,-0.0075,-0.015,0.19,0.024,0.0019,-0.15,0.0075,0.00061,-0.11,-2e-05,-3.6e-05,9.5e-07,0,0,-0.00034,0.2,0.002,0.44,0,0,0,0,0,2.8e-05,0.0022,0.0022,0.00028,0.73,0.73,0.095,0.2,0.2,0.086,2.3e-07,2.3e-07,2.9e-08,4e-06,4e-06,3e-06,0,0,0,0,0,0,0,0 -3390000,0.98,-0.0073,-0.014,0.19,0.019,0.0032,-0.15,0.0051,0.00045,-0.1,-2.1e-05,-3.8e-05,9.5e-07,0,0,-0.0004,0.2,0.002,0.44,0,0,0,0,0,2.7e-05,0.0017,0.0017,0.00027,0.53,0.53,0.095,0.14,0.14,0.085,1.9e-07,1.9e-07,2.7e-08,4e-06,4e-06,2.9e-06,0,0,0,0,0,0,0,0 -3490000,0.98,-0.0072,-0.014,0.19,0.024,0.0064,-0.15,0.0073,0.00092,-0.1,-2.1e-05,-3.8e-05,9.5e-07,0,0,-0.00044,0.2,0.002,0.44,0,0,0,0,0,2.6e-05,0.0019,0.0019,0.00027,0.65,0.65,0.095,0.19,0.19,0.086,1.9e-07,1.9e-07,2.5e-08,4e-06,4e-06,2.8e-06,0,0,0,0,0,0,0,0 -3590000,0.98,-0.0071,-0.014,0.19,0.021,0.0056,-0.15,0.0051,0.00082,-0.11,-2.2e-05,-4e-05,9.5e-07,0,0,-0.00047,0.2,0.002,0.44,0,0,0,0,0,2.6e-05,0.0015,0.0015,0.00026,0.48,0.48,0.094,0.13,0.13,0.086,1.6e-07,1.6e-07,2.3e-08,4e-06,4e-06,2.6e-06,0,0,0,0,0,0,0,0 -3690000,0.98,-0.0071,-0.014,0.19,0.023,0.0071,-0.15,0.0074,0.0014,-0.11,-2.1e-05,-4e-05,9.5e-07,0,0,-0.00052,0.2,0.002,0.44,0,0,0,0,0,2.5e-05,0.0017,0.0017,0.00025,0.59,0.59,0.093,0.18,0.18,0.085,1.6e-07,1.6e-07,2.1e-08,4e-06,4e-06,2.5e-06,0,0,0,0,0,0,0,0 -3790000,0.98,-0.007,-0.014,0.19,0.018,0.0098,-0.15,0.005,0.0012,-0.11,-2.2e-05,-4.2e-05,9.4e-07,0,0,-0.00055,0.2,0.002,0.44,0,0,0,0,0,2.4e-05,0.0014,0.0014,0.00025,0.44,0.44,0.093,0.12,0.12,0.086,1.4e-07,1.4e-07,2e-08,4e-06,4e-06,2.4e-06,0,0,0,0,0,0,0,0 -3890000,0.98,-0.007,-0.014,0.19,0.019,0.011,-0.14,0.0069,0.0023,-0.11,-2.2e-05,-4.2e-05,9.4e-07,0,0,-0.00059,0.2,0.002,0.44,0,0,0,0,0,2.4e-05,0.0015,0.0015,0.00024,0.54,0.54,0.091,0.17,0.17,0.086,1.4e-07,1.4e-07,1.8e-08,4e-06,4e-06,2.2e-06,0,0,0,0,0,0,0,0 -3990000,0.98,-0.007,-0.014,0.19,0.023,0.013,-0.14,0.0091,0.0035,-0.11,-2.2e-05,-4.2e-05,9.4e-07,0,0,-0.00064,0.2,0.002,0.44,0,0,0,0,0,2.3e-05,0.0017,0.0017,0.00024,0.66,0.66,0.089,0.22,0.22,0.085,1.4e-07,1.4e-07,1.7e-08,4e-06,4e-06,2.1e-06,0,0,0,0,0,0,0,0 -4090000,0.98,-0.0069,-0.014,0.19,0.02,0.011,-0.12,0.0067,0.0028,-0.098,-2.2e-05,-4.4e-05,9.3e-07,0,0,-0.00072,0.2,0.002,0.44,0,0,0,0,0,2.3e-05,0.0013,0.0013,0.00023,0.5,0.5,0.087,0.16,0.16,0.085,1.2e-07,1.2e-07,1.6e-08,4e-06,4e-06,2e-06,0,0,0,0,0,0,0,0 -4190000,0.98,-0.007,-0.014,0.19,0.023,0.011,-0.12,0.0088,0.0039,-0.1,-2.2e-05,-4.4e-05,9.3e-07,0,0,-0.00074,0.2,0.002,0.44,0,0,0,0,0,2.2e-05,0.0015,0.0015,0.00023,0.6,0.6,0.086,0.21,0.21,0.086,1.2e-07,1.2e-07,1.5e-08,4e-06,4e-06,1.9e-06,0,0,0,0,0,0,0,0 -4290000,0.98,-0.0072,-0.014,0.19,0.02,0.01,-0.12,0.0064,0.0029,-0.11,-2.2e-05,-4.6e-05,9.1e-07,0,0,-0.00077,0.2,0.002,0.44,0,0,0,0,0,2.2e-05,0.0012,0.0012,0.00022,0.46,0.46,0.084,0.15,0.15,0.085,9.6e-08,9.6e-08,1.4e-08,4e-06,4e-06,1.7e-06,0,0,0,0,0,0,0,0 -4390000,0.98,-0.0071,-0.014,0.19,0.024,0.01,-0.11,0.0087,0.0039,-0.095,-2.2e-05,-4.6e-05,9.1e-07,0,0,-0.00083,0.2,0.002,0.44,0,0,0,0,0,2.1e-05,0.0013,0.0013,0.00022,0.56,0.56,0.081,0.2,0.2,0.084,9.6e-08,9.6e-08,1.3e-08,4e-06,4e-06,1.6e-06,0,0,0,0,0,0,0,0 -4490000,0.98,-0.0072,-0.014,0.19,0.019,0.01,-0.11,0.0064,0.0029,-0.095,-2.1e-05,-4.8e-05,9e-07,0,0,-0.00086,0.2,0.002,0.44,0,0,0,0,0,2.1e-05,0.001,0.001,0.00021,0.43,0.43,0.08,0.14,0.14,0.085,7.9e-08,7.9e-08,1.2e-08,4e-06,4e-06,1.5e-06,0,0,0,0,0,0,0,0 -4590000,0.98,-0.0071,-0.014,0.19,0.023,0.01,-0.11,0.0085,0.0039,-0.098,-2.1e-05,-4.8e-05,8.9e-07,0,0,-0.00089,0.2,0.002,0.44,0,0,0,0,0,2.1e-05,0.0011,0.0011,0.00021,0.51,0.51,0.077,0.19,0.19,0.084,7.9e-08,7.9e-08,1.1e-08,4e-06,4e-06,1.4e-06,0,0,0,0,0,0,0,0 -4690000,0.98,-0.0071,-0.013,0.19,0.017,0.0083,-0.1,0.0062,0.0029,-0.09,-2.1e-05,-5e-05,8.8e-07,0,0,-0.00093,0.2,0.002,0.44,0,0,0,0,0,2e-05,0.00092,0.00092,0.0002,0.39,0.39,0.074,0.14,0.14,0.083,6.4e-08,6.4e-08,1.1e-08,4e-06,4e-06,1.3e-06,0,0,0,0,0,0,0,0 -4790000,0.98,-0.007,-0.013,0.19,0.014,0.0094,-0.099,0.0078,0.0038,-0.093,-2.1e-05,-5e-05,8.8e-07,0,0,-0.00095,0.2,0.002,0.44,0,0,0,0,0,2e-05,0.001,0.001,0.0002,0.47,0.47,0.073,0.18,0.18,0.084,6.4e-08,6.4e-08,1e-08,4e-06,4e-06,1.2e-06,0,0,0,0,0,0,0,0 -4890000,0.98,-0.007,-0.013,0.19,0.01,0.0054,-0.093,0.0052,0.0026,-0.088,-2.1e-05,-5.1e-05,8.7e-07,0,0,-0.00099,0.2,0.002,0.44,0,0,0,0,0,1.9e-05,0.0008,0.0008,0.0002,0.36,0.36,0.07,0.13,0.13,0.083,5.2e-08,5.2e-08,9.4e-09,4e-06,4e-06,1.1e-06,0,0,0,0,0,0,0,0 -4990000,0.98,-0.0069,-0.013,0.19,0.014,0.0072,-0.085,0.0064,0.0033,-0.083,-2.1e-05,-5.1e-05,8.7e-07,0,0,-0.001,0.2,0.002,0.44,0,0,0,0,0,1.9e-05,0.00087,0.00087,0.00019,0.43,0.43,0.067,0.17,0.17,0.082,5.2e-08,5.2e-08,8.9e-09,4e-06,4e-06,1e-06,0,0,0,0,0,0,0,0 -5090000,0.98,-0.0069,-0.013,0.19,0.01,0.0064,-0.082,0.0045,0.0023,-0.082,-2.1e-05,-5.2e-05,8.6e-07,0,0,-0.001,0.2,0.002,0.44,0,0,0,0,0,1.9e-05,0.0007,0.0007,0.00019,0.33,0.33,0.065,0.12,0.12,0.082,4.2e-08,4.2e-08,8.4e-09,4e-06,4e-06,9.8e-07,0,0,0,0,0,0,0,0 -5190000,0.98,-0.0067,-0.013,0.19,0.0089,0.0096,-0.08,0.0054,0.0031,-0.079,-2.1e-05,-5.2e-05,8.6e-07,0,0,-0.0011,0.2,0.002,0.44,0,0,0,0,0,1.8e-05,0.00075,0.00075,0.00019,0.39,0.39,0.063,0.16,0.16,0.081,4.2e-08,4.2e-08,7.9e-09,4e-06,4e-06,9.1e-07,0,0,0,0,0,0,0,0 -5290000,0.98,-0.0066,-0.013,0.19,0.0071,0.0092,-0.068,0.0037,0.0024,-0.072,-2.1e-05,-5.3e-05,8.5e-07,0,0,-0.0011,0.2,0.002,0.44,0,0,0,0,0,1.8e-05,0.0006,0.0006,0.00018,0.3,0.3,0.06,0.12,0.12,0.08,3.4e-08,3.4e-08,7.5e-09,4e-06,4e-06,8.4e-07,0,0,0,0,0,0,0,0 -5390000,0.98,-0.0066,-0.013,0.19,0.0055,0.012,-0.065,0.0044,0.0034,-0.067,-2.1e-05,-5.3e-05,8.5e-07,0,0,-0.0011,0.2,0.002,0.44,0,0,0,0,0,1.8e-05,0.00065,0.00065,0.00018,0.36,0.36,0.057,0.15,0.15,0.079,3.4e-08,3.4e-08,7.1e-09,4e-06,4e-06,7.8e-07,0,0,0,0,0,0,0,0 -5490000,0.98,-0.0066,-0.013,0.19,0.0043,0.013,-0.06,0.0028,0.0028,-0.065,-2.1e-05,-5.4e-05,8.4e-07,0,0,-0.0011,0.2,0.002,0.44,0,0,0,0,0,1.7e-05,0.00052,0.00052,0.00018,0.28,0.28,0.056,0.11,0.11,0.079,2.8e-08,2.8e-08,6.7e-09,4e-06,4e-06,7.3e-07,0,0,0,0,0,0,0,0 -5590000,0.98,-0.0066,-0.013,0.19,0.0043,0.017,-0.053,0.0033,0.0044,-0.058,-2.1e-05,-5.4e-05,8.4e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.7e-05,0.00056,0.00056,0.00017,0.32,0.32,0.053,0.15,0.15,0.078,2.8e-08,2.8e-08,6.4e-09,4e-06,4e-06,6.7e-07,0,0,0,0,0,0,0,0 -5690000,0.98,-0.0067,-0.013,0.19,0.0034,0.017,-0.052,0.0022,0.0036,-0.056,-2e-05,-5.5e-05,8.2e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.7e-05,0.00045,0.00045,0.00017,0.25,0.25,0.051,0.11,0.11,0.076,2.2e-08,2.2e-08,6.1e-09,4e-06,4e-06,6.2e-07,0,0,0,0,0,0,0,0 -5790000,0.98,-0.0065,-0.013,0.19,0.004,0.02,-0.049,0.0026,0.0054,-0.053,-2e-05,-5.5e-05,8.2e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.7e-05,0.00048,0.00048,0.00017,0.29,0.29,0.05,0.14,0.14,0.077,2.2e-08,2.2e-08,5.8e-09,4e-06,4e-06,5.8e-07,0,0,0,0,0,0,0,0 -5890000,0.98,-0.0066,-0.013,0.19,0.005,0.018,-0.048,0.0018,0.0044,-0.056,-1.9e-05,-5.5e-05,8.1e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.6e-05,0.00039,0.00039,0.00016,0.23,0.23,0.048,0.1,0.1,0.075,1.8e-08,1.8e-08,5.5e-09,4e-06,4e-06,5.4e-07,0,0,0,0,0,0,0,0 -5990000,0.98,-0.0065,-0.013,0.19,0.0061,0.019,-0.041,0.0024,0.0062,-0.05,-1.9e-05,-5.5e-05,8.1e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.6e-05,0.00041,0.00041,0.00016,0.27,0.27,0.045,0.13,0.13,0.074,1.8e-08,1.8e-08,5.2e-09,4e-06,4e-06,5e-07,0,0,0,0,0,0,0,0 -6090000,0.98,-0.0065,-0.013,0.19,0.0058,0.021,-0.039,0.003,0.0082,-0.048,-1.9e-05,-5.5e-05,8.1e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.6e-05,0.00044,0.00044,0.00016,0.31,0.31,0.044,0.17,0.17,0.074,1.8e-08,1.8e-08,5e-09,4e-06,4e-06,4.7e-07,0,0,0,0,0,0,0,0 -6190000,0.98,-0.0067,-0.013,0.19,0.0037,0.019,-0.037,0.0022,0.0066,-0.047,-1.9e-05,-5.6e-05,7.9e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.6e-05,0.00036,0.00036,0.00016,0.24,0.24,0.042,0.13,0.13,0.073,1.5e-08,1.5e-08,4.8e-09,4e-06,4e-06,4.4e-07,0,0,0,0,0,0,0,0 -6290000,0.98,-0.0066,-0.013,0.19,0.0022,0.021,-0.041,0.0026,0.0085,-0.053,-1.9e-05,-5.6e-05,7.9e-07,0,0,-0.0012,0.2,0.002,0.44,0,0,0,0,0,1.5e-05,0.00038,0.00038,0.00015,0.28,0.28,0.04,0.16,0.16,0.072,1.5e-08,1.5e-08,4.5e-09,4e-06,4e-06,4.1e-07,0,0,0,0,0,0,0,0 -6390000,0.98,-0.0066,-0.013,0.19,0.0032,0.018,-0.042,0.0017,0.0068,-0.056,-1.8e-05,-5.6e-05,7.8e-07,0,0,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.2e-05,0.0003,0.0003,0.0015,0.2,0.2,0.039,0.12,0.12,0.072,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,3.8e-07,0,0,0,0,0,0,0,0 -6490000,0.98,-0.0066,-0.013,0.19,0.00098,0.017,-0.039,0.0019,0.0085,-0.053,-1.8e-05,-5.6e-05,7.6e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,4.2e-05,0.0003,0.0003,0.00097,0.2,0.21,0.038,0.15,0.15,0.07,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,3.6e-07,0,0,0,0,0,0,0,0 -6590000,0.98,-0.0065,-0.013,0.19,0.00028,0.02,-0.042,0.002,0.01,-0.056,-1.8e-05,-5.6e-05,7.3e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,3.2e-05,0.0003,0.0003,0.00071,0.21,0.21,0.036,0.18,0.18,0.069,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,3.3e-07,0,0,0,0,0,0,0,0 -6690000,0.98,-0.0064,-0.013,0.19,-0.0024,0.022,-0.044,0.0019,0.012,-0.057,-1.8e-05,-5.6e-05,6.8e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.6e-05,0.0003,0.0003,0.00057,0.22,0.22,0.035,0.22,0.22,0.068,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,3.1e-07,0,0,0,0,0,0,0,0 -6790000,0.98,-0.0064,-0.013,0.19,-0.00059,0.024,-0.042,0.0017,0.015,-0.058,-1.8e-05,-5.6e-05,6.5e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2.3e-05,0.0003,0.0003,0.00048,0.23,0.23,0.034,0.26,0.26,0.068,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.9e-07,0,0,0,0,0,0,0,0 -6890000,0.98,-0.0062,-0.013,0.19,-0.0011,0.024,-0.038,0.0016,0.017,-0.055,-1.8e-05,-5.6e-05,6.2e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,2e-05,0.00031,0.00031,0.00041,0.25,0.25,0.032,0.31,0.31,0.067,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.7e-07,0,0,0,0,0,0,0,0 -6990000,0.98,-0.0062,-0.013,0.19,-0.0011,0.026,-0.037,0.0015,0.02,-0.055,-1.8e-05,-5.6e-05,6.1e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.8e-05,0.00031,0.00031,0.00036,0.27,0.27,0.031,0.36,0.36,0.066,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.6e-07,0,0,0,0,0,0,0,0 -7090000,0.98,-0.0061,-0.012,0.19,-0.002,0.032,-0.037,0.0013,0.023,-0.056,-1.8e-05,-5.6e-05,6.1e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.7e-05,0.00031,0.00031,0.00032,0.29,0.29,0.03,0.42,0.42,0.066,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.4e-07,0,0,0,0,0,0,0,0 -7190000,0.98,-0.006,-0.013,0.19,-0.0025,0.034,-0.036,0.0011,0.026,-0.058,-1.8e-05,-5.6e-05,5.4e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.6e-05,0.00032,0.00032,0.00029,0.32,0.32,0.029,0.49,0.49,0.065,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.3e-07,0,0,0,0,0,0,0,0 -7290000,0.98,-0.006,-0.013,0.19,-0.0017,0.038,-0.034,0.0008,0.029,-0.054,-1.8e-05,-5.6e-05,5.4e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.5e-05,0.00032,0.00032,0.00026,0.35,0.35,0.028,0.56,0.56,0.064,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2.1e-07,0,0,0,0,0,0,0,0 -7390000,0.98,-0.0058,-0.013,0.19,-0.0035,0.041,-0.032,0.00056,0.033,-0.052,-1.8e-05,-5.6e-05,5.6e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.4e-05,0.00033,0.00033,0.00024,0.38,0.38,0.027,0.64,0.64,0.064,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,2e-07,0,0,0,0,0,0,0,0 -7490000,0.98,-0.0058,-0.013,0.19,-0.0012,0.045,-0.026,0.00037,0.038,-0.046,-1.8e-05,-5.6e-05,6.7e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.3e-05,0.00034,0.00034,0.00022,0.42,0.42,0.026,0.73,0.73,0.063,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,1.9e-07,0,0,0,0,0,0,0,0 -7590000,0.98,-0.0059,-0.013,0.19,-0.00017,0.048,-0.023,0.00031,0.042,-0.041,-1.8e-05,-5.6e-05,6.7e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.3e-05,0.00034,0.00034,0.00021,0.45,0.45,0.025,0.83,0.83,0.062,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,1.8e-07,0,0,0,0,0,0,0,0 -7690000,0.98,-0.0059,-0.013,0.19,-0.00052,0.052,-0.022,0.00028,0.047,-0.036,-1.8e-05,-5.6e-05,6.4e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.2e-05,0.00035,0.00035,0.0002,0.5,0.5,0.025,0.95,0.95,0.062,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,1.7e-07,0,0,0,0,0,0,0,0 -7790000,0.98,-0.0058,-0.013,0.19,0.0011,0.054,-0.024,0.00029,0.052,-0.041,-1.8e-05,-5.6e-05,5.6e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.2e-05,0.00036,0.00036,0.00018,0.54,0.54,0.024,1.1,1.1,0.061,1.2e-08,1.2e-08,4.5e-09,4e-06,4e-06,1.6e-07,0,0,0,0,0,0,0,0 -7890000,0.98,-0.0058,-0.013,0.19,-0.00029,0.059,-0.025,0.00024,0.057,-0.045,-1.8e-05,-5.6e-05,5e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.2e-05,0.00037,0.00037,0.00017,0.59,0.59,0.023,1.2,1.2,0.06,1.2e-08,1.2e-08,4.4e-09,4e-06,4e-06,1.5e-07,0,0,0,0,0,0,0,0 -7990000,0.98,-0.0057,-0.013,0.19,-4.2e-05,0.062,-0.021,0.00022,0.063,-0.042,-1.8e-05,-5.6e-05,5e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.1e-05,0.00037,0.00037,0.00017,0.64,0.64,0.022,1.4,1.4,0.059,1.2e-08,1.2e-08,4.4e-09,4e-06,4e-06,1.4e-07,0,0,0,0,0,0,0,0 -8090000,0.98,-0.0056,-0.013,0.19,0.0014,0.067,-0.022,0.0003,0.069,-0.044,-1.8e-05,-5.6e-05,2.4e-07,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.1e-05,0.00038,0.00038,0.00016,0.7,0.7,0.022,1.5,1.5,0.059,1.2e-08,1.2e-08,4.4e-09,4e-06,4e-06,1.4e-07,0,0,0,0,0,0,0,0 -8190000,0.98,-0.0056,-0.013,0.19,0.002,0.073,-0.018,0.00044,0.076,-0.038,-1.8e-05,-5.6e-05,3.9e-08,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1.1e-05,0.00039,0.00039,0.00015,0.76,0.76,0.021,1.7,1.7,0.058,1.2e-08,1.2e-08,4.4e-09,4e-06,4e-06,1.3e-07,0,0,0,0,0,0,0,0 -8290000,0.98,-0.0056,-0.013,0.19,0.0042,0.077,-0.016,0.00073,0.082,-0.038,-1.8e-05,-5.6e-05,-4.3e-08,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1e-05,0.0004,0.0004,0.00015,0.81,0.81,0.02,1.9,1.9,0.057,1.1e-08,1.1e-08,4.4e-09,4e-06,4e-06,1.2e-07,0,0,0,0,0,0,0,0 -8390000,0.98,-0.0056,-0.013,0.19,0.002,0.08,-0.015,0.001,0.09,-0.036,-1.8e-05,-5.6e-05,-1.2e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,1e-05,0.00041,0.00041,0.00014,0.88,0.88,0.02,2.2,2.2,0.057,1.1e-08,1.1e-08,4.4e-09,4e-06,4e-06,1.2e-07,0,0,0,0,0,0,0,0 -8490000,0.98,-0.0055,-0.013,0.19,0.0018,0.084,-0.017,0.0012,0.096,-0.041,-1.8e-05,-5.6e-05,2.3e-08,0,0,-0.0013,0.2,0.002,0.43,0,0,0,0,0,1e-05,0.00042,0.00042,0.00014,0.94,0.94,0.019,2.4,2.4,0.056,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1.1e-07,0,0,0,0,0,0,0,0 -8590000,0.98,-0.0054,-0.013,0.19,0.0028,0.088,-0.012,0.0014,0.1,-0.036,-1.8e-05,-5.6e-05,-1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,1e-05,0.00043,0.00043,0.00013,1,1,0.018,2.7,2.7,0.055,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1.1e-07,0,0,0,0,0,0,0,0 -8690000,0.98,-0.0055,-0.013,0.19,0.0028,0.089,-0.014,0.0016,0.11,-0.037,-1.8e-05,-5.6e-05,-7.9e-08,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.8e-06,0.00043,0.00043,0.00013,1.1,1.1,0.018,2.9,2.9,0.055,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,1e-07,0,0,0,0,0,0,0,0 -8790000,0.98,-0.0054,-0.013,0.19,0.0041,0.093,-0.013,0.0018,0.12,-0.035,-1.8e-05,-5.6e-05,-3.1e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.7e-06,0.00045,0.00045,0.00013,1.2,1.2,0.018,3.3,3.3,0.055,1.1e-08,1.1e-08,4.3e-09,4e-06,4e-06,9.6e-08,0,0,0,0,0,0,0,0 -8890000,0.98,-0.0055,-0.013,0.19,0.0041,0.096,-0.0091,0.0022,0.13,-0.029,-1.8e-05,-5.6e-05,-1.4e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.6e-06,0.00045,0.00045,0.00012,1.2,1.2,0.017,3.5,3.6,0.054,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,9.1e-08,0,0,0,0,0,0,0,0 -8990000,0.98,-0.0054,-0.013,0.19,0.0033,0.1,-0.0083,0.0026,0.14,-0.032,-1.8e-05,-5.6e-05,1.2e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.5e-06,0.00046,0.00046,0.00012,1.3,1.3,0.017,4,4,0.054,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,8.8e-08,0,0,0,0,0,0,0,0 -9090000,0.98,-0.0055,-0.013,0.19,0.0036,0.1,-0.0093,0.0029,0.14,-0.032,-1.7e-05,-5.6e-05,4.2e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.4e-06,0.00046,0.00046,0.00012,1.3,1.3,0.016,4.2,4.2,0.053,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,8.4e-08,0,0,0,0,0,0,0,0 -9190000,0.98,-0.0054,-0.013,0.19,0.0072,0.11,-0.0088,0.0035,0.15,-0.032,-1.7e-05,-5.6e-05,6.9e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.3e-06,0.00047,0.00047,0.00012,1.4,1.4,0.016,4.7,4.7,0.052,1.1e-08,1.1e-08,4.2e-09,4e-06,4e-06,8e-08,0,0,0,0,0,0,0,0 -9290000,0.98,-0.0053,-0.013,0.19,0.0094,0.11,-0.0072,0.0042,0.15,-0.03,-1.7e-05,-5.6e-05,7.6e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.3e-06,0.00047,0.00047,0.00012,1.5,1.5,0.015,5,5,0.052,1.1e-08,1.1e-08,4.1e-09,4e-06,4e-06,7.6e-08,0,0,0,0,0,0,0,0 -9390000,0.98,-0.0052,-0.013,0.19,0.0094,0.11,-0.0061,0.0051,0.16,-0.03,-1.7e-05,-5.6e-05,4.5e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.2e-06,0.00048,0.00048,0.00011,1.6,1.6,0.015,5.5,5.5,0.052,1.1e-08,1.1e-08,4.1e-09,4e-06,4e-06,7.3e-08,0,0,0,0,0,0,0,0 -9490000,0.98,-0.0053,-0.013,0.19,0.0092,0.11,-0.0044,0.0057,0.17,-0.027,-1.7e-05,-5.7e-05,5.6e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.2e-06,0.00047,0.00047,0.00011,1.6,1.6,0.015,5.8,5.8,0.051,1e-08,1e-08,4e-09,4e-06,4e-06,7e-08,0,0,0,0,0,0,0,0 -9590000,0.98,-0.0054,-0.013,0.19,0.0091,0.11,-0.0043,0.0064,0.18,-0.029,-1.7e-05,-5.6e-05,-7.2e-08,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.1e-06,0.00049,0.00049,0.00011,1.7,1.7,0.014,6.4,6.4,0.05,1e-08,1e-08,4e-09,4e-06,4e-06,6.7e-08,0,0,0,0,0,0,0,0 -9690000,0.98,-0.0054,-0.013,0.19,0.0088,0.11,-0.0014,0.0069,0.18,-0.027,-1.7e-05,-5.7e-05,-2.6e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9.1e-06,0.00047,0.00047,0.00011,1.7,1.7,0.014,6.6,6.6,0.05,9.9e-09,9.9e-09,4e-09,4e-06,4e-06,6.5e-08,0,0,0,0,0,0,0,0 -9790000,0.98,-0.0054,-0.013,0.19,0.01,0.11,-0.0027,0.0077,0.19,-0.028,-1.7e-05,-5.7e-05,-8.8e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00049,0.00049,0.00011,1.8,1.8,0.014,7.3,7.3,0.05,9.9e-09,9.9e-09,3.9e-09,4e-06,4e-06,6.2e-08,0,0,0,0,0,0,0,0 -9890000,0.98,-0.0055,-0.013,0.19,0.012,0.11,-0.0014,0.0082,0.18,-0.029,-1.6e-05,-5.7e-05,-7.6e-07,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00047,0.00047,0.00011,1.8,1.8,0.013,7.4,7.4,0.049,9.6e-09,9.6e-09,3.9e-09,4e-06,4e-06,6e-08,0,0,0,0,0,0,0,0 -9990000,0.98,-0.0054,-0.013,0.19,0.014,0.11,-0.00073,0.0093,0.2,-0.031,-1.6e-05,-5.7e-05,-1.3e-06,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00049,0.00049,0.00011,1.9,1.9,0.013,8.2,8.2,0.049,9.6e-09,9.6e-09,3.8e-09,4e-06,4e-06,5.8e-08,0,0,0,0,0,0,0,0 -10090000,0.98,-0.0055,-0.013,0.19,0.012,0.11,0.00048,0.0096,0.19,-0.029,-1.6e-05,-5.7e-05,-1.9e-06,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,9e-06,0.00046,0.00046,0.00011,1.9,1.9,0.013,8.2,8.2,0.048,9.3e-09,9.3e-09,3.8e-09,4e-06,4e-06,5.6e-08,0,0,0,0,0,0,0,0 -10190000,0.98,-0.0055,-0.013,0.19,0.0099,0.11,0.0014,0.011,0.2,-0.03,-1.6e-05,-5.7e-05,-2.6e-06,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00048,0.00048,0.00011,2,2,0.012,9,9,0.048,9.3e-09,9.3e-09,3.7e-09,4e-06,4e-06,5.4e-08,0,0,0,0,0,0,0,0 -10290000,0.98,-0.0055,-0.013,0.19,0.01,0.11,0.00029,0.012,0.21,-0.029,-1.6e-05,-5.7e-05,-2.3e-06,0,0,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00049,0.00049,0.00011,2.1,2.1,0.012,9.9,9.9,0.048,9.3e-09,9.3e-09,3.7e-09,4e-06,4e-06,5.2e-08,0,0,0,0,0,0,0,0 -10390000,0.98,-0.0055,-0.012,0.19,0.007,0.0051,-0.0025,0.00075,0.00014,-0.028,-1.6e-05,-5.7e-05,-2.1e-06,-3.6e-10,2.5e-10,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00051,0.00051,0.00011,0.25,0.25,0.56,0.25,0.25,0.048,9.3e-09,9.3e-09,3.7e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10490000,0.98,-0.0054,-0.012,0.19,0.0083,0.0074,0.007,0.0015,0.00073,-0.023,-1.6e-05,-5.7e-05,-2.6e-06,-1.2e-08,8.3e-09,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00053,0.00053,0.00011,0.26,0.26,0.55,0.26,0.26,0.057,9.3e-09,9.3e-09,3.6e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10590000,0.98,-0.0053,-0.012,0.19,-0.0014,0.0053,0.013,-0.0012,-0.0054,-0.021,-1.6e-05,-5.7e-05,-2.3e-06,3.2e-06,6.7e-08,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00053,0.00053,0.00011,0.13,0.13,0.27,0.13,0.13,0.055,9.2e-09,9.2e-09,3.5e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10690000,0.98,-0.0053,-0.012,0.19,-0.00034,0.0064,0.016,-0.0013,-0.0049,-0.017,-1.6e-05,-5.7e-05,-2.5e-06,3.2e-06,7.4e-08,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00055,0.00055,0.00011,0.14,0.14,0.26,0.14,0.14,0.065,9.2e-09,9.2e-09,3.5e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10790000,0.98,-0.0054,-0.012,0.19,0.0015,0.0029,0.014,-0.00078,-0.0047,-0.015,-1.6e-05,-5.7e-05,-2.5e-06,5.1e-06,4.7e-06,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00054,0.00054,0.00011,0.1,0.1,0.17,0.091,0.091,0.062,9e-09,9e-09,3.4e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10890000,0.98,-0.0053,-0.013,0.19,0.0015,0.0063,0.01,-0.00063,-0.0043,-0.018,-1.6e-05,-5.7e-05,-1.9e-06,5.1e-06,4.7e-06,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00056,0.00056,0.00011,0.12,0.12,0.16,0.098,0.098,0.068,9e-09,9e-09,3.4e-09,4e-06,4e-06,5e-08,0,0,0,0,0,0,0,0 -10990000,0.98,-0.0054,-0.013,0.19,0.001,0.012,0.016,-0.00046,-0.003,-0.012,-1.6e-05,-5.7e-05,-2.1e-06,6.5e-06,7.2e-06,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00053,0.00053,0.00011,0.092,0.092,0.12,0.073,0.073,0.065,8.6e-09,8.6e-09,3.3e-09,3.9e-06,3.9e-06,5e-08,0,0,0,0,0,0,0,0 -11090000,0.98,-0.0055,-0.013,0.19,0.0019,0.017,0.02,-0.00036,-0.0016,-0.0075,-1.6e-05,-5.7e-05,-2.8e-06,6.5e-06,7.2e-06,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00054,0.00054,0.00011,0.11,0.11,0.11,0.08,0.08,0.069,8.6e-09,8.6e-09,3.3e-09,3.9e-06,3.9e-06,5e-08,0,0,0,0,0,0,0,0 -11190000,0.98,-0.0058,-0.013,0.19,0.0036,0.017,0.026,0.001,-0.0018,-0.00047,-1.5e-05,-5.7e-05,-3.1e-06,6.2e-06,1.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00049,0.00049,0.00011,0.092,0.092,0.083,0.063,0.063,0.066,8e-09,8e-09,3.2e-09,3.8e-06,3.8e-06,5e-08,0,0,0,0,0,0,0,0 -11290000,0.98,-0.0058,-0.013,0.19,0.0037,0.018,0.026,0.0014,2.6e-05,-0.00024,-1.5e-05,-5.7e-05,-3.3e-06,6.2e-06,1.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.9e-06,0.00051,0.00051,0.00011,0.11,0.11,0.077,0.07,0.07,0.069,8e-09,8e-09,3.2e-09,3.8e-06,3.8e-06,5e-08,0,0,0,0,0,0,0,0 -11390000,0.98,-0.0059,-0.013,0.19,0.0022,0.016,0.016,0.00085,-0.00079,-0.0087,-1.4e-05,-5.8e-05,-3.2e-06,1.1e-05,2.3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00044,0.00044,0.00011,0.093,0.093,0.062,0.057,0.057,0.066,7.4e-09,7.4e-09,3.1e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0 -11490000,0.98,-0.0059,-0.013,0.19,0.0012,0.017,0.02,0.00095,0.00085,-0.0026,-1.4e-05,-5.8e-05,-3.1e-06,1.1e-05,2.3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00046,0.00046,0.00011,0.11,0.11,0.057,0.065,0.065,0.067,7.4e-09,7.4e-09,3.1e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0 -11590000,0.98,-0.0062,-0.012,0.19,0.0031,0.013,0.018,0.00081,-0.00023,-0.0037,-1.3e-05,-5.8e-05,-3.1e-06,1.5e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00039,0.00039,0.00011,0.092,0.092,0.048,0.054,0.054,0.065,6.7e-09,6.7e-09,3e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0 -11690000,0.98,-0.0061,-0.012,0.19,0.0035,0.017,0.018,0.0012,0.0013,-0.0051,-1.3e-05,-5.8e-05,-2.8e-06,1.5e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.0004,0.0004,0.00011,0.11,0.11,0.044,0.062,0.062,0.066,6.7e-09,6.7e-09,3e-09,3.7e-06,3.7e-06,5e-08,0,0,0,0,0,0,0,0 -11790000,0.98,-0.0065,-0.012,0.19,0.0023,0.011,0.019,0.00068,-0.0016,-0.0021,-1.2e-05,-5.9e-05,-2.1e-06,2e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00034,0.00034,0.00011,0.089,0.09,0.037,0.052,0.052,0.063,6.2e-09,6.2e-09,2.9e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -11890000,0.98,-0.0066,-0.012,0.19,0.0049,0.013,0.017,0.00098,-0.00042,-0.0014,-1.2e-05,-5.9e-05,-2.2e-06,2e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00035,0.00035,0.00011,0.11,0.11,0.034,0.06,0.06,0.063,6.2e-09,6.2e-09,2.8e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -11990000,0.98,-0.0068,-0.012,0.19,0.0079,0.013,0.015,0.0021,-0.0016,-0.0051,-1.2e-05,-5.9e-05,-2e-06,2e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.0003,0.0003,0.00011,0.086,0.086,0.03,0.051,0.051,0.061,5.7e-09,5.7e-09,2.8e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -12090000,0.98,-0.0067,-0.012,0.19,0.0095,0.013,0.018,0.003,-0.00037,0.00096,-1.2e-05,-5.9e-05,-1.9e-06,2e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00031,0.00031,0.00011,0.1,0.1,0.027,0.059,0.059,0.06,5.7e-09,5.7e-09,2.7e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -12190000,0.98,-0.0066,-0.012,0.19,0.0076,0.012,0.017,0.0018,0.00055,0.0028,-1.2e-05,-5.9e-05,-2.1e-06,2.3e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00027,0.00027,0.00011,0.081,0.081,0.024,0.05,0.05,0.058,5.3e-09,5.3e-09,2.7e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -12290000,0.98,-0.0067,-0.012,0.19,0.0054,0.011,0.016,0.0024,0.0017,0.0038,-1.2e-05,-5.9e-05,-2.3e-06,2.3e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00028,0.00028,0.00011,0.096,0.096,0.022,0.059,0.059,0.058,5.3e-09,5.3e-09,2.6e-09,3.6e-06,3.6e-06,5e-08,0,0,0,0,0,0,0,0 -12390000,0.98,-0.0068,-0.012,0.19,0.004,0.0078,0.014,0.0017,0.00065,-0.0022,-1.2e-05,-5.9e-05,-2.2e-06,2.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00025,0.00025,0.00011,0.076,0.076,0.02,0.05,0.05,0.056,4.9e-09,4.9e-09,2.6e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12490000,0.98,-0.0068,-0.012,0.19,0.004,0.0089,0.018,0.0021,0.0015,-0.00019,-1.2e-05,-5.9e-05,-2.2e-06,2.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00026,0.00026,0.0001,0.089,0.089,0.018,0.058,0.058,0.055,4.9e-09,4.9e-09,2.5e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12590000,0.98,-0.007,-0.012,0.19,0.0078,0.0022,0.02,0.0033,-0.0012,0.0016,-1.1e-05,-5.9e-05,-2.2e-06,2.6e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.8e-06,0.00023,0.00023,0.00011,0.071,0.071,0.017,0.049,0.049,0.054,4.7e-09,4.7e-09,2.5e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12690000,0.98,-0.007,-0.012,0.19,0.0083,9.3e-05,0.019,0.004,-0.0011,0.0032,-1.1e-05,-5.9e-05,-2.1e-06,2.6e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00024,0.00024,0.0001,0.083,0.083,0.016,0.058,0.058,0.053,4.7e-09,4.7e-09,2.4e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12790000,0.98,-0.0072,-0.012,0.19,0.0099,-0.0034,0.021,0.0041,-0.0042,0.0053,-1.1e-05,-5.9e-05,-1.2e-06,2.7e-05,4.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00022,0.00022,0.0001,0.067,0.067,0.014,0.049,0.049,0.052,4.4e-09,4.4e-09,2.4e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12890000,0.98,-0.0072,-0.012,0.19,0.01,-0.0042,0.022,0.0051,-0.0046,0.0083,-1.1e-05,-5.9e-05,-6.4e-07,2.7e-05,4.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00023,0.00023,0.0001,0.077,0.077,0.013,0.058,0.058,0.051,4.4e-09,4.4e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -12990000,0.98,-0.0072,-0.012,0.19,0.008,-0.0023,0.022,0.0036,-0.0034,0.0095,-1.1e-05,-6e-05,-9.9e-08,2.7e-05,4.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00021,0.00021,0.0001,0.063,0.063,0.012,0.049,0.049,0.05,4.2e-09,4.2e-09,2.3e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13090000,0.98,-0.0072,-0.012,0.19,0.0089,-0.0022,0.02,0.0044,-0.0036,0.0084,-1.1e-05,-6e-05,5.9e-07,2.8e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00022,0.00022,0.0001,0.072,0.072,0.012,0.057,0.057,0.049,4.2e-09,4.2e-09,2.2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13190000,0.98,-0.0072,-0.012,0.19,0.0039,-0.0039,0.019,0.00096,-0.0044,0.009,-1.1e-05,-6e-05,1.1e-06,2.9e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00021,0.00021,0.0001,0.059,0.059,0.011,0.049,0.049,0.047,4e-09,4e-09,2.2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13290000,0.98,-0.0073,-0.012,0.19,0.0036,-0.0048,0.016,0.0013,-0.0048,0.0083,-1.1e-05,-6e-05,1.2e-06,2.9e-05,4.4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00021,0.00021,0.0001,0.067,0.067,0.01,0.057,0.057,0.047,4e-09,4e-09,2.1e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13390000,0.98,-0.0072,-0.012,0.19,0.0027,-0.0029,0.016,0.00085,-0.0036,0.009,-1.1e-05,-6e-05,1e-06,3e-05,4.4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.0002,0.0002,0.0001,0.056,0.056,0.0097,0.049,0.049,0.046,3.8e-09,3.8e-09,2.1e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13490000,0.98,-0.0072,-0.012,0.19,0.0032,-0.0012,0.015,0.0012,-0.0038,0.0051,-1.1e-05,-6e-05,1.2e-06,3.1e-05,4.3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.00021,0.00021,0.0001,0.063,0.063,0.0093,0.056,0.056,0.045,3.8e-09,3.8e-09,2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13590000,0.98,-0.0072,-0.012,0.19,0.0076,-0.0015,0.017,0.004,-0.0031,0.0036,-1.1e-05,-6e-05,1.1e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.7e-06,0.0002,0.0002,0.0001,0.053,0.053,0.0088,0.048,0.048,0.044,3.6e-09,3.6e-09,2e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13690000,0.98,-0.0071,-0.012,0.19,0.0076,-0.0029,0.017,0.0048,-0.0033,0.0063,-1.1e-05,-6e-05,1.5e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.06,0.06,0.0085,0.056,0.056,0.044,3.6e-09,3.6e-09,1.9e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13790000,0.98,-0.0071,-0.012,0.19,0.015,0.0012,0.017,0.0083,-0.00086,0.0058,-1.1e-05,-5.9e-05,1.4e-06,3.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.05,0.05,0.0082,0.048,0.048,0.043,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13890000,0.98,-0.007,-0.012,0.19,0.016,0.0019,0.018,0.0098,-0.0007,0.008,-1.1e-05,-5.9e-05,1.9e-06,3.6e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.057,0.057,0.008,0.056,0.056,0.042,3.5e-09,3.5e-09,1.9e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -13990000,0.98,-0.007,-0.012,0.19,0.015,0.002,0.017,0.0074,-0.0022,0.0069,-1.1e-05,-5.9e-05,2.4e-06,3.4e-05,4.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00019,0.00019,0.0001,0.048,0.048,0.0077,0.048,0.048,0.041,3.3e-09,3.3e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14090000,0.98,-0.0071,-0.012,0.19,0.013,-0.0024,0.018,0.0088,-0.0023,0.0033,-1.1e-05,-5.9e-05,1.5e-06,3.5e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.055,0.055,0.0076,0.055,0.055,0.041,3.3e-09,3.3e-09,1.8e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14190000,0.98,-0.007,-0.012,0.19,0.01,-0.0012,0.018,0.008,-0.0017,0.0035,-1.1e-05,-5.9e-05,1.1e-06,3.6e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00019,0.00019,0.0001,0.046,0.046,0.0074,0.048,0.048,0.04,3.1e-09,3.1e-09,1.7e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14290000,0.98,-0.007,-0.011,0.19,0.012,-0.0012,0.016,0.009,-0.0018,0.0078,-1.1e-05,-6e-05,1.2e-06,3.6e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.0002,0.0002,0.0001,0.052,0.052,0.0073,0.055,0.055,0.04,3.1e-09,3.1e-09,1.7e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14390000,0.98,-0.0071,-0.011,0.19,0.012,-0.0042,0.017,0.0084,-0.003,0.012,-1.1e-05,-6e-05,1.5e-06,3.5e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.6e-06,0.00019,0.00019,0.0001,0.045,0.045,0.0071,0.048,0.048,0.039,3e-09,3e-09,1.6e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14490000,0.98,-0.0072,-0.011,0.19,0.01,-0.004,0.021,0.0095,-0.0034,0.014,-1.1e-05,-6e-05,1e-06,3.4e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.0002,0.0002,0.0001,0.051,0.051,0.0071,0.055,0.055,0.038,3e-09,3e-09,1.6e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14590000,0.98,-0.0072,-0.011,0.19,0.0081,-0.0041,0.019,0.0059,-0.0041,0.01,-1.1e-05,-6e-05,9.9e-07,3.2e-05,3.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,0.0001,0.044,0.044,0.007,0.047,0.047,0.038,2.8e-09,2.8e-09,1.6e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14690000,0.98,-0.0072,-0.011,0.19,0.0073,-0.0041,0.019,0.0067,-0.0045,0.011,-1.1e-05,-6e-05,1.2e-06,3.3e-05,3.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,0.0001,0.049,0.049,0.007,0.054,0.054,0.037,2.8e-09,2.8e-09,1.5e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14790000,0.98,-0.0072,-0.011,0.19,0.009,0.0028,0.019,0.0053,0.00086,0.013,-1.2e-05,-6e-05,1.8e-06,3.3e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,9.9e-05,0.042,0.042,0.0069,0.047,0.047,0.037,2.6e-09,2.6e-09,1.5e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14890000,0.98,-0.0071,-0.011,0.19,0.0077,0.00044,0.023,0.0061,0.001,0.014,-1.2e-05,-6e-05,2.2e-06,3.4e-05,4.5e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,9.9e-05,0.048,0.048,0.007,0.054,0.054,0.037,2.6e-09,2.6e-09,1.5e-09,3.5e-06,3.5e-06,5e-08,0,0,0,0,0,0,0,0 -14990000,0.98,-0.0072,-0.011,0.19,0.0065,-0.0012,0.026,0.0048,-0.00061,0.016,-1.2e-05,-6e-05,2.5e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.5e-06,0.00019,0.00019,9.9e-05,0.042,0.042,0.0069,0.047,0.047,0.036,2.5e-09,2.5e-09,1.4e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15090000,0.98,-0.0072,-0.011,0.19,0.0065,-0.00012,0.03,0.0054,-0.00072,0.019,-1.2e-05,-6e-05,2.5e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00019,0.00019,9.8e-05,0.047,0.047,0.007,0.054,0.054,0.036,2.5e-09,2.5e-09,1.4e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15190000,0.98,-0.0073,-0.011,0.19,0.0046,-0.0012,0.03,0.0043,-0.00068,0.021,-1.2e-05,-6.1e-05,2.4e-06,3.2e-05,4.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00018,0.00018,9.8e-05,0.041,0.041,0.007,0.047,0.047,0.036,2.3e-09,2.3e-09,1.4e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15290000,0.98,-0.0074,-0.011,0.19,0.0053,-0.0023,0.03,0.0048,-0.00083,0.017,-1.2e-05,-6e-05,2.8e-06,3.5e-05,4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00019,0.00019,9.8e-05,0.046,0.046,0.0071,0.054,0.054,0.035,2.3e-09,2.3e-09,1.3e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15390000,0.98,-0.0075,-0.011,0.19,0.0055,7.3e-05,0.029,0.0038,-0.00056,0.018,-1.2e-05,-6.1e-05,2.8e-06,3.5e-05,3.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00018,0.00018,9.7e-05,0.04,0.04,0.007,0.047,0.047,0.035,2.1e-09,2.1e-09,1.3e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15490000,0.98,-0.0075,-0.011,0.19,0.0049,-0.0024,0.029,0.0043,-0.00065,0.019,-1.2e-05,-6.1e-05,2.8e-06,3.6e-05,3.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00019,0.00019,9.7e-05,0.045,0.045,0.0072,0.053,0.053,0.035,2.1e-09,2.1e-09,1.3e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15590000,0.98,-0.0077,-0.011,0.19,0.0084,-0.0063,0.029,0.0063,-0.0046,0.017,-1.1e-05,-6.1e-05,3.1e-06,3.9e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00018,0.00018,9.6e-05,0.04,0.04,0.0072,0.046,0.046,0.035,2e-09,2e-09,1.3e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15690000,0.98,-0.0077,-0.011,0.19,0.01,-0.0093,0.029,0.0072,-0.0054,0.018,-1.1e-05,-6.1e-05,3.5e-06,4e-05,2.7e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.4e-06,0.00018,0.00018,9.6e-05,0.044,0.045,0.0073,0.053,0.053,0.034,2e-09,2e-09,1.2e-09,3.4e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15790000,0.98,-0.0077,-0.011,0.19,0.0067,-0.0087,0.029,0.0055,-0.0042,0.02,-1.1e-05,-6.1e-05,4e-06,3.8e-05,3.1e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.3e-06,0.00018,0.00017,9.5e-05,0.039,0.039,0.0073,0.046,0.046,0.034,1.8e-09,1.8e-09,1.2e-09,3.3e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15890000,0.98,-0.0077,-0.011,0.19,0.0056,-0.0071,0.03,0.0062,-0.005,0.02,-1.1e-05,-6.1e-05,3.6e-06,4.1e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.3e-06,0.00018,0.00018,9.5e-05,0.044,0.044,0.0074,0.053,0.053,0.034,1.8e-09,1.8e-09,1.2e-09,3.3e-06,3.4e-06,5e-08,0,0,0,0,0,0,0,0 -15990000,0.98,-0.0075,-0.011,0.19,0.0035,-0.0057,0.027,0.0049,-0.0038,0.019,-1.2e-05,-6.1e-05,3.6e-06,4.2e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.3e-06,0.00017,0.00017,9.4e-05,0.038,0.038,0.0074,0.046,0.046,0.034,1.7e-09,1.7e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16090000,0.98,-0.0075,-0.011,0.19,0.0029,-0.0069,0.024,0.0051,-0.0045,0.019,-1.2e-05,-6.1e-05,3.4e-06,4.4e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.3e-06,0.00018,0.00017,9.4e-05,0.043,0.043,0.0076,0.053,0.053,0.034,1.7e-09,1.7e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16190000,0.98,-0.0074,-0.011,0.19,-0.001,-0.0046,0.023,0.0028,-0.0033,0.016,-1.2e-05,-6.1e-05,3.1e-06,4.5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.3e-06,0.00017,0.00017,9.4e-05,0.038,0.038,0.0076,0.046,0.046,0.034,1.5e-09,1.5e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16290000,0.98,-0.0075,-0.011,0.19,-0.00067,-0.0061,0.023,0.0027,-0.0039,0.017,-1.2e-05,-6.1e-05,3.3e-06,4.5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00017,0.00017,9.3e-05,0.043,0.043,0.0077,0.053,0.053,0.034,1.5e-09,1.5e-09,1.1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16390000,0.98,-0.0074,-0.011,0.19,0.0018,-0.0054,0.023,0.0037,-0.0029,0.017,-1.2e-05,-6.1e-05,3.6e-06,5.1e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00016,0.00016,9.3e-05,0.037,0.038,0.0077,0.046,0.046,0.034,1.4e-09,1.4e-09,1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16490000,0.98,-0.0075,-0.011,0.19,0.0037,-0.007,0.026,0.0039,-0.0036,0.021,-1.2e-05,-6.1e-05,3.5e-06,5e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00017,0.00017,9.2e-05,0.042,0.042,0.0079,0.052,0.053,0.034,1.4e-09,1.4e-09,1e-09,3.3e-06,3.3e-06,5e-08,0,0,0,0,0,0,0,0 -16590000,0.98,-0.0075,-0.011,0.19,0.0078,-0.0071,0.029,0.0034,-0.0028,0.021,-1.2e-05,-6.1e-05,3.5e-06,5.1e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00016,0.00016,9.2e-05,0.037,0.037,0.0079,0.046,0.046,0.034,1.3e-09,1.3e-09,1e-09,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -16690000,0.98,-0.0076,-0.011,0.19,0.0093,-0.012,0.029,0.0043,-0.0037,0.022,-1.2e-05,-6.1e-05,3.7e-06,5.2e-05,2.9e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.2e-06,0.00016,0.00016,9.1e-05,0.041,0.041,0.008,0.052,0.052,0.034,1.3e-09,1.3e-09,9.8e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -16790000,0.98,-0.0074,-0.011,0.19,0.01,-0.011,0.028,0.0033,-0.0027,0.022,-1.2e-05,-6.1e-05,3.8e-06,5.3e-05,3.2e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00015,9.1e-05,0.036,0.036,0.008,0.046,0.046,0.034,1.2e-09,1.2e-09,9.6e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -16890000,0.98,-0.0074,-0.011,0.19,0.0092,-0.011,0.029,0.0043,-0.0037,0.02,-1.2e-05,-6.1e-05,4.2e-06,5.6e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00016,0.00016,9e-05,0.041,0.041,0.0082,0.052,0.052,0.034,1.2e-09,1.2e-09,9.4e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -16990000,0.98,-0.0074,-0.011,0.19,0.0088,-0.01,0.029,0.0041,-0.0028,0.019,-1.3e-05,-6.1e-05,4.3e-06,6.2e-05,3e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00015,0.00015,9e-05,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-09,1.1e-09,9.2e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -17090000,0.98,-0.0075,-0.011,0.19,0.01,-0.013,0.028,0.0051,-0.004,0.018,-1.2e-05,-6.1e-05,4.2e-06,6.4e-05,2.8e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00015,0.00015,8.9e-05,0.04,0.04,0.0083,0.052,0.052,0.034,1.1e-09,1.1e-09,9e-10,3.2e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -17190000,0.98,-0.0076,-0.011,0.19,0.0092,-0.018,0.03,0.0034,-0.0075,0.021,-1.2e-05,-6.1e-05,3.8e-06,6.3e-05,2.6e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00015,0.00015,8.9e-05,0.035,0.035,0.0083,0.046,0.046,0.034,9.7e-10,9.7e-10,8.8e-10,3.1e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -17290000,0.98,-0.0076,-0.011,0.19,0.01,-0.019,0.03,0.0044,-0.0093,0.021,-1.2e-05,-6.1e-05,3.5e-06,6.5e-05,2.4e-05,-0.0014,0.2,0.002,0.43,0,0,0,0,0,8.1e-06,0.00015,0.00015,8.8e-05,0.039,0.039,0.0084,0.052,0.052,0.034,9.7e-10,9.7e-10,8.6e-10,3.1e-06,3.2e-06,5e-08,0,0,0,0,0,0,0,0 -17390000,0.98,-0.0074,-0.011,0.19,0.0069,-0.018,0.029,0.0058,-0.0059,0.021,-1.3e-05,-6e-05,3.8e-06,7.3e-05,2.9e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00014,0.00014,8.8e-05,0.035,0.035,0.0084,0.046,0.046,0.034,8.8e-10,8.8e-10,8.4e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17490000,0.98,-0.0074,-0.011,0.19,0.005,-0.019,0.029,0.0063,-0.0077,0.022,-1.3e-05,-6e-05,3.7e-06,7.3e-05,2.8e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00015,0.00014,8.8e-05,0.039,0.039,0.0085,0.052,0.052,0.034,8.8e-10,8.8e-10,8.2e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17590000,0.98,-0.0073,-0.011,0.19,0.0011,-0.015,0.028,0.0025,-0.0058,0.02,-1.3e-05,-6.1e-05,3.6e-06,7.2e-05,3.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00014,0.00014,8.7e-05,0.034,0.034,0.0085,0.046,0.046,0.034,7.9e-10,7.9e-10,8.1e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17690000,0.98,-0.0074,-0.011,0.19,0.00022,-0.016,0.029,0.0026,-0.0073,0.022,-1.3e-05,-6.1e-05,3.7e-06,7.3e-05,3.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,8e-06,0.00014,0.00014,8.6e-05,0.038,0.038,0.0086,0.052,0.052,0.034,7.9e-10,7.9e-10,7.9e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17790000,0.98,-0.0074,-0.011,0.19,0.0028,-0.014,0.029,0.0036,-0.0062,0.028,-1.4e-05,-6e-05,3.7e-06,7.6e-05,3.9e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00014,0.00013,8.6e-05,0.033,0.033,0.0086,0.045,0.045,0.034,7.2e-10,7.2e-10,7.7e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17890000,0.98,-0.0073,-0.011,0.19,0.005,-0.016,0.029,0.004,-0.0077,0.032,-1.4e-05,-6e-05,3.8e-06,7.5e-05,4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00014,0.00014,8.6e-05,0.037,0.037,0.0086,0.052,0.052,0.035,7.2e-10,7.2e-10,7.6e-10,3.1e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -17990000,0.98,-0.0071,-0.011,0.19,0.0043,-0.0093,0.029,0.0032,-0.002,0.033,-1.4e-05,-6e-05,3.8e-06,8e-05,5.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00013,0.00013,8.5e-05,0.033,0.033,0.0086,0.045,0.045,0.034,6.5e-10,6.5e-10,7.4e-10,3e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -18090000,0.98,-0.0072,-0.011,0.19,0.0039,-0.0098,0.028,0.0037,-0.003,0.031,-1.4e-05,-6e-05,3.7e-06,8.2e-05,4.9e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00013,0.00013,8.5e-05,0.036,0.036,0.0087,0.051,0.051,0.035,6.5e-10,6.5e-10,7.3e-10,3e-06,3.1e-06,5e-08,0,0,0,0,0,0,0,0 -18190000,0.98,-0.0072,-0.011,0.19,0.0039,-0.0088,0.028,0.0042,-0.0022,0.029,-1.4e-05,-6e-05,4.1e-06,8.8e-05,4.8e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00013,0.00013,8.4e-05,0.032,0.032,0.0086,0.045,0.045,0.035,5.9e-10,5.9e-10,7.1e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18290000,0.98,-0.0072,-0.011,0.19,0.0048,-0.0093,0.027,0.0046,-0.0031,0.027,-1.4e-05,-6e-05,4.1e-06,9e-05,4.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.9e-06,0.00013,0.00013,8.4e-05,0.035,0.035,0.0087,0.051,0.051,0.035,5.9e-10,5.9e-10,7e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18390000,0.98,-0.0071,-0.011,0.19,0.0057,-0.008,0.027,0.0062,-0.0023,0.026,-1.4e-05,-6e-05,4e-06,9.6e-05,4.5e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00012,0.00012,8.3e-05,0.031,0.031,0.0086,0.045,0.045,0.035,5.3e-10,5.3e-10,6.8e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18490000,0.98,-0.0072,-0.011,0.19,0.0084,-0.008,0.026,0.007,-0.0031,0.028,-1.4e-05,-6e-05,4.1e-06,9.7e-05,4.5e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00013,0.00013,8.3e-05,0.034,0.034,0.0087,0.051,0.051,0.035,5.3e-10,5.3e-10,6.7e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18590000,0.98,-0.007,-0.011,0.19,0.0068,-0.0074,0.026,0.0056,-0.0024,0.03,-1.5e-05,-6e-05,3.9e-06,9.5e-05,4.7e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00012,0.00012,8.2e-05,0.03,0.03,0.0087,0.045,0.045,0.035,4.8e-10,4.8e-10,6.6e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18690000,0.98,-0.007,-0.011,0.19,0.0069,-0.0063,0.024,0.0063,-0.0031,0.029,-1.5e-05,-6e-05,4e-06,9.8e-05,4.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00012,0.00012,8.2e-05,0.034,0.034,0.0087,0.051,0.051,0.035,4.8e-10,4.8e-10,6.4e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18790000,0.98,-0.007,-0.011,0.19,0.0058,-0.006,0.024,0.0063,-0.0025,0.027,-1.5e-05,-6e-05,4e-06,0.0001,4.4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00012,0.00012,8.2e-05,0.03,0.03,0.0087,0.045,0.045,0.035,4.4e-10,4.4e-10,6.3e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18890000,0.98,-0.0069,-0.011,0.19,0.0045,-0.0056,0.021,0.0068,-0.0032,0.023,-1.5e-05,-6e-05,3.8e-06,0.0001,4.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.8e-06,0.00012,0.00012,8.1e-05,0.033,0.033,0.0087,0.051,0.051,0.035,4.4e-10,4.4e-10,6.2e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -18990000,0.98,-0.0069,-0.011,0.19,0.0029,-0.0057,0.022,0.0056,-0.0025,0.026,-1.5e-05,-6e-05,3.8e-06,0.0001,4.4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00012,0.00011,8.1e-05,0.029,0.029,0.0086,0.045,0.045,0.035,4e-10,4e-10,6e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -19090000,0.98,-0.007,-0.011,0.19,0.00087,-0.0062,0.023,0.0059,-0.0031,0.022,-1.5e-05,-6e-05,3.9e-06,0.00011,4.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00012,0.00012,8e-05,0.032,0.032,0.0087,0.051,0.051,0.036,4e-10,4e-10,5.9e-10,3e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -19190000,0.98,-0.0069,-0.011,0.19,-0.00064,-0.0059,0.022,0.0049,-0.0025,0.021,-1.5e-05,-6e-05,3.5e-06,0.00011,4.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00011,0.00011,8e-05,0.028,0.028,0.0086,0.045,0.045,0.036,3.6e-10,3.6e-10,5.8e-10,2.9e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -19290000,0.98,-0.0068,-0.011,0.19,-0.0015,-0.0057,0.023,0.0048,-0.0031,0.02,-1.5e-05,-6e-05,3.5e-06,0.00011,4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00011,0.00011,7.9e-05,0.031,0.031,0.0087,0.05,0.05,0.036,3.6e-10,3.6e-10,5.7e-10,2.9e-06,3e-06,5e-08,0,0,0,0,0,0,0,0 -19390000,0.98,-0.0069,-0.011,0.19,-0.002,-0.0022,0.024,0.0041,-0.0012,0.019,-1.5e-05,-6e-05,3.3e-06,0.00011,4.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00011,0.00011,7.9e-05,0.028,0.028,0.0086,0.045,0.045,0.036,3.3e-10,3.3e-10,5.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19490000,0.98,-0.0069,-0.011,0.19,-0.0028,-0.0022,0.023,0.0039,-0.0014,0.019,-1.5e-05,-6e-05,3.1e-06,0.00011,4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.7e-06,0.00011,0.00011,7.9e-05,0.03,0.03,0.0087,0.05,0.05,0.036,3.3e-10,3.3e-10,5.5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19590000,0.98,-0.0069,-0.011,0.19,-0.0039,-0.0051,0.025,0.0044,-0.0024,0.019,-1.5e-05,-6e-05,3e-06,0.00012,3.8e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.00011,0.00011,7.8e-05,0.027,0.027,0.0086,0.044,0.044,0.036,3e-10,3e-10,5.4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19690000,0.98,-0.0069,-0.011,0.19,-0.0055,-0.0037,0.023,0.004,-0.0028,0.018,-1.5e-05,-6e-05,3.1e-06,0.00012,3.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.00011,0.00011,7.8e-05,0.03,0.03,0.0086,0.05,0.05,0.036,3e-10,3e-10,5.3e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19790000,0.98,-0.007,-0.011,0.19,-0.0056,-0.0022,0.022,0.0064,-0.0023,0.014,-1.5e-05,-6e-05,2.9e-06,0.00012,3.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.00011,0.0001,7.7e-05,0.026,0.026,0.0086,0.044,0.044,0.036,2.8e-10,2.8e-10,5.2e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19890000,0.98,-0.007,-0.011,0.19,-0.0056,-0.002,0.022,0.0058,-0.0025,0.013,-1.5e-05,-6e-05,2.7e-06,0.00013,3.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.00011,0.00011,7.7e-05,0.029,0.029,0.0086,0.05,0.05,0.036,2.8e-10,2.8e-10,5.1e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -19990000,0.98,-0.0071,-0.011,0.19,-0.0053,-0.0019,0.019,0.0062,-0.00091,0.0096,-1.5e-05,-5.9e-05,2.8e-06,0.00013,3.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.0001,0.0001,7.6e-05,0.026,0.026,0.0085,0.044,0.044,0.036,2.5e-10,2.5e-10,5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20090000,0.98,-0.0071,-0.011,0.19,-0.0048,-0.0041,0.019,0.0056,-0.0012,0.013,-1.5e-05,-5.9e-05,2.7e-06,0.00013,3.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.00011,0.0001,7.6e-05,0.028,0.028,0.0086,0.05,0.05,0.036,2.5e-10,2.5e-10,4.9e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20190000,0.98,-0.0071,-0.011,0.19,-0.0038,-0.0016,0.02,0.0066,-0.0009,0.013,-1.5e-05,-5.9e-05,2.5e-06,0.00013,3.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.6e-06,0.0001,0.0001,7.6e-05,0.025,0.025,0.0085,0.044,0.044,0.036,2.3e-10,2.3e-10,4.8e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20290000,0.98,-0.007,-0.011,0.19,-0.007,-0.0027,0.02,0.0061,-0.0011,0.013,-1.5e-05,-5.9e-05,2.4e-06,0.00013,3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,0.0001,0.0001,7.5e-05,0.027,0.027,0.0085,0.049,0.049,0.036,2.3e-10,2.3e-10,4.7e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20390000,0.98,-0.007,-0.011,0.19,-0.0077,-0.0015,0.021,0.0069,-0.00077,0.014,-1.5e-05,-5.9e-05,2.5e-06,0.00014,3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,0.0001,9.9e-05,7.5e-05,0.024,0.024,0.0084,0.044,0.044,0.036,2.1e-10,2.1e-10,4.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20490000,0.98,-0.007,-0.011,0.19,-0.012,-0.0024,0.02,0.006,-0.00095,0.012,-1.5e-05,-5.9e-05,2.5e-06,0.00014,2.8e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,0.0001,0.0001,7.4e-05,0.027,0.027,0.0085,0.049,0.049,0.036,2.1e-10,2.1e-10,4.6e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20590000,0.98,-0.007,-0.011,0.19,-0.011,-0.0034,0.02,0.007,-0.00079,0.011,-1.5e-05,-5.9e-05,2.7e-06,0.00014,2.7e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,9.9e-05,9.8e-05,7.4e-05,0.024,0.024,0.0084,0.044,0.044,0.036,2e-10,2e-10,4.5e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20690000,0.98,-0.0069,-0.011,0.19,-0.013,-0.0022,0.021,0.0058,-0.001,0.011,-1.5e-05,-5.9e-05,2.5e-06,0.00014,2.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,0.0001,9.9e-05,7.4e-05,0.026,0.026,0.0084,0.049,0.049,0.036,2e-10,2e-10,4.4e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20790000,0.98,-0.0063,-0.011,0.19,-0.016,0.00041,0.0055,0.0049,-0.00081,0.0096,-1.5e-05,-5.9e-05,2.6e-06,0.00014,2.5e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.5e-06,9.8e-05,9.6e-05,7.3e-05,0.023,0.023,0.0084,0.043,0.043,0.036,1.8e-10,1.8e-10,4.3e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20890000,0.98,0.0028,-0.0073,0.19,-0.021,0.012,-0.11,0.003,-0.00013,0.0033,-1.5e-05,-5.9e-05,2.5e-06,0.00015,2.4e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.8e-05,9.7e-05,7.3e-05,0.026,0.026,0.0084,0.049,0.049,0.036,1.8e-10,1.8e-10,4.2e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -20990000,0.98,0.0061,-0.0038,0.19,-0.032,0.03,-0.25,0.0023,0.00052,-0.011,-1.5e-05,-5.9e-05,2.5e-06,0.00015,2.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.6e-05,9.5e-05,7.2e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.7e-10,1.7e-10,4.2e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21090000,0.98,0.0045,-0.0042,0.19,-0.046,0.046,-0.37,-0.0016,0.0044,-0.042,-1.5e-05,-5.9e-05,2.5e-06,0.00015,2.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.7e-05,9.5e-05,7.2e-05,0.026,0.026,0.0084,0.048,0.048,0.036,1.7e-10,1.7e-10,4.1e-10,2.9e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21190000,0.98,0.0017,-0.0059,0.19,-0.049,0.05,-0.5,-0.0012,0.0035,-0.078,-1.4e-05,-5.9e-05,2.5e-06,0.00014,1.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.5e-05,9.3e-05,7.2e-05,0.023,0.023,0.0083,0.043,0.043,0.036,1.5e-10,1.5e-10,4e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21290000,0.98,-0.00049,-0.0072,0.19,-0.049,0.054,-0.63,-0.0061,0.0087,-0.14,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.6e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.5e-05,9.4e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.5e-10,1.5e-10,3.9e-10,2.8e-06,2.9e-06,5e-08,0,0,0,0,0,0,0,0 -21390000,0.98,-0.002,-0.0079,0.19,-0.044,0.05,-0.75,-0.005,0.011,-0.2,-1.4e-05,-5.9e-05,2.2e-06,0.00015,1.3e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.3e-05,9.1e-05,7.1e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-10,1.4e-10,3.9e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0092,0.016,-0.29,-1.4e-05,-5.9e-05,2.3e-06,0.00015,1.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.3e-05,9.2e-05,7.1e-05,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-10,1.4e-10,3.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21590000,0.98,-0.0033,-0.0083,0.19,-0.031,0.043,-1,-0.0079,0.016,-0.38,-1.4e-05,-5.9e-05,2.4e-06,0.00015,1.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.1e-05,9e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-10,1.3e-10,3.8e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21690000,0.98,-0.0036,-0.0082,0.19,-0.029,0.039,-1.1,-0.011,0.021,-0.49,-1.4e-05,-5.9e-05,2.6e-06,0.00015,9.2e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.4e-06,9.1e-05,9e-05,7e-05,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-10,1.3e-10,3.7e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.033,-1.3,-0.0037,0.018,-0.61,-1.4e-05,-5.8e-05,2.7e-06,0.00016,7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.9e-05,8.8e-05,7e-05,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.028,-1.4,-0.0057,0.021,-0.75,-1.4e-05,-5.8e-05,2.6e-06,0.00016,5.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.9e-05,8.8e-05,6.9e-05,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-10,1.2e-10,3.6e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -21990000,0.98,-0.005,-0.0088,0.19,-0.014,0.023,-1.4,-0.00032,0.017,-0.89,-1.4e-05,-5.8e-05,2.7e-06,0.00015,8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.7e-05,8.6e-05,6.9e-05,0.022,0.022,0.0082,0.043,0.043,0.036,1.1e-10,1.1e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22090000,0.98,-0.0057,-0.0096,0.19,-0.012,0.019,-1.4,-0.0016,0.019,-1,-1.4e-05,-5.8e-05,2.9e-06,0.00016,6.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.7e-05,8.6e-05,6.9e-05,0.024,0.024,0.0082,0.048,0.048,0.036,1.1e-10,1.1e-10,3.5e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22190000,0.98,-0.0062,-0.0099,0.19,-0.0035,0.013,-1.4,0.0061,0.013,-1.2,-1.4e-05,-5.8e-05,3e-06,0.00016,6.9e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.5e-05,8.4e-05,6.8e-05,0.021,0.021,0.0081,0.043,0.043,0.036,1.1e-10,1.1e-10,3.4e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22290000,0.98,-0.0069,-0.01,0.19,0.0017,0.0078,-1.4,0.006,0.014,-1.3,-1.4e-05,-5.8e-05,2.9e-06,0.00016,6e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.6e-05,8.4e-05,6.8e-05,0.023,0.023,0.0081,0.048,0.048,0.036,1.1e-10,1.1e-10,3.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22390000,0.98,-0.0072,-0.01,0.19,0.0067,-0.0018,-1.4,0.013,0.0042,-1.5,-1.4e-05,-5.8e-05,2.7e-06,0.00016,2.2e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.4e-05,8.3e-05,6.7e-05,0.021,0.021,0.0081,0.042,0.043,0.036,1e-10,1e-10,3.3e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22490000,0.98,-0.0074,-0.011,0.19,0.011,-0.0077,-1.4,0.014,0.0036,-1.6,-1.4e-05,-5.8e-05,2.6e-06,0.00016,1.5e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.3e-06,8.4e-05,8.3e-05,6.7e-05,0.022,0.022,0.0081,0.047,0.047,0.036,1e-10,1e-10,3.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22590000,0.98,-0.0073,-0.011,0.19,0.02,-0.017,-1.4,0.026,-0.0053,-1.7,-1.4e-05,-5.8e-05,2.7e-06,0.00016,-2.6e-07,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8.2e-05,8.1e-05,6.7e-05,0.02,0.02,0.0081,0.042,0.042,0.036,9.4e-11,9.4e-11,3.2e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22690000,0.98,-0.0072,-0.012,0.19,0.022,-0.021,-1.4,0.028,-0.0072,-1.9,-1.4e-05,-5.8e-05,2.6e-06,0.00016,-1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8.3e-05,8.2e-05,6.6e-05,0.021,0.021,0.0081,0.047,0.047,0.036,9.4e-11,9.4e-11,3.1e-10,2.8e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22790000,0.98,-0.0072,-0.012,0.19,0.028,-0.029,-1.4,0.038,-0.017,-2,-1.4e-05,-5.8e-05,2.5e-06,0.00016,-2.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8.1e-05,8e-05,6.6e-05,0.019,0.019,0.0081,0.042,0.042,0.036,8.8e-11,8.9e-11,3.1e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22890000,0.98,-0.0074,-0.012,0.19,0.031,-0.035,-1.4,0.041,-0.02,-2.2,-1.4e-05,-5.8e-05,2.7e-06,0.00016,-3e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8.1e-05,8e-05,6.6e-05,0.021,0.021,0.0081,0.047,0.047,0.036,8.9e-11,8.9e-11,3e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -22990000,0.98,-0.0074,-0.013,0.19,0.036,-0.04,-1.4,0.051,-0.031,-2.3,-1.4e-05,-5.8e-05,2.8e-06,0.00016,-4.3e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8e-05,7.9e-05,6.6e-05,0.019,0.019,0.0081,0.042,0.042,0.036,8.4e-11,8.4e-11,3e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23090000,0.98,-0.0074,-0.013,0.19,0.041,-0.044,-1.4,0.055,-0.035,-2.5,-1.4e-05,-5.8e-05,2.8e-06,0.00016,-4.6e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.2e-06,8e-05,7.9e-05,6.5e-05,0.02,0.02,0.0081,0.046,0.046,0.036,8.4e-11,8.4e-11,2.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23190000,0.98,-0.0074,-0.013,0.19,0.047,-0.046,-1.4,0.066,-0.045,-2.6,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-6.1e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.9e-05,7.8e-05,6.5e-05,0.018,0.018,0.008,0.042,0.042,0.035,7.9e-11,7.9e-11,2.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23290000,0.98,-0.0079,-0.013,0.19,0.052,-0.051,-1.4,0.071,-0.05,-2.8,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-6.7e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.9e-05,7.8e-05,6.5e-05,0.019,0.02,0.0081,0.046,0.046,0.036,7.9e-11,7.9e-11,2.9e-10,2.7e-06,2.8e-06,5e-08,0,0,0,0,0,0,0,0 -23390000,0.98,-0.0078,-0.014,0.19,0.057,-0.053,-1.4,0.082,-0.055,-2.9,-1.4e-05,-5.8e-05,2.6e-06,0.00017,-4e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.8e-05,7.7e-05,6.4e-05,0.018,0.018,0.008,0.041,0.041,0.036,7.5e-11,7.5e-11,2.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23490000,0.98,-0.0079,-0.014,0.19,0.061,-0.055,-1.4,0.088,-0.061,-3,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-4.4e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.9e-05,7.7e-05,6.4e-05,0.019,0.019,0.0081,0.046,0.046,0.036,7.5e-11,7.5e-11,2.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23590000,0.98,-0.0082,-0.014,0.18,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-1.4e-05,-5.8e-05,2.7e-06,0.00017,-5.6e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.8e-05,7.7e-05,6.4e-05,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-11,7.2e-11,2.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23690000,0.98,-0.0088,-0.014,0.18,0.062,-0.061,-1.3,0.1,-0.076,-3.3,-1.4e-05,-5.8e-05,2.8e-06,0.00017,-5.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.8e-05,7.7e-05,6.3e-05,0.018,0.018,0.0081,0.046,0.046,0.036,7.2e-11,7.2e-11,2.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23790000,0.98,-0.011,-0.017,0.18,0.057,-0.058,-0.96,0.11,-0.081,-3.4,-1.4e-05,-5.8e-05,3e-06,0.00017,-5.8e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.7e-05,7.6e-05,6.3e-05,0.016,0.016,0.008,0.041,0.041,0.035,6.8e-11,6.8e-11,2.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23890000,0.98,-0.014,-0.021,0.18,0.053,-0.059,-0.52,0.12,-0.087,-3.5,-1.4e-05,-5.8e-05,3e-06,0.00017,-5.9e-06,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.7e-05,7.6e-05,6.3e-05,0.017,0.017,0.008,0.045,0.045,0.035,6.9e-11,6.9e-11,2.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -23990000,0.98,-0.016,-0.024,0.18,0.054,-0.058,-0.14,0.13,-0.089,-3.6,-1.4e-05,-5.8e-05,3e-06,0.00018,-1.1e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.7e-05,7.6e-05,6.2e-05,0.015,0.015,0.008,0.041,0.041,0.036,6.6e-11,6.6e-11,2.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24090000,0.98,-0.016,-0.023,0.18,0.061,-0.066,0.09,0.13,-0.095,-3.6,-1.4e-05,-5.8e-05,2.9e-06,0.00018,-1.2e-05,-0.0013,0.2,0.002,0.43,0,0,0,0,0,7.1e-06,7.7e-05,7.6e-05,6.2e-05,0.016,0.016,0.0081,0.045,0.045,0.036,6.6e-11,6.6e-11,2.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24190000,0.98,-0.013,-0.019,0.18,0.072,-0.071,0.077,0.14,-0.1,-3.6,-1.4e-05,-5.8e-05,2.9e-06,0.00019,-1.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.2e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.3e-11,6.3e-11,2.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24290000,0.98,-0.01,-0.016,0.18,0.075,-0.075,0.055,0.15,-0.11,-3.6,-1.4e-05,-5.8e-05,2.9e-06,0.00019,-1.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.2e-05,0.016,0.016,0.0081,0.044,0.044,0.036,6.3e-11,6.3e-11,2.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24390000,0.98,-0.0096,-0.015,0.18,0.069,-0.069,0.071,0.15,-0.11,-3.6,-1.4e-05,-5.8e-05,3.1e-06,0.00019,-2.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.015,0.015,0.008,0.04,0.04,0.035,6.1e-11,6.1e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24490000,0.98,-0.0098,-0.015,0.18,0.064,-0.066,0.069,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-2.7e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.016,0.016,0.0081,0.044,0.044,0.035,6.1e-11,6.1e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24590000,0.98,-0.01,-0.015,0.18,0.061,-0.062,0.065,0.16,-0.11,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-3.4e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6.1e-05,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-11,5.9e-11,2.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24690000,0.98,-0.011,-0.014,0.18,0.059,-0.061,0.064,0.17,-0.12,-3.6,-1.4e-05,-5.8e-05,3.3e-06,0.00019,-3.4e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6e-05,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-11,5.9e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24790000,0.98,-0.011,-0.014,0.18,0.056,-0.059,0.056,0.17,-0.11,-3.6,-1.5e-05,-5.8e-05,3.3e-06,0.00019,-3.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,7e-06,7.7e-05,7.6e-05,6e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-11,5.7e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24890000,0.98,-0.011,-0.013,0.18,0.054,-0.062,0.045,0.18,-0.12,-3.6,-1.5e-05,-5.8e-05,3.4e-06,0.00019,-3.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6e-05,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-11,5.7e-11,2.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -24990000,0.98,-0.011,-0.013,0.18,0.045,-0.058,0.038,0.18,-0.11,-3.6,-1.5e-05,-5.8e-05,3.4e-06,0.00019,-4.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.7e-05,7.6e-05,6e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-11,5.5e-11,2.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25090000,0.98,-0.011,-0.013,0.18,0.042,-0.057,0.035,0.18,-0.12,-3.6,-1.5e-05,-5.8e-05,3.4e-06,0.00019,-4.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.6e-05,5.9e-05,0.016,0.016,0.0081,0.043,0.043,0.035,5.5e-11,5.5e-11,2.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25190000,0.98,-0.011,-0.013,0.18,0.037,-0.05,0.035,0.18,-0.11,-3.6,-1.5e-05,-5.8e-05,3.2e-06,0.00019,-5.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.6e-05,5.9e-05,0.015,0.015,0.008,0.039,0.039,0.035,5.3e-11,5.3e-11,2.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25290000,0.98,-0.011,-0.012,0.18,0.032,-0.052,0.029,0.18,-0.11,-3.6,-1.5e-05,-5.8e-05,3.1e-06,0.00019,-5.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.9e-05,0.016,0.016,0.0081,0.043,0.043,0.036,5.3e-11,5.3e-11,2.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25390000,0.98,-0.011,-0.012,0.18,0.024,-0.044,0.028,0.18,-0.1,-3.6,-1.5e-05,-5.8e-05,3.1e-06,0.00018,-5.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.9e-05,0.014,0.015,0.008,0.039,0.039,0.035,5.1e-11,5.1e-11,2.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25490000,0.98,-0.011,-0.012,0.18,0.019,-0.044,0.027,0.18,-0.11,-3.6,-1.5e-05,-5.8e-05,3e-06,0.00019,-5.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.8e-05,0.016,0.016,0.0081,0.043,0.043,0.035,5.1e-11,5.1e-11,2.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25590000,0.98,-0.011,-0.012,0.18,0.014,-0.04,0.028,0.18,-0.098,-3.6,-1.5e-05,-5.8e-05,2.9e-06,0.00018,-6.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.8e-05,0.014,0.014,0.008,0.039,0.039,0.035,5e-11,5e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25690000,0.98,-0.011,-0.011,0.18,0.013,-0.039,0.017,0.18,-0.1,-3.6,-1.5e-05,-5.8e-05,2.9e-06,0.00018,-6.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.8e-05,0.015,0.015,0.0081,0.043,0.043,0.035,5e-11,5e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25790000,0.98,-0.011,-0.011,0.18,0.0021,-0.031,0.017,0.17,-0.093,-3.6,-1.6e-05,-5.8e-05,2.8e-06,0.00018,-6.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.8e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.8e-11,4.8e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25890000,0.98,-0.011,-0.011,0.19,-0.0036,-0.029,0.019,0.17,-0.096,-3.6,-1.6e-05,-5.8e-05,2.6e-06,0.00019,-6.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.7e-05,0.015,0.015,0.0081,0.043,0.043,0.036,4.8e-11,4.8e-11,2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -25990000,0.98,-0.011,-0.011,0.19,-0.013,-0.022,0.013,0.16,-0.086,-3.6,-1.6e-05,-5.8e-05,2.5e-06,0.00019,-7.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.9e-06,7.8e-05,7.7e-05,5.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.7e-11,4.7e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26090000,0.98,-0.01,-0.011,0.18,-0.018,-0.022,0.011,0.16,-0.088,-3.6,-1.6e-05,-5.8e-05,2.6e-06,0.00019,-7.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.7e-05,5.7e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.7e-11,4.7e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26190000,0.98,-0.01,-0.011,0.18,-0.024,-0.015,0.0063,0.15,-0.081,-3.6,-1.6e-05,-5.8e-05,2.6e-06,0.00019,-7.3e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.8e-05,7.7e-05,5.7e-05,0.014,0.014,0.008,0.039,0.039,0.035,4.6e-11,4.6e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26290000,0.98,-0.01,-0.012,0.18,-0.026,-0.014,0.00052,0.15,-0.083,-3.6,-1.6e-05,-5.8e-05,2.5e-06,0.00019,-7.3e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.7e-05,5.6e-05,0.015,0.015,0.0081,0.042,0.042,0.036,4.6e-11,4.6e-11,1.9e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26390000,0.98,-0.0098,-0.012,0.18,-0.031,-0.0065,0.0045,0.13,-0.075,-3.6,-1.6e-05,-5.8e-05,2.4e-06,0.00019,-7.6e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.7e-05,5.6e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.4e-11,4.4e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26490000,0.98,-0.0096,-0.011,0.18,-0.035,-0.0033,0.014,0.13,-0.075,-3.6,-1.6e-05,-5.8e-05,2.3e-06,0.00019,-7.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.6e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.4e-11,4.5e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26590000,0.98,-0.009,-0.012,0.18,-0.036,0.0046,0.014,0.12,-0.068,-3.6,-1.6e-05,-5.8e-05,2.2e-06,0.00019,-7.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.6e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.3e-11,4.3e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26690000,0.98,-0.0089,-0.011,0.19,-0.038,0.0097,0.013,0.12,-0.067,-3.6,-1.6e-05,-5.8e-05,2e-06,0.00019,-7.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.5e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.3e-11,4.3e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26790000,0.98,-0.0086,-0.011,0.18,-0.045,0.013,0.012,0.11,-0.062,-3.6,-1.6e-05,-5.8e-05,1.9e-06,0.00019,-8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.5e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.2e-11,4.2e-11,1.8e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26890000,0.98,-0.008,-0.011,0.18,-0.051,0.016,0.007,0.1,-0.06,-3.6,-1.6e-05,-5.8e-05,1.9e-06,0.00019,-8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.5e-05,0.015,0.015,0.0081,0.042,0.042,0.036,4.2e-11,4.2e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -26990000,0.98,-0.0075,-0.011,0.18,-0.058,0.023,0.0061,0.088,-0.054,-3.6,-1.6e-05,-5.8e-05,1.8e-06,0.00019,-8.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.8e-06,7.9e-05,7.8e-05,5.5e-05,0.014,0.014,0.008,0.038,0.038,0.035,4.1e-11,4.1e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27090000,0.98,-0.0073,-0.012,0.18,-0.06,0.03,0.0089,0.082,-0.052,-3.6,-1.6e-05,-5.8e-05,1.8e-06,0.0002,-8.3e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,7.9e-05,7.8e-05,5.4e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4.1e-11,4.1e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27190000,0.98,-0.0074,-0.012,0.18,-0.066,0.035,0.011,0.071,-0.046,-3.6,-1.6e-05,-5.8e-05,1.7e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,7.9e-05,7.8e-05,5.4e-05,0.014,0.014,0.008,0.038,0.038,0.035,4e-11,4e-11,1.7e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27290000,0.98,-0.0075,-0.013,0.18,-0.073,0.041,0.12,0.064,-0.042,-3.6,-1.6e-05,-5.8e-05,1.7e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,7.9e-05,7.8e-05,5.4e-05,0.015,0.015,0.0081,0.042,0.042,0.035,4e-11,4e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27390000,0.98,-0.0089,-0.015,0.18,-0.078,0.047,0.45,0.055,-0.035,-3.5,-1.6e-05,-5.8e-05,1.6e-06,0.0002,-8.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,7.9e-05,7.8e-05,5.4e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27490000,0.98,-0.01,-0.017,0.18,-0.081,0.053,0.76,0.047,-0.03,-3.5,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.8e-05,5.4e-05,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27590000,0.98,-0.01,-0.016,0.18,-0.075,0.055,0.85,0.038,-0.025,-3.4,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.8e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27690000,0.98,-0.0089,-0.013,0.18,-0.072,0.052,0.76,0.031,-0.02,-3.3,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-11,3.9e-11,1.6e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27790000,0.98,-0.0076,-0.011,0.18,-0.071,0.05,0.75,0.025,-0.018,-3.3,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27890000,0.98,-0.0072,-0.012,0.19,-0.078,0.057,0.79,0.018,-0.012,-3.2,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.5e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-11,3.8e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -27990000,0.98,-0.0077,-0.012,0.19,-0.078,0.058,0.78,0.012,-0.011,-3.1,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.3e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28090000,0.98,-0.0079,-0.012,0.18,-0.082,0.059,0.78,0.0043,-0.0048,-3,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-8.2e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.2e-05,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28190000,0.98,-0.0074,-0.012,0.18,-0.082,0.055,0.79,-0.0022,-0.0043,-3,-1.6e-05,-5.8e-05,1.3e-06,0.0002,-7.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.2e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-11,3.6e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28290000,0.98,-0.0069,-0.012,0.18,-0.087,0.059,0.79,-0.011,0.0014,-2.9,-1.6e-05,-5.8e-05,1.4e-06,0.0002,-8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.7e-06,8e-05,7.9e-05,5.2e-05,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-11,3.7e-11,1.5e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28390000,0.98,-0.0069,-0.013,0.18,-0.088,0.061,0.79,-0.015,0.0044,-2.8,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-7.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8e-05,7.9e-05,5.2e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-11,3.6e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28490000,0.98,-0.0072,-0.014,0.18,-0.089,0.066,0.79,-0.024,0.011,-2.8,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,7.9e-05,5.2e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.6e-11,3.6e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28590000,0.98,-0.0072,-0.014,0.18,-0.083,0.061,0.79,-0.027,0.0084,-2.7,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-7.8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,7.9e-05,5.1e-05,0.013,0.013,0.008,0.038,0.038,0.035,3.5e-11,3.5e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28690000,0.98,-0.007,-0.013,0.18,-0.083,0.062,0.79,-0.036,0.015,-2.6,-1.5e-05,-5.8e-05,1.3e-06,0.0002,-7.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5.1e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.5e-11,3.5e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28790000,0.98,-0.0063,-0.013,0.18,-0.079,0.062,0.79,-0.038,0.016,-2.5,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-7.9e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5.1e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.5e-11,3.5e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28890000,0.98,-0.0062,-0.012,0.18,-0.084,0.064,0.78,-0.046,0.023,-2.5,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5.1e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.5e-11,3.5e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -28990000,0.98,-0.006,-0.013,0.18,-0.079,0.06,0.78,-0.046,0.022,-2.4,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.1e-05,-0.0012,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5.1e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-11,3.4e-11,1.4e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29090000,0.98,-0.0058,-0.013,0.18,-0.082,0.063,0.78,-0.054,0.028,-2.3,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.2e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29190000,0.98,-0.0058,-0.013,0.18,-0.078,0.061,0.78,-0.051,0.027,-2.2,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.4e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29290000,0.98,-0.006,-0.013,0.18,-0.08,0.067,0.78,-0.059,0.033,-2.2,-1.5e-05,-5.8e-05,1.4e-06,0.0002,-8.4e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-11,3.4e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29390000,0.98,-0.0065,-0.012,0.18,-0.075,0.065,0.78,-0.057,0.034,-2.1,-1.5e-05,-5.8e-05,1.5e-06,0.0002,-8.6e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29490000,0.98,-0.0065,-0.012,0.18,-0.078,0.066,0.78,-0.065,0.041,-2,-1.5e-05,-5.8e-05,1.6e-06,0.0002,-8.6e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,5e-05,0.014,0.014,0.0081,0.041,0.041,0.036,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29590000,0.98,-0.0064,-0.012,0.18,-0.074,0.064,0.78,-0.062,0.04,-1.9,-1.5e-05,-5.8e-05,1.7e-06,0.0002,-8.8e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29690000,0.98,-0.0064,-0.012,0.18,-0.078,0.063,0.78,-0.07,0.046,-1.9,-1.5e-05,-5.8e-05,1.7e-06,0.0002,-8.9e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,4.9e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-11,3.3e-11,1.3e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29790000,0.98,-0.0063,-0.013,0.18,-0.074,0.056,0.78,-0.065,0.044,-1.8,-1.4e-05,-5.8e-05,1.8e-06,0.0002,-9.2e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-11,3.2e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29890000,0.98,-0.0057,-0.013,0.18,-0.074,0.057,0.77,-0.072,0.049,-1.7,-1.4e-05,-5.8e-05,1.9e-06,0.0002,-9.4e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.6e-06,8.2e-05,8e-05,4.9e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-11,3.2e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -29990000,0.98,-0.0059,-0.013,0.18,-0.069,0.052,0.77,-0.068,0.044,-1.6,-1.4e-05,-5.8e-05,1.9e-06,0.00021,-9.8e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.9e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-11,3.2e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30090000,0.98,-0.006,-0.013,0.18,-0.069,0.053,0.77,-0.075,0.05,-1.6,-1.4e-05,-5.8e-05,1.8e-06,0.00021,-9.9e-05,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.2e-05,8e-05,4.9e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-11,3.2e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30190000,0.98,-0.006,-0.013,0.18,-0.063,0.05,0.77,-0.068,0.048,-1.5,-1.4e-05,-5.7e-05,1.6e-06,0.00021,-0.0001,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30290000,0.98,-0.006,-0.013,0.19,-0.062,0.05,0.77,-0.074,0.053,-1.4,-1.4e-05,-5.7e-05,1.6e-06,0.00021,-0.0001,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.2e-05,8e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30390000,0.98,-0.006,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.4,-1.4e-05,-5.7e-05,1.8e-06,0.00021,-0.00011,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.8e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30490000,0.98,-0.006,-0.013,0.19,-0.057,0.044,0.77,-0.072,0.054,-1.3,-1.4e-05,-5.7e-05,1.8e-06,0.00021,-0.00011,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.8e-05,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-11,3.1e-11,1.2e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30590000,0.98,-0.0064,-0.014,0.18,-0.053,0.041,0.76,-0.065,0.05,-1.2,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00011,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.8e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30690000,0.98,-0.0068,-0.014,0.18,-0.051,0.04,0.76,-0.07,0.054,-1.1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00011,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.8e-05,0.013,0.013,0.008,0.041,0.041,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30790000,0.98,-0.0064,-0.013,0.18,-0.044,0.035,0.76,-0.063,0.052,-1.1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30890000,0.98,-0.0058,-0.013,0.18,-0.044,0.032,0.76,-0.067,0.056,-1,-1.4e-05,-5.7e-05,1.9e-06,0.00022,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -30990000,0.98,-0.006,-0.013,0.18,-0.037,0.026,0.76,-0.057,0.049,-0.94,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.0079,0.037,0.037,0.035,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31090000,0.98,-0.0061,-0.013,0.19,-0.035,0.025,0.76,-0.061,0.051,-0.86,-1.4e-05,-5.7e-05,1.8e-06,0.00023,-0.00012,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.036,3e-11,3e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31190000,0.98,-0.0063,-0.013,0.19,-0.031,0.021,0.76,-0.052,0.046,-0.79,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31290000,0.98,-0.0066,-0.014,0.18,-0.028,0.018,0.76,-0.055,0.048,-0.72,-1.4e-05,-5.7e-05,2e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.7e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31390000,0.98,-0.0063,-0.013,0.18,-0.022,0.012,0.76,-0.046,0.042,-0.65,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,7.9e-05,4.6e-05,0.013,0.013,0.0079,0.037,0.037,0.035,2.9e-11,2.9e-11,1.1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31490000,0.98,-0.0061,-0.014,0.19,-0.022,0.009,0.76,-0.049,0.043,-0.58,-1.4e-05,-5.7e-05,1.9e-06,0.00023,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,8e-05,4.6e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-11,2.9e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31590000,0.98,-0.0059,-0.014,0.18,-0.018,0.0068,0.76,-0.038,0.039,-0.51,-1.4e-05,-5.7e-05,2e-06,0.00024,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8e-05,7.9e-05,4.6e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.9e-11,2.9e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31690000,0.98,-0.0059,-0.015,0.18,-0.02,0.0057,0.76,-0.04,0.039,-0.44,-1.4e-05,-5.7e-05,2.1e-06,0.00024,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8.1e-05,7.9e-05,4.6e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-11,2.9e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31790000,0.98,-0.0062,-0.015,0.18,-0.011,0.0031,0.76,-0.028,0.037,-0.36,-1.4e-05,-5.7e-05,2.1e-06,0.00024,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.5e-06,8e-05,7.9e-05,4.6e-05,0.012,0.012,0.008,0.037,0.037,0.035,2.8e-11,2.8e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31890000,0.98,-0.0059,-0.015,0.18,-0.008,0.00083,0.76,-0.029,0.038,-0.3,-1.4e-05,-5.7e-05,2.2e-06,0.00024,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.6e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-11,2.8e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -31990000,0.98,-0.0061,-0.015,0.18,-0.0001,0.00017,0.75,-0.017,0.034,-0.23,-1.3e-05,-5.7e-05,2.1e-06,0.00025,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.5e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-11,2.8e-11,1e-10,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32090000,0.98,-0.0065,-0.014,0.18,-0.00064,-0.0032,0.76,-0.017,0.034,-0.16,-1.3e-05,-5.7e-05,2.1e-06,0.00025,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.5e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-11,2.8e-11,9.9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32190000,0.98,-0.0067,-0.015,0.18,0.0047,-0.0064,0.76,-0.006,0.033,-0.092,-1.3e-05,-5.7e-05,2.1e-06,0.00025,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.5e-05,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-11,2.8e-11,9.8e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32290000,0.98,-0.0066,-0.015,0.18,0.0063,-0.0091,0.75,-0.0055,0.032,-0.024,-1.3e-05,-5.7e-05,2.2e-06,0.00025,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.5e-05,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-11,2.8e-11,9.7e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32390000,0.98,-0.0067,-0.015,0.18,0.013,-0.01,0.75,0.0057,0.03,0.051,-1.3e-05,-5.7e-05,2.1e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.8e-05,4.5e-05,0.012,0.012,0.008,0.037,0.037,0.035,2.7e-11,2.7e-11,9.7e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32490000,0.98,-0.0096,-0.013,0.18,0.039,-0.073,-0.12,0.0089,0.023,0.054,-1.3e-05,-5.7e-05,2.1e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,8e-05,7.9e-05,4.5e-05,0.015,0.015,0.0078,0.04,0.04,0.035,2.8e-11,2.8e-11,9.6e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32590000,0.98,-0.0095,-0.013,0.18,0.039,-0.074,-0.12,0.021,0.02,0.035,-1.4e-05,-5.7e-05,2.2e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.9e-05,7.7e-05,4.5e-05,0.016,0.016,0.0075,0.037,0.037,0.035,2.7e-11,2.7e-11,9.5e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32690000,0.98,-0.0095,-0.013,0.18,0.035,-0.08,-0.12,0.025,0.012,0.02,-1.4e-05,-5.7e-05,2.1e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.9e-05,7.8e-05,4.4e-05,0.019,0.019,0.0074,0.041,0.041,0.035,2.7e-11,2.7e-11,9.4e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32790000,0.98,-0.0092,-0.013,0.18,0.034,-0.078,-0.12,0.034,0.01,0.0042,-1.4e-05,-5.7e-05,2.2e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.6e-05,7.5e-05,4.4e-05,0.019,0.019,0.0071,0.037,0.037,0.035,2.7e-11,2.7e-11,9.3e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32890000,0.98,-0.0091,-0.013,0.18,0.033,-0.084,-0.13,0.038,0.0021,-0.011,-1.4e-05,-5.7e-05,2.2e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.6e-05,7.5e-05,4.4e-05,0.023,0.023,0.007,0.041,0.041,0.035,2.7e-11,2.7e-11,9.2e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -32990000,0.98,-0.0089,-0.013,0.18,0.03,-0.08,-0.13,0.045,-0.0011,-0.024,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.2e-05,7.1e-05,4.4e-05,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33090000,0.98,-0.0088,-0.013,0.18,0.026,-0.083,-0.12,0.048,-0.0092,-0.031,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00013,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,7.2e-05,7.1e-05,4.4e-05,0.028,0.029,0.0066,0.042,0.042,0.035,2.7e-11,2.7e-11,9.1e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33190000,0.98,-0.0085,-0.013,0.18,0.022,-0.079,-0.12,0.054,-0.011,-0.037,-1.4e-05,-5.6e-05,2.2e-06,0.00026,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,6.6e-05,6.6e-05,4.4e-05,0.029,0.029,0.0064,0.038,0.038,0.035,2.6e-11,2.6e-11,9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33290000,0.98,-0.0086,-0.013,0.18,0.019,-0.08,-0.12,0.056,-0.019,-0.045,-1.4e-05,-5.6e-05,2.3e-06,0.00026,-0.00014,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,6.7e-05,6.6e-05,4.4e-05,0.035,0.035,0.0063,0.043,0.043,0.034,2.6e-11,2.6e-11,8.9e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33390000,0.98,-0.0081,-0.013,0.18,0.014,-0.066,-0.12,0.059,-0.014,-0.053,-1.4e-05,-5.6e-05,2.3e-06,0.00025,-0.00016,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,6e-05,5.9e-05,4.4e-05,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-11,2.6e-11,8.8e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33490000,0.98,-0.0081,-0.013,0.18,0.0094,-0.067,-0.12,0.06,-0.021,-0.063,-1.4e-05,-5.6e-05,2.3e-06,0.00025,-0.00016,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.4e-06,6e-05,5.9e-05,4.3e-05,0.042,0.042,0.0061,0.044,0.044,0.034,2.6e-11,2.6e-11,8.8e-11,2.7e-06,2.7e-06,5e-08,0,0,0,0,0,0,0,0 -33590000,0.98,-0.0077,-0.013,0.18,0.0055,-0.058,-0.11,0.063,-0.017,-0.069,-1.4e-05,-5.6e-05,2.4e-06,0.00024,-0.00018,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,5.3e-05,5.2e-05,4.3e-05,0.04,0.041,0.006,0.04,0.04,0.034,2.6e-11,2.6e-11,8.7e-11,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -33690000,0.98,-0.0077,-0.013,0.18,0.00085,-0.058,-0.11,0.063,-0.023,-0.077,-1.4e-05,-5.6e-05,2.4e-06,0.00024,-0.00018,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,5.3e-05,5.2e-05,4.3e-05,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-11,2.6e-11,8.6e-11,2.6e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -33790000,0.98,-0.0075,-0.013,0.18,-0.0023,-0.048,-0.11,0.067,-0.018,-0.083,-1.4e-05,-5.6e-05,2.3e-06,0.00023,-0.00021,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,4.6e-05,4.6e-05,4.3e-05,0.044,0.045,0.0059,0.041,0.041,0.034,2.6e-11,2.6e-11,8.6e-11,2.5e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -33890000,0.98,-0.0075,-0.013,0.18,-0.0065,-0.046,-0.11,0.066,-0.023,-0.09,-1.4e-05,-5.6e-05,2.4e-06,0.00023,-0.00021,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,4.7e-05,4.6e-05,4.3e-05,0.052,0.052,0.0058,0.047,0.047,0.033,2.6e-11,2.6e-11,8.5e-11,2.5e-06,2.6e-06,5e-08,0,0,0,0,0,0,0,0 -33990000,0.98,-0.0072,-0.013,0.18,-0.0062,-0.031,-0.1,0.069,-0.015,-0.092,-1.4e-05,-5.6e-05,2.3e-06,0.00021,-0.00024,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,4.1e-05,4.1e-05,4.3e-05,0.047,0.047,0.0058,0.042,0.042,0.033,2.6e-11,2.6e-11,8.4e-11,2.5e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -34090000,0.98,-0.0072,-0.013,0.18,-0.011,-0.031,-0.1,0.069,-0.018,-0.096,-1.4e-05,-5.6e-05,2.3e-06,0.00021,-0.00024,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,4.1e-05,4.1e-05,4.3e-05,0.054,0.054,0.0058,0.049,0.049,0.033,2.6e-11,2.6e-11,8.4e-11,2.5e-06,2.5e-06,5e-08,0,0,0,0,0,0,0,0 -34190000,0.98,-0.0071,-0.013,0.18,-0.011,-0.021,-0.098,0.072,-0.013,-0.099,-1.4e-05,-5.6e-05,2.3e-06,0.00019,-0.00026,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.7e-05,3.6e-05,4.2e-05,0.047,0.048,0.0058,0.043,0.043,0.033,2.6e-11,2.6e-11,8.3e-11,2.4e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34290000,0.98,-0.0069,-0.013,0.18,-0.012,-0.02,-0.097,0.071,-0.015,-0.1,-1.4e-05,-5.6e-05,2.3e-06,0.00019,-0.00026,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.7e-05,3.6e-05,4.2e-05,0.054,0.055,0.0058,0.05,0.05,0.033,2.6e-11,2.6e-11,8.2e-11,2.4e-06,2.4e-06,5e-08,0,0,0,0,0,0,0,0 -34390000,0.98,-0.0068,-0.013,0.18,-0.013,-0.01,-0.092,0.073,-0.01,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00018,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.047,0.047,0.0058,0.044,0.044,0.033,2.6e-11,2.6e-11,8.2e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34490000,0.98,-0.0069,-0.013,0.18,-0.016,-0.0096,-0.09,0.071,-0.011,-0.11,-1.4e-05,-5.6e-05,2.3e-06,0.00017,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.3e-05,3.3e-05,4.2e-05,0.053,0.053,0.0059,0.051,0.051,0.032,2.6e-11,2.6e-11,8.1e-11,2.3e-06,2.3e-06,5e-08,0,0,0,0,0,0,0,0 -34590000,0.98,-0.0068,-0.013,0.18,-0.014,-0.0053,0.71,0.073,-0.0089,-0.081,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3e-05,4.2e-05,0.044,0.044,0.0059,0.045,0.045,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -34690000,0.98,-0.0068,-0.012,0.18,-0.017,-0.0032,1.7,0.071,-0.0093,0.037,-1.4e-05,-5.6e-05,2.3e-06,0.00016,-0.00027,-0.0011,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,3.1e-05,3.1e-05,4.2e-05,0.047,0.047,0.006,0.052,0.052,0.032,2.6e-11,2.6e-11,8e-11,2.2e-06,2.2e-06,5e-08,0,0,0,0,0,0,0,0 -34790000,0.98,-0.0068,-0.012,0.18,-0.018,0.0015,2.7,0.072,-0.0069,0.21,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.04,0.04,0.0061,0.045,0.045,0.032,2.6e-11,2.6e-11,7.9e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 -34890000,0.98,-0.0068,-0.012,0.18,-0.022,0.0039,3.6,0.07,-0.0065,0.5,-1.4e-05,-5.6e-05,2.2e-06,0.00018,-0.00029,-0.001,0.2,0.002,0.43,0,0,0,0,0,6.3e-06,2.9e-05,2.9e-05,4.2e-05,0.043,0.043,0.0061,0.052,0.052,0.032,2.6e-11,2.6e-11,7.8e-11,2e-06,2e-06,5e-08,0,0,0,0,0,0,0,0 +10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5.7e-07,0.0025,0.0025,4.3e-05,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +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,6.2e-07,0.0026,0.0026,0.00011,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +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,-1.5e-11,0,0,-6.8e-08,0,0,0,0,0,0,0,0,6.7e-07,0.0027,0.0027,0.00023,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +290000,1,-0.012,-0.011,0.00045,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.9e-09,-5.9e-09,-4.7e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,7.4e-07,0.0029,0.0029,0.00019,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +390000,1,-0.012,-0.011,0.0005,-0.0025,-0.006,-0.063,-0.00056,-0.00072,-0.013,-7.2e-09,-5.8e-09,-1.3e-09,0,0,8.8e-06,0,0,0,0,0,0,0,0,8.2e-07,0.0031,0.0031,0.0003,25,25,8.1,0.97,0.97,0.32,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +490000,1,-0.012,-0.012,0.00056,-0.0007,-0.0063,-0.069,-0.00015,-0.00047,-0.011,-1.2e-06,7.5e-07,-1.7e-07,0,0,1.6e-05,0,0,0,0,0,0,0,0,9.2e-07,0.0033,0.0034,0.00018,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,0,0,0,0,0,0,0 +590000,1,-0.012,-0.012,0.00058,-0.002,-0.009,-0.12,-0.00028,-0.0013,-0.029,-1.3e-06,7.8e-07,-1.8e-07,0,0,6.4e-05,0,0,0,0,0,0,0,0,1e-06,0.0037,0.0037,0.00025,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,0,0,0,0,0,0,0 +690000,1,-0.012,-0.012,0.00062,5.3e-05,-0.0089,-0.05,-8e-05,-0.00079,-0.0088,-5.6e-06,1.6e-06,-5.2e-07,0,0,-9.5e-05,0,0,0,0,0,0,0,0,1.2e-06,0.004,0.004,0.00015,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,0,0,0,0,0,0,0 +790000,1,-0.012,-0.012,0.00062,0.0022,-0.011,-0.054,-2.3e-05,-0.0017,-0.011,-5.4e-06,1.7e-06,-5.1e-07,0,0,-0.00013,0,0,0,0,0,0,0,0,1.3e-06,0.0044,0.0044,0.0002,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,0,0,0,0,0,0,0 +890000,1,-0.012,-0.013,0.00063,0.0031,-0.0085,-0.093,0.00015,-0.0011,-0.031,-2.2e-05,1.2e-06,-7.5e-07,0,0,-3.3e-05,0,0,0,0,0,0,0,0,1.5e-06,0.0048,0.0048,0.00013,1.3,1.3,1.3,0.2,0.2,0.25,0.0099,0.0099,0.00067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +990000,1,-0.012,-0.013,0.0006,0.006,-0.0099,-0.12,0.00062,-0.002,-0.046,-2.2e-05,1.2e-06,-7.5e-07,0,0,1.3e-05,0,0,0,0,0,0,0,0,1.7e-06,0.0053,0.0053,0.00016,1.5,1.5,0.95,0.3,0.3,0.23,0.0099,0.0099,0.00067,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1090000,1,-0.012,-0.013,0.00056,0.011,-0.013,-0.13,0.00077,-0.0015,-0.062,-6e-05,-1.5e-05,-1.8e-07,0,0,4.5e-05,0,0,0,0,0,0,0,0,1.8e-06,0.0056,0.0057,0.00011,0.92,0.92,0.69,0.17,0.17,0.2,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1190000,1,-0.012,-0.013,0.0005,0.015,-0.018,-0.11,0.0021,-0.003,-0.047,-5.8e-05,-1.3e-05,-2.2e-07,0,0,-0.00048,0,0,0,0,0,0,0,0,2.1e-06,0.0062,0.0063,0.00013,1.1,1.1,0.54,0.24,0.24,0.19,0.0098,0.0098,0.00042,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1290000,1,-0.012,-0.014,0.00045,0.019,-0.018,-0.11,0.002,-0.0024,-0.048,-0.00017,-9.5e-05,3e-06,0,0,-0.00075,0,0,0,0,0,0,0,0,2.1e-06,0.0064,0.0064,9.8e-05,0.87,0.87,0.41,0.15,0.15,0.18,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1390000,1,-0.012,-0.014,0.00042,0.026,-0.023,-0.097,0.0043,-0.0045,-0.038,-0.00016,-9e-05,2.8e-06,0,0,-0.0014,0,0,0,0,0,0,0,0,2.4e-06,0.007,0.007,0.00011,1.1,1.1,0.33,0.21,0.21,0.16,0.0095,0.0095,0.00028,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1490000,1,-0.012,-0.014,0.0004,0.024,-0.021,-0.12,0.0034,-0.0033,-0.053,-0.00039,-0.00033,1.1e-05,0,0,-0.0012,0,0,0,0,0,0,0,0,2.3e-06,0.0067,0.0067,8.7e-05,0.95,0.94,0.27,0.14,0.14,0.15,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1590000,1,-0.012,-0.014,0.00042,0.031,-0.025,-0.13,0.0062,-0.0056,-0.063,-0.00039,-0.00033,1.1e-05,0,0,-0.0014,0,0,0,0,0,0,0,0,2.6e-06,0.0073,0.0073,0.0001,1.3,1.3,0.23,0.2,0.2,0.14,0.0088,0.0088,0.0002,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1690000,1,-0.012,-0.014,0.00046,0.028,-0.02,-0.13,0.0044,-0.0038,-0.068,-0.00074,-0.00073,2.4e-05,0,0,-0.0018,0,0,0,0,0,0,0,0,2.3e-06,0.0063,0.0063,7.9e-05,1,1,0.19,0.14,0.14,0.13,0.0078,0.0078,0.00015,0.04,0.04,0.04,0,0,0,0,0,0,0,0 +1790000,1,-0.012,-0.014,0.00041,0.036,-0.025,-0.13,0.0076,-0.006,-0.067,-0.00073,-0.00072,2.4e-05,0,0,-0.0028,0,0,0,0,0,0,0,0,2.6e-06,0.0069,0.007,8.9e-05,1.3,1.3,0.16,0.2,0.2,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +1890000,1,-0.012,-0.014,0.00041,0.043,-0.026,-0.14,0.012,-0.0085,-0.075,-0.00073,-0.00072,2.4e-05,0,0,-0.0032,0,0,0,0,0,0,0,0,2.9e-06,0.0076,0.0076,0.0001,1.7,1.7,0.15,0.31,0.31,0.12,0.0078,0.0078,0.00015,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +1990000,1,-0.011,-0.014,0.0004,0.035,-0.018,-0.14,0.0082,-0.0055,-0.074,-0.0011,-0.0013,3.9e-05,0,0,-0.0046,0,0,0,0,0,0,0,0,2.3e-06,0.006,0.006,8e-05,1.3,1.3,0.13,0.2,0.2,0.11,0.0066,0.0066,0.00011,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +2090000,1,-0.011,-0.014,0.00042,0.042,-0.021,-0.14,0.012,-0.0076,-0.071,-0.0011,-0.0012,3.8e-05,0,0,-0.0065,0,0,0,0,0,0,0,0,2.6e-06,0.0066,0.0066,8.9e-05,1.7,1.7,0.12,0.31,0.31,0.11,0.0066,0.0066,0.00011,0.04,0.04,0.039,0,0,0,0,0,0,0,0 +2190000,1,-0.011,-0.014,0.00036,0.033,-0.014,-0.14,0.0081,-0.0045,-0.077,-0.0015,-0.0018,5.1e-05,0,0,-0.0075,0,0,0,0,0,0,0,0,2e-06,0.0049,0.005,7.2e-05,1.2,1.2,0.11,0.2,0.2,0.11,0.0055,0.0055,8.8e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2290000,1,-0.011,-0.014,0.00035,0.038,-0.015,-0.14,0.012,-0.0059,-0.075,-0.0014,-0.0018,5.1e-05,0,0,-0.0098,0,0,0,0,0,0,0,0,2.2e-06,0.0054,0.0054,8e-05,1.5,1.5,0.11,0.29,0.29,0.1,0.0055,0.0055,8.8e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2390000,1,-0.011,-0.013,0.00035,0.029,-0.01,-0.14,0.0075,-0.0034,-0.072,-0.0017,-0.0023,5.9e-05,0,0,-0.012,0,0,0,0,0,0,0,0,1.6e-06,0.004,0.004,6.6e-05,1,1,0.1,0.19,0.19,0.098,0.0045,0.0045,7e-05,0.04,0.04,0.038,0,0,0,0,0,0,0,0 +2490000,1,-0.011,-0.014,0.00043,0.033,-0.0092,-0.14,0.011,-0.0044,-0.079,-0.0017,-0.0023,5.9e-05,0,0,-0.013,0,0,0,0,0,0,0,0,1.7e-06,0.0044,0.0044,7.2e-05,1.3,1.3,0.1,0.28,0.28,0.097,0.0045,0.0045,7e-05,0.04,0.04,0.037,0,0,0,0,0,0,0,0 +2590000,1,-0.01,-0.013,0.00032,0.023,-0.0062,-0.15,0.0066,-0.0025,-0.085,-0.0019,-0.0027,6.4e-05,0,0,-0.015,0,0,0,0,0,0,0,0,1.3e-06,0.0032,0.0032,6.1e-05,0.89,0.89,0.099,0.18,0.18,0.094,0.0038,0.0038,5.7e-05,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +2690000,1,-0.01,-0.013,0.00036,0.027,-0.0055,-0.15,0.0091,-0.0031,-0.084,-0.0019,-0.0027,6.4e-05,0,0,-0.018,0,0,0,0,0,0,0,0,1.4e-06,0.0035,0.0035,6.6e-05,1.1,1.1,0.097,0.25,0.25,0.091,0.0038,0.0038,5.7e-05,0.04,0.04,0.036,0,0,0,0,0,0,0,0 +2790000,1,-0.01,-0.013,0.00029,0.022,-0.0031,-0.14,0.0059,-0.0017,-0.081,-0.002,-0.003,6.7e-05,0,0,-0.022,0,0,0,0,0,0,0,0,1e-06,0.0027,0.0027,5.6e-05,0.76,0.77,0.096,0.16,0.16,0.089,0.0032,0.0032,4.7e-05,0.04,0.04,0.035,0,0,0,0,0,0,0,0 +2890000,1,-0.01,-0.013,0.00021,0.026,-0.0049,-0.14,0.0083,-0.0022,-0.081,-0.002,-0.003,6.6e-05,0,0,-0.025,0,0,0,0,0,0,0,0,1.1e-06,0.0029,0.0029,6.1e-05,0.95,0.95,0.096,0.23,0.23,0.089,0.0032,0.0032,4.7e-05,0.04,0.04,0.034,0,0,0,0,0,0,0,0 +2990000,1,-0.01,-0.013,0.00022,0.02,-0.0038,-0.15,0.0054,-0.0013,-0.086,-0.002,-0.0033,6.7e-05,0,0,-0.028,0,0,0,0,0,0,0,0,8.5e-07,0.0023,0.0023,5.3e-05,0.67,0.67,0.095,0.15,0.15,0.088,0.0027,0.0027,3.9e-05,0.04,0.04,0.033,0,0,0,0,0,0,0,0 +3090000,1,-0.01,-0.013,0.00042,0.025,-0.0067,-0.15,0.0077,-0.0019,-0.087,-0.002,-0.0033,6.7e-05,0,0,-0.031,0,0,0,0,0,0,0,0,9.3e-07,0.0025,0.0025,5.7e-05,0.82,0.82,0.095,0.22,0.22,0.086,0.0027,0.0027,3.9e-05,0.04,0.04,0.032,0,0,0,0,0,0,0,0 +3190000,1,-0.01,-0.013,0.00045,0.021,-0.0065,-0.15,0.0051,-0.0014,-0.097,-0.0021,-0.0036,6.8e-05,0,0,-0.033,0,0,0,0,0,0,0,0,7.2e-07,0.002,0.002,4.9e-05,0.59,0.59,0.096,0.14,0.14,0.087,0.0023,0.0023,3.3e-05,0.04,0.04,0.031,0,0,0,0,0,0,0,0 +3290000,1,-0.01,-0.013,0.00047,0.024,-0.0067,-0.15,0.0074,-0.0021,-0.11,-0.0021,-0.0036,6.7e-05,0,0,-0.034,0,0,0,0,0,0,0,0,7.8e-07,0.0022,0.0022,5.3e-05,0.73,0.73,0.095,0.2,0.2,0.086,0.0023,0.0023,3.3e-05,0.04,0.04,0.03,0,0,0,0,0,0,0,0 +3390000,1,-0.0098,-0.013,0.00047,0.019,-0.0037,-0.15,0.005,-0.0014,-0.1,-0.0021,-0.0038,6.7e-05,0,0,-0.04,0,0,0,0,0,0,0,0,6e-07,0.0017,0.0017,4.6e-05,0.53,0.53,0.095,0.14,0.14,0.085,0.0019,0.0019,2.8e-05,0.04,0.04,0.029,0,0,0,0,0,0,0,0 +3490000,1,-0.0097,-0.013,0.00045,0.025,-0.0024,-0.15,0.0073,-0.0017,-0.1,-0.0021,-0.0038,6.7e-05,0,0,-0.044,0,0,0,0,0,0,0,0,6.5e-07,0.0019,0.0019,5e-05,0.65,0.65,0.095,0.19,0.19,0.086,0.0019,0.0019,2.8e-05,0.04,0.04,0.028,0,0,0,0,0,0,0,0 +3590000,1,-0.0096,-0.013,0.0004,0.022,-0.0019,-0.15,0.0051,-0.001,-0.11,-0.0022,-0.004,6.6e-05,0,0,-0.047,0,0,0,0,0,0,0,0,5e-07,0.0015,0.0015,4.4e-05,0.48,0.48,0.094,0.13,0.13,0.086,0.0016,0.0016,2.4e-05,0.04,0.04,0.026,0,0,0,0,0,0,0,0 +3690000,1,-0.0096,-0.013,0.00038,0.024,-0.0014,-0.15,0.0075,-0.0013,-0.11,-0.0022,-0.004,6.6e-05,0,0,-0.052,0,0,0,0,0,0,0,0,5.4e-07,0.0017,0.0017,4.7e-05,0.59,0.59,0.093,0.18,0.18,0.085,0.0016,0.0016,2.4e-05,0.04,0.04,0.025,0,0,0,0,0,0,0,0 +3790000,1,-0.0095,-0.012,0.00039,0.02,0.0031,-0.15,0.0052,-0.0006,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.055,0,0,0,0,0,0,0,0,4.3e-07,0.0014,0.0014,4.1e-05,0.44,0.44,0.093,0.12,0.12,0.086,0.0014,0.0014,2.1e-05,0.04,0.04,0.024,0,0,0,0,0,0,0,0 +3890000,1,-0.0094,-0.013,0.00047,0.022,0.0042,-0.14,0.0074,-0.00025,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.059,0,0,0,0,0,0,0,0,4.7e-07,0.0015,0.0015,4.4e-05,0.54,0.54,0.091,0.17,0.17,0.086,0.0014,0.0014,2.1e-05,0.04,0.04,0.022,0,0,0,0,0,0,0,0 +3990000,1,-0.0094,-0.013,0.00053,0.027,0.0038,-0.14,0.0099,7.5e-05,-0.11,-0.0022,-0.0042,6.5e-05,0,0,-0.064,0,0,0,0,0,0,0,0,5.1e-07,0.0016,0.0016,4.7e-05,0.66,0.66,0.089,0.22,0.22,0.085,0.0014,0.0014,2.1e-05,0.04,0.04,0.021,0,0,0,0,0,0,0,0 +4090000,1,-0.0094,-0.012,0.00059,0.023,0.0031,-0.12,0.0074,0.00032,-0.098,-0.0022,-0.0044,6.2e-05,0,0,-0.072,0,0,0,0,0,0,0,0,3.9e-07,0.0013,0.0013,4.2e-05,0.5,0.5,0.087,0.16,0.16,0.085,0.0011,0.0011,1.8e-05,0.04,0.04,0.02,0,0,0,0,0,0,0,0 +4190000,1,-0.0095,-0.012,0.00055,0.025,0.0027,-0.12,0.0098,0.00061,-0.1,-0.0022,-0.0044,6.2e-05,0,0,-0.074,0,0,0,0,0,0,0,0,4.3e-07,0.0015,0.0015,4.4e-05,0.6,0.6,0.086,0.21,0.21,0.086,0.0011,0.0011,1.8e-05,0.04,0.04,0.019,0,0,0,0,0,0,0,0 +4290000,1,-0.0095,-0.012,0.00055,0.023,0.0026,-0.12,0.0071,0.00052,-0.11,-0.0022,-0.0046,6e-05,0,0,-0.077,0,0,0,0,0,0,0,0,3.4e-07,0.0012,0.0012,4e-05,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,0,0,0,0,0,0,0 +4390000,1,-0.0095,-0.012,0.0005,0.026,0.00099,-0.11,0.0097,0.00061,-0.094,-0.0022,-0.0046,6e-05,0,0,-0.083,0,0,0,0,0,0,0,0,3.6e-07,0.0013,0.0013,4.2e-05,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,0,0,0,0,0,0,0 +4490000,1,-0.0095,-0.012,0.00056,0.022,0.0028,-0.11,0.0071,0.00052,-0.095,-0.0022,-0.0048,5.7e-05,0,0,-0.086,0,0,0,0,0,0,0,0,2.8e-07,0.001,0.001,3.8e-05,0.42,0.42,0.08,0.14,0.14,0.085,0.00078,0.00078,1.4e-05,0.04,0.04,0.015,0,0,0,0,0,0,0,0 +4590000,1,-0.0095,-0.012,0.00062,0.025,0.0015,-0.11,0.0095,0.00072,-0.098,-0.0022,-0.0048,5.7e-05,0,0,-0.089,0,0,0,0,0,0,0,0,3.1e-07,0.0011,0.0011,4e-05,0.51,0.51,0.077,0.19,0.19,0.084,0.00078,0.00078,1.4e-05,0.04,0.04,0.014,0,0,0,0,0,0,0,0 +4690000,1,-0.0095,-0.012,0.00055,0.019,0.0018,-0.1,0.0069,0.00051,-0.09,-0.0022,-0.005,5.5e-05,0,0,-0.093,0,0,0,0,0,0,0,0,2.4e-07,0.00092,0.00092,3.6e-05,0.39,0.39,0.074,0.14,0.14,0.083,0.00064,0.00064,1.3e-05,0.04,0.04,0.013,0,0,0,0,0,0,0,0 +4790000,1,-0.0094,-0.012,0.00064,0.017,0.0039,-0.099,0.0087,0.00084,-0.092,-0.0022,-0.005,5.5e-05,0,0,-0.095,0,0,0,0,0,0,0,0,2.6e-07,0.00099,0.00099,3.8e-05,0.47,0.47,0.073,0.18,0.18,0.084,0.00064,0.00064,1.3e-05,0.04,0.04,0.012,0,0,0,0,0,0,0,0 +4890000,1,-0.0093,-0.012,0.00067,0.012,0.0015,-0.093,0.0059,0.00065,-0.088,-0.0021,-0.0051,5.3e-05,0,0,-0.099,0,0,0,0,0,0,0,0,2e-07,0.0008,0.0008,3.5e-05,0.36,0.36,0.07,0.13,0.13,0.083,0.00052,0.00052,1.1e-05,0.04,0.04,0.011,0,0,0,0,0,0,0,0 +4990000,1,-0.0093,-0.012,0.00065,0.016,0.0021,-0.085,0.0073,0.00085,-0.083,-0.0021,-0.0051,5.3e-05,0,0,-0.1,0,0,0,0,0,0,0,0,2.1e-07,0.00086,0.00086,3.6e-05,0.43,0.43,0.067,0.17,0.17,0.082,0.00052,0.00052,1.1e-05,0.04,0.04,0.01,0,0,0,0,0,0,0,0 +5090000,1,-0.0092,-0.012,0.00072,0.012,0.0026,-0.082,0.0051,0.00061,-0.081,-0.0021,-0.0052,5.1e-05,0,0,-0.1,0,0,0,0,0,0,0,0,1.6e-07,0.00069,0.00069,3.3e-05,0.33,0.33,0.065,0.12,0.12,0.082,0.00042,0.00042,1e-05,0.04,0.04,0.0098,0,0,0,0,0,0,0,0 +5190000,1,-0.009,-0.012,0.00076,0.012,0.006,-0.08,0.0063,0.001,-0.079,-0.0021,-0.0052,5.1e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.8e-07,0.00075,0.00075,3.5e-05,0.39,0.39,0.063,0.16,0.16,0.081,0.00042,0.00042,1e-05,0.04,0.04,0.0091,0,0,0,0,0,0,0,0 +5290000,1,-0.0089,-0.012,0.00081,0.01,0.0062,-0.068,0.0044,0.00098,-0.072,-0.0021,-0.0053,4.9e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.3e-07,0.0006,0.0006,3.2e-05,0.3,0.3,0.06,0.12,0.12,0.08,0.00034,0.00034,9.1e-06,0.04,0.04,0.0084,0,0,0,0,0,0,0,0 +5390000,1,-0.0088,-0.012,0.00081,0.0099,0.0098,-0.065,0.0054,0.0017,-0.067,-0.0021,-0.0053,4.9e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.4e-07,0.00065,0.00065,3.4e-05,0.36,0.36,0.057,0.15,0.15,0.079,0.00034,0.00034,9.1e-06,0.04,0.04,0.0078,0,0,0,0,0,0,0,0 +5490000,1,-0.0089,-0.012,0.0008,0.0091,0.011,-0.06,0.0037,0.0017,-0.065,-0.0021,-0.0054,4.8e-05,0,0,-0.11,0,0,0,0,0,0,0,0,1.1e-07,0.00052,0.00052,3.1e-05,0.28,0.28,0.056,0.11,0.11,0.079,0.00028,0.00028,8.3e-06,0.04,0.04,0.0073,0,0,0,0,0,0,0,0 +5590000,1,-0.0089,-0.012,0.00072,0.011,0.015,-0.053,0.0048,0.003,-0.058,-0.0021,-0.0054,4.8e-05,0,0,-0.12,0,0,0,0,0,0,0,0,1.2e-07,0.00056,0.00056,3.2e-05,0.32,0.32,0.053,0.15,0.15,0.078,0.00028,0.00028,8.3e-06,0.04,0.04,0.0067,0,0,0,0,0,0,0,0 +5690000,1,-0.0089,-0.011,0.00062,0.0095,0.015,-0.052,0.0034,0.0027,-0.055,-0.002,-0.0055,4.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.3e-08,0.00045,0.00045,3e-05,0.25,0.25,0.051,0.11,0.11,0.076,0.00022,0.00022,7.5e-06,0.04,0.04,0.0062,0,0,0,0,0,0,0,0 +5790000,1,-0.0088,-0.011,0.00057,0.011,0.017,-0.049,0.0045,0.0042,-0.053,-0.002,-0.0055,4.6e-05,0,0,-0.12,0,0,0,0,0,0,0,0,9.8e-08,0.00048,0.00048,3.1e-05,0.29,0.29,0.05,0.14,0.14,0.077,0.00022,0.00022,7.5e-06,0.04,0.04,0.0058,0,0,0,0,0,0,0,0 +5890000,1,-0.0088,-0.011,0.0006,0.011,0.015,-0.048,0.0034,0.0035,-0.056,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,7.7e-08,0.00039,0.00039,2.9e-05,0.23,0.23,0.048,0.1,0.1,0.075,0.00018,0.00018,6.8e-06,0.04,0.04,0.0054,0,0,0,0,0,0,0,0 +5990000,1,-0.0088,-0.012,0.00056,0.013,0.016,-0.041,0.0046,0.005,-0.05,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.3e-08,0.00041,0.00041,3e-05,0.27,0.27,0.045,0.13,0.13,0.074,0.00018,0.00018,6.8e-06,0.04,0.04,0.005,0,0,0,0,0,0,0,0 +6090000,1,-0.0088,-0.012,0.00037,0.013,0.017,-0.039,0.0059,0.0067,-0.047,-0.002,-0.0055,4.4e-05,0,0,-0.12,0,0,0,0,0,0,0,0,8.8e-08,0.00044,0.00044,3.1e-05,0.31,0.31,0.044,0.17,0.17,0.074,0.00018,0.00018,6.8e-06,0.04,0.04,0.0047,0,0,0,0,0,0,0,0 +6190000,0.98,-0.0066,-0.013,0.19,0.0098,0.016,-0.037,0.0045,0.0054,-0.047,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.22,0.22,0.042,0.13,0.13,0.073,0.00015,0.00015,6.3e-06,0.04,0.04,0.0044,0,0,0,0,0,0,0,0 +6290000,0.98,-0.0065,-0.013,0.19,0.0083,0.018,-0.041,0.0055,0.0071,-0.053,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.22,0.22,0.04,0.16,0.16,0.072,0.00015,0.00015,6.3e-06,0.04,0.04,0.0041,0,0,0,0,0,0,0,0 +6390000,0.98,-0.0065,-0.013,0.19,0.011,0.02,-0.042,0.0064,0.009,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00034,0.00034,0.0054,0.23,0.23,0.039,0.19,0.19,0.072,0.00015,0.00015,6.3e-06,0.04,0.04,0.0038,0,0,0,0,0,0,0,0 +6490000,0.98,-0.0064,-0.013,0.19,0.0084,0.019,-0.039,0.0073,0.011,-0.053,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00034,0.0054,0.24,0.24,0.038,0.23,0.23,0.07,0.00015,0.00015,6.3e-06,0.04,0.04,0.0036,0,0,0,0,0,0,0,0 +6590000,0.98,-0.0064,-0.013,0.19,0.0078,0.022,-0.042,0.0082,0.013,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00035,0.0054,0.26,0.26,0.036,0.28,0.28,0.069,0.00015,0.00015,6.3e-06,0.04,0.04,0.0033,0,0,0,0,0,0,0,0 +6690000,0.98,-0.0063,-0.013,0.19,0.0052,0.025,-0.044,0.0088,0.015,-0.057,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00035,0.00035,0.0054,0.28,0.28,0.035,0.33,0.33,0.068,0.00015,0.00015,6.3e-06,0.04,0.04,0.0031,0,0,0,0,0,0,0,0 +6790000,0.98,-0.0063,-0.013,0.19,0.007,0.027,-0.042,0.0093,0.018,-0.058,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00036,0.00036,0.0054,0.3,0.3,0.034,0.38,0.38,0.068,0.00015,0.00015,6.3e-06,0.04,0.04,0.0029,0,0,0,0,0,0,0,0 +6890000,0.98,-0.0061,-0.013,0.19,0.0066,0.028,-0.038,0.01,0.02,-0.055,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00036,0.00036,0.0054,0.32,0.32,0.032,0.45,0.45,0.067,0.00015,0.00015,6.3e-06,0.04,0.04,0.0027,0,0,0,0,0,0,0,0 +6990000,0.98,-0.006,-0.013,0.19,0.0066,0.029,-0.037,0.011,0.023,-0.055,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00037,0.00037,0.0054,0.35,0.35,0.031,0.52,0.52,0.066,0.00015,0.00015,6.3e-06,0.04,0.04,0.0026,0,0,0,0,0,0,0,0 +7090000,0.98,-0.0059,-0.013,0.19,0.0057,0.035,-0.037,0.011,0.027,-0.056,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00037,0.00037,0.0054,0.39,0.39,0.03,0.6,0.6,0.066,0.00015,0.00015,6.3e-06,0.04,0.04,0.0024,0,0,0,0,0,0,0,0 +7190000,0.98,-0.0058,-0.013,0.19,0.0053,0.038,-0.036,0.012,0.03,-0.058,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00038,0.00038,0.0054,0.42,0.42,0.029,0.69,0.69,0.065,0.00015,0.00015,6.3e-06,0.04,0.04,0.0023,0,0,0,0,0,0,0,0 +7290000,0.98,-0.0058,-0.013,0.19,0.0062,0.043,-0.034,0.012,0.034,-0.054,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00039,0.00039,0.0054,0.46,0.46,0.028,0.79,0.79,0.064,0.00015,0.00015,6.3e-06,0.04,0.04,0.0021,0,0,0,0,0,0,0,0 +7390000,0.98,-0.0057,-0.013,0.19,0.0044,0.046,-0.032,0.013,0.038,-0.052,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.0004,0.0004,0.0054,0.51,0.51,0.027,0.9,0.9,0.064,0.00014,0.00014,6.3e-06,0.04,0.04,0.002,0,0,0,0,0,0,0,0 +7490000,0.98,-0.0057,-0.013,0.19,0.0068,0.05,-0.026,0.013,0.043,-0.046,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00041,0.00041,0.0054,0.56,0.56,0.026,1,1,0.063,0.00014,0.00014,6.3e-06,0.04,0.04,0.0019,0,0,0,0,0,0,0,0 +7590000,0.98,-0.0057,-0.013,0.19,0.0077,0.053,-0.023,0.014,0.048,-0.041,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00042,0.00041,0.0054,0.6,0.6,0.025,1.2,1.2,0.062,0.00014,0.00014,6.3e-06,0.04,0.04,0.0018,0,0,0,0,0,0,0,0 +7690000,0.98,-0.0057,-0.013,0.19,0.0075,0.058,-0.022,0.015,0.053,-0.036,-0.0019,-0.0056,4.2e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00043,0.00043,0.0054,0.66,0.66,0.025,1.3,1.3,0.062,0.00014,0.00014,6.3e-06,0.04,0.04,0.0017,0,0,0,0,0,0,0,0 +7790000,0.98,-0.0056,-0.013,0.19,0.009,0.06,-0.024,0.015,0.058,-0.041,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00044,0.00043,0.0054,0.72,0.72,0.024,1.5,1.5,0.061,0.00014,0.00014,6.3e-06,0.04,0.04,0.0016,0,0,0,0,0,0,0,0 +7890000,0.98,-0.0056,-0.013,0.19,0.0077,0.065,-0.025,0.016,0.065,-0.045,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00045,0.00045,0.0054,0.78,0.78,0.023,1.7,1.7,0.06,0.00014,0.00014,6.3e-06,0.04,0.04,0.0015,0,0,0,0,0,0,0,0 +7990000,0.98,-0.0056,-0.013,0.19,0.0079,0.068,-0.021,0.016,0.07,-0.041,-0.0019,-0.0056,4.1e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.0002,0.00045,0.00045,0.0054,0.84,0.84,0.022,1.9,1.9,0.059,0.00014,0.00014,6.3e-06,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 +8090000,0.98,-0.0054,-0.013,0.19,0.0092,0.074,-0.022,0.017,0.077,-0.044,-0.0019,-0.0056,4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00047,0.00047,0.0043,0.91,0.92,0.022,2.1,2.1,0.059,0.00014,0.00014,6.3e-06,0.04,0.04,0.0014,0,0,0,0,0,0,0,0 +8190000,0.98,-0.0054,-0.013,0.19,0.0099,0.08,-0.018,0.018,0.085,-0.038,-0.0019,-0.0056,4e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00048,0.00048,0.0043,0.99,0.99,0.021,2.4,2.4,0.058,0.00014,0.00014,6.3e-06,0.04,0.04,0.0013,0,0,0,0,0,0,0,0 +8290000,0.98,-0.0054,-0.013,0.19,0.012,0.084,-0.016,0.019,0.091,-0.038,-0.0019,-0.0056,3.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00049,0.00049,0.0043,1.1,1.1,0.02,2.6,2.6,0.057,0.00014,0.00014,6.3e-06,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 +8390000,0.98,-0.0054,-0.013,0.19,0.0098,0.088,-0.015,0.02,0.1,-0.036,-0.0019,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0005,0.0005,0.0043,1.1,1.1,0.02,2.9,2.9,0.057,0.00014,0.00014,6.3e-06,0.04,0.04,0.0012,0,0,0,0,0,0,0,0 +8490000,0.98,-0.0053,-0.013,0.19,0.0093,0.091,-0.017,0.02,0.11,-0.041,-0.0018,-0.0056,3.9e-05,0,0,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00051,0.00051,0.0043,1.2,1.2,0.019,3.2,3.2,0.056,0.00014,0.00014,6.3e-06,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 +8590000,0.98,-0.0052,-0.013,0.19,0.01,0.095,-0.012,0.021,0.12,-0.036,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00052,0.00052,0.0043,1.3,1.3,0.018,3.6,3.6,0.055,0.00014,0.00014,6.3e-06,0.04,0.04,0.0011,0,0,0,0,0,0,0,0 +8690000,0.98,-0.0053,-0.013,0.19,0.01,0.096,-0.014,0.021,0.12,-0.037,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00053,0.00052,0.0043,1.4,1.4,0.018,3.9,3.9,0.055,0.00013,0.00013,6.3e-06,0.04,0.04,0.001,0,0,0,0,0,0,0,0 +8790000,0.98,-0.0052,-0.013,0.19,0.012,0.1,-0.013,0.023,0.13,-0.035,-0.0018,-0.0056,3.9e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0043,1.5,1.5,0.018,4.3,4.3,0.055,0.00013,0.00013,6.3e-06,0.04,0.04,0.00096,0,0,0,0,0,0,0,0 +8890000,0.98,-0.0053,-0.013,0.19,0.011,0.1,-0.0091,0.023,0.14,-0.029,-0.0018,-0.0056,3.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0043,1.5,1.5,0.017,4.6,4.6,0.054,0.00013,0.00013,6.3e-06,0.04,0.04,0.00091,0,0,0,0,0,0,0,0 +8990000,0.98,-0.0052,-0.013,0.19,0.01,0.11,-0.0083,0.024,0.15,-0.032,-0.0018,-0.0056,3.8e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0043,1.6,1.6,0.017,5.2,5.2,0.054,0.00013,0.00013,6.3e-06,0.04,0.04,0.00088,0,0,0,0,0,0,0,0 +9090000,0.98,-0.0053,-0.013,0.19,0.01,0.11,-0.0093,0.024,0.15,-0.032,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0043,1.7,1.7,0.016,5.4,5.4,0.053,0.00013,0.00013,6.3e-06,0.04,0.04,0.00084,0,0,0,0,0,0,0,0 +9190000,0.98,-0.0053,-0.013,0.19,0.014,0.11,-0.0088,0.025,0.16,-0.032,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0043,1.8,1.8,0.016,6,6.1,0.052,0.00013,0.00013,6.3e-06,0.04,0.04,0.0008,0,0,0,0,0,0,0,0 +9290000,0.98,-0.0052,-0.013,0.19,0.016,0.11,-0.0072,0.025,0.16,-0.03,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00055,0.0044,1.8,1.8,0.015,6.3,6.3,0.052,0.00012,0.00012,6.3e-06,0.04,0.04,0.00076,0,0,0,0,0,0,0,0 +9390000,0.98,-0.0051,-0.013,0.19,0.016,0.12,-0.006,0.026,0.17,-0.03,-0.0018,-0.0056,3.7e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,1.9,1.9,0.015,7,7,0.052,0.00012,0.00012,6.3e-06,0.04,0.04,0.00073,0,0,0,0,0,0,0,0 +9490000,0.98,-0.0052,-0.013,0.19,0.015,0.11,-0.0044,0.026,0.17,-0.027,-0.0017,-0.0056,3.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0044,1.9,1.9,0.015,7.2,7.2,0.051,0.00012,0.00012,6.3e-06,0.04,0.04,0.0007,0,0,0,0,0,0,0,0 +9590000,0.98,-0.0052,-0.013,0.19,0.015,0.12,-0.0043,0.028,0.18,-0.028,-0.0017,-0.0056,3.6e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2,2,0.014,7.9,7.9,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00067,0,0,0,0,0,0,0,0 +9690000,0.98,-0.0053,-0.013,0.19,0.015,0.11,-0.0014,0.027,0.18,-0.027,-0.0017,-0.0056,3.5e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,2,2,0.014,8,8.1,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00065,0,0,0,0,0,0,0,0 +9790000,0.98,-0.0053,-0.013,0.19,0.016,0.12,-0.0027,0.028,0.19,-0.028,-0.0017,-0.0056,3.5e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2.1,2.1,0.014,8.9,8.9,0.05,0.00012,0.00012,6.3e-06,0.04,0.04,0.00062,0,0,0,0,0,0,0,0 +9890000,0.98,-0.0054,-0.013,0.19,0.018,0.11,-0.0014,0.027,0.19,-0.029,-0.0016,-0.0056,3.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00054,0.00054,0.0044,2.1,2.1,0.013,8.9,8.9,0.049,0.00011,0.00011,6.3e-06,0.04,0.04,0.0006,0,0,0,0,0,0,0,0 +9990000,0.98,-0.0054,-0.013,0.19,0.019,0.12,-0.00072,0.029,0.2,-0.031,-0.0016,-0.0056,3.4e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00056,0.00056,0.0044,2.2,2.2,0.013,9.8,9.8,0.049,0.00011,0.00011,6.3e-06,0.04,0.04,0.00058,0,0,0,0,0,0,0,0 +10090000,0.98,-0.0054,-0.013,0.19,0.017,0.11,0.00048,0.028,0.19,-0.029,-0.0016,-0.0057,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00053,0.00053,0.0044,2.1,2.1,0.013,9.7,9.7,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00056,0,0,0,0,0,0,0,0 +10190000,0.98,-0.0054,-0.013,0.19,0.015,0.11,0.0014,0.03,0.2,-0.03,-0.0016,-0.0057,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,2.3,2.3,0.012,11,11,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00054,0,0,0,0,0,0,0,0 +10290000,0.98,-0.0054,-0.013,0.19,0.016,0.11,0.0003,0.031,0.21,-0.029,-0.0016,-0.0057,3.3e-05,0,0,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,2.4,2.4,0.012,12,12,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.00052,0,0,0,0,0,0,0,0 +10390000,0.98,-0.0054,-0.013,0.19,0.007,0.0052,-0.0025,0.00076,0.00015,-0.028,-0.0016,-0.0057,3.3e-05,-3.6e-08,2.5e-08,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00059,0.00058,0.0044,0.25,0.25,0.56,0.25,0.25,0.048,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10490000,0.98,-0.0053,-0.013,0.19,0.0085,0.0077,0.007,0.0015,0.00076,-0.023,-0.0016,-0.0057,3.3e-05,-1.2e-06,8.3e-07,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0006,0.0006,0.0044,0.26,0.26,0.55,0.26,0.26,0.057,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10590000,0.98,-0.0053,-0.012,0.19,-0.0012,0.0055,0.013,-0.0012,-0.0054,-0.021,-0.0016,-0.0057,3.2e-05,0.00032,8.5e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00061,0.00061,0.0044,0.13,0.13,0.27,0.13,0.13,0.055,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10690000,0.98,-0.0052,-0.012,0.19,-8.2e-05,0.0068,0.016,-0.0012,-0.0048,-0.017,-0.0016,-0.0057,3.2e-05,0.00032,9.2e-06,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00063,0.00063,0.0044,0.15,0.15,0.26,0.14,0.14,0.065,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10790000,0.98,-0.0053,-0.013,0.19,0.0017,0.0031,0.014,-0.00078,-0.0047,-0.015,-0.0016,-0.0057,3.1e-05,0.00052,0.00048,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00062,0.00062,0.0044,0.1,0.1,0.17,0.091,0.091,0.062,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10890000,0.98,-0.0053,-0.013,0.19,0.0018,0.0066,0.01,-0.00059,-0.0042,-0.018,-0.0016,-0.0057,3.1e-05,0.00053,0.00048,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00064,0.00064,0.0044,0.12,0.12,0.16,0.098,0.098,0.068,0.00011,0.00011,6.3e-06,0.04,0.04,0.0005,0,0,0,0,0,0,0,0 +10990000,0.98,-0.0054,-0.013,0.19,0.0013,0.012,0.016,-0.00045,-0.003,-0.011,-0.0015,-0.0057,3e-05,0.00068,0.00072,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0006,0.0006,0.0044,0.096,0.096,0.12,0.073,0.073,0.065,9.9e-05,9.9e-05,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 +11090000,0.98,-0.0055,-0.013,0.19,0.0023,0.017,0.02,-0.00031,-0.0016,-0.0074,-0.0015,-0.0057,3e-05,0.00068,0.00072,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00062,0.00062,0.0044,0.12,0.12,0.11,0.08,0.08,0.069,9.9e-05,9.9e-05,6.3e-06,0.039,0.039,0.0005,0,0,0,0,0,0,0,0 +11190000,0.98,-0.0058,-0.013,0.19,0.004,0.017,0.026,0.0011,-0.0018,-0.00035,-0.0015,-0.0057,2.8e-05,0.00067,0.0016,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00055,0.00055,0.0044,0.096,0.096,0.083,0.063,0.063,0.066,9.1e-05,9.1e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11290000,0.98,-0.0058,-0.013,0.19,0.0042,0.018,0.026,0.0015,-1.6e-05,-0.00012,-0.0015,-0.0057,2.8e-05,0.00067,0.0016,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00057,0.00057,0.0044,0.12,0.12,0.077,0.07,0.07,0.069,9.1e-05,9.1e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11390000,0.98,-0.006,-0.013,0.19,0.0025,0.015,0.016,0.00089,-0.00087,-0.0086,-0.0014,-0.0058,2.6e-05,0.0012,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00049,0.00049,0.0044,0.097,0.097,0.062,0.057,0.057,0.066,8.3e-05,8.3e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11490000,0.98,-0.0059,-0.013,0.19,0.0016,0.017,0.02,0.001,0.00073,-0.0025,-0.0014,-0.0058,2.6e-05,0.0012,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0005,0.0005,0.0044,0.12,0.12,0.057,0.065,0.065,0.067,8.3e-05,8.3e-05,6.3e-06,0.038,0.038,0.0005,0,0,0,0,0,0,0,0 +11590000,0.98,-0.0062,-0.012,0.19,0.0034,0.013,0.018,0.00084,-0.00036,-0.0036,-0.0013,-0.0058,2.4e-05,0.0015,0.0028,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00042,0.00042,0.0044,0.096,0.096,0.048,0.054,0.054,0.065,7.5e-05,7.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 +11690000,0.98,-0.0062,-0.012,0.19,0.0038,0.017,0.018,0.0012,0.0011,-0.0049,-0.0013,-0.0058,2.4e-05,0.0015,0.0028,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00044,0.00044,0.0044,0.12,0.12,0.044,0.062,0.062,0.066,7.5e-05,7.5e-05,6.3e-06,0.037,0.037,0.0005,0,0,0,0,0,0,0,0 +11790000,0.98,-0.0066,-0.012,0.19,0.0025,0.011,0.019,0.0007,-0.0018,-0.002,-0.0012,-0.0059,2e-05,0.002,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00037,0.00037,0.0044,0.092,0.092,0.037,0.052,0.052,0.063,6.8e-05,6.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +11890000,0.98,-0.0067,-0.012,0.19,0.0052,0.012,0.017,0.001,-0.00069,-0.0013,-0.0012,-0.0059,2e-05,0.002,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00038,0.00038,0.0044,0.11,0.11,0.034,0.061,0.061,0.063,6.8e-05,6.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +11990000,0.98,-0.0069,-0.012,0.19,0.0081,0.012,0.015,0.0021,-0.0018,-0.0049,-0.0012,-0.0059,2e-05,0.002,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00032,0.00032,0.0044,0.088,0.088,0.03,0.051,0.051,0.061,6.2e-05,6.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12090000,0.98,-0.0068,-0.012,0.19,0.0097,0.012,0.018,0.003,-0.00063,0.0011,-0.0012,-0.0059,2e-05,0.002,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00033,0.00033,0.0044,0.11,0.11,0.027,0.06,0.06,0.06,6.2e-05,6.2e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12190000,0.98,-0.0067,-0.012,0.19,0.0078,0.011,0.017,0.0018,0.00039,0.0029,-0.0012,-0.0059,2e-05,0.0023,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00029,0.00029,0.0044,0.083,0.083,0.024,0.05,0.05,0.058,5.7e-05,5.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12290000,0.98,-0.0068,-0.012,0.19,0.0055,0.011,0.016,0.0025,0.0015,0.0039,-0.0012,-0.0059,2e-05,0.0023,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0003,0.0003,0.0044,0.098,0.098,0.022,0.059,0.059,0.058,5.7e-05,5.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12390000,0.98,-0.0069,-0.012,0.19,0.004,0.0072,0.014,0.0017,0.00049,-0.0021,-0.0012,-0.006,1.8e-05,0.0025,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00026,0.00026,0.0044,0.078,0.078,0.02,0.05,0.05,0.056,5.3e-05,5.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12490000,0.98,-0.0069,-0.012,0.19,0.004,0.0082,0.018,0.0021,0.0013,-6.6e-05,-0.0012,-0.006,1.8e-05,0.0025,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00027,0.00027,0.0044,0.091,0.091,0.018,0.059,0.059,0.055,5.3e-05,5.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12590000,0.98,-0.0071,-0.012,0.19,0.0078,0.0015,0.02,0.0033,-0.0013,0.0017,-0.0011,-0.006,1.7e-05,0.0025,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00024,0.00024,0.0044,0.072,0.072,0.017,0.049,0.049,0.054,5e-05,5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12690000,0.98,-0.007,-0.012,0.19,0.0083,-0.00063,0.019,0.0041,-0.0013,0.0033,-0.0011,-0.006,1.7e-05,0.0025,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00025,0.00025,0.0044,0.084,0.084,0.016,0.058,0.058,0.053,5e-05,5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12790000,0.98,-0.0073,-0.012,0.19,0.0099,-0.004,0.021,0.0041,-0.0044,0.0054,-0.0011,-0.006,1.6e-05,0.0026,0.0042,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00023,0.00023,0.0044,0.068,0.068,0.014,0.049,0.049,0.052,4.7e-05,4.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12890000,0.98,-0.0073,-0.012,0.19,0.01,-0.005,0.022,0.0052,-0.0049,0.0085,-0.0011,-0.006,1.6e-05,0.0026,0.0042,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00024,0.00024,0.0044,0.078,0.078,0.013,0.058,0.058,0.051,4.7e-05,4.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +12990000,0.98,-0.0072,-0.012,0.19,0.0079,-0.0029,0.022,0.0036,-0.0036,0.0096,-0.0011,-0.006,1.6e-05,0.0026,0.0041,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00022,0.0044,0.064,0.064,0.012,0.049,0.049,0.05,4.5e-05,4.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13090000,0.98,-0.0073,-0.012,0.19,0.0088,-0.0028,0.02,0.0044,-0.0038,0.0085,-0.0011,-0.006,1.6e-05,0.0027,0.0041,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00023,0.00023,0.0044,0.073,0.073,0.012,0.057,0.057,0.049,4.5e-05,4.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13190000,0.98,-0.0073,-0.012,0.19,0.0038,-0.0045,0.019,0.00094,-0.0046,0.0091,-0.0011,-0.006,1.6e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.06,0.06,0.011,0.049,0.049,0.047,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13290000,0.98,-0.0073,-0.012,0.19,0.0034,-0.0055,0.016,0.0013,-0.005,0.0085,-0.0011,-0.006,1.6e-05,0.0028,0.004,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00022,0.0044,0.068,0.068,0.01,0.057,0.057,0.047,4.3e-05,4.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13390000,0.98,-0.0072,-0.012,0.19,0.0026,-0.0035,0.016,0.00083,-0.0037,0.0091,-0.0011,-0.006,1.6e-05,0.0028,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.056,0.056,0.0097,0.049,0.049,0.046,4e-05,4e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13490000,0.98,-0.0072,-0.012,0.19,0.003,-0.0018,0.015,0.0011,-0.0039,0.0053,-0.0011,-0.006,1.6e-05,0.0029,0.0039,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00022,0.00021,0.0044,0.064,0.064,0.0093,0.057,0.057,0.045,4e-05,4e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13590000,0.98,-0.0072,-0.012,0.19,0.0075,-0.002,0.017,0.004,-0.0032,0.0038,-0.0011,-0.006,1.6e-05,0.0031,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.053,0.053,0.0088,0.048,0.048,0.044,3.8e-05,3.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13690000,0.98,-0.0072,-0.012,0.19,0.0075,-0.0035,0.017,0.0047,-0.0034,0.0064,-0.0011,-0.006,1.6e-05,0.0031,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.061,0.061,0.0085,0.056,0.056,0.044,3.8e-05,3.8e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13790000,0.98,-0.0071,-0.012,0.19,0.014,0.00071,0.017,0.0083,-0.00094,0.0059,-0.0011,-0.0059,1.7e-05,0.0035,0.0037,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.051,0.051,0.0082,0.048,0.048,0.043,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13890000,0.98,-0.007,-0.012,0.19,0.016,0.0014,0.018,0.0098,-0.00082,0.0081,-0.0011,-0.0059,1.7e-05,0.0035,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.058,0.058,0.008,0.056,0.056,0.042,3.7e-05,3.7e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +13990000,0.98,-0.0071,-0.012,0.19,0.015,0.0015,0.017,0.0074,-0.0023,0.007,-0.0011,-0.006,1.6e-05,0.0033,0.0036,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.049,0.049,0.0077,0.048,0.048,0.041,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14090000,0.98,-0.0071,-0.012,0.19,0.013,-0.0029,0.018,0.0088,-0.0024,0.0034,-0.0011,-0.006,1.6e-05,0.0034,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00021,0.00021,0.0044,0.055,0.055,0.0076,0.055,0.055,0.041,3.5e-05,3.5e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14190000,0.98,-0.0071,-0.012,0.19,0.0099,-0.0016,0.018,0.008,-0.0018,0.0036,-0.0011,-0.006,1.7e-05,0.0035,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.047,0.047,0.0074,0.048,0.048,0.04,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14290000,0.98,-0.007,-0.011,0.19,0.012,-0.0017,0.016,0.009,-0.002,0.0079,-0.0011,-0.006,1.7e-05,0.0035,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.053,0.053,0.0073,0.055,0.055,0.04,3.3e-05,3.3e-05,6.3e-06,0.036,0.036,0.0005,0,0,0,0,0,0,0,0 +14390000,0.98,-0.0071,-0.011,0.19,0.012,-0.0047,0.017,0.0084,-0.0031,0.012,-0.0011,-0.006,1.6e-05,0.0033,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.045,0.045,0.0071,0.048,0.048,0.039,3.1e-05,3.1e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14490000,0.98,-0.0072,-0.011,0.19,0.01,-0.0045,0.021,0.0094,-0.0036,0.015,-0.0011,-0.006,1.6e-05,0.0033,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.051,0.051,0.0071,0.055,0.055,0.038,3.1e-05,3.1e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14590000,0.98,-0.0073,-0.011,0.19,0.0078,-0.0046,0.019,0.0059,-0.0042,0.011,-0.0011,-0.006,1.6e-05,0.0031,0.0033,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0044,0.044,0.044,0.007,0.047,0.047,0.038,2.9e-05,2.9e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14690000,0.98,-0.0073,-0.011,0.19,0.0071,-0.0046,0.019,0.0066,-0.0047,0.011,-0.0011,-0.006,1.5e-05,0.0031,0.0032,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.05,0.05,0.007,0.054,0.054,0.037,2.9e-05,2.9e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14790000,0.98,-0.0072,-0.011,0.19,0.0087,0.0025,0.019,0.0053,0.00078,0.014,-0.0012,-0.006,1.8e-05,0.0032,0.0041,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0044,0.043,0.043,0.0069,0.047,0.047,0.037,2.7e-05,2.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14890000,0.98,-0.0071,-0.011,0.19,0.0074,3.9e-05,0.023,0.006,0.00092,0.014,-0.0012,-0.006,1.8e-05,0.0032,0.0041,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0044,0.049,0.049,0.007,0.054,0.054,0.037,2.7e-05,2.7e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +14990000,0.98,-0.0072,-0.011,0.19,0.0061,-0.0016,0.026,0.0047,-0.0007,0.017,-0.0012,-0.0061,1.7e-05,0.0031,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.042,0.042,0.0069,0.047,0.047,0.036,2.6e-05,2.6e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15090000,0.98,-0.0072,-0.011,0.19,0.0062,-0.00054,0.03,0.0053,-0.00085,0.019,-0.0012,-0.0061,1.7e-05,0.0031,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.0002,0.0002,0.0045,0.048,0.048,0.007,0.054,0.054,0.036,2.6e-05,2.6e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15190000,0.98,-0.0073,-0.011,0.19,0.0043,-0.0016,0.03,0.0042,-0.00076,0.021,-0.0012,-0.0061,1.7e-05,0.003,0.0038,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.041,0.041,0.007,0.047,0.047,0.036,2.4e-05,2.4e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15290000,0.98,-0.0074,-0.011,0.19,0.0049,-0.0027,0.03,0.0047,-0.00096,0.018,-0.0012,-0.0061,1.7e-05,0.0033,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00019,0.00019,0.0045,0.047,0.047,0.0071,0.054,0.054,0.035,2.4e-05,2.4e-05,6.3e-06,0.035,0.035,0.0005,0,0,0,0,0,0,0,0 +15390000,0.98,-0.0075,-0.011,0.19,0.0051,-0.00029,0.029,0.0037,-0.00065,0.018,-0.0012,-0.0061,1.7e-05,0.0034,0.0035,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00016,0.00018,0.00018,0.0045,0.041,0.041,0.0071,0.047,0.047,0.035,2.2e-05,2.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15490000,0.98,-0.0075,-0.011,0.19,0.0045,-0.0028,0.029,0.0042,-0.00078,0.019,-0.0012,-0.0061,1.7e-05,0.0035,0.0034,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00019,0.00019,0.0045,0.046,0.046,0.0072,0.053,0.053,0.035,2.2e-05,2.2e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15590000,0.98,-0.0077,-0.011,0.19,0.0081,-0.0067,0.029,0.0062,-0.0047,0.018,-0.0011,-0.0061,1.5e-05,0.0038,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.04,0.04,0.0072,0.047,0.047,0.035,2.1e-05,2.1e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15690000,0.98,-0.0077,-0.011,0.19,0.0099,-0.0098,0.029,0.0072,-0.0055,0.019,-0.0011,-0.0061,1.5e-05,0.0039,0.0022,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00019,0.00019,0.0045,0.045,0.045,0.0073,0.053,0.053,0.034,2.1e-05,2.1e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15790000,0.98,-0.0077,-0.011,0.19,0.0065,-0.0091,0.029,0.0055,-0.0043,0.02,-0.0011,-0.0061,1.6e-05,0.0039,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.04,0.04,0.0073,0.046,0.046,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15890000,0.98,-0.0078,-0.011,0.19,0.0053,-0.0076,0.03,0.0061,-0.0052,0.02,-0.0011,-0.0061,1.6e-05,0.0041,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.045,0.045,0.0074,0.053,0.053,0.034,1.9e-05,1.9e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +15990000,0.98,-0.0076,-0.011,0.19,0.0033,-0.0061,0.027,0.0048,-0.004,0.019,-0.0012,-0.0061,1.6e-05,0.0043,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.039,0.039,0.0074,0.046,0.046,0.034,1.7e-05,1.7e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +16090000,0.98,-0.0075,-0.011,0.19,0.0026,-0.0074,0.024,0.0051,-0.0046,0.019,-0.0012,-0.0061,1.6e-05,0.0044,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00018,0.00018,0.0045,0.044,0.044,0.0076,0.053,0.053,0.034,1.7e-05,1.7e-05,6.3e-06,0.034,0.034,0.0005,0,0,0,0,0,0,0,0 +16190000,0.98,-0.0074,-0.011,0.19,-0.0013,-0.005,0.023,0.0027,-0.0034,0.016,-0.0012,-0.0061,1.7e-05,0.0045,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.039,0.039,0.0076,0.046,0.046,0.034,1.6e-05,1.6e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16290000,0.98,-0.0075,-0.011,0.19,-0.00097,-0.0066,0.023,0.0026,-0.004,0.017,-0.0012,-0.0061,1.7e-05,0.0046,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.043,0.043,0.0077,0.053,0.053,0.034,1.6e-05,1.6e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16390000,0.98,-0.0074,-0.011,0.19,0.0015,-0.0058,0.023,0.0036,-0.0031,0.017,-0.0012,-0.0061,1.7e-05,0.0051,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.038,0.038,0.0077,0.046,0.046,0.034,1.5e-05,1.5e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16490000,0.98,-0.0075,-0.011,0.19,0.0035,-0.0074,0.026,0.0039,-0.0038,0.021,-0.0012,-0.0061,1.7e-05,0.005,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00017,0.0045,0.043,0.043,0.0079,0.053,0.053,0.034,1.5e-05,1.5e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16590000,0.98,-0.0075,-0.011,0.19,0.0075,-0.0074,0.029,0.0034,-0.0029,0.021,-0.0012,-0.0061,1.8e-05,0.0052,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.037,0.037,0.0079,0.046,0.046,0.034,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16690000,0.98,-0.0076,-0.011,0.19,0.009,-0.012,0.029,0.0042,-0.0039,0.022,-0.0012,-0.0061,1.8e-05,0.0053,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00017,0.00016,0.0045,0.042,0.042,0.008,0.053,0.053,0.034,1.3e-05,1.3e-05,6.3e-06,0.033,0.033,0.0005,0,0,0,0,0,0,0,0 +16790000,0.98,-0.0074,-0.011,0.19,0.0099,-0.011,0.028,0.0033,-0.0028,0.022,-0.0012,-0.0061,1.8e-05,0.0054,0.0027,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.037,0.037,0.008,0.046,0.046,0.034,1.2e-05,1.2e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +16890000,0.98,-0.0074,-0.011,0.19,0.0089,-0.011,0.029,0.0043,-0.0039,0.02,-0.0012,-0.0061,1.8e-05,0.0057,0.0024,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00016,0.0045,0.041,0.041,0.0082,0.052,0.052,0.034,1.2e-05,1.2e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +16990000,0.98,-0.0074,-0.011,0.19,0.0086,-0.011,0.029,0.004,-0.0029,0.019,-0.0013,-0.0061,1.9e-05,0.0063,0.0025,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.036,0.036,0.0082,0.046,0.046,0.034,1.1e-05,1.1e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17090000,0.98,-0.0075,-0.011,0.19,0.01,-0.013,0.028,0.005,-0.0041,0.018,-0.0013,-0.0061,1.9e-05,0.0065,0.0023,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00016,0.00015,0.0045,0.04,0.041,0.0083,0.052,0.052,0.034,1.1e-05,1.1e-05,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17190000,0.98,-0.0076,-0.011,0.19,0.009,-0.018,0.03,0.0033,-0.0076,0.021,-0.0012,-0.0061,1.9e-05,0.0063,0.0021,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.036,0.036,0.0083,0.046,0.046,0.034,9.9e-06,9.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17290000,0.98,-0.0076,-0.011,0.19,0.0099,-0.019,0.03,0.0043,-0.0094,0.021,-0.0012,-0.0061,1.9e-05,0.0065,0.0019,-0.14,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.04,0.04,0.0084,0.052,0.052,0.034,9.9e-06,9.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17390000,0.98,-0.0074,-0.011,0.19,0.0066,-0.018,0.029,0.0057,-0.0059,0.021,-0.0013,-0.006,2e-05,0.0073,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.035,0.035,0.0084,0.046,0.046,0.034,8.9e-06,8.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17490000,0.98,-0.0074,-0.011,0.19,0.0047,-0.019,0.029,0.0062,-0.0078,0.023,-0.0013,-0.006,2e-05,0.0074,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00015,0.00015,0.0045,0.039,0.039,0.0085,0.052,0.052,0.034,8.9e-06,8.9e-06,6.3e-06,0.032,0.032,0.0005,0,0,0,0,0,0,0,0 +17590000,0.98,-0.0073,-0.011,0.19,0.00086,-0.015,0.028,0.0024,-0.0058,0.02,-0.0013,-0.0061,2.1e-05,0.0072,0.0029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.034,0.034,0.0085,0.046,0.046,0.034,8.1e-06,8.1e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17690000,0.98,-0.0074,-0.011,0.19,-8.7e-05,-0.016,0.029,0.0025,-0.0074,0.023,-0.0013,-0.0061,2.1e-05,0.0073,0.0029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.038,0.038,0.0086,0.052,0.052,0.034,8.1e-06,8.1e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17790000,0.98,-0.0074,-0.011,0.19,0.0026,-0.014,0.029,0.0035,-0.0062,0.028,-0.0014,-0.006,2.2e-05,0.0076,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.034,0.034,0.0086,0.046,0.046,0.034,7.3e-06,7.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17890000,0.98,-0.0073,-0.011,0.19,0.0047,-0.016,0.029,0.0039,-0.0077,0.032,-0.0014,-0.006,2.2e-05,0.0075,0.0036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00014,0.00014,0.0045,0.037,0.037,0.0086,0.052,0.052,0.035,7.3e-06,7.3e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +17990000,0.98,-0.0071,-0.011,0.19,0.0041,-0.0093,0.029,0.0031,-0.002,0.033,-0.0014,-0.006,2.4e-05,0.0079,0.0046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.033,0.033,0.0086,0.045,0.045,0.034,6.6e-06,6.6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18090000,0.98,-0.0072,-0.011,0.19,0.0036,-0.0099,0.028,0.0035,-0.003,0.031,-0.0014,-0.006,2.4e-05,0.0082,0.0044,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.036,0.036,0.0087,0.052,0.052,0.035,6.6e-06,6.6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18190000,0.98,-0.0072,-0.011,0.19,0.0036,-0.0088,0.028,0.0041,-0.0022,0.029,-0.0014,-0.006,2.4e-05,0.0087,0.0043,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0045,0.032,0.032,0.0086,0.045,0.045,0.035,6e-06,6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18290000,0.98,-0.0072,-0.011,0.19,0.0045,-0.0093,0.027,0.0045,-0.0031,0.027,-0.0014,-0.006,2.4e-05,0.009,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0046,0.036,0.036,0.0087,0.051,0.051,0.035,6e-06,6e-06,6.3e-06,0.031,0.031,0.0005,0,0,0,0,0,0,0,0 +18390000,0.98,-0.0071,-0.011,0.19,0.0054,-0.008,0.027,0.0061,-0.0023,0.026,-0.0014,-0.006,2.4e-05,0.0096,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00012,0.0046,0.031,0.031,0.0086,0.045,0.045,0.035,5.4e-06,5.4e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 +18490000,0.98,-0.0072,-0.011,0.19,0.0082,-0.008,0.026,0.0069,-0.0032,0.028,-0.0014,-0.006,2.4e-05,0.0096,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00013,0.00013,0.0046,0.035,0.035,0.0087,0.051,0.051,0.035,5.4e-06,5.4e-06,6.3e-06,0.03,0.031,0.0005,0,0,0,0,0,0,0,0 +18590000,0.98,-0.007,-0.011,0.19,0.0066,-0.0074,0.026,0.0055,-0.0024,0.031,-0.0015,-0.006,2.5e-05,0.0095,0.0043,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.031,0.031,0.0087,0.045,0.045,0.035,4.9e-06,4.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18690000,0.98,-0.007,-0.011,0.19,0.0066,-0.0063,0.024,0.0062,-0.0031,0.029,-0.0015,-0.006,2.5e-05,0.0097,0.0041,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.034,0.034,0.0087,0.051,0.051,0.035,4.9e-06,4.9e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18790000,0.98,-0.0069,-0.011,0.19,0.0056,-0.006,0.024,0.0062,-0.0025,0.027,-0.0015,-0.006,2.5e-05,0.01,0.004,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.03,0.03,0.0087,0.045,0.045,0.035,4.4e-06,4.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18890000,0.98,-0.0069,-0.011,0.19,0.0043,-0.0057,0.021,0.0067,-0.0032,0.023,-0.0015,-0.006,2.5e-05,0.01,0.0037,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.033,0.033,0.0087,0.051,0.051,0.035,4.4e-06,4.4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +18990000,0.98,-0.0069,-0.011,0.19,0.0026,-0.0058,0.022,0.0055,-0.0025,0.026,-0.0015,-0.006,2.5e-05,0.01,0.0039,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.029,0.029,0.0086,0.045,0.045,0.035,4e-06,4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19090000,0.98,-0.007,-0.011,0.19,0.00064,-0.0063,0.023,0.0058,-0.0031,0.022,-0.0015,-0.006,2.5e-05,0.011,0.0036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00012,0.0046,0.032,0.032,0.0087,0.051,0.051,0.036,4e-06,4e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19190000,0.98,-0.0069,-0.011,0.19,-0.00084,-0.0059,0.022,0.0048,-0.0025,0.022,-0.0015,-0.006,2.5e-05,0.011,0.0036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.028,0.028,0.0086,0.045,0.045,0.036,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19290000,0.98,-0.0068,-0.011,0.19,-0.0017,-0.0058,0.023,0.0047,-0.0031,0.021,-0.0015,-0.006,2.5e-05,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00012,0.00011,0.0046,0.031,0.031,0.0087,0.05,0.051,0.036,3.7e-06,3.7e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19390000,0.98,-0.0069,-0.011,0.19,-0.0022,-0.0023,0.024,0.004,-0.0012,0.019,-0.0015,-0.006,2.6e-05,0.011,0.0036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.028,0.028,0.0086,0.045,0.045,0.036,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19490000,0.98,-0.0069,-0.011,0.19,-0.003,-0.0023,0.023,0.0038,-0.0014,0.019,-0.0015,-0.006,2.6e-05,0.011,0.0035,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.03,0.03,0.0087,0.05,0.05,0.036,3.3e-06,3.3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19590000,0.98,-0.0069,-0.011,0.19,-0.0041,-0.0052,0.025,0.0044,-0.0024,0.019,-0.0015,-0.006,2.6e-05,0.012,0.0032,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.027,0.027,0.0086,0.044,0.044,0.036,3e-06,3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19690000,0.98,-0.0069,-0.011,0.19,-0.0057,-0.0037,0.023,0.0039,-0.0028,0.019,-0.0015,-0.006,2.6e-05,0.012,0.0031,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.03,0.03,0.0086,0.05,0.05,0.036,3e-06,3e-06,6.3e-06,0.03,0.03,0.0005,0,0,0,0,0,0,0,0 +19790000,0.98,-0.007,-0.011,0.19,-0.0058,-0.0023,0.022,0.0063,-0.0023,0.014,-0.0015,-0.006,2.6e-05,0.012,0.0028,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.026,0.026,0.0086,0.044,0.044,0.036,2.8e-06,2.8e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 +19890000,0.98,-0.007,-0.011,0.19,-0.0058,-0.002,0.022,0.0057,-0.0025,0.013,-0.0015,-0.006,2.6e-05,0.013,0.0027,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.00011,0.0046,0.029,0.029,0.0086,0.05,0.05,0.036,2.8e-06,2.8e-06,6.3e-06,0.029,0.03,0.0005,0,0,0,0,0,0,0,0 +19990000,0.98,-0.007,-0.011,0.19,-0.0055,-0.002,0.019,0.0061,-0.00093,0.0097,-0.0015,-0.0059,2.6e-05,0.013,0.0026,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.0001,0.0046,0.026,0.026,0.0085,0.044,0.044,0.036,2.5e-06,2.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20090000,0.98,-0.0071,-0.011,0.19,-0.005,-0.0042,0.019,0.0055,-0.0012,0.013,-0.0015,-0.0059,2.6e-05,0.013,0.0026,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.00011,0.0001,0.0046,0.028,0.028,0.0086,0.05,0.05,0.036,2.5e-06,2.5e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20190000,0.98,-0.0071,-0.011,0.19,-0.0039,-0.0016,0.02,0.0065,-0.00092,0.013,-0.0015,-0.0059,2.6e-05,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.025,0.025,0.0085,0.044,0.044,0.036,2.3e-06,2.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20290000,0.98,-0.007,-0.011,0.19,-0.0071,-0.0027,0.02,0.006,-0.0011,0.013,-0.0015,-0.0059,2.6e-05,0.013,0.0025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.027,0.027,0.0085,0.049,0.049,0.036,2.3e-06,2.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20390000,0.98,-0.007,-0.011,0.19,-0.0078,-0.0015,0.021,0.0069,-0.00079,0.014,-0.0015,-0.0059,2.7e-05,0.014,0.0024,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.024,0.024,0.0084,0.044,0.044,0.036,2.1e-06,2.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20490000,0.98,-0.007,-0.011,0.19,-0.012,-0.0025,0.02,0.0059,-0.00097,0.012,-0.0015,-0.0059,2.7e-05,0.014,0.0023,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,0.0001,0.0046,0.027,0.027,0.0085,0.049,0.049,0.036,2.1e-06,2.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20590000,0.98,-0.007,-0.011,0.19,-0.011,-0.0035,0.02,0.0069,-0.00081,0.011,-0.0015,-0.0059,2.7e-05,0.014,0.0021,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,9.9e-05,0.0046,0.024,0.024,0.0084,0.044,0.044,0.036,2e-06,2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20690000,0.98,-0.0069,-0.011,0.19,-0.013,-0.0022,0.021,0.0057,-0.001,0.011,-0.0015,-0.0059,2.7e-05,0.014,0.0021,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,0.0001,9.9e-05,0.0046,0.026,0.026,0.0084,0.049,0.049,0.036,2e-06,2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20790000,0.98,-0.0063,-0.011,0.19,-0.016,0.00032,0.0055,0.0048,-0.00083,0.0097,-0.0015,-0.0059,2.7e-05,0.015,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.9e-05,9.7e-05,0.0047,0.023,0.023,0.0084,0.043,0.043,0.036,1.8e-06,1.8e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20890000,0.98,0.0028,-0.0073,0.19,-0.022,0.012,-0.11,0.0029,-0.00016,0.0034,-0.0015,-0.0059,2.7e-05,0.015,0.0019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.9e-05,9.8e-05,0.0047,0.026,0.026,0.0084,0.049,0.049,0.036,1.8e-06,1.8e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +20990000,0.98,0.0061,-0.0038,0.19,-0.032,0.03,-0.25,0.0022,0.00049,-0.011,-0.0015,-0.0059,2.7e-05,0.015,0.0018,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.7e-05,9.6e-05,0.0047,0.023,0.023,0.0083,0.043,0.043,0.036,1.7e-06,1.7e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21090000,0.98,0.0045,-0.0042,0.19,-0.046,0.046,-0.37,-0.0017,0.0044,-0.042,-0.0015,-0.0059,2.7e-05,0.015,0.0018,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.8e-05,9.7e-05,0.0047,0.026,0.026,0.0084,0.048,0.048,0.036,1.7e-06,1.7e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21190000,0.98,0.0017,-0.0059,0.19,-0.049,0.05,-0.5,-0.0013,0.0034,-0.078,-0.0014,-0.0059,2.6e-05,0.015,0.0012,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.4e-05,0.0047,0.023,0.023,0.0083,0.043,0.043,0.036,1.6e-06,1.6e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21290000,0.98,-0.0005,-0.0072,0.19,-0.049,0.054,-0.63,-0.0062,0.0087,-0.14,-0.0014,-0.0059,2.6e-05,0.015,0.0011,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.6e-05,9.5e-05,0.0047,0.026,0.026,0.0083,0.048,0.048,0.036,1.6e-06,1.6e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21390000,0.98,-0.002,-0.0079,0.19,-0.045,0.05,-0.75,-0.0051,0.011,-0.2,-0.0014,-0.0059,2.6e-05,0.015,0.00077,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.4e-05,9.2e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21490000,0.98,-0.0028,-0.0083,0.19,-0.04,0.047,-0.89,-0.0094,0.016,-0.29,-0.0014,-0.0059,2.6e-05,0.015,0.00067,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.5e-05,9.3e-05,0.0047,0.026,0.026,0.0083,0.048,0.048,0.036,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21590000,0.98,-0.0033,-0.0083,0.19,-0.032,0.043,-1,-0.008,0.016,-0.38,-0.0014,-0.0059,2.6e-05,0.015,0.00063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.2e-05,9.1e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21690000,0.98,-0.0036,-0.0082,0.19,-0.03,0.039,-1.1,-0.011,0.02,-0.49,-0.0014,-0.0059,2.6e-05,0.015,0.00046,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9.3e-05,9.1e-05,0.0047,0.025,0.025,0.0083,0.048,0.048,0.036,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0 +21790000,0.98,-0.004,-0.0084,0.19,-0.021,0.032,-1.3,-0.0038,0.018,-0.61,-0.0014,-0.0058,2.7e-05,0.016,0.00026,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,9e-05,8.9e-05,0.0047,0.023,0.023,0.0082,0.043,0.043,0.036,1.2e-06,1.2e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 +21890000,0.98,-0.0043,-0.0085,0.19,-0.018,0.028,-1.4,-0.0058,0.021,-0.75,-0.0014,-0.0058,2.7e-05,0.016,0.00013,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,9.1e-05,8.9e-05,0.0047,0.025,0.025,0.0082,0.048,0.048,0.036,1.2e-06,1.2e-06,6.3e-06,0.028,0.029,0.0005,0,0,0,0,0,0,0,0 +21990000,0.98,-0.005,-0.0088,0.19,-0.014,0.022,-1.4,-0.00041,0.017,-0.89,-0.0014,-0.0058,2.6e-05,0.015,0.00038,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.8e-05,8.7e-05,0.0047,0.022,0.022,0.0082,0.043,0.043,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22090000,0.98,-0.0057,-0.0096,0.19,-0.012,0.019,-1.4,-0.0017,0.019,-1,-0.0014,-0.0058,2.6e-05,0.016,0.00025,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.9e-05,8.7e-05,0.0047,0.024,0.024,0.0082,0.048,0.048,0.036,1.2e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22190000,0.98,-0.0061,-0.0099,0.19,-0.0035,0.013,-1.4,0.006,0.013,-1.2,-0.0014,-0.0058,2.6e-05,0.016,0.00029,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.7e-05,8.5e-05,0.0047,0.021,0.021,0.0081,0.043,0.043,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22290000,0.98,-0.0069,-0.01,0.19,0.0016,0.0079,-1.4,0.0059,0.014,-1.3,-0.0014,-0.0058,2.6e-05,0.016,0.00019,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.7e-05,8.6e-05,0.0047,0.023,0.023,0.0081,0.048,0.048,0.036,1.1e-06,1.1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22390000,0.98,-0.0072,-0.01,0.19,0.0067,-0.0017,-1.4,0.013,0.0041,-1.5,-0.0014,-0.0058,2.6e-05,0.016,-0.00015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.5e-05,8.4e-05,0.0047,0.021,0.021,0.0081,0.043,0.043,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22490000,0.98,-0.0074,-0.011,0.19,0.011,-0.0076,-1.4,0.014,0.0036,-1.6,-0.0014,-0.0058,2.6e-05,0.016,-0.0002,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.6e-05,8.4e-05,0.0047,0.022,0.022,0.0081,0.047,0.047,0.036,1e-06,1e-06,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22590000,0.98,-0.0073,-0.011,0.19,0.02,-0.017,-1.4,0.026,-0.0053,-1.7,-0.0014,-0.0058,2.6e-05,0.016,-0.00036,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.2e-05,0.0047,0.02,0.02,0.0081,0.042,0.042,0.036,9.4e-07,9.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22690000,0.98,-0.0072,-0.012,0.19,0.022,-0.021,-1.4,0.028,-0.0072,-1.9,-0.0014,-0.0058,2.6e-05,0.016,-0.00044,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.3e-05,0.0047,0.021,0.021,0.0081,0.047,0.047,0.036,9.5e-07,9.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22790000,0.98,-0.0072,-0.012,0.19,0.028,-0.029,-1.4,0.038,-0.017,-2,-0.0014,-0.0058,2.6e-05,0.016,-0.00056,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0047,0.019,0.019,0.0081,0.042,0.042,0.036,8.9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22890000,0.98,-0.0074,-0.012,0.19,0.032,-0.034,-1.4,0.041,-0.02,-2.2,-0.0014,-0.0058,2.6e-05,0.016,-0.00063,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0047,0.021,0.021,0.0081,0.047,0.047,0.036,8.9e-07,8.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +22990000,0.98,-0.0073,-0.013,0.19,0.036,-0.04,-1.4,0.051,-0.031,-2.3,-0.0014,-0.0058,2.6e-05,0.017,-0.00077,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.019,0.019,0.0081,0.042,0.042,0.036,8.4e-07,8.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23090000,0.98,-0.0074,-0.013,0.19,0.041,-0.044,-1.4,0.055,-0.035,-2.5,-0.0014,-0.0058,2.6e-05,0.017,-0.0008,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.0047,0.02,0.02,0.0081,0.046,0.046,0.036,8.4e-07,8.4e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23190000,0.98,-0.0074,-0.013,0.19,0.047,-0.046,-1.4,0.066,-0.045,-2.6,-0.0014,-0.0058,2.7e-05,0.017,-0.00095,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.018,0.018,0.008,0.042,0.042,0.035,8e-07,8e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23290000,0.98,-0.0079,-0.013,0.19,0.052,-0.051,-1.4,0.071,-0.05,-2.8,-0.0014,-0.0058,2.7e-05,0.017,-0.001,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.02,0.02,0.0081,0.046,0.046,0.036,8e-07,8e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23390000,0.98,-0.0078,-0.014,0.19,0.057,-0.053,-1.4,0.082,-0.055,-2.9,-0.0014,-0.0058,2.6e-05,0.017,-0.00075,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8e-05,7.8e-05,0.0048,0.018,0.018,0.008,0.041,0.041,0.036,7.6e-07,7.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23490000,0.98,-0.0079,-0.014,0.19,0.062,-0.055,-1.4,0.088,-0.061,-3,-0.0014,-0.0058,2.6e-05,0.017,-0.00079,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.9e-05,0.0048,0.019,0.019,0.0081,0.046,0.046,0.036,7.6e-07,7.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23590000,0.98,-0.0081,-0.014,0.19,0.064,-0.058,-1.4,0.095,-0.07,-3.2,-0.0014,-0.0058,2.7e-05,0.017,-0.00091,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8e-05,7.8e-05,0.0048,0.017,0.017,0.008,0.041,0.041,0.035,7.2e-07,7.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23690000,0.98,-0.0088,-0.014,0.19,0.062,-0.06,-1.3,0.1,-0.076,-3.3,-0.0014,-0.0058,2.7e-05,0.017,-0.00093,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0048,0.018,0.018,0.0081,0.046,0.046,0.036,7.2e-07,7.2e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23790000,0.98,-0.011,-0.017,0.19,0.057,-0.058,-0.96,0.11,-0.081,-3.4,-0.0014,-0.0058,2.7e-05,0.017,-0.00093,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.7e-05,0.0048,0.016,0.016,0.008,0.041,0.041,0.035,6.9e-07,6.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23890000,0.98,-0.014,-0.021,0.19,0.053,-0.058,-0.52,0.12,-0.087,-3.5,-0.0014,-0.0058,2.7e-05,0.017,-0.00094,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.8e-05,0.0048,0.017,0.017,0.008,0.045,0.045,0.035,6.9e-07,6.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +23990000,0.98,-0.016,-0.024,0.19,0.054,-0.058,-0.14,0.13,-0.089,-3.6,-0.0014,-0.0058,2.7e-05,0.018,-0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.041,0.041,0.036,6.6e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24090000,0.98,-0.016,-0.023,0.19,0.061,-0.066,0.09,0.13,-0.095,-3.6,-0.0014,-0.0058,2.7e-05,0.018,-0.0015,-0.13,0.37,0.0037,0.025,0,0,0,0,0,0.00017,8.1e-05,7.8e-05,0.0048,0.017,0.017,0.0081,0.045,0.045,0.036,6.6e-07,6.6e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24190000,0.98,-0.013,-0.019,0.19,0.072,-0.071,0.077,0.14,-0.1,-3.6,-0.0014,-0.0058,2.7e-05,0.019,-0.0022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.035,6.3e-07,6.3e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24290000,0.98,-0.01,-0.016,0.19,0.076,-0.074,0.055,0.15,-0.11,-3.6,-0.0014,-0.0058,2.7e-05,0.019,-0.0022,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.0081,0.044,0.044,0.036,6.4e-07,6.3e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24390000,0.98,-0.0095,-0.015,0.19,0.069,-0.069,0.071,0.15,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.003,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.035,6.1e-07,6.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24490000,0.98,-0.0098,-0.015,0.19,0.065,-0.066,0.069,0.16,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.003,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.008,0.044,0.044,0.035,6.1e-07,6.1e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24590000,0.98,-0.01,-0.015,0.19,0.061,-0.062,0.065,0.16,-0.11,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0037,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.04,0.04,0.036,5.9e-07,5.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24690000,0.98,-0.011,-0.015,0.19,0.059,-0.061,0.064,0.17,-0.12,-3.6,-0.0014,-0.0058,2.6e-05,0.019,-0.0037,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.016,0.016,0.0081,0.044,0.044,0.036,5.9e-07,5.9e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24790000,0.98,-0.011,-0.014,0.19,0.056,-0.059,0.056,0.17,-0.11,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0043,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.7e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.7e-07,5.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24890000,0.98,-0.011,-0.013,0.19,0.054,-0.062,0.045,0.18,-0.12,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.0043,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.016,0.016,0.008,0.043,0.043,0.035,5.7e-07,5.7e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +24990000,0.98,-0.011,-0.013,0.19,0.046,-0.058,0.038,0.18,-0.11,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.005,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.5e-07,5.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +25090000,0.98,-0.011,-0.013,0.19,0.042,-0.057,0.035,0.18,-0.12,-3.6,-0.0015,-0.0058,2.6e-05,0.019,-0.005,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.016,0.016,0.0081,0.043,0.043,0.035,5.5e-07,5.5e-07,6.3e-06,0.028,0.028,0.0005,0,0,0,0,0,0,0,0 +25190000,0.98,-0.011,-0.013,0.19,0.037,-0.05,0.035,0.18,-0.11,-3.6,-0.0015,-0.0058,2.5e-05,0.018,-0.0055,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,7.9e-05,7.8e-05,0.0048,0.015,0.015,0.008,0.039,0.039,0.035,5.3e-07,5.3e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25290000,0.98,-0.011,-0.012,0.19,0.032,-0.052,0.029,0.18,-0.11,-3.6,-0.0015,-0.0058,2.5e-05,0.018,-0.0055,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.016,0.016,0.0081,0.043,0.043,0.036,5.3e-07,5.3e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25390000,0.98,-0.011,-0.012,0.19,0.024,-0.045,0.028,0.18,-0.1,-3.6,-0.0015,-0.0058,2.6e-05,0.018,-0.0062,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.015,0.015,0.008,0.039,0.039,0.035,5.2e-07,5.1e-07,6.3e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25490000,0.98,-0.011,-0.012,0.19,0.019,-0.044,0.027,0.18,-0.11,-3.6,-0.0015,-0.0058,2.6e-05,0.018,-0.0062,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.016,0.016,0.008,0.043,0.043,0.035,5.2e-07,5.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25590000,0.98,-0.011,-0.012,0.19,0.014,-0.04,0.028,0.18,-0.098,-3.6,-0.0015,-0.0058,2.6e-05,0.018,-0.0065,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,5e-07,5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25690000,0.98,-0.011,-0.011,0.19,0.013,-0.039,0.017,0.18,-0.1,-3.6,-0.0015,-0.0058,2.6e-05,0.018,-0.0066,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.015,0.015,0.0081,0.043,0.043,0.035,5e-07,5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25790000,0.98,-0.011,-0.011,0.19,0.0022,-0.031,0.017,0.17,-0.093,-3.6,-0.0016,-0.0058,2.7e-05,0.018,-0.007,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.9e-07,4.8e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25890000,0.98,-0.011,-0.011,0.19,-0.0036,-0.029,0.019,0.17,-0.096,-3.6,-0.0016,-0.0058,2.7e-05,0.018,-0.007,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.043,0.043,0.036,4.9e-07,4.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +25990000,0.98,-0.011,-0.011,0.19,-0.013,-0.022,0.013,0.16,-0.086,-3.6,-0.0016,-0.0058,2.8e-05,0.018,-0.0074,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.8e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.7e-07,4.7e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26090000,0.98,-0.01,-0.011,0.19,-0.018,-0.022,0.011,0.16,-0.088,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0075,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.7e-07,4.7e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26190000,0.98,-0.01,-0.011,0.19,-0.024,-0.015,0.0063,0.15,-0.081,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0077,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.039,0.039,0.035,4.6e-07,4.6e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26290000,0.98,-0.01,-0.012,0.19,-0.026,-0.014,0.00052,0.15,-0.083,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0077,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.036,4.6e-07,4.6e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26390000,0.98,-0.0098,-0.012,0.19,-0.032,-0.0067,0.0045,0.13,-0.075,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.008,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.5e-07,4.5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26490000,0.98,-0.0095,-0.011,0.19,-0.035,-0.0036,0.014,0.13,-0.075,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0079,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.5e-07,4.5e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26590000,0.98,-0.009,-0.012,0.19,-0.036,0.0044,0.014,0.12,-0.068,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0081,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.4e-07,4.3e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26690000,0.98,-0.0088,-0.011,0.19,-0.038,0.0095,0.013,0.12,-0.067,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0081,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.035,4.4e-07,4.4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26790000,0.98,-0.0086,-0.011,0.19,-0.046,0.013,0.012,0.11,-0.062,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0083,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.3e-07,4.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26890000,0.98,-0.008,-0.011,0.19,-0.051,0.016,0.007,0.1,-0.06,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0083,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.015,0.015,0.0081,0.042,0.042,0.036,4.3e-07,4.2e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +26990000,0.98,-0.0074,-0.011,0.19,-0.058,0.023,0.0061,0.088,-0.054,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0086,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.0049,0.014,0.014,0.008,0.038,0.038,0.035,4.2e-07,4.1e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27090000,0.98,-0.0073,-0.012,0.19,-0.06,0.03,0.0089,0.082,-0.052,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.015,0.015,0.0081,0.042,0.042,0.035,4.2e-07,4.1e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27190000,0.98,-0.0073,-0.012,0.19,-0.066,0.035,0.011,0.071,-0.046,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.014,0.014,0.008,0.038,0.038,0.035,4.1e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27290000,0.98,-0.0075,-0.013,0.19,-0.073,0.041,0.12,0.064,-0.042,-3.6,-0.0016,-0.0058,2.8e-05,0.019,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.1e-05,7.9e-05,0.005,0.015,0.015,0.0081,0.042,0.042,0.035,4.1e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27390000,0.98,-0.0089,-0.015,0.19,-0.078,0.047,0.45,0.055,-0.035,-3.5,-0.0016,-0.0058,3e-05,0.02,-0.0092,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,7.9e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,4e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27490000,0.98,-0.01,-0.017,0.19,-0.082,0.053,0.76,0.047,-0.03,-3.5,-0.0016,-0.0058,3e-05,0.02,-0.0093,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.008,0.042,0.042,0.035,4e-07,4e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27590000,0.98,-0.01,-0.016,0.19,-0.076,0.055,0.85,0.038,-0.026,-3.4,-0.0016,-0.0058,3e-05,0.02,-0.0094,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.9e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27690000,0.98,-0.0089,-0.013,0.19,-0.072,0.052,0.76,0.031,-0.02,-3.3,-0.0016,-0.0058,3e-05,0.02,-0.0095,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.0081,0.042,0.042,0.035,3.9e-07,3.9e-07,6.4e-06,0.027,0.028,0.0005,0,0,0,0,0,0,0,0 +27790000,0.98,-0.0076,-0.011,0.19,-0.071,0.049,0.75,0.025,-0.018,-3.3,-0.0016,-0.0058,3e-05,0.02,-0.0089,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.8e-07,3.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +27890000,0.98,-0.0072,-0.012,0.19,-0.078,0.056,0.79,0.017,-0.012,-3.2,-0.0016,-0.0058,3e-05,0.02,-0.009,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.8e-07,3.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +27990000,0.98,-0.0077,-0.012,0.19,-0.079,0.058,0.78,0.012,-0.011,-3.1,-0.0016,-0.0058,3.1e-05,0.019,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28090000,0.98,-0.0079,-0.012,0.19,-0.082,0.059,0.78,0.0041,-0.0049,-3,-0.0016,-0.0058,3.1e-05,0.019,-0.0087,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.8e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28190000,0.98,-0.0074,-0.012,0.19,-0.083,0.055,0.79,-0.0024,-0.0044,-3,-0.0016,-0.0058,3.1e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.2e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28290000,0.98,-0.0069,-0.012,0.19,-0.088,0.059,0.79,-0.011,0.0013,-2.9,-0.0016,-0.0058,3.1e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.035,3.7e-07,3.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28390000,0.98,-0.0069,-0.013,0.19,-0.088,0.061,0.79,-0.015,0.0043,-2.8,-0.0015,-0.0058,3.3e-05,0.019,-0.0083,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28490000,0.98,-0.0072,-0.014,0.19,-0.09,0.065,0.79,-0.024,0.011,-2.8,-0.0015,-0.0058,3.3e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.6e-07,3.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28590000,0.98,-0.0072,-0.014,0.19,-0.083,0.06,0.79,-0.028,0.0083,-2.7,-0.0015,-0.0058,3.3e-05,0.019,-0.0082,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.038,0.038,0.035,3.6e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28690000,0.98,-0.007,-0.013,0.19,-0.083,0.061,0.79,-0.036,0.014,-2.6,-0.0015,-0.0058,3.3e-05,0.019,-0.0083,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.6e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28790000,0.98,-0.0063,-0.013,0.19,-0.08,0.061,0.79,-0.038,0.016,-2.5,-0.0015,-0.0058,3.5e-05,0.019,-0.0083,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.037,0.037,0.035,3.5e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28890000,0.98,-0.0062,-0.012,0.19,-0.084,0.064,0.78,-0.046,0.022,-2.5,-0.0015,-0.0058,3.5e-05,0.019,-0.0084,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.0081,0.041,0.041,0.036,3.5e-07,3.5e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +28990000,0.98,-0.006,-0.013,0.19,-0.079,0.06,0.78,-0.046,0.022,-2.4,-0.0015,-0.0058,3.7e-05,0.019,-0.0085,-0.12,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29090000,0.98,-0.0058,-0.013,0.19,-0.082,0.062,0.78,-0.054,0.028,-2.3,-0.0015,-0.0058,3.7e-05,0.02,-0.0086,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.005,0.014,0.014,0.008,0.041,0.041,0.035,3.5e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29190000,0.98,-0.0058,-0.013,0.19,-0.078,0.061,0.78,-0.051,0.027,-2.2,-0.0015,-0.0058,4e-05,0.02,-0.0088,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29290000,0.98,-0.006,-0.013,0.19,-0.08,0.067,0.78,-0.059,0.033,-2.2,-0.0015,-0.0058,4e-05,0.02,-0.0088,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.4e-07,3.4e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29390000,0.98,-0.0065,-0.012,0.19,-0.076,0.065,0.78,-0.057,0.034,-2.1,-0.0015,-0.0058,4.2e-05,0.02,-0.009,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00018,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29490000,0.98,-0.0065,-0.012,0.19,-0.078,0.066,0.78,-0.065,0.041,-2,-0.0015,-0.0058,4.2e-05,0.02,-0.009,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.0081,0.041,0.041,0.036,3.4e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29590000,0.98,-0.0064,-0.012,0.19,-0.074,0.064,0.78,-0.062,0.04,-1.9,-0.0015,-0.0058,4.4e-05,0.02,-0.0092,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29690000,0.98,-0.0064,-0.012,0.19,-0.078,0.063,0.78,-0.07,0.046,-1.9,-0.0015,-0.0058,4.4e-05,0.02,-0.0093,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-07,3.3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29790000,0.98,-0.0063,-0.013,0.19,-0.074,0.056,0.78,-0.065,0.043,-1.8,-0.0014,-0.0058,4.6e-05,0.02,-0.0096,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29890000,0.98,-0.0057,-0.013,0.19,-0.074,0.057,0.77,-0.073,0.049,-1.7,-0.0014,-0.0058,4.6e-05,0.02,-0.0097,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.3e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +29990000,0.98,-0.0059,-0.013,0.19,-0.069,0.052,0.77,-0.068,0.044,-1.6,-0.0014,-0.0058,4.7e-05,0.02,-0.01,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30090000,0.98,-0.006,-0.013,0.19,-0.069,0.053,0.77,-0.075,0.05,-1.6,-0.0014,-0.0058,4.7e-05,0.02,-0.01,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30190000,0.98,-0.006,-0.013,0.19,-0.063,0.05,0.77,-0.068,0.048,-1.5,-0.0014,-0.0057,5e-05,0.02,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.2e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30290000,0.98,-0.006,-0.013,0.19,-0.062,0.05,0.77,-0.074,0.053,-1.4,-0.0014,-0.0057,5e-05,0.02,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.035,3.2e-07,3.2e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30390000,0.98,-0.0061,-0.013,0.19,-0.055,0.044,0.76,-0.066,0.049,-1.4,-0.0014,-0.0057,5.2e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.0079,0.037,0.037,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30490000,0.98,-0.0061,-0.013,0.19,-0.058,0.044,0.77,-0.072,0.054,-1.3,-0.0014,-0.0057,5.2e-05,0.021,-0.011,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.014,0.008,0.041,0.041,0.036,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30590000,0.98,-0.0064,-0.013,0.19,-0.053,0.041,0.76,-0.065,0.05,-1.2,-0.0014,-0.0057,5.4e-05,0.021,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30690000,0.98,-0.0068,-0.014,0.19,-0.051,0.04,0.76,-0.07,0.054,-1.1,-0.0014,-0.0057,5.4e-05,0.021,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.014,0.013,0.008,0.041,0.041,0.035,3.1e-07,3.1e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30790000,0.98,-0.0065,-0.013,0.19,-0.044,0.035,0.76,-0.063,0.052,-1.1,-0.0014,-0.0057,5.6e-05,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30890000,0.98,-0.0058,-0.013,0.19,-0.044,0.032,0.76,-0.067,0.056,-1,-0.0014,-0.0057,5.6e-05,0.022,-0.012,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.4e-05,8.1e-05,0.0051,0.013,0.013,0.008,0.041,0.041,0.035,3.1e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +30990000,0.98,-0.006,-0.013,0.19,-0.037,0.026,0.76,-0.057,0.049,-0.94,-0.0014,-0.0057,5.7e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.0079,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31090000,0.98,-0.0062,-0.013,0.19,-0.036,0.025,0.76,-0.061,0.051,-0.86,-0.0014,-0.0057,5.7e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.041,0.04,0.036,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31190000,0.98,-0.0064,-0.013,0.19,-0.031,0.02,0.76,-0.052,0.046,-0.79,-0.0014,-0.0057,5.8e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.037,0.037,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31290000,0.98,-0.0066,-0.014,0.19,-0.028,0.018,0.76,-0.055,0.048,-0.72,-0.0014,-0.0057,5.8e-05,0.022,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,3e-07,3e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31390000,0.98,-0.0064,-0.013,0.19,-0.022,0.012,0.76,-0.046,0.042,-0.65,-0.0014,-0.0057,5.9e-05,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31490000,0.98,-0.0061,-0.014,0.19,-0.022,0.0088,0.76,-0.049,0.043,-0.58,-0.0014,-0.0057,5.9e-05,0.023,-0.013,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31590000,0.98,-0.006,-0.014,0.19,-0.018,0.0066,0.76,-0.038,0.039,-0.51,-0.0014,-0.0057,6e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31690000,0.98,-0.006,-0.015,0.19,-0.02,0.0055,0.76,-0.04,0.039,-0.44,-0.0014,-0.0057,6e-05,0.023,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8.1e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31790000,0.98,-0.0062,-0.015,0.19,-0.011,0.0029,0.76,-0.028,0.037,-0.36,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.008,0.037,0.037,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31890000,0.98,-0.0059,-0.015,0.19,-0.008,0.00064,0.76,-0.029,0.038,-0.3,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.9e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +31990000,0.98,-0.0062,-0.015,0.19,-0.00015,-2.2e-05,0.75,-0.017,0.034,-0.23,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32090000,0.98,-0.0065,-0.014,0.19,-0.00069,-0.0034,0.76,-0.017,0.034,-0.16,-0.0014,-0.0057,6.1e-05,0.024,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.9e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32190000,0.98,-0.0067,-0.015,0.19,0.0046,-0.0066,0.76,-0.006,0.033,-0.092,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0052,0.012,0.012,0.0079,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32290000,0.98,-0.0066,-0.015,0.19,0.0062,-0.0094,0.75,-0.0055,0.032,-0.024,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.3e-05,8e-05,0.0052,0.013,0.013,0.008,0.04,0.04,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32390000,0.98,-0.0068,-0.015,0.19,0.012,-0.011,0.75,0.0057,0.03,0.051,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0053,0.012,0.012,0.008,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32490000,0.98,-0.0096,-0.013,0.19,0.039,-0.074,-0.12,0.0089,0.023,0.054,-0.0013,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.2e-05,8e-05,0.0053,0.015,0.015,0.0078,0.04,0.04,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32590000,0.98,-0.0095,-0.013,0.19,0.039,-0.075,-0.12,0.021,0.02,0.035,-0.0014,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.1e-05,7.9e-05,0.0053,0.016,0.016,0.0075,0.037,0.037,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32690000,0.98,-0.0095,-0.013,0.19,0.035,-0.08,-0.12,0.025,0.012,0.02,-0.0014,-0.0057,6.2e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,8.1e-05,7.9e-05,0.0053,0.019,0.019,0.0074,0.041,0.041,0.035,2.8e-07,2.8e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32790000,0.98,-0.0092,-0.013,0.19,0.034,-0.078,-0.12,0.034,0.01,0.0044,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.8e-05,7.6e-05,0.0053,0.02,0.02,0.0071,0.037,0.037,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32890000,0.98,-0.0092,-0.013,0.19,0.033,-0.084,-0.13,0.038,0.002,-0.011,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.8e-05,7.7e-05,0.0053,0.023,0.024,0.007,0.041,0.041,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +32990000,0.98,-0.0089,-0.013,0.19,0.03,-0.08,-0.13,0.045,-0.0012,-0.024,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.4e-05,7.2e-05,0.0053,0.024,0.024,0.0067,0.038,0.038,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33090000,0.98,-0.0089,-0.013,0.19,0.026,-0.084,-0.12,0.048,-0.0093,-0.031,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,7.4e-05,7.2e-05,0.0053,0.029,0.029,0.0066,0.042,0.042,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33190000,0.98,-0.0085,-0.013,0.19,0.022,-0.079,-0.12,0.054,-0.011,-0.037,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.8e-05,6.7e-05,0.0053,0.029,0.029,0.0064,0.038,0.038,0.035,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33290000,0.98,-0.0086,-0.013,0.19,0.019,-0.08,-0.12,0.056,-0.019,-0.045,-0.0014,-0.0056,6.3e-05,0.025,-0.014,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.8e-05,6.7e-05,0.0053,0.035,0.035,0.0063,0.043,0.043,0.034,2.7e-07,2.7e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33390000,0.98,-0.0081,-0.013,0.19,0.014,-0.066,-0.12,0.059,-0.014,-0.053,-0.0014,-0.0056,6.4e-05,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.1e-05,6e-05,0.0053,0.035,0.035,0.0062,0.039,0.039,0.034,2.6e-07,2.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33490000,0.98,-0.0081,-0.013,0.19,0.0096,-0.067,-0.12,0.06,-0.021,-0.063,-0.0014,-0.0056,6.4e-05,0.025,-0.017,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.00019,6.2e-05,6e-05,0.0053,0.042,0.042,0.0061,0.044,0.044,0.034,2.7e-07,2.6e-07,6.4e-06,0.027,0.027,0.0005,0,0,0,0,0,0,0,0 +33590000,0.98,-0.0077,-0.013,0.19,0.0057,-0.058,-0.11,0.063,-0.017,-0.069,-0.0014,-0.0056,6.4e-05,0.024,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,5.4e-05,5.3e-05,0.0053,0.04,0.041,0.006,0.04,0.04,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 +33690000,0.98,-0.0077,-0.013,0.19,0.001,-0.058,-0.11,0.063,-0.023,-0.077,-0.0014,-0.0056,6.4e-05,0.024,-0.019,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,5.5e-05,5.3e-05,0.0053,0.048,0.048,0.0059,0.046,0.046,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.027,0.0005,0,0,0,0,0,0,0,0 +33790000,0.98,-0.0075,-0.013,0.19,-0.0022,-0.048,-0.11,0.067,-0.018,-0.083,-0.0014,-0.0056,6.4e-05,0.023,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.8e-05,4.7e-05,0.0053,0.045,0.045,0.0059,0.041,0.041,0.034,2.6e-07,2.6e-07,6.4e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 +33890000,0.98,-0.0075,-0.013,0.19,-0.0064,-0.045,-0.11,0.066,-0.023,-0.089,-0.0014,-0.0056,6.4e-05,0.023,-0.022,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.8e-05,4.7e-05,0.0053,0.052,0.053,0.0058,0.047,0.047,0.033,2.6e-07,2.6e-07,6.4e-06,0.026,0.026,0.0005,0,0,0,0,0,0,0,0 +33990000,0.98,-0.0072,-0.013,0.19,-0.006,-0.031,-0.1,0.069,-0.015,-0.092,-0.0014,-0.0056,6.5e-05,0.02,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.2e-05,4.1e-05,0.0054,0.047,0.047,0.0058,0.042,0.042,0.033,2.6e-07,2.6e-07,6.4e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 +34090000,0.98,-0.0072,-0.013,0.19,-0.01,-0.031,-0.1,0.069,-0.018,-0.096,-0.0014,-0.0056,6.5e-05,0.02,-0.025,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,4.2e-05,4.1e-05,0.0054,0.054,0.055,0.0058,0.049,0.049,0.033,2.6e-07,2.6e-07,6.4e-06,0.025,0.025,0.0005,0,0,0,0,0,0,0,0 +34190000,0.98,-0.0071,-0.013,0.19,-0.011,-0.02,-0.098,0.072,-0.013,-0.099,-0.0014,-0.0056,6.5e-05,0.018,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.8e-05,3.7e-05,0.0054,0.048,0.048,0.0058,0.043,0.043,0.033,2.6e-07,2.6e-07,6.4e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 +34290000,0.98,-0.0069,-0.013,0.19,-0.011,-0.02,-0.097,0.071,-0.015,-0.1,-0.0014,-0.0056,6.5e-05,0.018,-0.027,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.8e-05,3.7e-05,0.0054,0.055,0.055,0.0058,0.05,0.05,0.033,2.6e-07,2.6e-07,6.4e-06,0.024,0.024,0.0005,0,0,0,0,0,0,0,0 +34390000,0.98,-0.0068,-0.013,0.19,-0.013,-0.01,-0.092,0.073,-0.01,-0.11,-0.0014,-0.0056,6.5e-05,0.017,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.4e-05,3.3e-05,0.0054,0.047,0.047,0.0058,0.044,0.044,0.033,2.6e-07,2.6e-07,6.4e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 +34490000,0.98,-0.0069,-0.013,0.19,-0.015,-0.0092,-0.089,0.071,-0.011,-0.11,-0.0014,-0.0056,6.5e-05,0.017,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.5e-05,3.3e-05,0.0054,0.053,0.054,0.0059,0.051,0.051,0.032,2.6e-07,2.6e-07,6.4e-06,0.023,0.023,0.0005,0,0,0,0,0,0,0,0 +34590000,0.98,-0.0069,-0.013,0.19,-0.014,-0.0049,0.71,0.073,-0.0088,-0.081,-0.0014,-0.0056,6.5e-05,0.015,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.2e-05,3.1e-05,0.0054,0.044,0.044,0.0059,0.045,0.045,0.032,2.7e-07,2.6e-07,6.4e-06,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 +34690000,0.98,-0.0068,-0.012,0.19,-0.017,-0.0027,1.7,0.071,-0.0092,0.038,-0.0014,-0.0056,6.5e-05,0.015,-0.028,-0.11,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.2e-05,3.1e-05,0.0054,0.047,0.048,0.006,0.052,0.052,0.032,2.7e-07,2.6e-07,6.4e-06,0.022,0.022,0.0005,0,0,0,0,0,0,0,0 +34790000,0.98,-0.0068,-0.012,0.19,-0.018,0.0019,2.7,0.072,-0.0068,0.21,-0.0014,-0.0056,6.5e-05,0.017,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.1e-05,3e-05,0.0054,0.04,0.04,0.0061,0.045,0.045,0.032,2.7e-07,2.7e-07,6.4e-06,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 +34890000,0.98,-0.0068,-0.012,0.19,-0.022,0.0044,3.6,0.07,-0.0063,0.5,-0.0014,-0.0056,6.5e-05,0.017,-0.03,-0.1,0.37,0.0037,0.025,0,0,0,0,0,0.0002,3.1e-05,3e-05,0.0054,0.044,0.044,0.0061,0.052,0.052,0.032,2.7e-07,2.7e-07,6.4e-06,0.021,0.021,0.0005,0,0,0,0,0,0,0,0 diff --git a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt index 58bdcee733..9e9edc1f0e 100644 --- a/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt +++ b/src/modules/ekf2/test/sensor_simulator/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 ECL Development Team. All rights reserved. +# Copyright (c) 2019-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 @@ -12,7 +12,7 @@ # notice, this list of conditions and the following disclaimer in # the documentation and/or other materials provided with the # distribution. -# 3. Neither the name ECL nor the names of its contributors may be +# 3. Neither the name PX4 nor the names of its contributors may be # used to endorse or promote products derived from this software # without specific prior written permission. # diff --git a/src/modules/ekf2/test/sensor_simulator/airspeed.h b/src/modules/ekf2/test/sensor_simulator/airspeed.h index 0ac4aa5c89..261aa4fefc 100644 --- a/src/modules/ekf2/test/sensor_simulator/airspeed.h +++ b/src/modules/ekf2/test/sensor_simulator/airspeed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/baro.h b/src/modules/ekf2/test/sensor_simulator/baro.h index 018e5d88e0..2847935d45 100644 --- a/src/modules/ekf2/test/sensor_simulator/baro.h +++ b/src/modules/ekf2/test/sensor_simulator/baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py index 0e656a684a..1b3dd9dc05 100644 --- a/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py +++ b/src/modules/ekf2/test/sensor_simulator/convertULogToSensorData.py @@ -22,14 +22,14 @@ def getVioData(ulog: ULog) -> pd.DataFrame: def getOpticalFlowData(ulog: ULog) -> pd.DataFrame: - optical_flow = ulog.get_dataset("optical_flow").data + optical_flow = ulog.get_dataset("vehicle_optical_flow").data flow = pd.DataFrame({'timestamp': optical_flow['timestamp'], 'sensor' : 'flow', - 'pixel_flow_x_integral': optical_flow["pixel_flow_x_integral"], - 'pixel_flow_y_integral': optical_flow["pixel_flow_y_integral"], - 'gyro_x_rate_integral': optical_flow["gyro_x_rate_integral"], - 'gyro_y_rate_integral': optical_flow["gyro_y_rate_integral"], - 'gyro_z_rate_integral': optical_flow["gyro_z_rate_integral"], + 'pixel_flow_x': optical_flow["pixel_flow[0]"], + 'pixel_flow_y': optical_flow["pixel_flow[1]"], + 'delta_angle_x': optical_flow["delta_angle[0]"], + 'delta_angle_y': optical_flow["delta_angle[1]"], + 'delta_angle_z': optical_flow["delta_angle[2]"], 'quality': optical_flow["quality"] }) return flow diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h index 7a1c648d89..748adcc758 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_logger.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_logger.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index adf31b1c1c..596d087cf3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -205,6 +205,11 @@ bool EkfWrapper::isIntendingMag3DFusion() const return _ekf->control_status_flags().mag_3D; } +bool EkfWrapper::isMagHeadingConsistent() const +{ + return _ekf->control_status_flags().mag_heading_consistent; +} + void EkfWrapper::setMagFuseTypeNone() { _ekf_params->mag_fusion_type = MagFuseType::NONE; @@ -212,7 +217,17 @@ void EkfWrapper::setMagFuseTypeNone() void EkfWrapper::enableMagStrengthCheck() { - _ekf_params->check_mag_strength = 1; + _ekf_params->mag_check |= static_cast(MagCheckMask::STRENGTH); +} + +void EkfWrapper::enableMagInclinationCheck() +{ + _ekf_params->mag_check |= static_cast(MagCheckMask::INCLINATION); +} + +void EkfWrapper::enableMagCheckForceWMM() +{ + _ekf_params->mag_check |= static_cast(MagCheckMask::FORCE_WMM); } bool EkfWrapper::isWindVelocityEstimated() const @@ -265,11 +280,6 @@ float EkfWrapper::getYawAngle() const return euler(2); } -matrix::Vector4f EkfWrapper::getQuaternionVariance() const -{ - return matrix::Vector4f(_ekf->orientation_covariances().diag()); -} - int EkfWrapper::getQuaternionResetCounter() const { float tmp[4]; @@ -278,11 +288,6 @@ int EkfWrapper::getQuaternionResetCounter() const return static_cast(counter); } -matrix::Vector3f EkfWrapper::getDeltaVelBiasVariance() const -{ - return _ekf->covariances_diagonal().slice<3, 1>(13, 0); -} - void EkfWrapper::enableDragFusion() { _ekf_params->drag_ctrl = 1; @@ -299,3 +304,8 @@ void EkfWrapper::setDragFusionParameters(const float &bcoef_x, const float &bcoe _ekf_params->bcoef_y = bcoef_y; _ekf_params->mcoef = mcoef; } + +float EkfWrapper::getMagHeadingNoise() const +{ + return _ekf_params->mag_heading_noise; +} diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 73ea368166..beb43e64e3 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -100,8 +100,11 @@ public: bool isIntendingMagHeadingFusion() const; bool isIntendingMag3DFusion() const; + bool isMagHeadingConsistent() const; void setMagFuseTypeNone(); void enableMagStrengthCheck(); + void enableMagInclinationCheck(); + void enableMagCheckForceWMM(); bool isWindVelocityEstimated() const; @@ -115,15 +118,14 @@ public: Eulerf getEulerAngles() const; float getYawAngle() const; - matrix::Vector4f getQuaternionVariance() const; int getQuaternionResetCounter() const; - matrix::Vector3f getDeltaVelBiasVariance() const; - void enableDragFusion(); void disableDragFusion(); void setDragFusionParameters(const float &bcoef_x, const float &bcoef_y, const float &mcoef); + float getMagHeadingNoise() const; + private: std::shared_ptr _ekf; diff --git a/src/modules/ekf2/test/sensor_simulator/flow.h b/src/modules/ekf2/test/sensor_simulator/flow.h index 4820fedfd8..2c0cdf94fc 100644 --- a/src/modules/ekf2/test/sensor_simulator/flow.h +++ b/src/modules/ekf2/test/sensor_simulator/flow.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/gps.h b/src/modules/ekf2/test/sensor_simulator/gps.h index 1ecacb85a8..82d62df70f 100644 --- a/src/modules/ekf2/test/sensor_simulator/gps.h +++ b/src/modules/ekf2/test/sensor_simulator/gps.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/imu.h b/src/modules/ekf2/test/sensor_simulator/imu.h index 18600c3eb7..ccf6856edf 100644 --- a/src/modules/ekf2/test/sensor_simulator/imu.h +++ b/src/modules/ekf2/test/sensor_simulator/imu.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/mag.h b/src/modules/ekf2/test/sensor_simulator/mag.h index 5cfed8b892..202ba63fb9 100644 --- a/src/modules/ekf2/test/sensor_simulator/mag.h +++ b/src/modules/ekf2/test/sensor_simulator/mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/range_finder.h b/src/modules/ekf2/test/sensor_simulator/range_finder.h index e41a923cb7..0dcd068232 100644 --- a/src/modules/ekf2/test/sensor_simulator/range_finder.h +++ b/src/modules/ekf2/test/sensor_simulator/range_finder.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/sensor.h b/src/modules/ekf2/test/sensor_simulator/sensor.h index 271960eb47..317f922978 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h index ce61f43ddf..ee6a651e3a 100644 --- a/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h +++ b/src/modules/ekf2/test/sensor_simulator/sensor_simulator.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/sensor_simulator/vio.h b/src/modules/ekf2/test/sensor_simulator/vio.h index 683341024b..28d98135fc 100644 --- a/src/modules/ekf2/test/sensor_simulator/vio.h +++ b/src/modules/ekf2/test/sensor_simulator/vio.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index c92a3170ec..28964035c9 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp index 9d4ac39012..114a051e18 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed_fusion_generated.cpp @@ -62,7 +62,7 @@ TEST(AirspeedFusionGenerated, SympyVsSymforce) // Intermediate variables const float HK0 = vn - vwn; const float HK1 = ve - vwe; - const float HK2 = ecl::powf(HK0, 2) + ecl::powf(HK1, 2) + ecl::powf(vd, 2); + const float HK2 = powf(HK0, 2.f) + powf(HK1, 2.f) + powf(vd, 2.f); const float v_tas_pred = sqrtf(HK2); // predicted airspeed //const float HK3 = powf(HK2, -1.0F/2.0F); // calculation can be badly conditioned for very low airspeed values so don't fuse this time diff --git a/src/modules/ekf2/test/test_EKF_attitude_variance.cpp b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp new file mode 100644 index 0000000000..d851d6a7a6 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_attitude_variance.cpp @@ -0,0 +1,272 @@ +/**************************************************************************** + * + * 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 "EKF/ekf.h" +#include "test_helper/comparison_helper.h" + +#include "../EKF/python/ekf_derivation/generated/quat_var_to_rot_var.h" +#include "../EKF/python/ekf_derivation/generated/rot_var_ned_to_lower_triangular_quat_cov.h" +#include "../EKF/python/ekf_derivation/generated/compute_yaw_321_innov_var_and_h.h" +#include "../EKF/python/ekf_derivation/generated/compute_yaw_312_innov_var_and_h.h" + +using namespace matrix; + +Vector3f calcRotVarMatlab(const Quatf &q, const SquareMatrix24f &P) +{ + Vector3f rot_var_vec; + float q0, q1, q2, q3; + + if (q(0) >= 0.0f) { + q0 = q(0); + q1 = q(1); + q2 = q(2); + q3 = q(3); + + } else { + q0 = -q(0); + q1 = -q(1); + q2 = -q(2); + q3 = -q(3); + } + + float t2 = q0 * q0; + float t3 = acosf(q0); + float t4 = -t2 + 1.0f; + float t5 = t2 - 1.0f; + + if ((t4 > 1e-9f) && (t5 < -1e-9f)) { + float t6 = 1.0f / t5; + float t7 = q1 * t6 * 2.0f; + float t8 = 1.0f / powf(t4, 1.5f); + float t9 = q0 * q1 * t3 * t8 * 2.0f; + float t10 = t7 + t9; + float t11 = 1.0f / sqrtf(t4); + float t12 = q2 * t6 * 2.0f; + float t13 = q0 * q2 * t3 * t8 * 2.0f; + float t14 = t12 + t13; + float t15 = q3 * t6 * 2.0f; + float t16 = q0 * q3 * t3 * t8 * 2.0f; + float t17 = t15 + t16; + rot_var_vec(0) = t10 * (P(0, 0) * t10 + P(1, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 1) * t10 + P(1, + 1) * t3 * t11 * 2.0f) * 2.0f; + rot_var_vec(1) = t14 * (P(0, 0) * t14 + P(2, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 2) * t14 + P(2, + 2) * t3 * t11 * 2.0f) * 2.0f; + rot_var_vec(2) = t17 * (P(0, 0) * t17 + P(3, 0) * t3 * t11 * 2.0f) + t3 * t11 * (P(0, 3) * t17 + P(3, + 3) * t3 * t11 * 2.0f) * 2.0f; + + } else { + rot_var_vec = 4.0f * P.slice<3, 3>(1, 1).diag(); + } + + return rot_var_vec; +} + +TEST(AttitudeVariance, matlabVsSymforce) +{ + Quatf q(Eulerf(M_PI_F / 4.f, -M_PI_F / 6.f, M_PI_F)); + q = -q; // use non-canonical quaternion to cover special case + + const SquareMatrix24f P = createRandomCovarianceMatrix24f(); + Vector3f rot_var_matlab = calcRotVarMatlab(q, P); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + Vector3f rot_var_symforce; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce); + + EXPECT_EQ(rot_var_matlab, rot_var_symforce); +} + +TEST(AttitudeVariance, matlabVsSymforceZeroTilt) +{ + Quatf q; + + const SquareMatrix24f P = createRandomCovarianceMatrix24f(); + Vector3f rot_var_matlab = calcRotVarMatlab(q, P); + + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + Vector3f rot_var_symforce; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var_symforce); + + EXPECT_EQ(rot_var_matlab, rot_var_symforce); + + const Vector3f rot_var_true = 4.0f * P.slice<3, 3>(1, 1).diag(); // special case + EXPECT_EQ(rot_var_symforce, rot_var_true); +} + +void increaseYawVar(const Vector24f &state_vector, SquareMatrix24f &P, const float yaw_var) +{ + SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(0.f, 0.f, yaw_var), &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) += q_cov; +} + +void setTiltVar(const Vector24f &state_vector, SquareMatrix24f &P, const float tilt_var) +{ + SquareMatrix q_cov; + sym::RotVarNedToLowerTriangularQuatCov(state_vector, Vector3f(tilt_var, tilt_var, 0.f), &q_cov); + q_cov.copyLowerToUpperTriangle(); + P.slice<4, 4>(0, 0) = q_cov; +} + +float getYawVar(const Vector24f &state_vector, const SquareMatrix24f &P) +{ + Vector24f H_YAW; + float yaw_var = 0.f; + sym::ComputeYaw312InnovVarAndH(state_vector, P, 0.f, FLT_EPSILON, &yaw_var, &H_YAW); + return yaw_var; +} + +TEST(AttitudeVariance, increaseYawVarNoTilt) +{ + Quatf q; + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float yaw_var = radians(25.f); + increaseYawVar(state_vector, P, yaw_var); + + const float var_new = getYawVar(state_vector, P); + + EXPECT_NEAR(var_new, yaw_var, 1e-6f); +} + +TEST(AttitudeVariance, increaseYawVarPitch90) +{ + Quatf q(Eulerf(M_PI_F / 2.f, M_PI_F / 2.f, M_PI_F / 3.f)); + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float yaw_var = radians(25.f); + increaseYawVar(state_vector, P, yaw_var); + + const float var_new = getYawVar(state_vector, P); + + EXPECT_NEAR(var_new, yaw_var, 1e-6f); +} + +TEST(AttitudeVariance, increaseYawWithTilt) +{ + Quatf q(Eulerf(-M_PI_F, M_PI_F / 3.f, -M_PI_F / 5.f)); + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + // Set the yaw variance (from 0) + const float yaw_var_1 = radians(25.f); + increaseYawVar(state_vector, P, yaw_var_1); + + const float var_1 = getYawVar(state_vector, P); + EXPECT_NEAR(var_1, yaw_var_1, 1e-6f); + + // Increase it even more + const float yaw_var_2 = radians(12.f); + increaseYawVar(state_vector, P, yaw_var_2); + + const float var_2 = getYawVar(state_vector, P); + EXPECT_NEAR(var_2, yaw_var_1 + yaw_var_2, 1e-6f); +} + +TEST(AttitudeVariance, setRotVarNoTilt) +{ + Quatf q; + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + EXPECT_EQ(rot_var(2), 0.f); + + // Compare against known values (special case) + EXPECT_EQ(P(0, 0), 0.f); + EXPECT_EQ(P(1, 1), 0.25f * tilt_var); + EXPECT_EQ(P(2, 2), 0.25f * tilt_var); + EXPECT_EQ(P(3, 3), 0.25f * 0.f); // no yaw var +} + +TEST(AttitudeVariance, setRotVarPitch90) +{ + Quatf q(Eulerf(0.f, M_PI_F, 0.f)); + SquareMatrix24f P; + Vector24f state_vector{}; + state_vector(0) = q(0); + state_vector(1) = q(1); + state_vector(2) = q(2); + state_vector(3) = q(3); + + const float tilt_var = radians(1.2f); + setTiltVar(state_vector, P, tilt_var); + + Vector3f rot_var; + sym::QuatVarToRotVar(state_vector, P, FLT_EPSILON, &rot_var); + + // TODO: FIXME, due to the nonlinearity of the quaternion parameters, + // setting the variance and getting it back is approximate. + // The correct way would be to keep the uncertainty as a 3D vector in the tangent plane + // instead of converting it to the parameter space + // EXPECT_NEAR(rot_var(0), tilt_var, 1e-6f); + // EXPECT_NEAR(rot_var(1), tilt_var, 1e-6f); + // EXPECT_EQ(rot_var(2), 0.f); +} diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index d7b00f1b8d..99ce71674e 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -271,6 +271,8 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + EXPECT_FALSE(_ekf->global_origin_valid()); + _latitude_new = 45.0000005; _longitude_new = 111.0000005; _altitude_new = 1500.0; @@ -282,6 +284,11 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_DOUBLE_EQ(_longitude, _longitude_new); EXPECT_FLOAT_EQ(_altitude, _altitude_new); + // Global origin has been initialized but since there is no position aiding, the global + // position is still not valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + _sensor_simulator.runSeconds(1); float hpos = 0.f; @@ -299,4 +306,62 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) EXPECT_NEAR(vvel, 0.f, 0.02f); } +TEST_F(EkfBasicsTest, global_position_from_local_ev) +{ + // GIVEN: external vision (local) aiding + _ekf_wrapper.enableExternalVisionPositionFusion(); + _sensor_simulator._vio.setPositionFrameToLocalNED(); + _sensor_simulator.startExternalVision(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); +} + +TEST_F(EkfBasicsTest, global_position_from_opt_flow) +{ + // GIVEN: optical flow aiding + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + _sensor_simulator.startFlow(); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startRangeFinder(); + + _sensor_simulator.runSeconds(1); + + // THEN; since there is no origin, only the local position can be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_origin_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + _latitude_new = 45.0000005; + _longitude_new = 111.0000005; + _altitude_new = 1500.0; + + // BUT WHEN: the global origin is set (manually) + _ekf->setEkfGlobalOrigin(_latitude_new, _longitude_new, _altitude_new); + + // THEN: local and global positions are valid + EXPECT_TRUE(_ekf->global_origin_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + EXPECT_TRUE(_ekf->local_position_is_valid()); +} + // TODO: Add sampling tests diff --git a/src/modules/ekf2/test/test_EKF_covariance_prediction_generated.cpp b/src/modules/ekf2/test/test_EKF_covariance_prediction_generated.cpp deleted file mode 100644 index 2cc0dcf7d0..0000000000 --- a/src/modules/ekf2/test/test_EKF_covariance_prediction_generated.cpp +++ /dev/null @@ -1,816 +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. - * - ****************************************************************************/ - -#include -#include "EKF/ekf.h" -#include "test_helper/comparison_helper.h" - -#include "../EKF/python/ekf_derivation/generated/predict_covariance.h" - -using namespace matrix; - -TEST(CovariancePredictionGenerated, SympyVsSymforce) -{ - // Compare calculation of observation Jacobians and Kalman gains for sympy and symforce generated equations - const Quatf q(Eulerf(-M_PI_F / 2.f, M_PI_F / 3.f, M_PI_F * 4.f / 5.f)); - const float q0 = q(0); - const float q1 = q(1); - const float q2 = q(2); - const float q3 = q(3); - - const float dt = 0.01f; - - // up to 1 rad/sec of rate - const float dax = 2.0f * dt * ((float)randf() - 0.5f); - const float day = 2.0f * dt * ((float)randf() - 0.5f); - const float daz = 2.0f * dt * ((float)randf() - 0.5f); - - // up to 2g of accel - const float dvx = 2.0f * 20.0f * dt * ((float)randf() - 0.5f); - const float dvy = 2.0f * 20.0f * dt * ((float)randf() - 0.5f); - const float dvz = 2.0f * 20.0f * dt * ((float)randf() - 0.5f); - - // up to 0.1 rad/sec of gyro bias - const float dax_b = 2.0f * 0.1f * dt * ((float)randf() - 0.5f); - const float day_b = 2.0f * 0.1f * dt * ((float)randf() - 0.5f); - const float daz_b = 2.0f * 0.1f * dt * ((float)randf() - 0.5f); - - // up to 0.5 m/s/s of accel bias - const float dvx_b = 2.0f * 0.5f * dt * ((float)randf() - 0.5f); - const float dvy_b = 2.0f * 0.5f * dt * ((float)randf() - 0.5f); - const float dvz_b = 2.0f * 0.5f * dt * ((float)randf() - 0.5f); - - const float daxVar = sq(dt * 0.015f); - const float dayVar = daxVar; - const float dazVar = daxVar; - - const float dvxVar = sq(dt * 0.3f); - const float dvyVar = dvxVar; - const float dvzVar = dvxVar; - - SquareMatrix24f P = createRandomCovarianceMatrix24f(); - SquareMatrix24f nextP_sympy; - SquareMatrix24f nextP_symforce; - - { - SquareMatrix24f nextP; - // Equations for covariance matrix prediction, without process noise! - const float PS0 = (q1) * (q1); - const float PS1 = 0.25F * daxVar; - const float PS2 = (q2) * (q2); - const float PS3 = 0.25F * dayVar; - const float PS4 = (q3) * (q3); - const float PS5 = 0.25F * dazVar; - const float PS6 = 0.5F * q1; - const float PS7 = 0.5F * q2; - const float PS8 = P(10, 11) * PS7; - const float PS9 = 0.5F * q3; - const float PS10 = P(10, 12) * PS9; - const float PS11 = 0.5F * dax - 0.5F * dax_b; - const float PS12 = 0.5F * day - 0.5F * day_b; - const float PS13 = 0.5F * daz - 0.5F * daz_b; - const float PS14 = P(0, 10) - P(1, 10) * PS11 + P(10, 10) * PS6 - P(2, 10) * PS12 - P(3, 10) * PS13 + PS10 + PS8; - const float PS15 = P(10, 11) * PS6; - const float PS16 = P(11, 12) * PS9; - const float PS17 = P(0, 11) - P(1, 11) * PS11 + P(11, 11) * PS7 - P(2, 11) * PS12 - P(3, 11) * PS13 + PS15 + PS16; - const float PS18 = P(10, 12) * PS6; - const float PS19 = P(11, 12) * PS7; - const float PS20 = P(0, 12) - P(1, 12) * PS11 + P(12, 12) * PS9 - P(2, 12) * PS12 - P(3, 12) * PS13 + PS18 + PS19; - const float PS21 = P(1, 2) * PS12; - const float PS22 = -P(1, 3) * PS13; - const float PS23 = P(0, 1) - P(1, 1) * PS11 + P(1, 10) * PS6 + P(1, 11) * PS7 + P(1, 12) * PS9 - PS21 + PS22; - const float PS24 = -P(1, 2) * PS11; - const float PS25 = P(2, 3) * PS13; - const float PS26 = P(0, 2) + P(2, 10) * PS6 + P(2, 11) * PS7 + P(2, 12) * PS9 - P(2, 2) * PS12 + PS24 - PS25; - const float PS27 = P(1, 3) * PS11; - const float PS28 = -P(2, 3) * PS12; - const float PS29 = P(0, 3) + P(3, 10) * PS6 + P(3, 11) * PS7 + P(3, 12) * PS9 - P(3, 3) * PS13 - PS27 + PS28; - const float PS30 = P(0, 1) * PS11; - const float PS31 = P(0, 2) * PS12; - const float PS32 = P(0, 3) * PS13; - const float PS33 = P(0, 0) + P(0, 10) * PS6 + P(0, 11) * PS7 + P(0, 12) * PS9 - PS30 - PS31 - PS32; - const float PS34 = 0.5F * q0; - const float PS35 = q2 * q3; - const float PS36 = q0 * q1; - const float PS37 = q1 * q3; - const float PS38 = q0 * q2; - const float PS39 = q1 * q2; - const float PS40 = q0 * q3; - const float PS41 = 2 * PS2; - const float PS42 = 2 * PS4 - 1; - const float PS43 = PS41 + PS42; - const float PS44 = P(0, 13) - P(1, 13) * PS11 + P(10, 13) * PS6 + P(11, 13) * PS7 + P(12, 13) * PS9 - P(2, - 13) * PS12 - P(3, 13) * PS13; - const float PS45 = PS37 + PS38; - const float PS46 = P(0, 15) - P(1, 15) * PS11 + P(10, 15) * PS6 + P(11, 15) * PS7 + P(12, 15) * PS9 - P(2, - 15) * PS12 - P(3, 15) * PS13; - const float PS47 = 2 * PS46; - const float PS48 = dvy - dvy_b; - const float PS49 = PS48 * q0; - const float PS50 = dvz - dvz_b; - const float PS51 = PS50 * q1; - const float PS52 = dvx - dvx_b; - const float PS53 = PS52 * q3; - const float PS54 = PS49 - PS51 + 2 * PS53; - const float PS55 = 2 * PS29; - const float PS56 = -PS39 + PS40; - const float PS57 = P(0, 14) - P(1, 14) * PS11 + P(10, 14) * PS6 + P(11, 14) * PS7 + P(12, 14) * PS9 - P(2, - 14) * PS12 - P(3, 14) * PS13; - const float PS58 = 2 * PS57; - const float PS59 = PS48 * q2; - const float PS60 = PS50 * q3; - const float PS61 = PS59 + PS60; - const float PS62 = 2 * PS23; - const float PS63 = PS50 * q2; - const float PS64 = PS48 * q3; - const float PS65 = -PS64; - const float PS66 = PS63 + PS65; - const float PS67 = 2 * PS33; - const float PS68 = PS50 * q0; - const float PS69 = PS48 * q1; - const float PS70 = PS52 * q2; - const float PS71 = PS68 + PS69 - 2 * PS70; - const float PS72 = 2 * PS26; - const float PS73 = P(0, 4) - P(1, 4) * PS11 - P(2, 4) * PS12 - P(3, 4) * PS13 + P(4, 10) * PS6 + P(4, 11) * PS7 + P(4, - 12) * PS9; - const float PS74 = 2 * PS0; - const float PS75 = PS42 + PS74; - const float PS76 = PS39 + PS40; - const float PS77 = 2 * PS44; - const float PS78 = PS51 - PS53; - const float PS79 = -PS70; - const float PS80 = PS68 + 2 * PS69 + PS79; - const float PS81 = -PS35 + PS36; - const float PS82 = PS52 * q1; - const float PS83 = PS60 + PS82; - const float PS84 = PS52 * q0; - const float PS85 = PS63 - 2 * PS64 + PS84; - const float PS86 = P(0, 5) - P(1, 5) * PS11 - P(2, 5) * PS12 - P(3, 5) * PS13 + P(5, 10) * PS6 + P(5, 11) * PS7 + P(5, - 12) * PS9; - const float PS87 = PS41 + PS74 - 1; - const float PS88 = PS35 + PS36; - const float PS89 = 2 * PS63 + PS65 + PS84; - const float PS90 = -PS37 + PS38; - const float PS91 = PS59 + PS82; - const float PS92 = PS69 + PS79; - const float PS93 = PS49 - 2 * PS51 + PS53; - const float PS94 = P(0, 6) - P(1, 6) * PS11 - P(2, 6) * PS12 - P(3, 6) * PS13 + P(6, 10) * PS6 + P(6, 11) * PS7 + P(6, - 12) * PS9; - const float PS95 = (q0) * (q0); - const float PS96 = -P(10, 11) * PS34; - const float PS97 = P(0, 11) * PS11 + P(1, 11) + P(11, 11) * PS9 + P(2, 11) * PS13 - P(3, 11) * PS12 - PS19 + PS96; - const float PS98 = P(0, 2) * PS13; - const float PS99 = P(0, 3) * PS12; - const float PS100 = P(0, 0) * PS11 + P(0, 1) - P(0, 10) * PS34 + P(0, 11) * PS9 - P(0, 12) * PS7 + PS98 - PS99; - const float PS101 = P(0, 2) * PS11; - const float PS102 = P(1, 2) - P(2, 10) * PS34 + P(2, 11) * PS9 - P(2, 12) * PS7 + P(2, 2) * PS13 + PS101 + PS28; - const float PS103 = P(10, 11) * PS9; - const float PS104 = P(10, 12) * PS7; - const float PS105 = P(0, 10) * PS11 + P(1, 10) - P(10, 10) * PS34 + P(2, 10) * PS13 - P(3, 10) * PS12 + PS103 - PS104; - const float PS106 = -P(10, 12) * PS34; - const float PS107 = P(0, 12) * PS11 + P(1, 12) - P(12, 12) * PS7 + P(2, 12) * PS13 - P(3, 12) * PS12 + PS106 + PS16; - const float PS108 = P(0, 3) * PS11; - const float PS109 = P(1, 3) - P(3, 10) * PS34 + P(3, 11) * PS9 - P(3, 12) * PS7 - P(3, 3) * PS12 + PS108 + PS25; - const float PS110 = P(1, 2) * PS13; - const float PS111 = P(1, 3) * PS12; - const float PS112 = P(1, 1) - P(1, 10) * PS34 + P(1, 11) * PS9 - P(1, 12) * PS7 + PS110 - PS111 + PS30; - const float PS113 = P(0, 13) * PS11 + P(1, 13) - P(10, 13) * PS34 + P(11, 13) * PS9 - P(12, 13) * PS7 + P(2, - 13) * PS13 - P(3, 13) * PS12; - const float PS114 = P(0, 15) * PS11 + P(1, 15) - P(10, 15) * PS34 + P(11, 15) * PS9 - P(12, 15) * PS7 + P(2, - 15) * PS13 - P(3, 15) * PS12; - const float PS115 = 2 * PS114; - const float PS116 = 2 * PS109; - const float PS117 = P(0, 14) * PS11 + P(1, 14) - P(10, 14) * PS34 + P(11, 14) * PS9 - P(12, 14) * PS7 + P(2, - 14) * PS13 - P(3, 14) * PS12; - const float PS118 = 2 * PS117; - const float PS119 = 2 * PS112; - const float PS120 = 2 * PS100; - const float PS121 = 2 * PS102; - const float PS122 = P(0, 4) * PS11 + P(1, 4) + P(2, 4) * PS13 - P(3, 4) * PS12 - P(4, 10) * PS34 + P(4, 11) * PS9 - P(4, - 12) * PS7; - const float PS123 = 2 * PS113; - const float PS124 = P(0, 5) * PS11 + P(1, 5) + P(2, 5) * PS13 - P(3, 5) * PS12 - P(5, 10) * PS34 + P(5, 11) * PS9 - P(5, - 12) * PS7; - const float PS125 = P(0, 6) * PS11 + P(1, 6) + P(2, 6) * PS13 - P(3, 6) * PS12 - P(6, 10) * PS34 + P(6, 11) * PS9 - P(6, - 12) * PS7; - const float PS126 = -P(11, 12) * PS34; - const float PS127 = P(0, 12) * PS12 - P(1, 12) * PS13 + P(12, 12) * PS6 + P(2, 12) + P(3, 12) * PS11 - PS10 + PS126; - const float PS128 = P(2, 3) - P(3, 10) * PS9 - P(3, 11) * PS34 + P(3, 12) * PS6 + P(3, 3) * PS11 + PS22 + PS99; - const float PS129 = P(0, 1) * PS13; - const float PS130 = P(0, 0) * PS12 - P(0, 10) * PS9 - P(0, 11) * PS34 + P(0, 12) * PS6 + P(0, 2) + PS108 - PS129; - const float PS131 = P(11, 12) * PS6; - const float PS132 = P(0, 11) * PS12 - P(1, 11) * PS13 - P(11, 11) * PS34 + P(2, 11) + P(3, 11) * PS11 - PS103 + PS131; - const float PS133 = P(0, 10) * PS12 - P(1, 10) * PS13 - P(10, 10) * PS9 + P(2, 10) + P(3, 10) * PS11 + PS18 + PS96; - const float PS134 = P(0, 1) * PS12; - const float PS135 = -P(1, 1) * PS13 - P(1, 10) * PS9 - P(1, 11) * PS34 + P(1, 12) * PS6 + P(1, 2) + PS134 + PS27; - const float PS136 = P(2, 3) * PS11; - const float PS137 = -P(2, 10) * PS9 - P(2, 11) * PS34 + P(2, 12) * PS6 + P(2, 2) - PS110 + PS136 + PS31; - const float PS138 = P(0, 13) * PS12 - P(1, 13) * PS13 - P(10, 13) * PS9 - P(11, 13) * PS34 + P(12, 13) * PS6 + P(2, - 13) + P(3, 13) * PS11; - const float PS139 = P(0, 15) * PS12 - P(1, 15) * PS13 - P(10, 15) * PS9 - P(11, 15) * PS34 + P(12, 15) * PS6 + P(2, - 15) + P(3, 15) * PS11; - const float PS140 = 2 * PS139; - const float PS141 = 2 * PS128; - const float PS142 = P(0, 14) * PS12 - P(1, 14) * PS13 - P(10, 14) * PS9 - P(11, 14) * PS34 + P(12, 14) * PS6 + P(2, - 14) + P(3, 14) * PS11; - const float PS143 = 2 * PS142; - const float PS144 = 2 * PS135; - const float PS145 = 2 * PS130; - const float PS146 = 2 * PS137; - const float PS147 = P(0, 4) * PS12 - P(1, 4) * PS13 + P(2, 4) + P(3, 4) * PS11 - P(4, 10) * PS9 - P(4, 11) * PS34 + P(4, - 12) * PS6; - const float PS148 = 2 * PS138; - const float PS149 = P(0, 5) * PS12 - P(1, 5) * PS13 + P(2, 5) + P(3, 5) * PS11 - P(5, 10) * PS9 - P(5, 11) * PS34 + P(5, - 12) * PS6; - const float PS150 = P(0, 6) * PS12 - P(1, 6) * PS13 + P(2, 6) + P(3, 6) * PS11 - P(6, 10) * PS9 - P(6, 11) * PS34 + P(6, - 12) * PS6; - const float PS151 = P(0, 10) * PS13 + P(1, 10) * PS12 + P(10, 10) * PS7 - P(2, 10) * PS11 + P(3, 10) + PS106 - PS15; - const float PS152 = P(1, 1) * PS12 + P(1, 10) * PS7 - P(1, 11) * PS6 - P(1, 12) * PS34 + P(1, 3) + PS129 + PS24; - const float PS153 = P(0, 0) * PS13 + P(0, 10) * PS7 - P(0, 11) * PS6 - P(0, 12) * PS34 + P(0, 3) - PS101 + PS134; - const float PS154 = P(0, 12) * PS13 + P(1, 12) * PS12 - P(12, 12) * PS34 - P(2, 12) * PS11 + P(3, 12) + PS104 - PS131; - const float PS155 = P(0, 11) * PS13 + P(1, 11) * PS12 - P(11, 11) * PS6 - P(2, 11) * PS11 + P(3, 11) + PS126 + PS8; - const float PS156 = P(2, 10) * PS7 - P(2, 11) * PS6 - P(2, 12) * PS34 - P(2, 2) * PS11 + P(2, 3) + PS21 + PS98; - const float PS157 = P(3, 10) * PS7 - P(3, 11) * PS6 - P(3, 12) * PS34 + P(3, 3) + PS111 - PS136 + PS32; - const float PS158 = P(0, 13) * PS13 + P(1, 13) * PS12 + P(10, 13) * PS7 - P(11, 13) * PS6 - P(12, 13) * PS34 - P(2, - 13) * PS11 + P(3, 13); - const float PS159 = P(0, 15) * PS13 + P(1, 15) * PS12 + P(10, 15) * PS7 - P(11, 15) * PS6 - P(12, 15) * PS34 - P(2, - 15) * PS11 + P(3, 15); - const float PS160 = 2 * PS159; - const float PS161 = 2 * PS157; - const float PS162 = P(0, 14) * PS13 + P(1, 14) * PS12 + P(10, 14) * PS7 - P(11, 14) * PS6 - P(12, 14) * PS34 - P(2, - 14) * PS11 + P(3, 14); - const float PS163 = 2 * PS162; - const float PS164 = 2 * PS152; - const float PS165 = 2 * PS153; - const float PS166 = 2 * PS156; - const float PS167 = P(0, 4) * PS13 + P(1, 4) * PS12 - P(2, 4) * PS11 + P(3, 4) + P(4, 10) * PS7 - P(4, 11) * PS6 - P(4, - 12) * PS34; - const float PS168 = 2 * PS158; - const float PS169 = P(0, 5) * PS13 + P(1, 5) * PS12 - P(2, 5) * PS11 + P(3, 5) + P(5, 10) * PS7 - P(5, 11) * PS6 - P(5, - 12) * PS34; - const float PS170 = P(0, 6) * PS13 + P(1, 6) * PS12 - P(2, 6) * PS11 + P(3, 6) + P(6, 10) * PS7 - P(6, 11) * PS6 - P(6, - 12) * PS34; - const float PS171 = 2 * PS45; - const float PS172 = 2 * PS56; - const float PS173 = 2 * PS61; - const float PS174 = 2 * PS66; - const float PS175 = 2 * PS71; - const float PS176 = 2 * PS54; - const float PS177 = P(0, 13) * PS174 + P(1, 13) * PS173 + P(13, 13) * PS43 + P(13, 14) * PS172 - P(13, - 15) * PS171 + P(2, 13) * PS175 - P(3, 13) * PS176 + P(4, 13); - const float PS178 = P(0, 15) * PS174 + P(1, 15) * PS173 + P(13, 15) * PS43 + P(14, 15) * PS172 - P(15, - 15) * PS171 + P(2, 15) * PS175 - P(3, 15) * PS176 + P(4, 15); - const float PS179 = P(0, 3) * PS174 + P(1, 3) * PS173 + P(2, 3) * PS175 + P(3, 13) * PS43 + P(3, 14) * PS172 - P(3, - 15) * PS171 - P(3, 3) * PS176 + P(3, 4); - const float PS180 = P(0, 14) * PS174 + P(1, 14) * PS173 + P(13, 14) * PS43 + P(14, 14) * PS172 - P(14, - 15) * PS171 + P(2, 14) * PS175 - P(3, 14) * PS176 + P(4, 14); - const float PS181 = P(0, 1) * PS174 + P(1, 1) * PS173 + P(1, 13) * PS43 + P(1, 14) * PS172 - P(1, 15) * PS171 + P(1, - 2) * PS175 - P(1, 3) * PS176 + P(1, 4); - const float PS182 = P(0, 0) * PS174 + P(0, 1) * PS173 + P(0, 13) * PS43 + P(0, 14) * PS172 - P(0, 15) * PS171 + P(0, - 2) * PS175 - P(0, 3) * PS176 + P(0, 4); - const float PS183 = P(0, 2) * PS174 + P(1, 2) * PS173 + P(2, 13) * PS43 + P(2, 14) * PS172 - P(2, 15) * PS171 + P(2, - 2) * PS175 - P(2, 3) * PS176 + P(2, 4); - const float PS184 = 4 * dvyVar; - const float PS185 = 4 * dvzVar; - const float PS186 = P(0, 4) * PS174 + P(1, 4) * PS173 + P(2, 4) * PS175 - P(3, 4) * PS176 + P(4, 13) * PS43 + P(4, - 14) * PS172 - P(4, 15) * PS171 + P(4, 4); - const float PS187 = 2 * PS177; - const float PS188 = 2 * PS182; - const float PS189 = 2 * PS181; - const float PS190 = 2 * PS81; - const float PS191 = 2 * PS183; - const float PS192 = 2 * PS179; - const float PS193 = 2 * PS76; - const float PS194 = PS43 * dvxVar; - const float PS195 = PS75 * dvyVar; - const float PS196 = P(0, 5) * PS174 + P(1, 5) * PS173 + P(2, 5) * PS175 - P(3, 5) * PS176 + P(4, 5) + P(5, - 13) * PS43 + P(5, 14) * PS172 - P(5, 15) * PS171; - const float PS197 = 2 * PS88; - const float PS198 = PS87 * dvzVar; - const float PS199 = 2 * PS90; - const float PS200 = P(0, 6) * PS174 + P(1, 6) * PS173 + P(2, 6) * PS175 - P(3, 6) * PS176 + P(4, 6) + P(6, - 13) * PS43 + P(6, 14) * PS172 - P(6, 15) * PS171; - const float PS201 = 2 * PS83; - const float PS202 = 2 * PS78; - const float PS203 = 2 * PS85; - const float PS204 = 2 * PS80; - const float PS205 = -P(0, 14) * PS202 - P(1, 14) * PS204 - P(13, 14) * PS193 + P(14, 14) * PS75 + P(14, - 15) * PS190 + P(2, 14) * PS201 + P(3, 14) * PS203 + P(5, 14); - const float PS206 = -P(0, 13) * PS202 - P(1, 13) * PS204 - P(13, 13) * PS193 + P(13, 14) * PS75 + P(13, - 15) * PS190 + P(2, 13) * PS201 + P(3, 13) * PS203 + P(5, 13); - const float PS207 = -P(0, 0) * PS202 - P(0, 1) * PS204 - P(0, 13) * PS193 + P(0, 14) * PS75 + P(0, 15) * PS190 + P(0, - 2) * PS201 + P(0, 3) * PS203 + P(0, 5); - const float PS208 = -P(0, 1) * PS202 - P(1, 1) * PS204 - P(1, 13) * PS193 + P(1, 14) * PS75 + P(1, 15) * PS190 + P(1, - 2) * PS201 + P(1, 3) * PS203 + P(1, 5); - const float PS209 = -P(0, 15) * PS202 - P(1, 15) * PS204 - P(13, 15) * PS193 + P(14, 15) * PS75 + P(15, - 15) * PS190 + P(2, 15) * PS201 + P(3, 15) * PS203 + P(5, 15); - const float PS210 = -P(0, 2) * PS202 - P(1, 2) * PS204 - P(2, 13) * PS193 + P(2, 14) * PS75 + P(2, 15) * PS190 + P(2, - 2) * PS201 + P(2, 3) * PS203 + P(2, 5); - const float PS211 = -P(0, 3) * PS202 - P(1, 3) * PS204 + P(2, 3) * PS201 - P(3, 13) * PS193 + P(3, 14) * PS75 + P(3, - 15) * PS190 + P(3, 3) * PS203 + P(3, 5); - const float PS212 = 4 * dvxVar; - const float PS213 = -P(0, 5) * PS202 - P(1, 5) * PS204 + P(2, 5) * PS201 + P(3, 5) * PS203 - P(5, 13) * PS193 + P(5, - 14) * PS75 + P(5, 15) * PS190 + P(5, 5); - const float PS214 = 2 * PS89; - const float PS215 = 2 * PS91; - const float PS216 = 2 * PS92; - const float PS217 = 2 * PS93; - const float PS218 = -P(0, 6) * PS202 - P(1, 6) * PS204 + P(2, 6) * PS201 + P(3, 6) * PS203 + P(5, 6) - P(6, - 13) * PS193 + P(6, 14) * PS75 + P(6, 15) * PS190; - const float PS219 = P(0, 15) * PS216 + P(1, 15) * PS217 + P(13, 15) * PS199 - P(14, 15) * PS197 + P(15, - 15) * PS87 - P(2, 15) * PS214 + P(3, 15) * PS215 + P(6, 15); - const float PS220 = P(0, 14) * PS216 + P(1, 14) * PS217 + P(13, 14) * PS199 - P(14, 14) * PS197 + P(14, - 15) * PS87 - P(2, 14) * PS214 + P(3, 14) * PS215 + P(6, 14); - const float PS221 = P(0, 13) * PS216 + P(1, 13) * PS217 + P(13, 13) * PS199 - P(13, 14) * PS197 + P(13, - 15) * PS87 - P(2, 13) * PS214 + P(3, 13) * PS215 + P(6, 13); - const float PS222 = P(0, 6) * PS216 + P(1, 6) * PS217 - P(2, 6) * PS214 + P(3, 6) * PS215 + P(6, 13) * PS199 - P(6, - 14) * PS197 + P(6, 15) * PS87 + P(6, 6); - - - nextP(0, 0) = PS0 * PS1 - PS11 * PS23 - PS12 * PS26 - PS13 * PS29 + PS14 * PS6 + PS17 * PS7 + PS2 * PS3 + PS20 * PS9 + - PS33 + PS4 * PS5; - nextP(0, 1) = -PS1 * PS36 + PS11 * PS33 - PS12 * PS29 + PS13 * PS26 - PS14 * PS34 + PS17 * PS9 - PS20 * PS7 + PS23 + PS3 - * PS35 - PS35 * PS5; - nextP(1, 1) = PS1 * PS95 + PS100 * PS11 + PS102 * PS13 - PS105 * PS34 - PS107 * PS7 - PS109 * PS12 + PS112 + PS2 * PS5 + - PS3 * PS4 + PS9 * PS97; - nextP(0, 2) = -PS1 * PS37 + PS11 * PS29 + PS12 * PS33 - PS13 * PS23 - PS14 * PS9 - PS17 * PS34 + PS20 * PS6 + PS26 - PS3 - * PS38 + PS37 * PS5; - nextP(1, 2) = PS1 * PS40 + PS100 * PS12 + PS102 - PS105 * PS9 + PS107 * PS6 + PS109 * PS11 - PS112 * PS13 - PS3 * PS40 - - PS34 * PS97 - PS39 * PS5; - nextP(2, 2) = PS0 * PS5 + PS1 * PS4 + PS11 * PS128 + PS12 * PS130 + PS127 * PS6 - PS13 * PS135 - PS132 * PS34 - PS133 * - PS9 + PS137 + PS3 * PS95; - nextP(0, 3) = PS1 * PS39 - PS11 * PS26 + PS12 * PS23 + PS13 * PS33 + PS14 * PS7 - PS17 * PS6 - PS20 * PS34 + PS29 - PS3 - * PS39 - PS40 * PS5; - nextP(1, 3) = -PS1 * PS38 + PS100 * PS13 - PS102 * PS11 + PS105 * PS7 - PS107 * PS34 + PS109 + PS112 * PS12 - PS3 * PS37 - + PS38 * PS5 - PS6 * PS97; - nextP(2, 3) = -PS1 * PS35 - PS11 * PS137 + PS12 * PS135 - PS127 * PS34 + PS128 + PS13 * PS130 - PS132 * PS6 + PS133 * - PS7 + PS3 * PS36 - PS36 * PS5; - nextP(3, 3) = PS0 * PS3 + PS1 * PS2 - PS11 * PS156 + PS12 * PS152 + PS13 * PS153 + PS151 * PS7 - PS154 * PS34 - PS155 * - PS6 + PS157 + PS5 * PS95; - nextP(0, 4) = PS43 * PS44 - PS45 * PS47 - PS54 * PS55 + PS56 * PS58 + PS61 * PS62 + PS66 * PS67 + PS71 * PS72 + PS73; - nextP(1, 4) = PS113 * PS43 - PS115 * PS45 - PS116 * PS54 + PS118 * PS56 + PS119 * PS61 + PS120 * PS66 + PS121 * PS71 + - PS122; - nextP(2, 4) = PS138 * PS43 - PS140 * PS45 - PS141 * PS54 + PS143 * PS56 + PS144 * PS61 + PS145 * PS66 + PS146 * PS71 + - PS147; - nextP(3, 4) = PS158 * PS43 - PS160 * PS45 - PS161 * PS54 + PS163 * PS56 + PS164 * PS61 + PS165 * PS66 + PS166 * PS71 + - PS167; - nextP(4, 4) = -PS171 * PS178 + PS172 * PS180 + PS173 * PS181 + PS174 * PS182 + PS175 * PS183 - PS176 * PS179 + PS177 * - PS43 + PS184 * (PS56) * (PS56) + PS185 * (PS45) * (PS45) + PS186 + (PS43) * (PS43) * dvxVar; - nextP(0, 5) = PS47 * PS81 + PS55 * PS85 + PS57 * PS75 - PS62 * PS80 - PS67 * PS78 + PS72 * PS83 - PS76 * PS77 + PS86; - nextP(1, 5) = PS115 * PS81 + PS116 * PS85 + PS117 * PS75 - PS119 * PS80 - PS120 * PS78 + PS121 * PS83 - PS123 * PS76 + - PS124; - nextP(2, 5) = PS140 * PS81 + PS141 * PS85 + PS142 * PS75 - PS144 * PS80 - PS145 * PS78 + PS146 * PS83 - PS148 * PS76 + - PS149; - nextP(3, 5) = PS160 * PS81 + PS161 * PS85 + PS162 * PS75 - PS164 * PS80 - PS165 * PS78 + PS166 * PS83 - PS168 * PS76 + - PS169; - nextP(4, 5) = PS172 * PS195 + PS178 * PS190 + PS180 * PS75 - PS185 * PS45 * PS81 - PS187 * PS76 - PS188 * PS78 - PS189 * - PS80 + PS191 * PS83 + PS192 * PS85 - PS193 * PS194 + PS196; - nextP(5, 5) = PS185 * (PS81) * (PS81) + PS190 * PS209 - PS193 * PS206 + PS201 * PS210 - PS202 * PS207 + PS203 * PS211 - - PS204 * PS208 + PS205 * PS75 + PS212 * (PS76) * (PS76) + PS213 + (PS75) * (PS75) * dvyVar; - nextP(0, 6) = PS46 * PS87 + PS55 * PS91 - PS58 * PS88 + PS62 * PS93 + PS67 * PS92 - PS72 * PS89 + PS77 * PS90 + PS94; - nextP(1, 6) = PS114 * PS87 + PS116 * PS91 - PS118 * PS88 + PS119 * PS93 + PS120 * PS92 - PS121 * PS89 + PS123 * PS90 + - PS125; - nextP(2, 6) = PS139 * PS87 + PS141 * PS91 - PS143 * PS88 + PS144 * PS93 + PS145 * PS92 - PS146 * PS89 + PS148 * PS90 + - PS150; - nextP(3, 6) = PS159 * PS87 + PS161 * PS91 - PS163 * PS88 + PS164 * PS93 + PS165 * PS92 - PS166 * PS89 + PS168 * PS90 + - PS170; - nextP(4, 6) = -PS171 * PS198 + PS178 * PS87 - PS180 * PS197 - PS184 * PS56 * PS88 + PS187 * PS90 + PS188 * PS92 + PS189 - * PS93 - PS191 * PS89 + PS192 * PS91 + PS194 * PS199 + PS200; - nextP(5, 6) = PS190 * PS198 - PS195 * PS197 - PS197 * PS205 + PS199 * PS206 + PS207 * PS216 + PS208 * PS217 + PS209 * - PS87 - PS210 * PS214 + PS211 * PS215 - PS212 * PS76 * PS90 + PS218; - nextP(6, 6) = PS184 * (PS88) * (PS88) - PS197 * PS220 + PS199 * PS221 + PS212 * (PS90) * (PS90) - PS214 * (P(0, - 2) * PS216 + P(1, 2) * PS217 + P(2, 13) * PS199 - P(2, 14) * PS197 + P(2, 15) * PS87 - P(2, 2) * PS214 + P(2, - 3) * PS215 + P(2, 6)) + PS215 * (P(0, 3) * PS216 + P(1, 3) * PS217 - P(2, 3) * PS214 + P(3, 13) * PS199 - P(3, - 14) * PS197 + P(3, 15) * PS87 + P(3, 3) * PS215 + P(3, 6)) + PS216 * (P(0, 0) * PS216 + P(0, 1) * PS217 + P(0, - 13) * PS199 - P(0, 14) * PS197 + P(0, 15) * PS87 - P(0, 2) * PS214 + P(0, 3) * PS215 + P(0, 6)) + PS217 * (P(0, - 1) * PS216 + P(1, 1) * PS217 + P(1, 13) * PS199 - P(1, 14) * PS197 + P(1, 15) * PS87 - P(1, 2) * PS214 + P(1, - 3) * PS215 + P(1, 6)) + PS219 * PS87 + PS222 + (PS87) * (PS87) * dvzVar; - nextP(0, 7) = P(0, 7) - P(1, 7) * PS11 - P(2, 7) * PS12 - P(3, 7) * PS13 + P(7, 10) * PS6 + P(7, 11) * PS7 + P(7, - 12) * PS9 + PS73 * dt; - nextP(1, 7) = P(0, 7) * PS11 + P(1, 7) + P(2, 7) * PS13 - P(3, 7) * PS12 - P(7, 10) * PS34 + P(7, 11) * PS9 - P(7, - 12) * PS7 + PS122 * dt; - nextP(2, 7) = P(0, 7) * PS12 - P(1, 7) * PS13 + P(2, 7) + P(3, 7) * PS11 - P(7, 10) * PS9 - P(7, 11) * PS34 + P(7, - 12) * PS6 + PS147 * dt; - nextP(3, 7) = P(0, 7) * PS13 + P(1, 7) * PS12 - P(2, 7) * PS11 + P(3, 7) + P(7, 10) * PS7 - P(7, 11) * PS6 - P(7, - 12) * PS34 + PS167 * dt; - nextP(4, 7) = P(0, 7) * PS174 + P(1, 7) * PS173 + P(2, 7) * PS175 - P(3, 7) * PS176 + P(4, 7) + P(7, 13) * PS43 + P(7, - 14) * PS172 - P(7, 15) * PS171 + PS186 * dt; - nextP(5, 7) = -P(0, 7) * PS202 - P(1, 7) * PS204 + P(2, 7) * PS201 + P(3, 7) * PS203 + P(5, 7) - P(7, 13) * PS193 + P(7, - 14) * PS75 + P(7, 15) * PS190 + dt * (-P(0, 4) * PS202 - P(1, 4) * PS204 + P(2, 4) * PS201 + P(3, 4) * PS203 - P(4, - 13) * PS193 + P(4, 14) * PS75 + P(4, 15) * PS190 + P(4, 5)); - nextP(6, 7) = P(0, 7) * PS216 + P(1, 7) * PS217 - P(2, 7) * PS214 + P(3, 7) * PS215 + P(6, 7) + P(7, 13) * PS199 - P(7, - 14) * PS197 + P(7, 15) * PS87 + dt * (P(0, 4) * PS216 + P(1, 4) * PS217 - P(2, 4) * PS214 + P(3, 4) * PS215 + P(4, - 13) * PS199 - P(4, 14) * PS197 + P(4, 15) * PS87 + P(4, 6)); - nextP(7, 7) = P(4, 7) * dt + P(7, 7) + dt * (P(4, 4) * dt + P(4, 7)); - nextP(0, 8) = P(0, 8) - P(1, 8) * PS11 - P(2, 8) * PS12 - P(3, 8) * PS13 + P(8, 10) * PS6 + P(8, 11) * PS7 + P(8, - 12) * PS9 + PS86 * dt; - nextP(1, 8) = P(0, 8) * PS11 + P(1, 8) + P(2, 8) * PS13 - P(3, 8) * PS12 - P(8, 10) * PS34 + P(8, 11) * PS9 - P(8, - 12) * PS7 + PS124 * dt; - nextP(2, 8) = P(0, 8) * PS12 - P(1, 8) * PS13 + P(2, 8) + P(3, 8) * PS11 - P(8, 10) * PS9 - P(8, 11) * PS34 + P(8, - 12) * PS6 + PS149 * dt; - nextP(3, 8) = P(0, 8) * PS13 + P(1, 8) * PS12 - P(2, 8) * PS11 + P(3, 8) + P(8, 10) * PS7 - P(8, 11) * PS6 - P(8, - 12) * PS34 + PS169 * dt; - nextP(4, 8) = P(0, 8) * PS174 + P(1, 8) * PS173 + P(2, 8) * PS175 - P(3, 8) * PS176 + P(4, 8) + P(8, 13) * PS43 + P(8, - 14) * PS172 - P(8, 15) * PS171 + PS196 * dt; - nextP(5, 8) = -P(0, 8) * PS202 - P(1, 8) * PS204 + P(2, 8) * PS201 + P(3, 8) * PS203 + P(5, 8) - P(8, 13) * PS193 + P(8, - 14) * PS75 + P(8, 15) * PS190 + PS213 * dt; - nextP(6, 8) = P(0, 8) * PS216 + P(1, 8) * PS217 - P(2, 8) * PS214 + P(3, 8) * PS215 + P(6, 8) + P(8, 13) * PS199 - P(8, - 14) * PS197 + P(8, 15) * PS87 + dt * (P(0, 5) * PS216 + P(1, 5) * PS217 - P(2, 5) * PS214 + P(3, 5) * PS215 + P(5, - 13) * PS199 - P(5, 14) * PS197 + P(5, 15) * PS87 + P(5, 6)); - nextP(7, 8) = P(4, 8) * dt + P(7, 8) + dt * (P(4, 5) * dt + P(5, 7)); - nextP(8, 8) = P(5, 8) * dt + P(8, 8) + dt * (P(5, 5) * dt + P(5, 8)); - nextP(0, 9) = P(0, 9) - P(1, 9) * PS11 - P(2, 9) * PS12 - P(3, 9) * PS13 + P(9, 10) * PS6 + P(9, 11) * PS7 + P(9, - 12) * PS9 + PS94 * dt; - nextP(1, 9) = P(0, 9) * PS11 + P(1, 9) + P(2, 9) * PS13 - P(3, 9) * PS12 - P(9, 10) * PS34 + P(9, 11) * PS9 - P(9, - 12) * PS7 + PS125 * dt; - nextP(2, 9) = P(0, 9) * PS12 - P(1, 9) * PS13 + P(2, 9) + P(3, 9) * PS11 - P(9, 10) * PS9 - P(9, 11) * PS34 + P(9, - 12) * PS6 + PS150 * dt; - nextP(3, 9) = P(0, 9) * PS13 + P(1, 9) * PS12 - P(2, 9) * PS11 + P(3, 9) + P(9, 10) * PS7 - P(9, 11) * PS6 - P(9, - 12) * PS34 + PS170 * dt; - nextP(4, 9) = P(0, 9) * PS174 + P(1, 9) * PS173 + P(2, 9) * PS175 - P(3, 9) * PS176 + P(4, 9) + P(9, 13) * PS43 + P(9, - 14) * PS172 - P(9, 15) * PS171 + PS200 * dt; - nextP(5, 9) = -P(0, 9) * PS202 - P(1, 9) * PS204 + P(2, 9) * PS201 + P(3, 9) * PS203 + P(5, 9) - P(9, 13) * PS193 + P(9, - 14) * PS75 + P(9, 15) * PS190 + PS218 * dt; - nextP(6, 9) = P(0, 9) * PS216 + P(1, 9) * PS217 - P(2, 9) * PS214 + P(3, 9) * PS215 + P(6, 9) + P(9, 13) * PS199 - P(9, - 14) * PS197 + P(9, 15) * PS87 + PS222 * dt; - nextP(7, 9) = P(4, 9) * dt + P(7, 9) + dt * (P(4, 6) * dt + P(6, 7)); - nextP(8, 9) = P(5, 9) * dt + P(8, 9) + dt * (P(5, 6) * dt + P(6, 8)); - nextP(9, 9) = P(6, 9) * dt + P(9, 9) + dt * (P(6, 6) * dt + P(6, 9)); - nextP(0, 10) = PS14; - nextP(1, 10) = PS105; - nextP(2, 10) = PS133; - nextP(3, 10) = PS151; - nextP(4, 10) = P(0, 10) * PS174 + P(1, 10) * PS173 + P(10, 13) * PS43 + P(10, 14) * PS172 - P(10, 15) * PS171 + P(2, - 10) * PS175 - P(3, 10) * PS176 + P(4, 10); - nextP(5, 10) = -P(0, 10) * PS202 - P(1, 10) * PS204 - P(10, 13) * PS193 + P(10, 14) * PS75 + P(10, 15) * PS190 + P(2, - 10) * PS201 + P(3, 10) * PS203 + P(5, 10); - nextP(6, 10) = P(0, 10) * PS216 + P(1, 10) * PS217 + P(10, 13) * PS199 - P(10, 14) * PS197 + P(10, 15) * PS87 - P(2, - 10) * PS214 + P(3, 10) * PS215 + P(6, 10); - nextP(7, 10) = P(4, 10) * dt + P(7, 10); - nextP(8, 10) = P(5, 10) * dt + P(8, 10); - nextP(9, 10) = P(6, 10) * dt + P(9, 10); - nextP(10, 10) = P(10, 10); - nextP(0, 11) = PS17; - nextP(1, 11) = PS97; - nextP(2, 11) = PS132; - nextP(3, 11) = PS155; - nextP(4, 11) = P(0, 11) * PS174 + P(1, 11) * PS173 + P(11, 13) * PS43 + P(11, 14) * PS172 - P(11, 15) * PS171 + P(2, - 11) * PS175 - P(3, 11) * PS176 + P(4, 11); - nextP(5, 11) = -P(0, 11) * PS202 - P(1, 11) * PS204 - P(11, 13) * PS193 + P(11, 14) * PS75 + P(11, 15) * PS190 + P(2, - 11) * PS201 + P(3, 11) * PS203 + P(5, 11); - nextP(6, 11) = P(0, 11) * PS216 + P(1, 11) * PS217 + P(11, 13) * PS199 - P(11, 14) * PS197 + P(11, 15) * PS87 - P(2, - 11) * PS214 + P(3, 11) * PS215 + P(6, 11); - nextP(7, 11) = P(4, 11) * dt + P(7, 11); - nextP(8, 11) = P(5, 11) * dt + P(8, 11); - nextP(9, 11) = P(6, 11) * dt + P(9, 11); - nextP(10, 11) = P(10, 11); - nextP(11, 11) = P(11, 11); - nextP(0, 12) = PS20; - nextP(1, 12) = PS107; - nextP(2, 12) = PS127; - nextP(3, 12) = PS154; - nextP(4, 12) = P(0, 12) * PS174 + P(1, 12) * PS173 + P(12, 13) * PS43 + P(12, 14) * PS172 - P(12, 15) * PS171 + P(2, - 12) * PS175 - P(3, 12) * PS176 + P(4, 12); - nextP(5, 12) = -P(0, 12) * PS202 - P(1, 12) * PS204 - P(12, 13) * PS193 + P(12, 14) * PS75 + P(12, 15) * PS190 + P(2, - 12) * PS201 + P(3, 12) * PS203 + P(5, 12); - nextP(6, 12) = P(0, 12) * PS216 + P(1, 12) * PS217 + P(12, 13) * PS199 - P(12, 14) * PS197 + P(12, 15) * PS87 - P(2, - 12) * PS214 + P(3, 12) * PS215 + P(6, 12); - nextP(7, 12) = P(4, 12) * dt + P(7, 12); - nextP(8, 12) = P(5, 12) * dt + P(8, 12); - nextP(9, 12) = P(6, 12) * dt + P(9, 12); - nextP(10, 12) = P(10, 12); - nextP(11, 12) = P(11, 12); - nextP(12, 12) = P(12, 12); - nextP(0, 13) = PS44; - nextP(1, 13) = PS113; - nextP(2, 13) = PS138; - nextP(3, 13) = PS158; - nextP(4, 13) = PS177; - nextP(5, 13) = PS206; - nextP(6, 13) = PS221; - nextP(7, 13) = P(4, 13) * dt + P(7, 13); - nextP(8, 13) = P(5, 13) * dt + P(8, 13); - nextP(9, 13) = P(6, 13) * dt + P(9, 13); - nextP(10, 13) = P(10, 13); - nextP(11, 13) = P(11, 13); - nextP(12, 13) = P(12, 13); - nextP(13, 13) = P(13, 13); - nextP(0, 14) = PS57; - nextP(1, 14) = PS117; - nextP(2, 14) = PS142; - nextP(3, 14) = PS162; - nextP(4, 14) = PS180; - nextP(5, 14) = PS205; - nextP(6, 14) = PS220; - nextP(7, 14) = P(4, 14) * dt + P(7, 14); - nextP(8, 14) = P(5, 14) * dt + P(8, 14); - nextP(9, 14) = P(6, 14) * dt + P(9, 14); - nextP(10, 14) = P(10, 14); - nextP(11, 14) = P(11, 14); - nextP(12, 14) = P(12, 14); - nextP(13, 14) = P(13, 14); - nextP(14, 14) = P(14, 14); - nextP(0, 15) = PS46; - nextP(1, 15) = PS114; - nextP(2, 15) = PS139; - nextP(3, 15) = PS159; - nextP(4, 15) = PS178; - nextP(5, 15) = PS209; - nextP(6, 15) = PS219; - nextP(7, 15) = P(4, 15) * dt + P(7, 15); - nextP(8, 15) = P(5, 15) * dt + P(8, 15); - nextP(9, 15) = P(6, 15) * dt + P(9, 15); - nextP(10, 15) = P(10, 15); - nextP(11, 15) = P(11, 15); - nextP(12, 15) = P(12, 15); - nextP(13, 15) = P(13, 15); - nextP(14, 15) = P(14, 15); - nextP(15, 15) = P(15, 15); - nextP(0, 16) = P(0, 16) - P(1, 16) * PS11 + P(10, 16) * PS6 + P(11, 16) * PS7 + P(12, 16) * PS9 - P(2, 16) * PS12 - P(3, - 16) * PS13; - nextP(1, 16) = P(0, 16) * PS11 + P(1, 16) - P(10, 16) * PS34 + P(11, 16) * PS9 - P(12, 16) * PS7 + P(2, - 16) * PS13 - P(3, 16) * PS12; - nextP(2, 16) = P(0, 16) * PS12 - P(1, 16) * PS13 - P(10, 16) * PS9 - P(11, 16) * PS34 + P(12, 16) * PS6 + P(2, - 16) + P(3, 16) * PS11; - nextP(3, 16) = P(0, 16) * PS13 + P(1, 16) * PS12 + P(10, 16) * PS7 - P(11, 16) * PS6 - P(12, 16) * PS34 - P(2, - 16) * PS11 + P(3, 16); - nextP(4, 16) = P(0, 16) * PS174 + P(1, 16) * PS173 + P(13, 16) * PS43 + P(14, 16) * PS172 - P(15, 16) * PS171 + P(2, - 16) * PS175 - P(3, 16) * PS176 + P(4, 16); - nextP(5, 16) = -P(0, 16) * PS202 - P(1, 16) * PS204 - P(13, 16) * PS193 + P(14, 16) * PS75 + P(15, 16) * PS190 + P(2, - 16) * PS201 + P(3, 16) * PS203 + P(5, 16); - nextP(6, 16) = P(0, 16) * PS216 + P(1, 16) * PS217 + P(13, 16) * PS199 - P(14, 16) * PS197 + P(15, 16) * PS87 - P(2, - 16) * PS214 + P(3, 16) * PS215 + P(6, 16); - nextP(7, 16) = P(4, 16) * dt + P(7, 16); - nextP(8, 16) = P(5, 16) * dt + P(8, 16); - nextP(9, 16) = P(6, 16) * dt + P(9, 16); - nextP(10, 16) = P(10, 16); - nextP(11, 16) = P(11, 16); - nextP(12, 16) = P(12, 16); - nextP(13, 16) = P(13, 16); - nextP(14, 16) = P(14, 16); - nextP(15, 16) = P(15, 16); - nextP(16, 16) = P(16, 16); - nextP(0, 17) = P(0, 17) - P(1, 17) * PS11 + P(10, 17) * PS6 + P(11, 17) * PS7 + P(12, 17) * PS9 - P(2, 17) * PS12 - P(3, - 17) * PS13; - nextP(1, 17) = P(0, 17) * PS11 + P(1, 17) - P(10, 17) * PS34 + P(11, 17) * PS9 - P(12, 17) * PS7 + P(2, - 17) * PS13 - P(3, 17) * PS12; - nextP(2, 17) = P(0, 17) * PS12 - P(1, 17) * PS13 - P(10, 17) * PS9 - P(11, 17) * PS34 + P(12, 17) * PS6 + P(2, - 17) + P(3, 17) * PS11; - nextP(3, 17) = P(0, 17) * PS13 + P(1, 17) * PS12 + P(10, 17) * PS7 - P(11, 17) * PS6 - P(12, 17) * PS34 - P(2, - 17) * PS11 + P(3, 17); - nextP(4, 17) = P(0, 17) * PS174 + P(1, 17) * PS173 + P(13, 17) * PS43 + P(14, 17) * PS172 - P(15, 17) * PS171 + P(2, - 17) * PS175 - P(3, 17) * PS176 + P(4, 17); - nextP(5, 17) = -P(0, 17) * PS202 - P(1, 17) * PS204 - P(13, 17) * PS193 + P(14, 17) * PS75 + P(15, 17) * PS190 + P(2, - 17) * PS201 + P(3, 17) * PS203 + P(5, 17); - nextP(6, 17) = P(0, 17) * PS216 + P(1, 17) * PS217 + P(13, 17) * PS199 - P(14, 17) * PS197 + P(15, 17) * PS87 - P(2, - 17) * PS214 + P(3, 17) * PS215 + P(6, 17); - nextP(7, 17) = P(4, 17) * dt + P(7, 17); - nextP(8, 17) = P(5, 17) * dt + P(8, 17); - nextP(9, 17) = P(6, 17) * dt + P(9, 17); - nextP(10, 17) = P(10, 17); - nextP(11, 17) = P(11, 17); - nextP(12, 17) = P(12, 17); - nextP(13, 17) = P(13, 17); - nextP(14, 17) = P(14, 17); - nextP(15, 17) = P(15, 17); - nextP(16, 17) = P(16, 17); - nextP(17, 17) = P(17, 17); - nextP(0, 18) = P(0, 18) - P(1, 18) * PS11 + P(10, 18) * PS6 + P(11, 18) * PS7 + P(12, 18) * PS9 - P(2, 18) * PS12 - P(3, - 18) * PS13; - nextP(1, 18) = P(0, 18) * PS11 + P(1, 18) - P(10, 18) * PS34 + P(11, 18) * PS9 - P(12, 18) * PS7 + P(2, - 18) * PS13 - P(3, 18) * PS12; - nextP(2, 18) = P(0, 18) * PS12 - P(1, 18) * PS13 - P(10, 18) * PS9 - P(11, 18) * PS34 + P(12, 18) * PS6 + P(2, - 18) + P(3, 18) * PS11; - nextP(3, 18) = P(0, 18) * PS13 + P(1, 18) * PS12 + P(10, 18) * PS7 - P(11, 18) * PS6 - P(12, 18) * PS34 - P(2, - 18) * PS11 + P(3, 18); - nextP(4, 18) = P(0, 18) * PS174 + P(1, 18) * PS173 + P(13, 18) * PS43 + P(14, 18) * PS172 - P(15, 18) * PS171 + P(2, - 18) * PS175 - P(3, 18) * PS176 + P(4, 18); - nextP(5, 18) = -P(0, 18) * PS202 - P(1, 18) * PS204 - P(13, 18) * PS193 + P(14, 18) * PS75 + P(15, 18) * PS190 + P(2, - 18) * PS201 + P(3, 18) * PS203 + P(5, 18); - nextP(6, 18) = P(0, 18) * PS216 + P(1, 18) * PS217 + P(13, 18) * PS199 - P(14, 18) * PS197 + P(15, 18) * PS87 - P(2, - 18) * PS214 + P(3, 18) * PS215 + P(6, 18); - nextP(7, 18) = P(4, 18) * dt + P(7, 18); - nextP(8, 18) = P(5, 18) * dt + P(8, 18); - nextP(9, 18) = P(6, 18) * dt + P(9, 18); - nextP(10, 18) = P(10, 18); - nextP(11, 18) = P(11, 18); - nextP(12, 18) = P(12, 18); - nextP(13, 18) = P(13, 18); - nextP(14, 18) = P(14, 18); - nextP(15, 18) = P(15, 18); - nextP(16, 18) = P(16, 18); - nextP(17, 18) = P(17, 18); - nextP(18, 18) = P(18, 18); - nextP(0, 19) = P(0, 19) - P(1, 19) * PS11 + P(10, 19) * PS6 + P(11, 19) * PS7 + P(12, 19) * PS9 - P(2, 19) * PS12 - P(3, - 19) * PS13; - nextP(1, 19) = P(0, 19) * PS11 + P(1, 19) - P(10, 19) * PS34 + P(11, 19) * PS9 - P(12, 19) * PS7 + P(2, - 19) * PS13 - P(3, 19) * PS12; - nextP(2, 19) = P(0, 19) * PS12 - P(1, 19) * PS13 - P(10, 19) * PS9 - P(11, 19) * PS34 + P(12, 19) * PS6 + P(2, - 19) + P(3, 19) * PS11; - nextP(3, 19) = P(0, 19) * PS13 + P(1, 19) * PS12 + P(10, 19) * PS7 - P(11, 19) * PS6 - P(12, 19) * PS34 - P(2, - 19) * PS11 + P(3, 19); - nextP(4, 19) = P(0, 19) * PS174 + P(1, 19) * PS173 + P(13, 19) * PS43 + P(14, 19) * PS172 - P(15, 19) * PS171 + P(2, - 19) * PS175 - P(3, 19) * PS176 + P(4, 19); - nextP(5, 19) = -P(0, 19) * PS202 - P(1, 19) * PS204 - P(13, 19) * PS193 + P(14, 19) * PS75 + P(15, 19) * PS190 + P(2, - 19) * PS201 + P(3, 19) * PS203 + P(5, 19); - nextP(6, 19) = P(0, 19) * PS216 + P(1, 19) * PS217 + P(13, 19) * PS199 - P(14, 19) * PS197 + P(15, 19) * PS87 - P(2, - 19) * PS214 + P(3, 19) * PS215 + P(6, 19); - nextP(7, 19) = P(4, 19) * dt + P(7, 19); - nextP(8, 19) = P(5, 19) * dt + P(8, 19); - nextP(9, 19) = P(6, 19) * dt + P(9, 19); - nextP(10, 19) = P(10, 19); - nextP(11, 19) = P(11, 19); - nextP(12, 19) = P(12, 19); - nextP(13, 19) = P(13, 19); - nextP(14, 19) = P(14, 19); - nextP(15, 19) = P(15, 19); - nextP(16, 19) = P(16, 19); - nextP(17, 19) = P(17, 19); - nextP(18, 19) = P(18, 19); - nextP(19, 19) = P(19, 19); - nextP(0, 20) = P(0, 20) - P(1, 20) * PS11 + P(10, 20) * PS6 + P(11, 20) * PS7 + P(12, 20) * PS9 - P(2, 20) * PS12 - P(3, - 20) * PS13; - nextP(1, 20) = P(0, 20) * PS11 + P(1, 20) - P(10, 20) * PS34 + P(11, 20) * PS9 - P(12, 20) * PS7 + P(2, - 20) * PS13 - P(3, 20) * PS12; - nextP(2, 20) = P(0, 20) * PS12 - P(1, 20) * PS13 - P(10, 20) * PS9 - P(11, 20) * PS34 + P(12, 20) * PS6 + P(2, - 20) + P(3, 20) * PS11; - nextP(3, 20) = P(0, 20) * PS13 + P(1, 20) * PS12 + P(10, 20) * PS7 - P(11, 20) * PS6 - P(12, 20) * PS34 - P(2, - 20) * PS11 + P(3, 20); - nextP(4, 20) = P(0, 20) * PS174 + P(1, 20) * PS173 + P(13, 20) * PS43 + P(14, 20) * PS172 - P(15, 20) * PS171 + P(2, - 20) * PS175 - P(3, 20) * PS176 + P(4, 20); - nextP(5, 20) = -P(0, 20) * PS202 - P(1, 20) * PS204 - P(13, 20) * PS193 + P(14, 20) * PS75 + P(15, 20) * PS190 + P(2, - 20) * PS201 + P(3, 20) * PS203 + P(5, 20); - nextP(6, 20) = P(0, 20) * PS216 + P(1, 20) * PS217 + P(13, 20) * PS199 - P(14, 20) * PS197 + P(15, 20) * PS87 - P(2, - 20) * PS214 + P(3, 20) * PS215 + P(6, 20); - nextP(7, 20) = P(4, 20) * dt + P(7, 20); - nextP(8, 20) = P(5, 20) * dt + P(8, 20); - nextP(9, 20) = P(6, 20) * dt + P(9, 20); - nextP(10, 20) = P(10, 20); - nextP(11, 20) = P(11, 20); - nextP(12, 20) = P(12, 20); - nextP(13, 20) = P(13, 20); - nextP(14, 20) = P(14, 20); - nextP(15, 20) = P(15, 20); - nextP(16, 20) = P(16, 20); - nextP(17, 20) = P(17, 20); - nextP(18, 20) = P(18, 20); - nextP(19, 20) = P(19, 20); - nextP(20, 20) = P(20, 20); - nextP(0, 21) = P(0, 21) - P(1, 21) * PS11 + P(10, 21) * PS6 + P(11, 21) * PS7 + P(12, 21) * PS9 - P(2, 21) * PS12 - P(3, - 21) * PS13; - nextP(1, 21) = P(0, 21) * PS11 + P(1, 21) - P(10, 21) * PS34 + P(11, 21) * PS9 - P(12, 21) * PS7 + P(2, - 21) * PS13 - P(3, 21) * PS12; - nextP(2, 21) = P(0, 21) * PS12 - P(1, 21) * PS13 - P(10, 21) * PS9 - P(11, 21) * PS34 + P(12, 21) * PS6 + P(2, - 21) + P(3, 21) * PS11; - nextP(3, 21) = P(0, 21) * PS13 + P(1, 21) * PS12 + P(10, 21) * PS7 - P(11, 21) * PS6 - P(12, 21) * PS34 - P(2, - 21) * PS11 + P(3, 21); - nextP(4, 21) = P(0, 21) * PS174 + P(1, 21) * PS173 + P(13, 21) * PS43 + P(14, 21) * PS172 - P(15, 21) * PS171 + P(2, - 21) * PS175 - P(3, 21) * PS176 + P(4, 21); - nextP(5, 21) = -P(0, 21) * PS202 - P(1, 21) * PS204 - P(13, 21) * PS193 + P(14, 21) * PS75 + P(15, 21) * PS190 + P(2, - 21) * PS201 + P(3, 21) * PS203 + P(5, 21); - nextP(6, 21) = P(0, 21) * PS216 + P(1, 21) * PS217 + P(13, 21) * PS199 - P(14, 21) * PS197 + P(15, 21) * PS87 - P(2, - 21) * PS214 + P(3, 21) * PS215 + P(6, 21); - nextP(7, 21) = P(4, 21) * dt + P(7, 21); - nextP(8, 21) = P(5, 21) * dt + P(8, 21); - nextP(9, 21) = P(6, 21) * dt + P(9, 21); - nextP(10, 21) = P(10, 21); - nextP(11, 21) = P(11, 21); - nextP(12, 21) = P(12, 21); - nextP(13, 21) = P(13, 21); - nextP(14, 21) = P(14, 21); - nextP(15, 21) = P(15, 21); - nextP(16, 21) = P(16, 21); - nextP(17, 21) = P(17, 21); - nextP(18, 21) = P(18, 21); - nextP(19, 21) = P(19, 21); - nextP(20, 21) = P(20, 21); - nextP(21, 21) = P(21, 21); - nextP(0, 22) = P(0, 22) - P(1, 22) * PS11 + P(10, 22) * PS6 + P(11, 22) * PS7 + P(12, 22) * PS9 - P(2, 22) * PS12 - P(3, - 22) * PS13; - nextP(1, 22) = P(0, 22) * PS11 + P(1, 22) - P(10, 22) * PS34 + P(11, 22) * PS9 - P(12, 22) * PS7 + P(2, - 22) * PS13 - P(3, 22) * PS12; - nextP(2, 22) = P(0, 22) * PS12 - P(1, 22) * PS13 - P(10, 22) * PS9 - P(11, 22) * PS34 + P(12, 22) * PS6 + P(2, - 22) + P(3, 22) * PS11; - nextP(3, 22) = P(0, 22) * PS13 + P(1, 22) * PS12 + P(10, 22) * PS7 - P(11, 22) * PS6 - P(12, 22) * PS34 - P(2, - 22) * PS11 + P(3, 22); - nextP(4, 22) = P(0, 22) * PS174 + P(1, 22) * PS173 + P(13, 22) * PS43 + P(14, 22) * PS172 - P(15, 22) * PS171 + P(2, - 22) * PS175 - P(3, 22) * PS176 + P(4, 22); - nextP(5, 22) = -P(0, 22) * PS202 - P(1, 22) * PS204 - P(13, 22) * PS193 + P(14, 22) * PS75 + P(15, 22) * PS190 + P(2, - 22) * PS201 + P(3, 22) * PS203 + P(5, 22); - nextP(6, 22) = P(0, 22) * PS216 + P(1, 22) * PS217 + P(13, 22) * PS199 - P(14, 22) * PS197 + P(15, 22) * PS87 - P(2, - 22) * PS214 + P(3, 22) * PS215 + P(6, 22); - nextP(7, 22) = P(4, 22) * dt + P(7, 22); - nextP(8, 22) = P(5, 22) * dt + P(8, 22); - nextP(9, 22) = P(6, 22) * dt + P(9, 22); - nextP(10, 22) = P(10, 22); - nextP(11, 22) = P(11, 22); - nextP(12, 22) = P(12, 22); - nextP(13, 22) = P(13, 22); - nextP(14, 22) = P(14, 22); - nextP(15, 22) = P(15, 22); - nextP(16, 22) = P(16, 22); - nextP(17, 22) = P(17, 22); - nextP(18, 22) = P(18, 22); - nextP(19, 22) = P(19, 22); - nextP(20, 22) = P(20, 22); - nextP(21, 22) = P(21, 22); - nextP(22, 22) = P(22, 22); - nextP(0, 23) = P(0, 23) - P(1, 23) * PS11 + P(10, 23) * PS6 + P(11, 23) * PS7 + P(12, 23) * PS9 - P(2, 23) * PS12 - P(3, - 23) * PS13; - nextP(1, 23) = P(0, 23) * PS11 + P(1, 23) - P(10, 23) * PS34 + P(11, 23) * PS9 - P(12, 23) * PS7 + P(2, - 23) * PS13 - P(3, 23) * PS12; - nextP(2, 23) = P(0, 23) * PS12 - P(1, 23) * PS13 - P(10, 23) * PS9 - P(11, 23) * PS34 + P(12, 23) * PS6 + P(2, - 23) + P(3, 23) * PS11; - nextP(3, 23) = P(0, 23) * PS13 + P(1, 23) * PS12 + P(10, 23) * PS7 - P(11, 23) * PS6 - P(12, 23) * PS34 - P(2, - 23) * PS11 + P(3, 23); - nextP(4, 23) = P(0, 23) * PS174 + P(1, 23) * PS173 + P(13, 23) * PS43 + P(14, 23) * PS172 - P(15, 23) * PS171 + P(2, - 23) * PS175 - P(3, 23) * PS176 + P(4, 23); - nextP(5, 23) = -P(0, 23) * PS202 - P(1, 23) * PS204 - P(13, 23) * PS193 + P(14, 23) * PS75 + P(15, 23) * PS190 + P(2, - 23) * PS201 + P(3, 23) * PS203 + P(5, 23); - nextP(6, 23) = P(0, 23) * PS216 + P(1, 23) * PS217 + P(13, 23) * PS199 - P(14, 23) * PS197 + P(15, 23) * PS87 - P(2, - 23) * PS214 + P(3, 23) * PS215 + P(6, 23); - nextP(7, 23) = P(4, 23) * dt + P(7, 23); - nextP(8, 23) = P(5, 23) * dt + P(8, 23); - nextP(9, 23) = P(6, 23) * dt + P(9, 23); - nextP(10, 23) = P(10, 23); - nextP(11, 23) = P(11, 23); - nextP(12, 23) = P(12, 23); - nextP(13, 23) = P(13, 23); - nextP(14, 23) = P(14, 23); - nextP(15, 23) = P(15, 23); - nextP(16, 23) = P(16, 23); - nextP(17, 23) = P(17, 23); - nextP(18, 23) = P(18, 23); - nextP(19, 23) = P(19, 23); - nextP(20, 23) = P(20, 23); - nextP(21, 23) = P(21, 23); - nextP(22, 23) = P(22, 23); - nextP(23, 23) = P(23, 23); - - // save output - for (int col = 0; col <= 23; col++) { - for (int row = 0; row <= col; row++) { - nextP_sympy(row, col) = nextP(row, col); - } - } - } - - { - Vector24f state_vector{}; - state_vector(0) = q0; - state_vector(1) = q1; - state_vector(2) = q2; - state_vector(3) = q3; - state_vector(10) = dax_b; - state_vector(11) = day_b; - state_vector(12) = daz_b; - state_vector(13) = dvx_b; - state_vector(14) = dvy_b; - state_vector(15) = dvz_b; - - const Vector3f d_vel(dvx, dvy, dvz); - const Vector3f d_ang(dax, day, daz); - - EXPECT_FLOAT_EQ(daxVar, dayVar); - EXPECT_FLOAT_EQ(daxVar, dazVar); - - const float d_ang_var = daxVar; // derivation assumes same variance on all gyro axes - const Vector3f d_vel_var(dvxVar, dvyVar, dvzVar); - - sym::PredictCovariance(state_vector, P, d_vel, d_vel_var, d_ang, d_ang_var, dt, &nextP_symforce); - } - - DiffRatioReport report = computeDiffRatioSquareMatrix24f(nextP_sympy, nextP_symforce); - EXPECT_LT(report.max_diff_fraction, 2e-5f) << "Max diff fraction = " << report.max_diff_fraction << - " location index = " << report.max_row << " sympy = " << report.max_v1 << " symforce = " << report.max_v2; -} diff --git a/src/modules/ekf2/test/test_EKF_externalVision.cpp b/src/modules/ekf2/test/test_EKF_externalVision.cpp index 3be67e2da7..aa8376dc6c 100644 --- a/src/modules/ekf2/test/test_EKF_externalVision.cpp +++ b/src/modules/ekf2/test/test_EKF_externalVision.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -61,9 +61,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 553a0af47c..0302849dd4 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index cfcc62d660..0bafab37f3 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -183,7 +183,6 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow) const float max_ground_distance = 50.f; _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); - _sensor_simulator.startFlow(); _ekf_wrapper.enableFlowFusion(); _ekf->set_in_air_status(true); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 9d0355fbfa..1669b12180 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -79,15 +79,51 @@ public: TEST_F(EkfGpsTest, gpsTimeout) { // GIVEN:EKF that fuses GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); - // WHEN: setting the PDOP to high + // WHEN: the number of satellites drops below the minimum _sensor_simulator._gps.setNumberOfSatellites(3); - // THEN: EKF should stop fusing GPS + // THEN: the GNSS fusion continues because it is the only source of position/velocity _sensor_simulator.runSeconds(20); - - // TODO: this is not happening as expected EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // BUT: if we have another velocity aiding source + const float max_flow_rate = 5.f; + const float min_ground_distance = 0.f; + const float max_ground_distance = 50.f; + _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); + + _sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest()); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + + _sensor_simulator._rng.setData(0.2, 100); + _sensor_simulator._rng.setLimits(0.1f, 9.f); + _sensor_simulator.startRangeFinder(); + _sensor_simulator.runSeconds(5); + + // THEN: the GNSS fusion stops + EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); +} + +TEST_F(EkfGpsTest, gpsFixLoss) +{ + // GIVEN:EKF that fuses GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // WHEN: the fix is loss + _sensor_simulator._gps.setFixType(0); + + // THEN: after dead-reconing for a couple of seconds, the local position gets invalidated + _sensor_simulator.runSeconds(5); + EXPECT_TRUE(_ekf->control_status_flags().inertial_dead_reckoning); + EXPECT_FALSE(_ekf->local_position_is_valid()); + + // The control logic takes a bit more time to deactivate the GNSS fusion completely + _sensor_simulator.runSeconds(5); + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); } TEST_F(EkfGpsTest, resetToGpsVelocity) diff --git a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp index f166a93647..781133dde2 100644 --- a/src/modules/ekf2/test/test_EKF_gps_yaw.cpp +++ b/src/modules/ekf2/test/test_EKF_gps_yaw.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -61,9 +61,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); @@ -304,9 +303,12 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir) _sensor_simulator.runSeconds(7.5); // THEN: after a few seconds, the fusion should stop and - // the estimator should fall back to mag fusion + // 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_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + 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()); } TEST_F(EkfGpsHeadingTest, stopOnGround) @@ -331,10 +333,10 @@ TEST_F(EkfGpsHeadingTest, stopOnGround) _ekf_wrapper.setMagFuseTypeNone(); // WHEN: running without yaw aiding - const matrix::Vector4f quat_variance_before = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_before = _ekf->getQuaternionVariance(); _sensor_simulator.runSeconds(20.0); - const matrix::Vector4f quat_variance_after = _ekf_wrapper.getQuaternionVariance(); + const matrix::Vector4f quat_variance_after = _ekf->getQuaternionVariance(); - // THEN: the yaw variance is constrained by fusing constant data - EXPECT_LT(quat_variance_after(3), quat_variance_before(3)); + // THEN: the yaw variance increases + EXPECT_GT(quat_variance_after(3), quat_variance_before(3)); } diff --git a/src/modules/ekf2/test/test_EKF_gyroscope.cpp b/src/modules/ekf2/test/test_EKF_gyroscope.cpp new file mode 100644 index 0000000000..8d3daac169 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_gyroscope.cpp @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Gyro-related tests (bias convergence, ...) + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" + + +class EkfGyroscopeTest : public ::testing::Test +{ +public: + + EkfGyroscopeTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + // Init, then manually set in air and at rest (default for a real vehicle) + _ekf->init(0); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } + + void testBias(const Vector3f &bias, float duration, const Vector3f &tolerance); +}; + +void EkfGyroscopeTest::testBias(const Vector3f &bias, float duration, const Vector3f &tolerance) +{ + _sensor_simulator._imu.setGyroData(bias); + _sensor_simulator.runSeconds(duration); + EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest); + + const Vector3f estimated_bias = _ekf->getGyroBias(); + + for (int i = 0; i < 3; i++) { + EXPECT_NEAR(estimated_bias(i), bias(i), tolerance(i)) << "index " << i; + } +} + +TEST_F(EkfGyroscopeTest, biasEstimateZero) +{ + testBias(Vector3f(), 10, Vector3f()); +} + +TEST_F(EkfGyroscopeTest, biasEstimatePositive) +{ + // The estimate should track a slowly changing bias + const float biases[4] = {0.001f, 0.002f, 0.0035f, 0.005f}; + Vector3f bias; + + for (int i = 0; i < 4; i ++) { + bias.setAll(biases[i]); + // The Z gyro bias takes more time to converge as the Z rotation variance is higher + testBias(bias, 30, Vector3f(0.0008f, 0.0008f, 0.004f)); + } +} + +TEST_F(EkfGyroscopeTest, biasEstimateNegative) +{ + const float biases[4] = {-0.001f, -0.002f, -0.0035, -0.005f}; + Vector3f bias; + + for (int i = 0; i < 4; i ++) { + bias.setAll(biases[i]); + testBias(bias, 30, Vector3f(0.0008f, 0.0008f, 0.004f)); + } +} diff --git a/src/modules/ekf2/test/test_EKF_imuSampling.cpp b/src/modules/ekf2/test/test_EKF_imuSampling.cpp index c037c2c44b..bd300c5385 100644 --- a/src/modules/ekf2/test/test_EKF_imuSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_imuSampling.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_initialization.cpp b/src/modules/ekf2/test/test_EKF_initialization.cpp index 71b09cc3e7..5dad09e2e8 100644 --- a/src/modules/ekf2/test/test_EKF_initialization.cpp +++ b/src/modules/ekf2/test/test_EKF_initialization.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 @@ -50,12 +50,15 @@ public: SensorSimulator _sensor_simulator; EkfWrapper _ekf_wrapper; - const float _init_tilt_period = 0.3f; // seconds + const float _init_tilt_period = 0.7f; // seconds - // GTests is calling this + // Setup the Ekf with synthetic measurements void SetUp() override { + // first init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); } // Use this method to clean up any memory, network etc. after each test @@ -66,28 +69,25 @@ public: void initializedOrienationIsMatchingGroundTruth(Quatf true_quaternion) { const Quatf quat_est = _ekf->getQuaternion(); - const float precision = 0.0002f; // TODO: this is only required for the pitch90 test to pass - EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion, precision)) + EXPECT_TRUE(matrix::isEqual(quat_est, true_quaternion)) << "quat est = " << quat_est(0) << ", " << quat_est(1) << ", " << quat_est(2) << ", " << quat_est(3) << "\nquat true = " << true_quaternion(0) << ", " << true_quaternion(1) << ", " << true_quaternion(2) << ", " << true_quaternion(3); } - void validStateAfterOrientationInitialization() + void quaternionVarianceBigEnoughAfterOrientationInitialization(float quat_variance_limit = 0.00001f) { - quaternionVarianceBigEnoughAfterOrientationInitialization(); - velocityAndPositionCloseToZero(); - velocityAndPositionVarianceBigEnoughAfterOrientationInitialization(); + const matrix::Vector4f quat_variance = _ekf->getQuaternionVariance(); + 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); + EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3): " << quat_variance(3); } - void quaternionVarianceBigEnoughAfterOrientationInitialization() + void yawVarianceBigEnoughAfterHeadingReset() { - const matrix::Vector4f quat_variance = _ekf_wrapper.getQuaternionVariance(); - const float quat_variance_limit = 0.0001f; - 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); - EXPECT_TRUE(quat_variance(3) > quat_variance_limit) << "quat_variance(3)" << quat_variance(3); + // The yaw variance is smaller than its reset value as we do not probe instantly after the reset + EXPECT_GT(sqrtf(_ekf->getYawVar()), _ekf_wrapper.getMagHeadingNoise() / 5.f); } void velocityAndPositionCloseToZero() @@ -101,32 +101,34 @@ public: << "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2); } - void velocityAndPositionVarianceBigEnoughAfterOrientationInitialization() + void velocityVarianceBigEnoughAfterOrientationInitialization(float vel_variance_limit) { - const Vector3f pos_var = _ekf->getPositionVariance(); const Vector3f vel_var = _ekf->getVelocityVariance(); - const float pos_variance_limit = 0.01f; // Fake fusion obs var when at rest - EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0)" << pos_var(0); - EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1)" << pos_var(1); - EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2)" << pos_var(2); + EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(0): " << vel_var(0); + EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1): " << vel_var(1); + EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2): " << vel_var(2); + } - const float vel_variance_limit = 0.0001f; // zero velocity update obs var when at rest - EXPECT_TRUE(vel_var(0) > vel_variance_limit) << "vel_var(0)" << vel_var(0); - EXPECT_TRUE(vel_var(1) > vel_variance_limit) << "vel_var(1)" << vel_var(1); - EXPECT_TRUE(vel_var(2) > vel_variance_limit) << "vel_var(2)" << vel_var(2); + void positionVarianceBigEnoughAfterOrientationInitialization(float pos_variance_limit) + { + const Vector3f pos_var = _ekf->getPositionVariance(); + + EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0): " << pos_var(0); + EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1): " << pos_var(1); + EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2): " << pos_var(2); } void learningCorrectAccelBias() { const Dcmf R_to_earth = Dcmf(_ekf->getQuaternion()); - const Vector3f dvel_bias_var = _ekf_wrapper.getDeltaVelBiasVariance(); + const Vector3f accel_bias_var = _ekf->getAccelBiasVariance(); const Vector3f accel_bias = _ekf->getAccelBias(); for (int i = 0; i < 3; i++) { if (fabsf(R_to_earth(2, i)) > 0.8f) { // Highly observable, the variance decreases - EXPECT_LT(dvel_bias_var(i), 4.0e-6f) << "axis " << i; + EXPECT_LT(accel_bias_var(i), 4.0e-2f) << "axis " << i; } EXPECT_LT(accel_bias(i), 4.0e-6f) << "axis " << i; @@ -144,8 +146,46 @@ TEST_F(EkfInitializationTest, initializeWithZeroTilt) _sensor_simulator.simulateOrientation(quat_sim); _sensor_simulator.runSeconds(_init_tilt_period); + EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest); + EXPECT_FALSE(_ekf->control_status_flags().in_air); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + initializedOrienationIsMatchingGroundTruth(quat_sim); - validStateAfterOrientationInitialization(); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + // Fake position fusion obs var when at rest sq(0.01f) + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); +} + +TEST_F(EkfInitializationTest, initializeWithZeroTiltNotAtRest) +{ + const float pitch = math::radians(0.0f); + const float roll = math::radians(0.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _sensor_simulator.simulateOrientation(quat_sim); + //_sensor_simulator.runSeconds(_init_tilt_period); + _sensor_simulator.runSeconds(7); + + 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(); @@ -222,8 +262,21 @@ TEST_F(EkfInitializationTest, initializeHeadingWithZeroTilt) _sensor_simulator.simulateOrientation(quat_sim); _sensor_simulator.runSeconds(_init_tilt_period); + EXPECT_TRUE(_ekf->control_status_flags().vehicle_at_rest); + EXPECT_FALSE(_ekf->control_status_flags().in_air); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + EXPECT_TRUE(_ekf->control_status_flags().yaw_align); + initializedOrienationIsMatchingGroundTruth(quat_sim); - validStateAfterOrientationInitialization(); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + yawVarianceBigEnoughAfterHeadingReset(); + + velocityAndPositionCloseToZero(); + + // Fake position fusion obs var when at rest sq(0.01f) + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); _sensor_simulator.runSeconds(1.f); learningCorrectAccelBias(); @@ -239,8 +292,45 @@ TEST_F(EkfInitializationTest, initializeWithTilt) _sensor_simulator.simulateOrientation(quat_sim); _sensor_simulator.runSeconds(_init_tilt_period); + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + EXPECT_TRUE(_ekf->control_status_flags().yaw_align); + initializedOrienationIsMatchingGroundTruth(quat_sim); - validStateAfterOrientationInitialization(); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + yawVarianceBigEnoughAfterHeadingReset(); + + velocityAndPositionCloseToZero(); + + // Fake position fusion obs var when at rest sq(0.01f) + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); + + _sensor_simulator.runSeconds(1.f); + learningCorrectAccelBias(); +} + +TEST_F(EkfInitializationTest, initializeWithTiltNotAtRest) +{ + const float pitch = math::radians(30.0f); + const float roll = math::radians(60.0f); + const Eulerf euler_angles_sim(roll, pitch, 0.0f); + const Quatf quat_sim(euler_angles_sim); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _sensor_simulator.simulateOrientation(quat_sim); + //_sensor_simulator.runSeconds(_init_tilt_period); + _sensor_simulator.runSeconds(7); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + positionVarianceBigEnoughAfterOrientationInitialization(0.01f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); _sensor_simulator.runSeconds(1.f); learningCorrectAccelBias(); @@ -253,13 +343,25 @@ TEST_F(EkfInitializationTest, initializeWithPitch90) const Eulerf euler_angles_sim(roll, pitch, 0.0f); const Quatf quat_sim(euler_angles_sim); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); _sensor_simulator.simulateOrientation(quat_sim); - _sensor_simulator.runSeconds(_init_tilt_period); + //_sensor_simulator.runSeconds(_init_tilt_period); + _sensor_simulator.runSeconds(10); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); - initializedOrienationIsMatchingGroundTruth(quat_sim); // TODO: Quaternion Variance is smaller and vel x is larger // in this case than in the other cases - validStateAfterOrientationInitialization(); + + initializedOrienationIsMatchingGroundTruth(quat_sim); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + // Fake position fusion obs var when at rest sq(0.01f) + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); _sensor_simulator.runSeconds(1.f); learningCorrectAccelBias(); @@ -272,11 +374,22 @@ TEST_F(EkfInitializationTest, initializeWithRoll90) const Eulerf euler_angles_sim(roll, pitch, 0.0f); const Quatf quat_sim(euler_angles_sim); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); _sensor_simulator.simulateOrientation(quat_sim); - _sensor_simulator.runSeconds(_init_tilt_period); + //_sensor_simulator.runSeconds(_init_tilt_period); + _sensor_simulator.runSeconds(10); + + EXPECT_TRUE(_ekf->control_status_flags().tilt_align); initializedOrienationIsMatchingGroundTruth(quat_sim); - validStateAfterOrientationInitialization(); + quaternionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + + velocityAndPositionCloseToZero(); + + // Fake position fusion obs var when at rest sq(0.01f) + positionVarianceBigEnoughAfterOrientationInitialization(0.00001f); + velocityVarianceBigEnoughAfterOrientationInitialization(0.0001f); _sensor_simulator.runSeconds(1.f); learningCorrectAccelBias(); diff --git a/src/modules/ekf2/test/test_EKF_mag.cpp b/src/modules/ekf2/test/test_EKF_mag.cpp index 2add65b786..11a079958b 100644 --- a/src/modules/ekf2/test/test_EKF_mag.cpp +++ b/src/modules/ekf2/test/test_EKF_mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2023 ECL 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 @@ -57,9 +57,8 @@ public: // Setup the Ekf with synthetic measurements void SetUp() override { - // run briefly to init, then manually set in air and at rest (default for a real vehicle) + // Init, then manually set in air and at rest (default for a real vehicle) _ekf->init(0); - _sensor_simulator.runSeconds(0.1); _ekf->set_in_air_status(false); _ekf->set_vehicle_at_rest(true); } @@ -83,6 +82,25 @@ 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); + + // THEN: the earth mag field is reset to the WMM + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2); + + Vector3f mag_earth = _ekf->getMagEarthField(); + float mag_decl = atan2f(mag_earth(1), mag_earth(0)); + float mag_decl_wmm_deg = 0.f; + _ekf->get_mag_decl_deg(mag_decl_wmm_deg); + EXPECT_NEAR(degrees(mag_decl), mag_decl_wmm_deg, 1e-6f); + + float mag_incl = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f)); + float mag_incl_wmm_deg = 0.f; + _ekf->get_mag_inc_deg(mag_incl_wmm_deg); + EXPECT_NEAR(degrees(mag_incl), mag_incl_wmm_deg, 1e-6f); } TEST_F(EkfMagTest, noInitLargeStrength) @@ -127,3 +145,70 @@ TEST_F(EkfMagTest, suddenLargeStrength) EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); } + +TEST_F(EkfMagTest, noInitLargeInclination) +{ + // GIVEN: a really large magnetic field + _ekf_wrapper.enableMagInclinationCheck(); + // To prevent an early pass of the inclination check, "force WMM" must be set + _ekf_wrapper.enableMagCheckForceWMM(); + _sensor_simulator.startGps(); + Vector3f mag_data(0.4f, 0.f, 0.f); + _sensor_simulator._mag.setData(mag_data); + + const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter(); + _sensor_simulator.runSeconds(_init_duration_s + 10.f); // live some extra time fo GNSS checks to pass + + // THEN: the fusion shouldn't start + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + EXPECT_EQ(0, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter); + + // BUT then: as soon as there is some meaningful data + const float mag_heading = -M_PI_F / 7.f; + mag_data = Vector3f(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + _sensor_simulator.runSeconds(2.f); + + float decl_deg = 0.f; + _ekf->get_mag_decl_deg(decl_deg); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align); + EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1); +} + +TEST_F(EkfMagTest, suddenInclinationChange) +{ + _ekf_wrapper.enableMagInclinationCheck(); + _ekf_wrapper.enableMagCheckForceWMM(); + _sensor_simulator.startGps(); + + // GIVEN: some meaningful mag data + const float mag_heading = -M_PI_F / 7.f; + Vector3f mag_data(0.2f * cosf(mag_heading), -0.2f * sinf(mag_heading), 0.4f); + _sensor_simulator._mag.setData(mag_data); + + _sensor_simulator.runSeconds(_init_duration_s + 10.f); + + float decl_deg = 0.f; + _ekf->get_mag_decl_deg(decl_deg); + + // THEN: the fusion initializes using the mag data and runs normally + EXPECT_NEAR(_ekf_wrapper.getYawAngle(), mag_heading + radians(decl_deg), radians(1.f)); + EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); + + // BUT WHEN: the mag field inclination suddenly changes + mag_data(2) = -mag_data(2); + _sensor_simulator._mag.setData(mag_data); + _sensor_simulator.runSeconds(6.f); + + // THEN: the mag fusion should stop after some time + EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion()); + EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion()); +} diff --git a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp index a3e0e77186..367ae41534 100644 --- a/src/modules/ekf2/test/test_EKF_measurementSampling.cpp +++ b/src/modules/ekf2/test/test_EKF_measurementSampling.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp index 44cf3a2e43..0338bea816 100644 --- a/src/modules/ekf2/test/test_EKF_ringbuffer.cpp +++ b/src/modules/ekf2/test/test_EKF_ringbuffer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp index 4220d6559f..72d0016160 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 ECL Development Team. All rights reserved. + * Copyright (c) 2020-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 @@ -40,6 +40,7 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" class EkfTerrainTest : public ::testing::Test { @@ -182,3 +183,30 @@ TEST_F(EkfTerrainTest, testRngForTerrainFusion) const float estimated_distance_to_ground = _ekf->getTerrainVertPos(); 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(); + + 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); + + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + // WHEN: the baro height is suddenly changed to trigger a height reset + 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); + + // THEN: a height reset occured 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); +} diff --git a/src/modules/ekf2/test/test_EKF_utils.cpp b/src/modules/ekf2/test/test_EKF_utils.cpp index 77a99a35d4..056be5ecff 100644 --- a/src/modules/ekf2/test/test_EKF_utils.cpp +++ b/src/modules/ekf2/test/test_EKF_utils.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -42,8 +42,6 @@ #include #include -#include "EKF/utils.hpp" - TEST(eclPowfTest, compareToStandardImplementation) { std::vector exponents = {-3, -2, -1, -0, 0, 1, 2, 3}; @@ -51,7 +49,7 @@ TEST(eclPowfTest, compareToStandardImplementation) for (auto const exponent : exponents) { for (auto const basis : bases) { - EXPECT_EQ(ecl::powf(basis, exponent), + EXPECT_EQ(powf(basis, exponent), std::pow(basis, static_cast(exponent))); } } diff --git a/src/modules/ekf2/test/test_EKF_withReplayData.cpp b/src/modules/ekf2/test/test_EKF_withReplayData.cpp index b9ee8cea15..b97637440a 100644 --- a/src/modules/ekf2/test/test_EKF_withReplayData.cpp +++ b/src/modules/ekf2/test/test_EKF_withReplayData.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019 ECL Development Team. All rights reserved. + * Copyright (c) 2019-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 diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 518f02a233..a49cd5c66f 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2021 ECL Development Team. All rights reserved. + * Copyright (c) 2021-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 @@ -106,10 +106,10 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 2 resets: 1 after IMU+GNSS yaw alignment and 1 when starting GNSS aiding + // 1 reset when starting GNSS aiding reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(2)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); EXPECT_TRUE(_ekf->local_position_is_valid()); EXPECT_TRUE(_ekf->global_position_is_valid()); diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp index 98537b1cba..a152275ca3 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator_generated.cpp @@ -74,9 +74,9 @@ void sympyYawEstUpdate(const SquareMatrix3f &P, float velObsVar, SquareMatrix FLT_EPSILON) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, - _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, yaw_sp, _sticks.getYawExpo(), _yaw, _deltatime); // Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input _yaw_sp_aligned = true; diff --git a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp index 3ebc783581..85ba49ea0b 100644 --- a/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp +++ b/src/modules/flight_mode_manager/tasks/Descend/FlightTaskDescend.cpp @@ -60,8 +60,7 @@ bool FlightTaskDescend::update() // Nudging if (_param_mpc_land_rc_help.get() && _sticks.checkAndUpdateStickInputs()) { - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp index c4b9649046..063b78dcf6 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp @@ -112,6 +112,7 @@ void FlightTask::_evaluateVehicleLocalPosition() // yaw _yaw = _sub_vehicle_local_position.get().heading; + _unaided_yaw = _sub_vehicle_local_position.get().unaided_heading; _is_yaw_good_for_control = _sub_vehicle_local_position.get().heading_good_for_control; // position diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp index 1df5351fb4..3b9ddbd466 100644 --- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp +++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp @@ -139,13 +139,12 @@ public: const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; } /** - * All setpoints are set to NAN (uncontrolled). Timestampt zero. + * All setpoints are set to NAN (uncontrolled), timestamp to zero */ static const trajectory_setpoint_s empty_trajectory_setpoint; /** - * Empty constraints. - * All constraints are set to NAN. + * All constraints are set to NAN, timestamp to zero */ static const vehicle_constraints_s empty_constraints; @@ -219,6 +218,7 @@ protected: matrix::Vector3f _velocity; /**< current vehicle velocity */ float _yaw{}; /**< current vehicle yaw heading */ + float _unaided_yaw{}; bool _is_yaw_good_for_control{}; /**< true if the yaw estimate can be used for yaw control */ float _dist_to_bottom{}; /**< current height above ground level if dist_bottom is valid */ float _dist_to_ground{}; /**< equals _dist_to_bottom if available, height above home otherwise */ diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp index 219868f64b..a1ea2b6b80 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.cpp @@ -61,9 +61,6 @@ bool FlightTaskManualAcceleration::update() { bool ret = FlightTaskManualAltitudeSmoothVel::update(); - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _is_yaw_good_for_control, - _deltatime); - _stick_acceleration_xy.generateSetpoints(_sticks.getPitchRollExpo(), _yaw, _yaw_setpoint, _position, _velocity_setpoint_feedback.xy(), _deltatime); _stick_acceleration_xy.getSetpoints(_position_setpoint, _velocity_setpoint, _acceleration_setpoint); diff --git a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp index d88f2d0bd6..a73d60e7fc 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAcceleration/FlightTaskManualAcceleration.hpp @@ -58,7 +58,5 @@ private: void _ekfResetHandlerVelocityXY(const matrix::Vector2f &delta_vxy) override; StickAccelerationXY _stick_acceleration_xy{this}; - StickYaw _stick_yaw{this}; - WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ }; diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp index 90d5dccb1f..ab90d10e12 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp @@ -64,6 +64,7 @@ bool FlightTaskManualAltitude::activate(const trajectory_setpoint_s &last_setpoi _acceleration_setpoint = Vector3f(0.f, 0.f, NAN); // altitude is controlled from position/velocity _position_setpoint(2) = _position(2); _velocity_setpoint(2) = 0.f; + _stick_yaw.reset(_yaw, _unaided_yaw); _setDefaultConstraints(); _updateConstraintsFromEstimator(); @@ -90,10 +91,6 @@ void FlightTaskManualAltitude::_updateConstraintsFromEstimator() void FlightTaskManualAltitude::_scaleSticks() { - // Use stick input with deadzone, exponential curve and first order lpf for yawspeed - _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, - _is_yaw_good_for_control, _deltatime); - // Use sticks input with deadzone and exponential curve for vertical velocity const float vel_max_z = (_sticks.getPosition()(2) > 0.0f) ? _param_mpc_z_vel_max_dn.get() : _param_mpc_z_vel_max_up.get(); @@ -273,51 +270,19 @@ void FlightTaskManualAltitude::_respectGroundSlowdown() } } -void FlightTaskManualAltitude::_updateHeadingSetpoints() -{ - if (_isYawInput() || !_is_yaw_good_for_control) { - _unlockYaw(); - - } else { - _lockYaw(); - } -} - -bool FlightTaskManualAltitude::_isYawInput() -{ - /* - * A threshold larger than FLT_EPSILON is required because the - * _yawspeed_setpoint comes from an IIR filter and takes too much - * time to reach zero. - */ - return fabsf(_yawspeed_setpoint) > 0.001f; -} - -void FlightTaskManualAltitude::_unlockYaw() -{ - // no fixed heading when rotating around yaw by stick - _yaw_setpoint = NAN; -} - -void FlightTaskManualAltitude::_lockYaw() -{ - // hold the current heading when no more rotation commanded - if (!PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint = _yaw; - } -} - void FlightTaskManualAltitude::_ekfResetHandlerHeading(float delta_psi) { // Only reset the yaw setpoint when the heading is locked if (PX4_ISFINITE(_yaw_setpoint)) { - _yaw_setpoint += delta_psi; + _yaw_setpoint = wrap_pi(_yaw_setpoint + delta_psi); } + + _stick_yaw.ekfResetHandler(delta_psi); } void FlightTaskManualAltitude::_updateSetpoints() { - _updateHeadingSetpoints(); // get yaw setpoint + _stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _yaw_setpoint, _sticks.getYawExpo(), _yaw, _deltatime, _unaided_yaw); _acceleration_setpoint.xy() = _stick_tilt_xy.generateAccelerationSetpoints(_sticks.getPitchRoll(), _deltatime, _yaw, _yaw_setpoint); _updateAltitudeLock(); diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp index 0b9fbe72a3..bf7ec2dc49 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.hpp @@ -40,7 +40,6 @@ #pragma once #include "FlightTask.hpp" -#include #include "Sticks.hpp" #include "StickTiltXY.hpp" #include "StickYaw.hpp" @@ -56,7 +55,6 @@ public: bool update() override; protected: - void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */ void _ekfResetHandlerHeading(float delta_psi) override; /**< adjust heading setpoint in case of EKF reset event */ virtual void _updateSetpoints(); /**< updates all setpoints */ virtual void _scaleSticks(); /**< scales sticks to velocity in z */ @@ -89,10 +87,6 @@ protected: _param_mpc_tko_speed /**< desired upwards speed when still close to the ground */ ) private: - bool _isYawInput(); - void _unlockYaw(); - void _lockYaw(); - /** * Terrain following. * During terrain following, the position setpoint is adjusted @@ -120,6 +114,8 @@ private: void setGearAccordingToSwitch(); + bool _updateYawCorrection(); + uint8_t _reset_counter = 0; /**< counter for estimator resets in z-direction */ bool _terrain_follow{false}; /**< true when the vehicle is following the terrain height */ bool _terrain_hold{false}; /**< true when vehicle is controlling height above a static ground position */ diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp index 8a2512279c..edc82d6c98 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.cpp @@ -35,23 +35,80 @@ #include +using matrix::wrap_pi; + StickYaw::StickYaw(ModuleParams *parent) : ModuleParams(parent) {} -void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, - const float yaw, const bool is_yaw_good_for_control, const float deltatime) +void StickYaw::reset(const float yaw, const float unaided_yaw) { + if (PX4_ISFINITE(unaided_yaw)) { + _yaw_error_lpf.reset(wrap_pi(yaw - unaided_yaw)); + } +} + +void StickYaw::ekfResetHandler(const float delta_yaw) +{ + _yaw_error_lpf.reset(wrap_pi(_yaw_error_lpf.getState() + delta_yaw)); + _yaw_error_ref = wrap_pi(_yaw_error_ref + delta_yaw); +} + +void StickYaw::generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, const float stick_yaw, + const float yaw, const float deltatime, const float unaided_yaw) +{ + _yaw_error_lpf.setParameters(deltatime, _kYawErrorTimeConstant); + const float yaw_correction_prev = _yaw_correction; + const bool reset_setpoint = updateYawCorrection(yaw, unaided_yaw); + + if (reset_setpoint) { + yaw_setpoint = NAN; + } + _yawspeed_filter.setParameters(deltatime, _param_mpc_man_y_tau.get()); yawspeed_setpoint = _yawspeed_filter.update(stick_yaw * math::radians(_param_mpc_man_y_max.get())); - yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, is_yaw_good_for_control); + yaw_setpoint = updateYawLock(yaw, yawspeed_setpoint, yaw_setpoint, yaw_correction_prev); +} + +bool StickYaw::updateYawCorrection(const float yaw, const float unaided_yaw) +{ + if (!PX4_ISFINITE(unaided_yaw)) { + _yaw_correction = 0.f; + return false; + } + + // Detect the convergence phase of the yaw estimate by monitoring its relative + // distance from an unaided yaw source. + const float yaw_error = wrap_pi(yaw - unaided_yaw); + + // Run it through a high-pass filter to detect transients + const float yaw_error_hpf = wrap_pi(yaw_error - _yaw_error_lpf.getState()); + _yaw_error_lpf.update(yaw_error); + + const bool was_converging = _yaw_estimate_converging; + _yaw_estimate_converging = fabsf(yaw_error_hpf) > _kYawErrorChangeThreshold; + + bool reset_setpoint = false; + + if (!_yaw_estimate_converging) { + _yaw_error_ref = yaw_error; + + if (was_converging) { + // Force a reset of the locking mechanism + reset_setpoint = true; + } + } + + _yaw_correction = wrap_pi(yaw_error - _yaw_error_ref); + + return reset_setpoint; } float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, const float yaw_setpoint, - const bool is_yaw_good_for_control) + const float yaw_correction_prev) const { // Yaw-lock depends on desired yawspeed input. If not locked, yaw_sp is set to NAN. - if ((fabsf(yawspeed_setpoint) > FLT_EPSILON) || !is_yaw_good_for_control) { + if (fabsf(yawspeed_setpoint) > FLT_EPSILON) { // no fixed heading when rotating around yaw by stick return NAN; @@ -61,7 +118,7 @@ float StickYaw::updateYawLock(const float yaw, const float yawspeed_setpoint, co return yaw; } else { - return yaw_setpoint; + return wrap_pi(yaw_setpoint - yaw_correction_prev + _yaw_correction); } } } diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp index 8e9e4aea1e..185c490110 100644 --- a/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp +++ b/src/modules/flight_mode_manager/tasks/Utility/StickYaw.hpp @@ -48,12 +48,23 @@ public: StickYaw(ModuleParams *parent); ~StickYaw() = default; - void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, - bool is_yaw_good_for_control, float deltatime); + void reset(float yaw, float unaided_yaw = NAN); + void ekfResetHandler(float delta_yaw); + void generateYawSetpoint(float &yawspeed_setpoint, float &yaw_setpoint, float stick_yaw, float yaw, float deltatime, + float unaided_yaw = NAN); private: AlphaFilter _yawspeed_filter; + float _yaw_error_ref{0.f}; + float _yaw_correction{0.f}; + bool _yaw_estimate_converging{false}; + AlphaFilter _yaw_error_lpf{0.01f}; ///< used to create a high-pass filter + static constexpr float _kYawErrorTimeConstant{1.f}; ///< time constant of the high-pass filter used to detect yaw convergence + static constexpr float _kYawErrorChangeThreshold{radians(1.f)}; ///< we consider the yaw estimate as "converging" when above this threshold + + bool updateYawCorrection(float yaw, float unaided_yaw); + /** * Lock yaw when not currently turning * When applying a yawspeed the vehicle is turning, when the speed is @@ -65,7 +76,7 @@ private: * @param yaw current yaw setpoint which then will be overwritten by the return value * @return yaw setpoint to execute to have a yaw lock at the correct moment in time */ - static float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, bool is_yaw_good_for_control); + float updateYawLock(float yaw, float yawspeed_setpoint, float yaw_setpoint, float yaw_correction_prev) const; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_man_y_max, ///< Maximum yaw speed with full stick deflection diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 66d80ee90e..554eac360c 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,10 +44,7 @@ * Attitude Roll Time Constant * * This defines the latency between a roll step input and the achieved setpoint - * (inverse to a P gain). Half a second is a good start value and fits for - * most average systems. Smaller systems may require smaller values, but as - * this will wear out servos faster, the value should only be decreased as - * needed. + * (inverse to a P gain). Smaller systems may require smaller values. * * @unit s * @min 0.2 @@ -62,10 +59,7 @@ PARAM_DEFINE_FLOAT(FW_R_TC, 0.4f); * Attitude pitch time constant * * This defines the latency between a pitch step input and the achieved setpoint - * (inverse to a P gain). Half a second is a good start value and fits for - * most average systems. Smaller systems may require smaller values, but as - * this will wear out servos faster, the value should only be decreased as - * needed. + * (inverse to a P gain). Smaller systems may require smaller values. * * @unit s * @min 0.2 @@ -168,9 +162,6 @@ PARAM_DEFINE_FLOAT(FW_WR_I, 0.1f); /** * Wheel steering rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -197,8 +188,6 @@ PARAM_DEFINE_FLOAT(FW_W_RMAX, 30.0f); /** * Wheel steering rate feed forward * - * Direct feed forward from rate setpoint to control surface output - * * @unit %/rad/s * @min 0.0 * @max 10 @@ -228,8 +217,7 @@ PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f); * Maximum manually added yaw rate * * This is the maximally added yaw rate setpoint from the yaw stick in any attitude controlled flight mode. - * The controller already generates a yaw rate setpoint to coordinate a turn, and this value is added to it. - * This is an absolute value, which is applied symmetrically to the negative and positive side. + * It is added to the yaw rate setpoint generated by the controller for turn coordination. * * @unit deg/s * @min 0 @@ -242,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_YR_MAX, 30.f); /** * Maximum manual roll angle * - * Maximum manual roll angle setpoint (positive & negative) in manual attitude-only stabilized mode + * Applies to both directions in all manual modes with attitude stabilization * * @unit deg * @min 0.0 @@ -256,7 +244,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f); /** * Maximum manual pitch angle * - * Maximum manual pitch angle setpoint (positive & negative) in manual attitude-only stabilized mode + * Applies to both directions in all manual modes with attitude stabilization but without altitude control * * @unit deg * @min 0.0 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 ec274f28da..eee05880fd 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 @@ -180,7 +180,11 @@ void FwAutotuneAttitudeControl::Run() Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f); _kiff(0) = kid(0); _kiff(1) = kid(1); - _attitude_p = 8.f / (M_PI_F * (_kiff(2) + _kiff(0))); // Maximum control power at an attitude error of pi/8 + + // To compute the attitude gain, use the following empirical rule: + // "An error of 60 degrees should produce the maximum control output" + // or K_att * (K_rate + K_ff) * rad(60) = 1 + _attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f); const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized() @@ -638,19 +642,21 @@ const Vector3f FwAutotuneAttitudeControl::getIdentificationSignal() if (_state == state::roll || _state == state::test) { // Scale the signal such that the attitude controller is // able to cancel it completely at an attitude error of pi/8 - signal_scaled = signal * M_PI_F / (8.f * _param_fw_r_tc.get()); + signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_r_tc.get()), math::radians(_param_fw_r_rmax.get())); rate_sp(0) = signal_scaled - _signal_filter.getState(); } if (_state == state::pitch || _state == state::test) { - signal_scaled = signal * M_PI_F / (8.f * _param_fw_p_tc.get()); + const float pitch_rate_max_deg = math::min(_param_fw_p_rmax_pos.get(), _param_fw_p_rmax_neg.get()); + signal_scaled = math::min(signal * M_PI_F / (8.f * _param_fw_p_tc.get()), math::radians(pitch_rate_max_deg)); rate_sp(1) = signal_scaled - _signal_filter.getState(); } if (_state == state::yaw) { // Do not send a signal that produces more than a full deflection of the rudder - signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get())); + signal_scaled = math::min(signal, 1.f / (_param_fw_yr_ff.get() + _param_fw_yr_p.get()), + math::radians(_param_fw_y_rmax.get())); rate_sp(2) = signal_scaled - _signal_filter.getState(); } 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 213518ef72..9be5f55596 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 @@ -193,14 +193,18 @@ private: (ParamFloat) _param_fw_rr_p, (ParamFloat) _param_fw_rr_i, (ParamFloat) _param_fw_rr_ff, + (ParamFloat) _param_fw_r_rmax, (ParamFloat) _param_fw_r_tc, (ParamFloat) _param_fw_pr_p, (ParamFloat) _param_fw_pr_i, (ParamFloat) _param_fw_pr_ff, + (ParamFloat) _param_fw_p_rmax_pos, + (ParamFloat) _param_fw_p_rmax_neg, (ParamFloat) _param_fw_p_tc, (ParamFloat) _param_fw_yr_p, (ParamFloat) _param_fw_yr_i, - (ParamFloat) _param_fw_yr_ff + (ParamFloat) _param_fw_yr_ff, + (ParamFloat) _param_fw_y_rmax ) static constexpr float _publishing_dt_s = 100e-3f; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index da1938ffc6..a7f5bb5eab 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -76,6 +76,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) : /* fetch initial parameter values */ parameters_update(); + + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_trim.get()); } FixedwingPositionControl::~FixedwingPositionControl() @@ -399,26 +401,9 @@ FixedwingPositionControl::get_manual_airspeed_setpoint() float FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed) + float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation) { - if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { - calibrated_airspeed_setpoint = _param_fw_airspd_trim.get(); - } - - // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained - if (!_wind_valid) { - /* - * This error value ensures that a plane (as long as its throttle capability is - * not exceeded) travels towards a waypoint (and is not pushed more and more away - * by wind). Not countering this would lead to a fly-away. Only non-zero in presence - * of sufficient wind. "minimum ground speed undershoot". - */ - const float ground_speed_body = _body_velocity_x; - - if (ground_speed_body < _param_fw_gnd_spd_min.get()) { - calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; - } - } + // --- airspeed *constraint adjustments --- float load_factor_from_bank_angle = 1.0f; @@ -441,27 +426,53 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio); // Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds. - if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { + if (!in_takeoff_situation && _airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) { calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() * _wind_vel.length(), _param_fw_airspd_max.get()); } + // --- airspeed *setpoint adjustments --- + + if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) { + calibrated_airspeed_setpoint = _param_fw_airspd_trim.get(); + } + + // Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained + if (!_wind_valid && !in_takeoff_situation) { + /* + * This error value ensures that a plane (as long as its throttle capability is + * not exceeded) travels towards a waypoint (and is not pushed more and more away + * by wind). Not countering this would lead to a fly-away. Only non-zero in presence + * of sufficient wind. "minimum ground speed undershoot". + */ + const float ground_speed_body = _body_velocity_x; + + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { + calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body; + } + } + calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed, _param_fw_airspd_max.get()); - // initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case - const bool slewed_airspeed_outside_of_limits = _airspeed_slew_rate_controller.getState() < calibrated_min_airspeed - || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get(); - - if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || slewed_airspeed_outside_of_limits) { + if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) { _airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint); - - } else if (control_interval > FLT_EPSILON) { - // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s - calibrated_airspeed_setpoint = _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); } - return calibrated_airspeed_setpoint; + if (control_interval > FLT_EPSILON) { + // constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s + _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval); + } + + if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) { + _airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed); + } + + if (_airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get()) { + _airspeed_slew_rate_controller.setForcedValue(_param_fw_airspd_max.get()); + } + + return _airspeed_slew_rate_controller.getState(); } void @@ -712,7 +723,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now) _skipping_takeoff_detection = false; if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) || - _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) { + _control_mode.flag_control_offboard_enabled) && (_position_setpoint_current_valid + || _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE)) { + + // Enter this mode only if the current waypoint has valid 3D position setpoints or is of type IDLE. + // A setpoint of type IDLE can be published by Navigator without a valid position, and is handled here in FW_POSCTRL_MODE_AUTO. if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { @@ -1025,21 +1040,6 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { const float acc_rad = _npfg.switchDistance(500.0f); - Vector2d curr_wp{0, 0}; - Vector2d prev_wp{0, 0}; - - /* current waypoint (the one currently heading for) */ - curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon); - - if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - prev_wp(0) = pos_sp_prev.lat; - prev_wp(1) = pos_sp_prev.lon; - - } else { - // No valid previous waypoint, go along the line between aircraft and current waypoint - prev_wp = curr_pos; - } - float tecs_fw_thr_min; float tecs_fw_thr_max; @@ -1062,14 +1062,14 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) || (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) ) { - const float d_curr_prev = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), - pos_sp_prev.lat, pos_sp_prev.lon); + const float d_curr_prev = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, pos_sp_prev.lat, + pos_sp_prev.lon); // Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one if (d_curr_prev > math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius))) { // Calculate distance to current waypoint - const float d_curr = get_distance_to_next_waypoint((double)curr_wp(0), (double)curr_wp(1), - _current_latitude, _current_longitude); + const float d_curr = get_distance_to_next_waypoint(pos_sp_curr.lat, pos_sp_curr.lon, _current_latitude, + _current_longitude); // Save distance to waypoint if it is the smallest ever achieved, however make sure that // _min_current_sp_distance_xy is never larger than the distance between the current and the previous wp @@ -1093,8 +1093,7 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co _param_fw_airspd_min.get(), ground_speed); Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); - Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1)); + Vector2f curr_wp_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); @@ -1107,8 +1106,12 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature); - } else { + } else if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + Vector2f prev_wp_local = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon); navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + + } else { + navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel); } _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1313,16 +1316,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo adjusted_min_airspeed = takeoff_airspeed; } - float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, - ground_speed); - if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { _runway_takeoff.init(now, _yaw, global_position); - _takeoff_ground_alt = _current_altitude; - _launch_current_yaw = _yaw; + _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway"); } @@ -1353,17 +1352,23 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP - Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)}; + float takeoff_bearing = _launch_current_yaw; if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { // the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded - takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local); + const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - start_pos_local; + + if (takeoff_bearing_vector.norm() > FLT_EPSILON) { + takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0)); + } } + float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed, ground_speed, + true); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed, - _wind_vel, 0.0f); + navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint()); @@ -1433,6 +1438,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _launch_global_position = global_position; _takeoff_ground_alt = _current_altitude; _launch_current_yaw = _yaw; + _airspeed_slew_rate_controller.setForcedValue(takeoff_airspeed); } const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0), @@ -1440,11 +1446,15 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon); // by default set the takeoff bearing to the takeoff yaw, but override in a mission takeoff with bearing to takeoff WP - Vector2f takeoff_bearing_vector = {cosf(_launch_current_yaw), sinf(_launch_current_yaw)}; + float takeoff_bearing = _launch_current_yaw; if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) { // the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded - takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local); + const Vector2f takeoff_bearing_vector = takeoff_waypoint_local - launch_local_position; + + if (takeoff_bearing_vector.norm() > FLT_EPSILON) { + takeoff_bearing = atan2f(takeoff_bearing_vector(1), takeoff_bearing_vector(0)); + } } /* Set control values depending on the detection state */ @@ -1452,10 +1462,12 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo && _param_fw_laun_detcn_on.get()) { /* 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, + true); + _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel, - 0.0f); + navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; @@ -1603,7 +1615,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1682,7 +1694,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _npfg.setAirspeedNom(target_airspeed * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); + navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel); target_airspeed = _npfg.getAirspeedRef() / _eas2tas; _att_sp.roll_body = _npfg.getRollSetpoint(); @@ -1944,7 +1956,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, updateManualTakeoffStatus(); const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _param_fw_airspd_min.get(), ground_speed); + _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -1985,7 +1997,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, updateManualTakeoffStatus(); float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(), - _param_fw_airspd_min.get(), ground_speed); + _param_fw_airspd_min.get(), ground_speed, !_completed_manual_takeoff); const float height_rate_sp = getManualHeightRateSetpoint(); // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are @@ -2001,6 +2013,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } /* 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 && fabsf(_manual_control_setpoint.yaw) < HDG_HOLD_MAN_INPUT_THRESH) { @@ -2025,28 +2038,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _hdg_hold_enabled = true; _hdg_hold_yaw = _yaw; - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true); + _hdg_hold_position.lat = _current_latitude; + _hdg_hold_position.lon = _current_longitude; } - /* we have a valid heading hold position, are we too close? */ - const float dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _hdg_hold_curr_wp.lat, - _hdg_hold_curr_wp.lon); - - if (dist < HDG_HOLD_REACHED_DIST) { - get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); - } - - Vector2d prev_wp{_hdg_hold_prev_wp.lat, _hdg_hold_prev_wp.lon}; - Vector2d curr_wp{_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon}; - Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; - Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1)); - Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), - prev_wp(1)); + Vector2f curr_wp_local = _global_local_proj_ref.project(_hdg_hold_position.lat, _hdg_hold_position.lon); _npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas); _npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas); - navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel); + navigateLine(curr_wp_local, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel); _att_sp.roll_body = _npfg.getRollSetpoint(); calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas; @@ -2055,7 +2056,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, } tecs_update_pitch_throttle(control_interval, - _current_altitude, + _current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude. calibrated_airspeed_sp, min_pitch, radians(_param_fw_p_lim_max.get()), @@ -2147,6 +2148,13 @@ FixedwingPositionControl::Run() _current_longitude = gpos.lon; } + if (_local_pos.z_global && PX4_ISFINITE(_local_pos.ref_alt)) { + _reference_altitude = _local_pos.ref_alt; + + } else { + _reference_altitude = 0.f; + } + _current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters // handle estimator reset events. we only adjust setpoins for manual modes @@ -2174,9 +2182,16 @@ FixedwingPositionControl::Run() || (_global_local_proj_ref.getProjectionReferenceTimestamp() != _local_pos.ref_timestamp) || (_local_pos.vxy_reset_counter != _pos_reset_counter)) { - _global_local_proj_ref.initReference(_local_pos.ref_lat, _local_pos.ref_lon, + double reference_latitude = 0.; + double reference_longitude = 0.; + + if (_local_pos.xy_global && PX4_ISFINITE(_local_pos.ref_lat) && PX4_ISFINITE(_local_pos.ref_lon)) { + reference_latitude = _local_pos.ref_lat; + reference_longitude = _local_pos.ref_lon; + } + + _global_local_proj_ref.initReference(reference_latitude, reference_longitude, _local_pos.ref_timestamp); - _global_local_alt0 = _local_pos.ref_alt; } if (_control_mode.flag_control_offboard_enabled) { @@ -2205,7 +2220,7 @@ FixedwingPositionControl::Run() _pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; _pos_sp_triplet.current.lat = lat; _pos_sp_triplet.current.lon = lon; - _pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2]; + _pos_sp_triplet.current.alt = _reference_altitude - trajectory_setpoint.position[2]; } } @@ -2616,27 +2631,6 @@ FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, con return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike); } -Vector2f -FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position, - const Vector2f &takeoff_waypoint) const -{ - Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position; - - if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) { - takeoff_bearing_vector.normalize(); - - } else { - // TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need - // for this check - - // takeoff in the direction of the airframe - takeoff_bearing_vector(0) = cosf(_yaw); - takeoff_bearing_vector(1) = sinf(_yaw); - } - - return takeoff_bearing_vector; -} - void FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev, const float land_point_altitude, const Vector2f &local_position, const Vector2f &local_land_point) @@ -2806,7 +2800,7 @@ void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpo local_position_setpoint.x = current_setpoint(0); local_position_setpoint.y = current_setpoint(1); - local_position_setpoint.z = _global_local_alt0 - current_waypoint.alt; + local_position_setpoint.z = _reference_altitude - current_waypoint.alt; local_position_setpoint.yaw = NAN; local_position_setpoint.yawspeed = NAN; local_position_setpoint.vx = NAN; @@ -2840,47 +2834,97 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_ _orbit_status_pub.publish(orbit_status); } -void FixedwingPositionControl::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoint_B, +void FixedwingPositionControl::navigateWaypoints(const Vector2f &start_waypoint, const Vector2f &end_waypoint, const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) { - // similar to logic found in ECL_L1_Pos_Controller method of same name - // BUT no arbitrary max approach angle, approach entirely determined by generated - // bearing vectors + const Vector2f start_waypoint_to_end_waypoint = end_waypoint - start_waypoint; + const Vector2f start_waypoint_to_vehicle = vehicle_pos - start_waypoint; + const Vector2f end_waypoint_to_vehicle = vehicle_pos - end_waypoint; - const Vector2f vector_A_to_B = waypoint_B - waypoint_A; - const Vector2f vector_B_to_A = -vector_A_to_B; - const Vector2f vector_A_to_vehicle = vehicle_pos - waypoint_A; - const Vector2f vector_B_to_vehicle = vehicle_pos - waypoint_B; - Vector2f desired_path = vector_A_to_B; // this is the default path tangent, but is overridden below - - if (vector_A_to_B.norm() < FLT_EPSILON) { - // the waypoints are on top of each other and should be considered as a - // single waypoint, fly directly to it - if (vector_A_to_vehicle.norm() > FLT_EPSILON) { - desired_path = -vector_A_to_vehicle; - - } else { - // Fly to a point and on it. Stay to the current control. Do not update the npfg library to get last output. - return; - } - - } else if ((vector_A_to_B.dot(vector_A_to_vehicle) < -FLT_EPSILON)) { - // we are in front of waypoint A, fly directly to it until we are within switch distance. - if (vector_A_to_vehicle.norm() > _npfg.switchDistance(500.0f)) { - desired_path = -vector_A_to_vehicle; - } - - } else if (vector_B_to_A.dot(vector_B_to_vehicle) < -FLT_EPSILON) { - // we are behind waypoint B, fly directly to it - desired_path = -vector_B_to_vehicle; + if (start_waypoint_to_end_waypoint.norm() < FLT_EPSILON) { + // degenerate case: the waypoints are on top of each other, this should only happen when someone uses this + // method incorrectly. just as a safe guard, call the singular waypoint navigation method. + navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); + return; } - // track the line segment - Vector2f unit_path_tangent{desired_path.normalized()}; + if ((start_waypoint_to_end_waypoint.dot(start_waypoint_to_vehicle) < -FLT_EPSILON) + && (start_waypoint_to_vehicle.norm() > _npfg.switchDistance(500.0f))) { + // we are in front of the start waypoint, fly directly to it until we are within switch distance + navigateWaypoint(start_waypoint, vehicle_pos, ground_vel, wind_vel); + return; + } + + if (start_waypoint_to_end_waypoint.dot(end_waypoint_to_vehicle) > FLT_EPSILON) { + // we are beyond the end waypoint, fly back to it + // NOTE: this logic ideally never gets executed, as a waypoint switch should happen before passing the + // end waypoint. however this included here as a safety precaution if any navigator (module) switch condition + // is missed for any reason. in the future this logic should all be handled in one place in a dedicated + // flight mode state machine. + navigateWaypoint(end_waypoint, vehicle_pos, ground_vel, wind_vel); + return; + } + + // follow the line segment between the start and end waypoints + navigateLine(start_waypoint, end_waypoint, vehicle_pos, ground_vel, wind_vel); +} + +void FixedwingPositionControl::navigateWaypoint(const Vector2f &waypoint_pos, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f vehicle_to_waypoint = waypoint_pos - vehicle_pos; + + if (vehicle_to_waypoint.norm() < FLT_EPSILON) { + // degenerate case: the vehicle is on top of the single waypoint. (can happen). maintain the last npfg command. + return; + } + + const Vector2f unit_path_tangent = vehicle_to_waypoint.normalized(); + _closest_point_on_path = waypoint_pos; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); - _closest_point_on_path = waypoint_A + vector_A_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; - _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, waypoint_A, 0.0f); -} // navigateWaypoints +} + +void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f line_segment = point_on_line_2 - point_on_line_1; + + if (line_segment.norm() <= FLT_EPSILON) { + // degenerate case: line segment has zero length. maintain the last npfg command. + return; + } + + const Vector2f unit_path_tangent = line_segment.normalized(); + + const Vector2f point_1_to_vehicle = vehicle_pos - point_on_line_1; + _closest_point_on_path = point_on_line_1 + point_1_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. + _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); +} + +void FixedwingPositionControl::navigateLine(const Vector2f &point_on_line, const float line_bearing, + const Vector2f &vehicle_pos, const Vector2f &ground_vel, const Vector2f &wind_vel) +{ + const Vector2f unit_path_tangent{cosf(line_bearing), sinf(line_bearing)}; + + const Vector2f point_on_line_to_vehicle = vehicle_pos - point_on_line; + _closest_point_on_path = point_on_line + point_on_line_to_vehicle.dot(unit_path_tangent) * unit_path_tangent; + + const float path_curvature = 0.f; + _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, _closest_point_on_path, path_curvature); + + // for logging - note we are abusing path tangent vs bearing definitions here. npfg interfaces need to be refined. + _target_bearing = line_bearing; +} void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos, float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel) @@ -2919,19 +2963,23 @@ void FixedwingPositionControl::navigateLoiter(const Vector2f &loiter_center, con _closest_point_on_path = unit_vec_center_to_closest_pt * radius + loiter_center; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, loiter_center + unit_vec_center_to_closest_pt * radius, path_curvature); -} // navigateLoiter - +} void FixedwingPositionControl::navigatePathTangent(const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &position_setpoint, const matrix::Vector2f &tangent_setpoint, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel, const float &curvature) { + if (tangent_setpoint.norm() <= FLT_EPSILON) { + // degenerate case: no direction. maintain the last npfg command. + return; + } + const Vector2f unit_path_tangent{tangent_setpoint.normalized()}; _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = position_setpoint; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, tangent_setpoint.normalized(), position_setpoint, curvature); -} // navigatePathTangent +} void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_pos, float bearing, const Vector2f &ground_vel, const Vector2f &wind_vel) @@ -2941,7 +2989,7 @@ void FixedwingPositionControl::navigateBearing(const matrix::Vector2f &vehicle_p _target_bearing = atan2f(unit_path_tangent(1), unit_path_tangent(0)); _closest_point_on_path = vehicle_pos; _npfg.guideToPath(vehicle_pos, ground_vel, wind_vel, unit_path_tangent, vehicle_pos, 0.0f); -} // navigateBearing +} int FixedwingPositionControl::task_spawn(int argc, char *argv[]) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fe4e6aa580..e46c14c005 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -266,7 +266,8 @@ private: float _body_velocity_x{0.f}; MapProjection _global_local_proj_ref{}; - float _global_local_alt0{NAN}; + + float _reference_altitude{NAN}; // [m AMSL] altitude of the local projection reference point bool _landed{true}; @@ -287,8 +288,7 @@ private: bool _hdg_hold_enabled{false}; // heading hold enabled bool _yaw_lock_engaged{false}; // yaw is locked for heading hold - position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started - position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies + position_setpoint_s _hdg_hold_position{}; // position where heading hold started // [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands float _manual_control_setpoint_for_height_rate{0.0f}; @@ -670,10 +670,11 @@ private: * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s] * @param calibrated_min_airspeed Minimum calibrated airspeed [m/s] * @param ground_speed Vehicle ground velocity vector (NE) [m/s] + * @param in_takeoff_situation Vehicle is currently in a takeoff situation * @return Adjusted calibrated airspeed setpoint [m/s] */ float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint, - float calibrated_min_airspeed, const Vector2f &ground_speed); + float calibrated_min_airspeed, const Vector2f &ground_speed, bool in_takeoff_situation = false); void reset_takeoff_state(); void reset_landing_state(); @@ -732,15 +733,6 @@ private: */ float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const; - /** - * @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont. - * - * @param launch_position Vehicle launch position in local coordinates (NE) [m] - * @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m] - * @return Unit takeoff bearing vector - */ - Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const; - /** * @brief Calculates the touchdown position for landing with optional manual lateral adjustments. * @@ -789,28 +781,68 @@ private: /* * Waypoint handling logic following closely to the ECL_L1_Pos_Controller - * method of the same name. Takes two waypoints and determines the relevant - * parameters for evaluating the NPFG guidance law, then updates control setpoints. + * method of the same name. Takes two waypoints, steering the vehicle to track + * the line segment between them. * - * @param[in] waypoint_A Waypoint A (segment start) position in WGS84 coordinates - * (lat,lon) [deg] - * @param[in] waypoint_B Waypoint B (segment end) position in WGS84 coordinates - * (lat,lon) [deg] - * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] start_waypoint Segment starting position in local coordinates. (N,E) [m] + * @param[in] end_waypoint Segment end position in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] */ - void navigateWaypoints(const matrix::Vector2f &waypoint_A, const matrix::Vector2f &waypoint_B, + void navigateWaypoints(const matrix::Vector2f &start_waypoint, const matrix::Vector2f &end_waypoint, const matrix::Vector2f &vehicle_pos, const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + /* + * Takes one waypoint and steers the vehicle towards this. + * + * NOTE: this *will lead to "flowering" behavior if no higher level state machine or + * switching condition changes the waypoint. + * + * @param[in] waypoint_pos Waypoint position in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateWaypoint(const matrix::Vector2f &waypoint_pos, const matrix::Vector2f &vehicle_pos, + const matrix::Vector2f &ground_vel, const matrix::Vector2f &wind_vel); + + /* + * Line (infinite) following logic. Two points on the line are used to define the + * line in 2D space (first to second point determines the direction). Determines the + * relevant parameters for evaluating the NPFG guidance law, then updates control setpoints. + * + * @param[in] point_on_line_1 Arbitrary first position on line in local coordinates. (N,E) [m] + * @param[in] point_on_line_2 Arbitrary second position on line in local coordinates. (N,E) [m] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateLine(const Vector2f &point_on_line_1, const Vector2f &point_on_line_2, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); + + /* + * Line (infinite) following logic. One point on the line and a line bearing are used to define + * the line in 2D space. Determines the relevant parameters for evaluating the NPFG guidance law, + * then updates control setpoints. + * + * @param[in] point_on_line Arbitrary position on line in local coordinates. (N,E) [m] + * @param[in] line_bearing Line bearing [rad] (from north) + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] ground_vel Vehicle ground velocity vector [m/s] + * @param[in] wind_vel Wind velocity vector [m/s] + */ + void navigateLine(const Vector2f &point_on_line, const float line_bearing, const Vector2f &vehicle_pos, + const Vector2f &ground_vel, const Vector2f &wind_vel); + /* * Loitering (unlimited) logic. Takes loiter center, radius, and direction and * determines the relevant parameters for evaluating the NPFG guidance law, * then updates control setpoints. * * @param[in] loiter_center The position of the center of the loiter circle [m] - * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] radius Loiter radius [m] * @param[in] loiter_direction_counter_clockwise Specifies loiter direction * @param[in] ground_vel Vehicle ground velocity vector [m/s] @@ -824,8 +856,10 @@ private: * Path following logic. Takes poisiton, path tangent, curvature and * then updates control setpoints to follow a path setpoint. * - * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] - * @param[in] position_setpoint closest point on a path in WGS84 coordinates (lat,lon) [deg] + * TODO: deprecate this function with a proper API to NPFG. + * + * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m] + * @param[in] position_setpoint closest point on a path in local coordinates. (N,E) [m] * @param[in] tangent_setpoint unit tangent vector of the path [m] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] @@ -839,9 +873,9 @@ private: * Navigate on a fixed bearing. * * This only holds a certain (ground relative) direction and does not perform - * cross track correction. Helpful for semi-autonomous modes. Similar to navigateHeading. + * cross track correction. Helpful for semi-autonomous modes. * - * @param[in] vehicle_pos vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg] + * @param[in] vehicle_pos vehicle_pos Vehicle position in local coordinates. (N,E) [m] * @param[in] bearing Bearing angle [rad] * @param[in] ground_vel Vehicle ground velocity vector [m/s] * @param[in] wind_vel Wind velocity vector [m/s] 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 86d5875b0b..af04e328cf 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2016 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -249,9 +249,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that - * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. + * Maximum throttle limit in altitude controlled modes. + * Should be set accordingly to achieve FW_T_CLMB_MAX. * * @unit norm * @min 0.0 @@ -265,11 +264,9 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide - * some aerodynamic drag from a turning prop to improve the descent rate. + * Minimum throttle limit in altitude controlled modes. + * 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. @@ -382,10 +379,7 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 1); * * 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. This shifts the (often large) - * altitude and airspeed errors caused by the configuration change away from the ground such that - * these are not so critical. It also gives the controller enough time to adapt to the new - * configuration such that the landing approach starts with a cleaner initial state. + * final loiter-down (loiter-to-alt) waypoint before the landing approach. * * @boolean * @@ -989,8 +983,6 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); * system more robust against disturbances (turbulence) in high wind. * Only applies to AUTO flight mode. * - * airspeed_min_adjusted = FW_AIRSPD_MIN + FW_WIND_ARSP_SC * wind.length() - * * @min 0 * @decimal 2 * @increment 0.01 @@ -999,10 +991,9 @@ PARAM_DEFINE_INT32(FW_LND_ABORT, 3); PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f); /** - * FW Launch detection + * Fixed-wing launch detection * * Enables automatic launch detection based on measured acceleration. Use for hand- or catapult-launched vehicles. - * Only available for fixed-wing vehicles. * Not compatible with runway takeoff. * * @boolean diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 7948e6eb11..4d1bab51bd 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -48,6 +48,8 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) : _vehicle_thrust_setpoint_pub(vtol ? ORB_ID(vehicle_thrust_setpoint_virtual_fw) : ORB_ID(vehicle_thrust_setpoint)), _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { + _handle_param_vt_fw_difthr_en = param_find("VT_FW_DIFTHR_EN"); + /* fetch initial parameter values */ parameters_update(); @@ -86,6 +88,10 @@ FixedwingRateControl::parameters_update() // set FF gains to 0 as we add the FF control outside of the rate controller Vector3f(0.f, 0.f, 0.f)); + if (_handle_param_vt_fw_difthr_en != PARAM_INVALID) { + param_get(_handle_param_vt_fw_difthr_en, &_param_vt_fw_difthr_en); + } + return PX4_OK; } @@ -278,26 +284,41 @@ void FixedwingRateControl::Run() _rate_control.resetIntegral(); } - // update saturation status from control allocation feedback + // Update saturation status from control allocation feedback + // TODO: send the unallocated value directly for better anti-windup + Vector3 diffthr_enabled( + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::ROLL_BIT), + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::PITCH_BIT), + _param_vt_fw_difthr_en & static_cast(VTOLFixedWingDifferentialThrustEnabledBit::YAW_BIT) + ); + + if (_vehicle_status.is_vtol_tailsitter) { + // Swap roll and yaw + diffthr_enabled.swapRows(0, 2); + } + + // saturation handling for axis controlled by differential thrust (VTOL only) control_allocator_status_s control_allocator_status; - if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { - Vector saturation_positive; - Vector saturation_negative; - - if (!control_allocator_status.torque_setpoint_achieved) { - for (size_t i = 0; i < 3; i++) { - if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) { - saturation_positive(i) = true; - - } else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) { - saturation_negative(i) = true; - } + // Set saturation flags for VTOL differential thrust feature + // If differential thrust is enabled in an axis, assume it's the only torque authority and only update saturation using matrix 0 allocating the motors. + if (_control_allocator_status_subs[0].update(&control_allocator_status)) { + for (size_t i = 0; i < 3; i++) { + if (diffthr_enabled(i)) { + _rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON); + _rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON); } } + } - // TODO: send the unallocated value directly for better anti-windup - _rate_control.setSaturationStatus(saturation_positive, saturation_negative); + // Set saturation flags for control surface controlled axes + if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) { + for (size_t i = 0; i < 3; i++) { + if (!diffthr_enabled(i)) { + _rate_control.setPositiveSaturationFlag(i, control_allocator_status.unallocated_torque[i] > FLT_EPSILON); + _rate_control.setNegativeSaturationFlag(i, control_allocator_status.unallocated_torque[i] < -FLT_EPSILON); + } + } } /* bi-linear interpolation over airspeed for actuator trim scheduling */ diff --git a/src/modules/fw_rate_control/FixedwingRateControl.hpp b/src/modules/fw_rate_control/FixedwingRateControl.hpp index 37c70a800a..14f4a6427f 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.hpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.hpp @@ -144,6 +144,16 @@ private: bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states + // enum for bitmask of VT_FW_DIFTHR_EN parameter options + enum class VTOLFixedWingDifferentialThrustEnabledBit : int32_t { + YAW_BIT = (1 << 0), + ROLL_BIT = (1 << 1), + PITCH_BIT = (1 << 2), + }; + + param_t _handle_param_vt_fw_difthr_en{PARAM_INVALID}; + int32_t _param_vt_fw_difthr_en{0}; + DEFINE_PARAMETERS( (ParamFloat) _param_fw_acro_x_max, (ParamFloat) _param_fw_acro_y_max, diff --git a/src/modules/fw_rate_control/fw_rate_control_params.c b/src/modules/fw_rate_control/fw_rate_control_params.c index f52ed4b59d..cffd790401 100644 --- a/src/modules/fw_rate_control/fw_rate_control_params.c +++ b/src/modules/fw_rate_control/fw_rate_control_params.c @@ -46,10 +46,9 @@ * The minimal airspeed (calibrated airspeed) the user is able to command. * Further, if the airspeed falls below this value, the TECS controller will try to * increase airspeed more aggressively. - * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL), - * with some margin between the stall speed and minimum airspeed. + * Should be set (with some margin) above the vehicle stall speed. * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight), - * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). + * and is automatically adapated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE). * * @unit m/s * @min 0.5 @@ -142,9 +141,6 @@ PARAM_DEFINE_FLOAT(FW_PR_D, 0.f); /** * Pitch rate integrator gain. * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * * @unit %/rad * @min 0.0 * @max 10 @@ -157,9 +153,6 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); /** * Pitch rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -169,7 +162,7 @@ PARAM_DEFINE_FLOAT(FW_PR_I, 0.1f); PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); /** - * Roll rate proportional Gain + * Roll rate proportional gain * * @unit %/rad/s * @min 0.0 @@ -181,10 +174,7 @@ PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f); PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); /** - * Roll rate derivative Gain - * - * Roll rate differential gain. Small values help reduce fast oscillations. - * If value is too big oscillations will appear again. + * Roll rate derivative gain * * @unit %/rad/s * @min 0.0 @@ -193,13 +183,10 @@ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); * @increment 0.005 * @group FW Rate Control */ -PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); +PARAM_DEFINE_FLOAT(FW_RR_D, 0.0f); /** - * Roll rate integrator Gain - * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. + * Roll rate integrator gain * * @unit %/rad * @min 0.0 @@ -211,9 +198,7 @@ PARAM_DEFINE_FLOAT(FW_RR_D, 0.00f); PARAM_DEFINE_FLOAT(FW_RR_I, 0.1f); /** - * Roll integrator anti-windup - * - * The portion of the integrator part in the control surface deflection is limited to this value. + * Roll integrator limit * * @min 0.0 * @max 1.0 @@ -238,9 +223,6 @@ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); /** * Yaw rate derivative gain * - * Yaw rate differential gain. Small values help reduce fast oscillations. - * If value is too big oscillations will appear again. - * * @unit %/rad/s * @min 0.0 * @max 10 @@ -253,9 +235,6 @@ PARAM_DEFINE_FLOAT(FW_YR_D, 0.0f); /** * Yaw rate integrator gain * - * This gain defines how much control response will result out of a steady - * state error. It trims any constant error. - * * @unit %/rad * @min 0.0 * @max 10 @@ -268,9 +247,6 @@ PARAM_DEFINE_FLOAT(FW_YR_I, 0.1f); /** * Yaw rate integrator limit * - * The portion of the integrator part in the control surface deflection is - * limited to this value - * * @min 0.0 * @max 1.0 * @decimal 2 @@ -282,9 +258,7 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f); /** * Roll rate feed forward * - * Direct feed forward from rate setpoint to control surface output. Use this - * to obtain a tigher response of the controller without introducing - * noise amplification. + * Direct feed forward from rate setpoint to control surface output. * * @unit %/rad/s * @min 0.0 @@ -324,10 +298,7 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f); PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); /** - * Acro body x max rate. - * - * This is the rate the controller is trying to achieve if the user applies full roll - * stick input in acro mode. + * Acro body roll max rate setpoint * * @min 10 * @max 720 @@ -337,7 +308,7 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f); PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); /** - * Acro body pitch max rate setpoint. + * Acro body pitch max rate setpoint * * @min 10 * @max 720 @@ -347,7 +318,7 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90); PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90); /** - * Acro body yaw max rate setpoint. + * Acro body yaw max rate setpoint * * @min 10 * @max 720 diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp index 3783c3d852..5e09de57d7 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.cpp +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.cpp @@ -100,13 +100,13 @@ void LandingTargetEstimator::update() } } - if (!_new_sensorReport) { + if (!_new_irlockReport) { // nothing to do return; } // mark this sensor measurement as consumed - _new_sensorReport = false; + _new_irlockReport = false; if (!_estimator_initialized) { @@ -254,30 +254,7 @@ void LandingTargetEstimator::_update_topics() _target_position_report.rel_pos_x += _params.offset_x; _target_position_report.rel_pos_y += _params.offset_y; - _new_sensorReport = true; - - } else if (_uwbDistanceSub.update(&_uwbDistance)) { - if (!_vehicleAttitude_valid || !_vehicleLocalPosition_valid) { - // don't have the data needed for an update - PX4_INFO("Attitude: %d, Local pos: %d", _vehicleAttitude_valid, _vehicleLocalPosition_valid); - return; - } - - if (!matrix::Vector3f(_uwbDistance.position).isAllFinite()) { - PX4_WARN("Marker position reading invalid!"); - return; - } - - _new_sensorReport = true; - - // The coordinate system is NED (north-east-down) - // the uwb_distance msg contains the Position in NED, Vehicle relative to LP - // The coordinates "rel_pos_*" are the position of the landing point relative to the vehicle. - // To change POV we negate every Axis: - _target_position_report.timestamp = _uwbDistance.timestamp; - _target_position_report.rel_pos_x = -_uwbDistance.position[0]; - _target_position_report.rel_pos_y = -_uwbDistance.position[1]; - _target_position_report.rel_pos_z = -_uwbDistance.position[2]; + _new_irlockReport = true; } } diff --git a/src/modules/landing_target_estimator/LandingTargetEstimator.h b/src/modules/landing_target_estimator/LandingTargetEstimator.h index 521a1e3763..5c78147329 100644 --- a/src/modules/landing_target_estimator/LandingTargetEstimator.h +++ b/src/modules/landing_target_estimator/LandingTargetEstimator.h @@ -54,9 +54,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -153,14 +150,11 @@ private: uORB::Subscription _attitudeSub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::Subscription _irlockReportSub{ORB_ID(irlock_report)}; - uORB::Subscription _uwbDistanceSub{ORB_ID(uwb_distance)}; vehicle_local_position_s _vehicleLocalPosition{}; vehicle_attitude_s _vehicleAttitude{}; vehicle_acceleration_s _vehicle_acceleration{}; irlock_report_s _irlockReport{}; - uwb_grid_s _uwbGrid{}; - uwb_distance_s _uwbDistance{}; // keep track of which topics we have received bool _vehicleLocalPosition_valid{false}; diff --git a/src/modules/landing_target_estimator/landing_target_estimator_params.c b/src/modules/landing_target_estimator/landing_target_estimator_params.c index 24ee9b772f..92048efc31 100644 --- a/src/modules/landing_target_estimator/landing_target_estimator_params.c +++ b/src/modules/landing_target_estimator/landing_target_estimator_params.c @@ -50,7 +50,7 @@ * * @min 0 * @max 1 - * @group Landing target Estimator + * @group Landing Target Estimator * @value 0 Moving * @value 1 Stationary */ @@ -66,7 +66,7 @@ PARAM_DEFINE_INT32(LTEST_MODE, 0); * @min 0.01 * @decimal 2 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); @@ -79,7 +79,7 @@ PARAM_DEFINE_FLOAT(LTEST_ACC_UNC, 10.0f); * @unit tan(rad)^2 * @decimal 4 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); @@ -92,7 +92,7 @@ PARAM_DEFINE_FLOAT(LTEST_MEAS_UNC, 0.005f); * @min 0.001 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); @@ -105,7 +105,7 @@ PARAM_DEFINE_FLOAT(LTEST_POS_UNC_IN, 0.1f); * @min 0.001 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); @@ -117,7 +117,7 @@ PARAM_DEFINE_FLOAT(LTEST_VEL_UNC_IN, 0.1f); * @min 0.01 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); @@ -129,7 +129,7 @@ PARAM_DEFINE_FLOAT(LTEST_SCALE_X, 1.0f); * @min 0.01 * @decimal 3 * - * @group Landing target Estimator + * @group Landing Target Estimator */ PARAM_DEFINE_FLOAT(LTEST_SCALE_Y, 1.0f); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 508a542754..9ece702888 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -600,7 +600,10 @@ void BlockLocalPositionEstimator::publishLocalPos() _pub_lpos.get().z = xLP(X_z); // down } - _pub_lpos.get().heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + const float heading = matrix::Eulerf(matrix::Quatf(_sub_att.get().q)).psi(); + _pub_lpos.get().heading = heading; + _pub_lpos.get().heading_good_for_control = PX4_ISFINITE(heading); + _pub_lpos.get().unaided_heading = NAN; _pub_lpos.get().vx = xLP(X_vx); // north _pub_lpos.get().vy = xLP(X_vy); // east diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index acfef88097..d3306e8d69 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -92,9 +92,9 @@ int BlockLocalPositionEstimator::gpsMeasure(Vector &y) { // gps measurement y.setZero(); - y(0) = _sub_gps.get().lat * 1e-7; - y(1) = _sub_gps.get().lon * 1e-7; - y(2) = _sub_gps.get().alt * 1e-3; + y(0) = _sub_gps.get().latitude_deg; + y(1) = _sub_gps.get().longitude_deg; + y(2) = _sub_gps.get().altitude_msl_m; y(3) = (double)_sub_gps.get().vel_n_m_s; y(4) = (double)_sub_gps.get().vel_e_m_s; y(5) = (double)_sub_gps.get().vel_d_m_s; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3e729489a9..e9f3c3004a 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -330,7 +330,7 @@ void LoggedTopics::add_estimator_replay_topics() // current EKF2 subscriptions add_topic("airspeed"); - add_topic("optical_flow"); + add_topic("vehicle_optical_flow"); add_topic("sensor_combined"); add_topic("sensor_selection"); add_topic("vehicle_air_data"); diff --git a/src/modules/logger/logged_topics.h b/src/modules/logger/logged_topics.h index 4a990d5ba7..de27574ce8 100644 --- a/src/modules/logger/logged_topics.h +++ b/src/modules/logger/logged_topics.h @@ -88,7 +88,7 @@ public: RequestedSubscription sub[MAX_TOPICS_NUM]; int count{0}; - uint8_t excluded_optional_topic_ids[MAX_EXCLUDED_OPTIONAL_TOPICS_NUM]; + orb_id_size_t excluded_optional_topic_ids[MAX_EXCLUDED_OPTIONAL_TOPICS_NUM]; int num_excluded_optional_topic_ids{0}; }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 1068d511bd..247c0b9e84 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -2103,7 +2103,8 @@ void Logger::write_version(LogType type) // data versioning: increase this on every larger data change (format/semantic) // 1: switch to FIFO drivers (disabled on-chip DLPF) - write_info(type, "ver_data_format", static_cast(1)); + // 2: changed lat/lon/alt* to double to accommodate RTK GPS centimeter level precision + write_info(type, "ver_data_format", static_cast(2)); #ifndef BOARD_HAS_NO_UUID diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h index 57b324d0c0..1531ac8b73 100644 --- a/src/modules/logger/logger.h +++ b/src/modules/logger/logger.h @@ -365,7 +365,7 @@ private: uint16_t _event_sequence_offset{0}; ///< event sequence offset to account for skipped (not logged) messages uint16_t _event_sequence_offset_mission{0}; - uint8_t _excluded_optional_topic_ids[LoggedTopics::MAX_EXCLUDED_OPTIONAL_TOPICS_NUM]; + orb_id_size_t _excluded_optional_topic_ids[LoggedTopics::MAX_EXCLUDED_OPTIONAL_TOPICS_NUM]; int _num_excluded_optional_topic_ids{0}; LogWriter _writer; diff --git a/src/modules/manual_control/CMakeLists.txt b/src/modules/manual_control/CMakeLists.txt index c96478afdf..279d9e8993 100644 --- a/src/modules/manual_control/CMakeLists.txt +++ b/src/modules/manual_control/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ px4_add_module( - MODULE module__manual_control + MODULE modules__manual_control MAIN manual_control COMPILE_FLAGS SRCS @@ -42,7 +42,9 @@ px4_add_module( ManualControlSelector.hpp ManualControlSelector.cpp DEPENDS + hysteresis px4_work_queue ) -px4_add_unit_gtest(SRC ManualControlSelectorTest.cpp LINKLIBS module__manual_control) +px4_add_functional_gtest(SRC ManualControlTest.cpp LINKLIBS modules__manual_control) +px4_add_unit_gtest(SRC ManualControlSelectorTest.cpp LINKLIBS modules__manual_control) diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp index c3b42c258e..9c01750f31 100644 --- a/src/modules/manual_control/ManualControl.cpp +++ b/src/modules/manual_control/ManualControl.cpp @@ -41,6 +41,7 @@ ManualControl::ManualControl() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) { + updateParams(); } ManualControl::~ManualControl() @@ -66,10 +67,21 @@ void ManualControl::Run() perf_begin(_loop_perf); perf_count(_loop_interval_perf); + processInput(hrt_absolute_time()); + + // reschedule to detect timeouts + ScheduleDelayed(200_ms); + + perf_end(_loop_perf); +} + +void ManualControl::processInput(hrt_abstime now) +{ 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); _system_id = vehicle_status.system_id; _rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); _vtol = vehicle_status.is_vtol; @@ -83,83 +95,25 @@ void ManualControl::Run() _parameter_update_sub.copy(¶m_update); updateParams(); - - _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); - _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); - _button_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); - - _selector.setRcInMode(_param_com_rc_in_mode.get()); - _selector.setTimeout(_param_com_rc_loss_t.get() * 1_s); - - // MAN_ARM_GESTURE - if (_param_man_arm_gesture.get() == 1) { - // RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured - param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW"); - - if (param_rc_map_arm_sw != PARAM_INVALID) { - int32_t rc_map_arm_sw = 0; - param_get(param_rc_map_arm_sw, &rc_map_arm_sw); - - if (rc_map_arm_sw > 0) { - _param_man_arm_gesture.set(0); // disable arm gesture - _param_man_arm_gesture.commit(); - - orb_advert_t mavlink_log_pub = nullptr; - mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t") - /* EVENT - * @description MAN_ARM_GESTURE is now set to disable arm/disarm stick gesture. - */ - events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled}, - "Arm stick gesture disabled if arm switch in use"); - } - } - - // MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture - if ((_param_man_arm_gesture.get() == 1) && (_rotary_wing || _vtol)) { - param_t param_mc_airmode = param_find("MC_AIRMODE"); - - if (param_mc_airmode != PARAM_INVALID) { - int32_t airmode = 0; - param_get(param_mc_airmode, &airmode); - - if (airmode == 2) { - airmode = 1; // change to roll/pitch airmode - param_set(param_mc_airmode, &airmode); - - orb_advert_t mavlink_log_pub = nullptr; - mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t") - /* EVENT - * @description MC_AIRMODE is now set to roll/pitch airmode. - */ - events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled}, - "Yaw Airmode requires disabling the stick arm gesture"); - } - } - } - } } - const hrt_abstime now = hrt_absolute_time(); _selector.updateValidityOfChosenInput(now); for (int i = 0; i < MAX_MANUAL_INPUT_COUNT; i++) { manual_control_setpoint_s manual_control_input; - if (_manual_control_setpoint_subs[i].update(&manual_control_input)) { + if (_manual_control_input_subs[i].update(&manual_control_input)) { _selector.updateWithNewInputSample(now, manual_control_input, i); } } - manual_control_switches_s switches; - bool switches_updated = _manual_control_switches_sub.update(&switches); - if (_selector.setpoint().valid) { _published_invalid_once = false; processStickArming(_selector.setpoint()); // User override by stick - const float dt_s = (now - _last_time) / 1e6f; + const float dt_s = (now - _timestamp_last_loop) / 1e6f; const float minimum_stick_change = 0.01f * _param_com_rc_stick_ov.get(); _selector.setpoint().sticks_moving = (fabsf(_roll_diff.update(_selector.setpoint().roll, dt_s)) > minimum_stick_change) @@ -167,136 +121,20 @@ void ManualControl::Run() || (fabsf(_yaw_diff.update(_selector.setpoint().yaw, dt_s)) > minimum_stick_change) || (fabsf(_throttle_diff.update(_selector.setpoint().throttle, dt_s)) > minimum_stick_change); - if (switches_updated) { - // Only use switches if current source is RC as well. - if (_selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) { - if (_previous_switches_initialized) { - if (switches.mode_slot != _previous_switches.mode_slot) { - evaluateModeSlot(switches.mode_slot); - } - - if (_param_com_arm_swisbtn.get()) { - // Arming button - const bool previous_button_hysteresis = _button_hysteresis.get_state(); - _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); - - if (!previous_button_hysteresis && _button_hysteresis.get_state()) { - sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_request_s::SOURCE_RC_BUTTON); - } - - } else { - // Arming switch - if (switches.arm_switch != _previous_switches.arm_switch) { - if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_SWITCH); - - } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { - sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_SWITCH); - } - } - } - - if (switches.return_switch != _previous_switches.return_switch) { - if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); - - } else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) { - evaluateModeSlot(switches.mode_slot); - } - } - - if (switches.loiter_switch != _previous_switches.loiter_switch) { - if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); - - } else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) { - evaluateModeSlot(switches.mode_slot); - } - } - - if (switches.offboard_switch != _previous_switches.offboard_switch) { - if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, - vehicle_status_s::NAVIGATION_STATE_OFFBOARD); - - } else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) { - evaluateModeSlot(switches.mode_slot); - } - } - - if (switches.kill_switch != _previous_switches.kill_switch) { - if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH); - - } else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { - sendActionRequest(action_request_s::ACTION_UNKILL, action_request_s::SOURCE_RC_SWITCH); - } - } - - if (switches.gear_switch != _previous_switches.gear_switch - && _previous_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) { - - if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { - publishLandingGear(landing_gear_s::GEAR_UP); - - } else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { - publishLandingGear(landing_gear_s::GEAR_DOWN); - } - } - - if (switches.transition_switch != _previous_switches.transition_switch) { - if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) { - sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING, action_request_s::SOURCE_RC_SWITCH); - - } else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) { - sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER, action_request_s::SOURCE_RC_SWITCH); - } - } - - if (switches.photo_switch != _previous_switches.photo_switch) { - if (switches.photo_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_camera_mode_command(CameraMode::Image); - send_photo_command(); - } - } - - if (switches.video_switch != _previous_switches.video_switch) { - if (switches.video_switch == manual_control_switches_s::SWITCH_POS_ON) { - send_camera_mode_command(CameraMode::Video); - send_video_command(); - } - } - - } else { - // Send an initial request to switch to the mode requested by RC - evaluateModeSlot(switches.mode_slot); - } - - _previous_switches_initialized = true; - _previous_switches = switches; - - } else { - _previous_switches_initialized = false; - } - } - _selector.setpoint().timestamp = now; _manual_control_setpoint_pub.publish(_selector.setpoint()); - // If it's valid, this should really be valid but better safe than sorry. + // Attach scheduling to new samples of the chosen input const int instance = _selector.instance(); - // Attach scheduling to new samples of the chosen input if (instance != _previous_manual_control_input_instance) { if ((0 <= _previous_manual_control_input_instance) && (_previous_manual_control_input_instance < MAX_MANUAL_INPUT_COUNT)) { - _manual_control_setpoint_subs[_previous_manual_control_input_instance].unregisterCallback(); + _manual_control_input_subs[_previous_manual_control_input_instance].unregisterCallback(); } if ((0 <= instance) && (instance < MAX_MANUAL_INPUT_COUNT)) { - _manual_control_setpoint_subs[instance].registerCallback(); + _manual_control_input_subs[instance].registerCallback(); } _previous_manual_control_input_instance = instance; @@ -319,12 +157,191 @@ void ManualControl::Run() _button_hysteresis.set_state_and_update(false, now); } - _last_time = now; + processSwitches(now); - // reschedule timeout - ScheduleDelayed(200_ms); + _timestamp_last_loop = now; +} - perf_end(_loop_perf); +void ManualControl::processSwitches(hrt_abstime &now) +{ + manual_control_switches_s switches; + const bool switches_updated = _manual_control_switches_sub.update(&switches); + + // Only use switches if the currently valid source is RC as well + if (_selector.setpoint().valid + && _selector.setpoint().data_source == manual_control_setpoint_s::SOURCE_RC) { + if (switches_updated) { + if (_previous_switches_initialized) { + if (switches.mode_slot != _previous_switches.mode_slot) { + evaluateModeSlot(switches.mode_slot); + } + + if (_param_com_arm_swisbtn.get()) { + // Arming button + const bool previous_button_hysteresis = _button_hysteresis.get_state(); + _button_hysteresis.set_state_and_update(switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON, now); + + if (!previous_button_hysteresis && _button_hysteresis.get_state()) { + sendActionRequest(action_request_s::ACTION_TOGGLE_ARMING, action_request_s::SOURCE_RC_BUTTON); + } + + } else { + // Arming switch + if (switches.arm_switch != _previous_switches.arm_switch) { + if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_ARM, action_request_s::SOURCE_RC_SWITCH); + + } else if (switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF) { + sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_SWITCH); + } + } + } + + if (switches.return_switch != _previous_switches.return_switch) { + if (switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, + vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + + } else if (switches.return_switch == manual_control_switches_s::SWITCH_POS_OFF) { + evaluateModeSlot(switches.mode_slot); + } + } + + if (switches.loiter_switch != _previous_switches.loiter_switch) { + if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, + vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER); + + } else if (switches.loiter_switch == manual_control_switches_s::SWITCH_POS_OFF) { + evaluateModeSlot(switches.mode_slot); + } + } + + if (switches.offboard_switch != _previous_switches.offboard_switch) { + if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_SWITCH_MODE, action_request_s::SOURCE_RC_SWITCH, + vehicle_status_s::NAVIGATION_STATE_OFFBOARD); + + } else if (switches.offboard_switch == manual_control_switches_s::SWITCH_POS_OFF) { + evaluateModeSlot(switches.mode_slot); + } + } + + if (switches.kill_switch != _previous_switches.kill_switch) { + if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_SWITCH); + + } else if (switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) { + sendActionRequest(action_request_s::ACTION_UNKILL, action_request_s::SOURCE_RC_SWITCH); + } + } + + if (switches.gear_switch != _previous_switches.gear_switch + && _previous_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) { + + if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) { + publishLandingGear(landing_gear_s::GEAR_UP); + + } else if (switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) { + publishLandingGear(landing_gear_s::GEAR_DOWN); + } + } + + if (switches.transition_switch != _previous_switches.transition_switch) { + if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_ON) { + sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING, action_request_s::SOURCE_RC_SWITCH); + + } else if (switches.transition_switch == manual_control_switches_s::SWITCH_POS_OFF) { + sendActionRequest(action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER, action_request_s::SOURCE_RC_SWITCH); + } + } + + if (switches.photo_switch != _previous_switches.photo_switch) { + if (switches.photo_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_camera_mode_command(CameraMode::Image); + send_photo_command(); + } + } + + if (switches.video_switch != _previous_switches.video_switch) { + if (switches.video_switch == manual_control_switches_s::SWITCH_POS_ON) { + send_camera_mode_command(CameraMode::Video); + send_video_command(); + } + } + + } else if (!_armed) { + // Directly initialize mode using RC switch but only before arming + evaluateModeSlot(switches.mode_slot); + } + + _previous_switches = switches; + _previous_switches_initialized = true; + } + + } else { + // Don't react on switch changes while RC was not in use + _previous_switches_initialized = false; + } +} + +void ManualControl::updateParams() +{ + ModuleParams::updateParams(); + + _stick_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); + _stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); + _button_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms); + + _selector.setRcInMode(_param_com_rc_in_mode.get()); + _selector.setTimeout(_param_com_rc_loss_t.get() * 1_s); + + // MAN_ARM_GESTURE + if (_param_man_arm_gesture.get() == 1) { + // RC_MAP_ARM_SW & MAN_ARM_GESTURE: disable arm gesture if an arm switch is configured + param_t param_rc_map_arm_sw = param_find("RC_MAP_ARM_SW"); + + if (param_rc_map_arm_sw != PARAM_INVALID) { + int32_t rc_map_arm_sw = 0; + param_get(param_rc_map_arm_sw, &rc_map_arm_sw); + + if (rc_map_arm_sw > 0) { + _param_man_arm_gesture.set(0); // disable arm gesture + _param_man_arm_gesture.commit(); + + orb_advert_t mavlink_log_pub = nullptr; + mavlink_log_critical(&mavlink_log_pub, "Arm stick gesture disabled if arm switch in use\t") + /* EVENT + * @description MAN_ARM_GESTURE is now set to disable arm/disarm stick gesture. + */ + events::send(events::ID("rc_update_arm_stick_gesture_disabled_with_switch"), {events::Log::Info, events::LogInternal::Disabled}, + "Arm stick gesture disabled if arm switch in use"); + } + } + + // MC_AIRMODE & MAN_ARM_GESTURE: check for unsafe Airmode settings: yaw airmode requires disabling the stick arm gesture + if ((_param_man_arm_gesture.get() == 1) && (_rotary_wing || _vtol)) { + param_t param_mc_airmode = param_find("MC_AIRMODE"); + + if (param_mc_airmode != PARAM_INVALID) { + int32_t airmode = 0; + param_get(param_mc_airmode, &airmode); + + if (airmode == 2) { + airmode = 1; // change to roll/pitch airmode + param_set(param_mc_airmode, &airmode); + + orb_advert_t mavlink_log_pub = nullptr; + mavlink_log_critical(&mavlink_log_pub, "Yaw Airmode requires disabling the stick arm gesture\t") + /* EVENT + * @description MC_AIRMODE is now set to roll/pitch airmode. + */ + events::send(events::ID("commander_airmode_requires_no_arm_gesture"), {events::Log::Error, events::LogInternal::Disabled}, + "Yaw Airmode requires disabling the stick arm gesture"); + } + } + } + } } void ManualControl::processStickArming(const manual_control_setpoint_s &input) diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp index d28166112e..8e8bce13a5 100644 --- a/src/modules/manual_control/ManualControl.hpp +++ b/src/modules/manual_control/ManualControl.hpp @@ -74,21 +74,23 @@ public: int print_status() override; +protected: + // protected for testing + void processInput(hrt_abstime now); + static int8_t navStateFromParam(int32_t param_value); + private: static constexpr int MAX_MANUAL_INPUT_COUNT = 3; void Run() override; + void updateParams() override; void processStickArming(const manual_control_setpoint_s &input); - - static int8_t navStateFromParam(int32_t param_value); + void processSwitches(hrt_abstime &now); void evaluateModeSlot(uint8_t mode_slot); void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0); void publishLandingGear(int8_t action); - uORB::Publication _action_request_pub{ORB_ID(action_request)}; - uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; - enum class CameraMode { Image = 0, Video = 1 @@ -97,39 +99,48 @@ private: void send_photo_command(); void send_video_command(); - uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; - - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; - - int _previous_manual_control_input_instance{-1}; - uORB::SubscriptionCallbackWorkItem _manual_control_setpoint_subs[MAX_MANUAL_INPUT_COUNT] { + uORB::SubscriptionCallbackWorkItem _manual_control_input_subs[MAX_MANUAL_INPUT_COUNT] { {this, ORB_ID(manual_control_input), 0}, {this, ORB_ID(manual_control_input), 1}, {this, ORB_ID(manual_control_input), 2}, }; uORB::SubscriptionCallbackWorkItem _manual_control_switches_sub{this, ORB_ID(manual_control_switches)}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + + uORB::Publication _action_request_pub{ORB_ID(action_request)}; + uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; + uORB::Publication _manual_control_setpoint_pub{ORB_ID(manual_control_setpoint)}; + + ManualControlSelector _selector; + + hrt_abstime _timestamp_last_loop{0}; + int _previous_manual_control_input_instance{-1}; + bool _previous_switches_initialized{false}; + manual_control_switches_s _previous_switches{}; + bool _published_invalid_once{false}; systemlib::Hysteresis _stick_arm_hysteresis{false}; systemlib::Hysteresis _stick_disarm_hysteresis{false}; systemlib::Hysteresis _button_hysteresis{false}; - ManualControlSelector _selector; - bool _published_invalid_once{false}; - MovingDiff _roll_diff{}; MovingDiff _pitch_diff{}; MovingDiff _yaw_diff{}; MovingDiff _throttle_diff{}; - manual_control_switches_s _previous_switches{}; - bool _previous_switches_initialized{false}; - - hrt_abstime _last_time{0}; - perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + // Camera control state TODO: hopefully there is a command soon to toggle without keeping state + unsigned _image_sequence{0}; + bool _video_recording{false}; + + bool _armed{false}; + uint8_t _system_id{1}; + bool _rotary_wing{false}; + bool _vtol{false}; + DEFINE_PARAMETERS( (ParamInt) _param_com_rc_in_mode, (ParamFloat) _param_com_rc_loss_t, @@ -144,11 +155,4 @@ private: (ParamInt) _param_fltmode_5, (ParamInt) _param_fltmode_6 ) - - unsigned _image_sequence {0}; - bool _video_recording {false}; // TODO: hopefully there is a command soon to toggle without keeping state - - uint8_t _system_id{1}; - bool _rotary_wing{false}; - bool _vtol{false}; }; diff --git a/src/modules/manual_control/ManualControlSelector.cpp b/src/modules/manual_control/ManualControlSelector.cpp index 3f0c79e053..f498534f72 100644 --- a/src/modules/manual_control/ManualControlSelector.cpp +++ b/src/modules/manual_control/ManualControlSelector.cpp @@ -46,7 +46,7 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_ // First check if the chosen input got invalid, so it can get replaced updateValidityOfChosenInput(now); - const bool update_existing_input = _setpoint.valid && input.data_source == _setpoint.data_source; + const bool update_existing_input = _setpoint.valid && (input.data_source == _setpoint.data_source); const bool start_using_new_input = !_setpoint.valid; // Switch to new input if it's valid and we don't already have a valid one diff --git a/src/modules/manual_control/ManualControlTest.cpp b/src/modules/manual_control/ManualControlTest.cpp new file mode 100644 index 0000000000..20591b48dc --- /dev/null +++ b/src/modules/manual_control/ManualControlTest.cpp @@ -0,0 +1,331 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#define MODULE_NAME "manual_control" + +#include +#include "ManualControl.hpp" + +static constexpr uint64_t SOME_TIME = 12345678; + +static constexpr uint8_t ACTION_KILL = action_request_s::ACTION_KILL; +static constexpr uint8_t ACTION_UNKILL = action_request_s::ACTION_UNKILL; +static constexpr uint8_t ACTION_VTOL_TRANSITION_TO_FIXEDWING = action_request_s::ACTION_VTOL_TRANSITION_TO_FIXEDWING; +static constexpr uint8_t ACTION_VTOL_TRANSITION_TO_MULTICOPTER = + action_request_s::ACTION_VTOL_TRANSITION_TO_MULTICOPTER; +static constexpr uint8_t ACTION_SWITCH_MODE = action_request_s::ACTION_SWITCH_MODE; + +static constexpr uint8_t NAVIGATION_STATE_MANUAL = vehicle_status_s::NAVIGATION_STATE_MANUAL; +static constexpr uint8_t NAVIGATION_STATE_ALTCTL = vehicle_status_s::NAVIGATION_STATE_ALTCTL; +static constexpr uint8_t NAVIGATION_STATE_POSCTL = vehicle_status_s::NAVIGATION_STATE_POSCTL; +static constexpr uint8_t NAVIGATION_STATE_AUTO_MISSION = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; +static constexpr uint8_t NAVIGATION_STATE_AUTO_LOITER = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER; +static constexpr uint8_t NAVIGATION_STATE_ACRO = vehicle_status_s::NAVIGATION_STATE_ACRO; + +class TestManualControl : public ManualControl +{ +public: + void processInput(hrt_abstime now) { ManualControl::processInput(now); } + static int8_t navStateFromParam(int32_t param_value) { return ManualControl::navStateFromParam(param_value); } +}; + +class SwitchTest : public ::testing::Test +{ +public: + void SetUp() override + { + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + + // Set stick input timeout to half a second + const float com_rc_loss_t = .5f; + param_set(param_find("COM_RC_LOSS_T"), &com_rc_loss_t); + + int32_t mode = NAVIGATION_STATE_ACRO; + param_set(param_find("COM_FLTMODE1"), &mode); + mode = NAVIGATION_STATE_MANUAL; + param_set(param_find("COM_FLTMODE2"), &mode); + mode = NAVIGATION_STATE_ALTCTL; + param_set(param_find("COM_FLTMODE3"), &mode); + mode = NAVIGATION_STATE_POSCTL; + param_set(param_find("COM_FLTMODE4"), &mode); + mode = NAVIGATION_STATE_AUTO_LOITER; + param_set(param_find("COM_FLTMODE5"), &mode); + mode = NAVIGATION_STATE_AUTO_MISSION; + param_set(param_find("COM_FLTMODE6"), &mode); + } + + uORB::Publication _manual_control_switches_pub{ORB_ID(manual_control_switches)}; + uORB::Publication _manual_control_input_pub{ORB_ID(manual_control_input)}; + uORB::SubscriptionData _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; + uORB::SubscriptionData _action_request_sub{ORB_ID(action_request)}; + + TestManualControl _manual_control; + hrt_abstime _timestamp{SOME_TIME}; +}; + + +TEST_F(SwitchTest, KillSwitch) +{ + // GIVEN: valid stick input from RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and kill switch in off position + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .kill_switch = manual_control_switches_s::SWITCH_POS_ON}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is published for use + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: the kill switch is switched on + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .kill_switch = manual_control_switches_s::SWITCH_POS_OFF}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: a kill action request is published + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_UNKILL); + + // WHEN: the kill switch is switched off again + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .kill_switch = manual_control_switches_s::SWITCH_POS_ON}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: an unkill action request is published + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_KILL); +} + +TEST_F(SwitchTest, TransitionSwitch) +{ + // GIVEN: valid stick input from RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and transition switch in off position + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_OFF}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is published for use + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: the transition switch is switched on + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_ON}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: a forward transition action request is published + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_VTOL_TRANSITION_TO_FIXEDWING); + + // WHEN: the kill switch is switched off again + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_OFF}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: an backward transition action request is published + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_VTOL_TRANSITION_TO_MULTICOPTER); +} + +TEST_F(SwitchTest, TransitionSwitchStaysRcLoss) +{ + // GIVEN: valid stick input from the RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and transition switch in off position + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_OFF}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is published for use + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: RC signal times out + _manual_control.processInput(_timestamp += 1_s); + + // THEN: the stick input is invalidated + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_FALSE(_manual_control_setpoint_sub.get().valid); + // and there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: RC signal comes back + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is avaialble again + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is still no action requested + EXPECT_FALSE(_action_request_sub.update()); +} + +TEST_F(SwitchTest, TransitionSwitchChangesRcLoss) +{ + // GIVEN: valid stick input from the RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and transition switch in off position + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_OFF}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is published for use + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: RC signal times out + _manual_control.processInput(_timestamp += 1_s); + + // THEN: the stick input is invalidated + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_FALSE(_manual_control_setpoint_sub.get().valid); + // and there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // WHEN: RC signal comes back with the switch on + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .transition_switch = manual_control_switches_s::SWITCH_POS_ON}); + _manual_control.processInput(_timestamp += 100_ms); + + // THEN: the stick input is avaialble again + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // but there is still no action requested + EXPECT_FALSE(_action_request_sub.update()); +} + +TEST_F(SwitchTest, ModeSwitch) +{ + // GIVEN: valid stick input from RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and mode switch in position 1 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1}); + _manual_control.processInput(_timestamp += 10_ms); + + // THEN: the stick input is published for use + EXPECT_TRUE(_manual_control_setpoint_sub.update()); + EXPECT_TRUE(_manual_control_setpoint_sub.get().valid); + // and action requested to switch to mode 1 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_ACRO)); + + // WHEN: the mode switch is switched to 2 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_2}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: action requested to switch to mode 2 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_MANUAL)); + + // WHEN: the mode switch is switched to 3 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_3}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: action requested to switch to mode 3 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_ALTCTL)); + + // WHEN: the mode switch is switched to 4 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_4}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: action requested to switch to mode 4 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_POSCTL)); + + // WHEN: the mode switch is switched to 5 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_5}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: action requested to switch to mode 5 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_AUTO_LOITER)); + + // WHEN: the mode switch is switched to 6 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_6}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: action requested to switch to mode 6 + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_AUTO_MISSION)); +} + +TEST_F(SwitchTest, ModeSwitchInitialization) +{ + // GIVEN: the mode switch is already in position 1 but there's no valid RC stick input yet + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: there is no action requested + EXPECT_FALSE(_action_request_sub.update()); + + // GIVEN: new valid stick input from RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: there is still no action requested because the switches were not updated yet + EXPECT_FALSE(_action_request_sub.update()); + + // GIVEN: switch update with the same positions + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: the mode switch is requested + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_ACRO)); +} + +TEST_F(SwitchTest, ModeSwitchInitializationArmed) +{ + // GIVEN: vehicle is armed + uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; + vehicle_status_pub.publish({.arming_state = vehicle_status_s::ARMING_STATE_ARMED}); + + // GIVEN: valid stick input from RC + _manual_control_input_pub.publish({.timestamp_sample = _timestamp, .valid = true, .data_source = manual_control_setpoint_s::SOURCE_RC}); + // and mode switch in position 1 + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_1}); + _manual_control.processInput(_timestamp += 10_ms); + + // THEN: no action requested because the vehicle is flying + EXPECT_FALSE(_action_request_sub.update()); + + // GIVEN: the switch changes position + _manual_control_switches_pub.publish({.timestamp_sample = _timestamp, .mode_slot = manual_control_switches_s::MODE_SLOT_2}); + _manual_control.processInput(_timestamp += 10_ms); + // THEN: the mode switch is requested + EXPECT_TRUE(_action_request_sub.update()); + EXPECT_EQ(_action_request_sub.get().action, ACTION_SWITCH_MODE); + EXPECT_EQ(_action_request_sub.get().mode, TestManualControl::navStateFromParam(NAVIGATION_STATE_MANUAL)); +} diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 45f7737e6e..01ccb41a77 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -131,6 +131,7 @@ px4_add_module( adsb airspeed component_general_json # for checksums.h + dataman_client drivers_accelerometer drivers_gyroscope drivers_magnetometer diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink index 18955a04c7..81524c2b34 160000 --- a/src/modules/mavlink/mavlink +++ b/src/modules/mavlink/mavlink @@ -1 +1 @@ -Subproject commit 18955a04c7c7467e00ea42b704addb4a9c12b53a +Subproject commit 81524c2b34aa08768f13091b1d94c421e64f96c3 diff --git a/src/modules/mavlink/mavlink_log_handler.cpp b/src/modules/mavlink/mavlink_log_handler.cpp index 8ae5061b3f..d4ed61d428 100644 --- a/src/modules/mavlink/mavlink_log_handler.cpp +++ b/src/modules/mavlink/mavlink_log_handler.cpp @@ -275,7 +275,7 @@ MavlinkLogHandler::_log_send_listing() mavlink_msg_log_entry_send_struct(_mavlink->get_channel(), &response); //-- If we're done listing, flag it. - if (_next_entry == _last_entry) { + if (_next_entry >= _last_entry) { _current_status = LogHandlerState::Idle; } else { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 40f6cbf819..f7ee306dd1 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1060,7 +1060,6 @@ Mavlink::send_autopilot_capabilities() msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; - msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2; msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 43f7fab016..ef8f1c3b55 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -228,8 +228,13 @@ static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast(ROTATION_PITCH_315), "Pitch: 315"); static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast(ROTATION_ROLL_90_PITCH_315), "Roll: 90, Pitch: 315"); + +// Note: Update the number (41, as of writing) below to the number of 'normal' rotation enums in MAVLink spec: +// https://mavlink.io/en/messages/common.html#MAV_SENSOR_ORIENTATION static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync"); +static_assert(MAV_SENSOR_ROTATION_CUSTOM == static_cast(ROTATION_CUSTOM), "Custom Rotation"); + static const StreamListItem streams_list[] = { #if defined(HEARTBEAT_HPP) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index e89f0f1f42..c4bf9c42f8 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -62,6 +62,7 @@ uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 }; int32_t MavlinkMissionManager::_current_seq = 0; bool MavlinkMissionManager::_transfer_in_progress = false; constexpr uint16_t MavlinkMissionManager::MAX_COUNT[]; +uint16_t MavlinkMissionManager::_mission_update_counter = 0; uint16_t MavlinkMissionManager::_geofence_update_counter = 0; uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; @@ -73,128 +74,102 @@ uint16_t MavlinkMissionManager::_safepoint_update_counter = 0; MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : _mavlink(mavlink) -{ - init_offboard_mission(); -} - -void -MavlinkMissionManager::init_offboard_mission() { if (!_dataman_init) { _dataman_init = true; - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); - - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); - } - mission_s mission_state; - int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); + if (success) { + init_offboard_mission(mission_state); + load_geofence_stats(); + load_safepoint_stats(); + + } else { + PX4_WARN("offboard mission init failed"); } - - if (ret > 0) { - _dataman_id = (dm_item_t)mission_state.dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; - _current_seq = mission_state.current_seq; - - } else if (ret < 0) { - PX4_WARN("offboard mission init failed (%i)", ret); - } - - load_geofence_stats(); - - load_safepoint_stats(); } _my_dataman_id = _dataman_id; } -int +void +MavlinkMissionManager::init_offboard_mission(mission_s mission_state) +{ + _dataman_id = (dm_item_t)mission_state.dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = mission_state.count; + _current_seq = mission_state.current_seq; + _land_start_marker = mission_state.land_start_index; + _land_marker = mission_state.land_index; + _mission_update_counter = mission_state.mission_update_counter; +} + +bool MavlinkMissionManager::load_geofence_stats() { mission_stats_entry_s stats; // initialize fence points count - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = stats.num_items; _geofence_update_counter = stats.update_counter; } - return ret; + return success; } -int +bool MavlinkMissionManager::load_safepoint_stats() { mission_stats_entry_s stats; // initialize safe points count - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (ret == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = stats.num_items; + _safepoint_update_counter = stats.update_counter; } - return ret; + return success; } /** * Publish mission topic to notify navigator about changes. */ -int -MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq) +void +MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman) { - // We want to make sure the whole struct is initialized including padding before getting written by dataman. mission_s mission{}; mission.timestamp = hrt_absolute_time(); mission.dataman_id = dataman_id; mission.count = count; mission.current_seq = seq; + mission.mission_update_counter = _mission_update_counter; + mission.geofence_update_counter = _geofence_update_counter; + mission.safe_points_update_counter = _safepoint_update_counter; + mission.land_start_index = _land_start_marker; + mission.land_index = _land_marker; - /* update mission state in dataman */ + /* update active mission state */ + _dataman_id = dataman_id; + _count[MAV_MISSION_TYPE_MISSION] = count; + _current_seq = seq; + _my_dataman_id = _dataman_id; - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); + _offboard_mission_pub.publish(mission); - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); - } + if (write_to_dataman) { + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); - int res = dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); - - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); - } - - if (res == sizeof(mission_s)) { - /* update active mission state */ - _dataman_id = dataman_id; - _count[MAV_MISSION_TYPE_MISSION] = count; - _current_seq = seq; - _my_dataman_id = _dataman_id; - - /* mission state saved successfully, publish offboard_mission topic */ - _offboard_mission_pub.publish(mission); - - return PX4_OK; - - } else { - PX4_ERR("WPM: can't save mission state"); - - if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { - _mavlink->send_statustext_critical("Mission storage: Unable to write to microSD\t"); - events::send(events::ID("mavlink_mission_storage_write_failure"), events::Log::Critical, - "Mission: Unable to write to storage"); + if (!success) { + PX4_ERR("Can't update mission state in Dataman"); } - - return PX4_ERROR; } } @@ -203,12 +178,13 @@ MavlinkMissionManager::update_geofence_count(unsigned count) { mission_stats_entry_s stats; stats.num_items = count; - stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data + stats.update_counter = ++_geofence_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_FENCE] = count; } else { @@ -222,6 +198,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count) return PX4_ERROR; } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); return PX4_OK; } @@ -233,9 +210,10 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) stats.update_counter = ++_safepoint_update_counter; /* update stats in dataman */ - int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + bool success = _dataman_client.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); - if (res == sizeof(mission_stats_entry_s)) { + if (success) { _count[MAV_MISSION_TYPE_RALLY] = count; } else { @@ -249,6 +227,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count) return PX4_ERROR; } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], _current_seq, false); return PX4_OK; } @@ -298,21 +277,20 @@ void MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) { mission_item_s mission_item{}; - int16_t bytes_read = 0; bool read_success = false; switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: { - bytes_read = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)); - read_success = (bytes_read == sizeof(mission_item_s)); + read_success = _dataman_client.readSync(_dataman_id, seq, reinterpret_cast(&mission_item), + sizeof(mission_item_s)); } break; case MAV_MISSION_TYPE_FENCE: { // Read a geofence point mission_fence_point_s mission_fence_point; - bytes_read = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)); - read_success = (bytes_read == sizeof(mission_fence_point_s)); + read_success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); mission_item.nav_cmd = mission_fence_point.nav_cmd; mission_item.frame = mission_fence_point.frame; @@ -332,8 +310,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point mission_safe_point_s mission_safe_point; - bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s)); - read_success = (bytes_read == sizeof(mission_safe_point_s)); + read_success = _dataman_client.readSync(DM_KEY_SAFE_POINTS, seq + 1, reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s)); mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT; mission_item.frame = mission_safe_point.frame; @@ -385,13 +363,12 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t if (_filesystem_errcount++ < FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT) { mavlink_log_critical(_mavlink->get_mavlink_log_pub(), - "Mission storage: Unable to read from storage, type: %" PRId8 " bytes read: %" PRId16 "\t", - (uint8_t)_mission_type, bytes_read); + "Mission storage: Unable to read from storage, type: %" PRId8 "\t", (uint8_t)_mission_type); /* EVENT - * @description Mission type: {1}. Number of bytes read: {2} + * @description Mission type: {1} */ - events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, - "Mission: Unable to read from storage", _mission_type, bytes_read); + events::send(events::ID("mavlink_mission_storage_read_failure"), events::Log::Error, + "Mission: Unable to read from storage", _mission_type); } PX4_DEBUG("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); @@ -494,8 +471,8 @@ MavlinkMissionManager::send() } else { _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 ({1} \\< {2})", mission_result.seq_current, mission_result.seq_total); + 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); } } } @@ -679,16 +656,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) _time_last_recv = hrt_absolute_time(); if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) { - if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); - - } else { - PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); - - _mavlink->send_statustext_critical("WPM: WP CURR CMD: Error setting ID\t"); - events::send(events::ID("mavlink_mission_err_id"), events::Log::Error, - "Failed to write current mission ID to storage"); - } + update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq); } else { PX4_ERR("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); @@ -916,6 +884,10 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: + _land_start_marker = -1; + _land_marker = -1; + ++_mission_update_counter; + /* alternate dataman ID anyway to let navigator know about changes */ if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) { @@ -955,21 +927,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission _transfer_current_seq = -1; - - if (_mission_type == MAV_MISSION_TYPE_FENCE) { - // We're about to write new geofence items, so take the lock. It will be released when - // switching back to idle - PX4_DEBUG("locking fence dataman items"); - - int ret = dm_lock(DM_KEY_FENCE_POINTS); - - if (ret == 0) { - _geofence_locked = true; - - } else { - PX4_ERR("locking failed (%i)", errno); - } - } + _transfer_land_start_marker = -1; + _transfer_land_marker = -1; } else if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -1006,15 +965,6 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) void MavlinkMissionManager::switch_to_idle_state() { - // when switching to idle, we *always* check if the lock was held and release it. - // This is to ensure we don't end up in a state where we forget to release it. - if (_geofence_locked) { - dm_unlock(DM_KEY_FENCE_POINTS); - _geofence_locked = false; - - PX4_DEBUG("unlocking geofence"); - } - _state = MAVLINK_WPM_STATE_IDLE; } @@ -1129,9 +1079,24 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) check_failed = true; } else { - dm_item_t dm_item = _transfer_dataman_id; - write_failed = dm_write(dm_item, wp.seq, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s); + write_failed = !_dataman_client.writeSync(_transfer_dataman_id, wp.seq, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); + + // Check for land start marker + if ((mission_item.nav_cmd == MAV_CMD_DO_LAND_START) && (_transfer_land_start_marker == -1)) { + _transfer_land_start_marker = wp.seq; + } + + // Check for land index + if (((mission_item.nav_cmd == MAV_CMD_NAV_VTOL_LAND) || (mission_item.nav_cmd == MAV_CMD_NAV_LAND)) + && (_transfer_land_marker == -1)) { + _transfer_land_marker = wp.seq; + + if (_transfer_land_start_marker == -1) { + _transfer_land_start_marker = _transfer_land_marker; + } + } if (!write_failed) { /* waypoint marked as current */ @@ -1167,8 +1132,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_fence_point.frame = mission_item.frame; if (!check_failed) { - write_failed = dm_write(DM_KEY_FENCE_POINTS, wp.seq + 1, &mission_fence_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_FENCE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_fence_point), sizeof(mission_fence_point_s)); } } @@ -1180,8 +1145,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) mission_safe_point.lon = mission_item.lon; mission_safe_point.alt = mission_item.altitude; mission_safe_point.frame = mission_item.frame; - write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point, - sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s); + write_failed = !_dataman_client.writeSync(DM_KEY_SAFE_POINTS, wp.seq + 1, + reinterpret_cast(&mission_safe_point), sizeof(mission_safe_point_s), 2_s); } break; @@ -1226,7 +1191,10 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg) switch (_mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); + ++_mission_update_counter; + _land_start_marker = _transfer_land_start_marker; + _land_marker = _transfer_land_marker; + update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq); break; case MAV_MISSION_TYPE_FENCE: @@ -1280,8 +1248,11 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) switch (wpca.mission_type) { case MAV_MISSION_TYPE_MISSION: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ++_mission_update_counter; + _land_start_marker = -1; + _land_marker = -1; + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); break; case MAV_MISSION_TYPE_FENCE: @@ -1293,9 +1264,12 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) break; case MAV_MISSION_TYPE_ALL: - ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : - DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); - ret = update_geofence_count(0) || ret; + ++_mission_update_counter; + _land_start_marker = -1; + _land_marker = -1; + update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0); + ret = update_geofence_count(0); ret = update_safepoint_count(0) || ret; break; @@ -1795,11 +1769,24 @@ void MavlinkMissionManager::check_active_mission() return; } - if (!(_my_dataman_id == _dataman_id)) { - PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); + if (_mission_sub.updated()) { + _mission_sub.update(); - _my_dataman_id = _dataman_id; - send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], - MAV_MISSION_TYPE_MISSION); + if (_mission_sub.get().geofence_update_counter != _geofence_update_counter) { + load_geofence_stats(); + } + + if (_mission_sub.get().safe_points_update_counter != _safepoint_update_counter) { + load_safepoint_stats(); + } + + if ((_mission_sub.get().mission_update_counter != _mission_update_counter) + || (_my_dataman_id != (dm_item_t)_mission_sub.get().dataman_id)) { + PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating"); + init_offboard_mission(_mission_sub.get()); + _my_dataman_id = _dataman_id; + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION], + MAV_MISSION_TYPE_MISSION); + } } } diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 64f6607ef3..b4bb8bccaa 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -45,7 +45,7 @@ #pragma once -#include +#include #include #include #include @@ -95,6 +95,8 @@ private: enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible) + DatamanClient _dataman_client{}; + uint64_t _time_last_recv{0}; uint64_t _time_last_sent{0}; @@ -123,16 +125,21 @@ private: uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission + int32_t _transfer_land_start_marker{-1}; ///< index of land start mission item in current transmission (if unavailable, index of land mission item, -1 otherwise) + int32_t _transfer_land_marker{-1}; ///< index of land mission item in current transmission (-1 if unavailable) static bool _transfer_in_progress; ///< Global variable checking for current transmission uORB::Subscription _mission_result_sub{ORB_ID(mission_result)}; + uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; uORB::Publication _offboard_mission_pub{ORB_ID(mission)}; + static uint16_t _mission_update_counter; static uint16_t _geofence_update_counter; static uint16_t _safepoint_update_counter; - bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress) + int32_t _land_start_marker{-1}; ///< index of loaded land start mission item (if unavailable, index of land mission item, -1 otherwise) + int32_t _land_marker{-1}; ///< index of loaded land mission item (-1 if unavailable) MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz @@ -157,9 +164,9 @@ private: MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager &operator = (const MavlinkMissionManager &); - void init_offboard_mission(); + void init_offboard_mission(mission_s mission_state); - int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq); + void update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool write_to_dataman = true); /** store the geofence count to dataman */ int update_geofence_count(unsigned count); @@ -168,10 +175,10 @@ private: int update_safepoint_count(unsigned count); /** load geofence stats from dataman */ - int load_geofence_stats(); + bool load_geofence_stats(); /** load safe point stats from dataman */ - int load_safepoint_stats(); + bool load_safepoint_stats(); /** * @brief Sends an waypoint ack message diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 26103f656a..de603c6b17 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2351,10 +2351,10 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) gps.device_id = device_id.devid; - gps.lat = hil_gps.lat; - gps.lon = hil_gps.lon; - gps.alt = hil_gps.alt; - gps.alt_ellipsoid = hil_gps.alt; + gps.latitude_deg = hil_gps.lat * 1e-7; + gps.longitude_deg = hil_gps.lon * 1e-7; + gps.altitude_msl_m = hil_gps.alt * 1e-3; + gps.altitude_ellipsoid_m = hil_gps.alt * 1e-3; gps.s_variance_m_s = 0.25f; gps.c_variance_rad = 0.5f; @@ -2713,6 +2713,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) matrix::Eulerf euler{matrix::Quatf(hil_state.attitude_quaternion)}; hil_local_pos.heading = euler.psi(); + hil_local_pos.heading_good_for_control = PX4_ISFINITE(euler.psi()); + hil_local_pos.unaided_heading = NAN; hil_local_pos.xy_global = true; hil_local_pos.z_global = true; hil_local_pos.vxy_max = INFINITY; @@ -3267,7 +3269,9 @@ MavlinkReceiver::run() _mission_manager.check_active_mission(); _mission_manager.send(); - _parameters_manager.send(); + if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE::MAVLINK_MODE_IRIDIUM) { + _parameters_manager.send(); + } if (_mavlink->ftp_enabled()) { _mavlink_ftp.send(); diff --git a/src/modules/mavlink/mavlink_shell.cpp b/src/modules/mavlink/mavlink_shell.cpp index deb5286dfd..723c535781 100644 --- a/src/modules/mavlink/mavlink_shell.cpp +++ b/src/modules/mavlink/mavlink_shell.cpp @@ -186,7 +186,13 @@ int MavlinkShell::shell_start_thread(int argc, char *argv[]) #ifdef __PX4_NUTTX dup2(1, 2); //redirect stderror to stdout - nsh_consolemain(0, NULL); + const int ret = nsh_consolemain(0, NULL); + + if (ret) { + PX4_ERR("Mavlink shell failed: %d%s", ret, (ret == -ENOMEM) ? " (out of memory)" : ""); + return ret; + } + #endif /* __PX4_NUTTX */ #ifdef __PX4_POSIX diff --git a/src/modules/mavlink/streams/BATTERY_STATUS.hpp b/src/modules/mavlink/streams/BATTERY_STATUS.hpp index 709eefb12d..73068f0de3 100644 --- a/src/modules/mavlink/streams/BATTERY_STATUS.hpp +++ b/src/modules/mavlink/streams/BATTERY_STATUS.hpp @@ -142,16 +142,41 @@ private: static constexpr int mavlink_cell_slots = (sizeof(bat_msg.voltages) / sizeof(bat_msg.voltages[0])); static constexpr int mavlink_cell_slots_extension = (sizeof(bat_msg.voltages_ext) / sizeof(bat_msg.voltages_ext[0])); - uint16_t cell_voltages[mavlink_cell_slots + mavlink_cell_slots_extension]; - for (auto &voltage : cell_voltages) { - voltage = UINT16_MAX; + // Fill defaults first, voltage fields 1-10 + for (int i = 0; i < mavlink_cell_slots; ++i) { + bat_msg.voltages[i] = UINT16_MAX; + } + + // And extensions fields 11-14: 0 if unused for backwards compatibility and 0 truncation. + for (int i = 0; i < mavlink_cell_slots_extension; ++i) { + bat_msg.voltages_ext[i] = 0; } if (battery_status.connected) { - // We don't know the cell count or we don't know the indpendent cell voltages so we report the total voltage in the first cell + // We don't know the cell count or we don't know the indpendent cell voltages, + // so we report the total voltage in the first cell, or cell(s) if the voltage + // doesn't "fit" in the UINT16. if (battery_status.cell_count == 0 || battery_status.voltage_cell_v[0] < 0.0001f) { - cell_voltages[0] = battery_status.voltage_filtered_v * 1000.f; + // 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; + + if (num_fields_required <= mavlink_cell_slots) { + float remaining_voltage = battery_status.voltage_filtered_v * 1000.f; + + for (int i = 0; i < num_fields_required - 1; ++i) { + bat_msg.voltages[i] = UINT16_MAX - 1; + remaining_voltage -= UINT16_MAX - 1; + } + + bat_msg.voltages[num_fields_required - 1] = remaining_voltage; + + } else { + // Leave it default/unknown. We're out of spec. + } + } else { static constexpr int uorb_cell_slots = @@ -161,22 +186,17 @@ private: uorb_cell_slots, mavlink_cell_slots + mavlink_cell_slots_extension); - for (int cell = 0; cell < cell_slots; cell++) { - cell_voltages[cell] = battery_status.voltage_cell_v[cell] * 1000.f; + for (int i = 0; i < cell_slots; ++i) { + if (i < mavlink_cell_slots) { + bat_msg.voltages[i] = battery_status.voltage_cell_v[i] * 1000.f; + + } else if ((i - mavlink_cell_slots) < mavlink_cell_slots_extension) { + bat_msg.voltages_ext[i - mavlink_cell_slots] = battery_status.voltage_cell_v[i] * 1000.f; + } } } } - // voltage fields 1-10 - for (int cell = 0; cell < mavlink_cell_slots; cell++) { - bat_msg.voltages[cell] = cell_voltages[cell]; - } - - // voltage fields 11-14 into the extension - for (int cell = 0; cell < mavlink_cell_slots_extension; cell++) { - bat_msg.voltages_ext[cell] = cell_voltages[mavlink_cell_slots + cell]; - } - mavlink_msg_battery_status_send_struct(_mavlink->get_channel(), &bat_msg); updated = true; } diff --git a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp index e64a772d2d..3d475932bf 100644 --- a/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp +++ b/src/modules/mavlink/streams/CAMERA_TRIGGER.hpp @@ -72,6 +72,8 @@ private: }; int _sequence {1}; + static constexpr uint8_t num_cameras = 6; // Mavlink has reserved component IDs for six cameras + bool send() override { camera_trigger_s camera_trigger; @@ -86,39 +88,41 @@ private: _camera_status_sub.update(&_camera_status); - vehicle_command_s vcmd{}; - vcmd.timestamp = hrt_absolute_time(); - vcmd.param1 = 0.0f; // all cameras - vcmd.param2 = 0.0f; // duration 0 because only taking one picture - vcmd.param3 = 1.0f; // only take one - vcmd.param4 = (float)_sequence++; - vcmd.param5 = (double)NAN; - vcmd.param6 = (double)NAN; - vcmd.param7 = NAN; - vcmd.command = MAV_CMD_IMAGE_START_CAPTURE; - vcmd.target_system = mavlink_system.sysid; - vcmd.target_component = _camera_status.active_comp_id; + for (int i_camera = 0; i_camera < num_cameras; i_camera++) { + vehicle_command_s vcmd{}; + vcmd.timestamp = hrt_absolute_time(); + vcmd.param1 = 0.0f; // all cameras + vcmd.param2 = 0.0f; // duration 0 because only taking one picture + vcmd.param3 = 1.0f; // only take one + vcmd.param4 = (float)_sequence++; + vcmd.param5 = (double)NAN; + vcmd.param6 = (double)NAN; + vcmd.param7 = NAN; + vcmd.command = MAV_CMD_IMAGE_START_CAPTURE; + vcmd.target_system = mavlink_system.sysid; + vcmd.target_component = i_camera + MAV_COMP_ID_CAMERA; - MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel()); + MavlinkCommandSender::instance().handle_vehicle_command(vcmd, _mavlink->get_channel()); - // TODO: move this camera_trigger and publish as a vehicle_command - /* send MAV_CMD_DO_DIGICAM_CONTROL*/ - mavlink_command_long_t command_long_msg{}; + // TODO: move this camera_trigger and publish as a vehicle_command + /* send MAV_CMD_DO_DIGICAM_CONTROL*/ + mavlink_command_long_t command_long_msg{}; - command_long_msg.target_system = _camera_status.active_sys_id; - command_long_msg.target_component = _camera_status.active_comp_id; - command_long_msg.command = MAV_CMD_DO_DIGICAM_CONTROL; - command_long_msg.confirmation = 0; - command_long_msg.param1 = NAN; - command_long_msg.param2 = NAN; - command_long_msg.param3 = NAN; - command_long_msg.param4 = NAN; - command_long_msg.param5 = 1; // take 1 picture - command_long_msg.param6 = NAN; - command_long_msg.param7 = NAN; + command_long_msg.target_system = _camera_status.active_sys_id; + command_long_msg.target_component = i_camera + MAV_COMP_ID_CAMERA; + command_long_msg.command = MAV_CMD_DO_DIGICAM_CONTROL; + command_long_msg.confirmation = 0; + command_long_msg.param1 = NAN; + command_long_msg.param2 = NAN; + command_long_msg.param3 = NAN; + command_long_msg.param4 = NAN; + command_long_msg.param5 = 1; // take 1 picture + command_long_msg.param6 = NAN; + command_long_msg.param7 = NAN; - mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &command_long_msg); + mavlink_msg_command_long_send_struct(_mavlink->get_channel(), &command_long_msg); + } return true; } diff --git a/src/modules/mavlink/streams/GPS2_RAW.hpp b/src/modules/mavlink/streams/GPS2_RAW.hpp index eb77c8c319..ef91b95f27 100644 --- a/src/modules/mavlink/streams/GPS2_RAW.hpp +++ b/src/modules/mavlink/streams/GPS2_RAW.hpp @@ -71,9 +71,9 @@ private: msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; - msg.lat = gps.lat; - msg.lon = gps.lon; - msg.alt = gps.alt; + msg.lat = static_cast(round(gps.latitude_deg * 1e7)); + msg.lon = static_cast(round(gps.longitude_deg * 1e7)); + msg.alt = static_cast(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm] msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) diff --git a/src/modules/mavlink/streams/GPS_RAW_INT.hpp b/src/modules/mavlink/streams/GPS_RAW_INT.hpp index f6887c73c9..ca2077d5a7 100644 --- a/src/modules/mavlink/streams/GPS_RAW_INT.hpp +++ b/src/modules/mavlink/streams/GPS_RAW_INT.hpp @@ -70,9 +70,9 @@ private: if (_sensor_gps_sub.update(&gps)) { msg.time_usec = gps.timestamp; msg.fix_type = gps.fix_type; - msg.lat = gps.lat; - msg.lon = gps.lon; - msg.alt = gps.alt; + msg.lat = static_cast(round(gps.latitude_deg * 1e7)); + msg.lon = static_cast(round(gps.longitude_deg * 1e7)); + msg.alt = static_cast(round(gps.altitude_msl_m * 1e3)); // convert [m] to [mm] msg.eph = gps.hdop * 100; // GPS HDOP horizontal dilution of position (unitless) msg.epv = gps.vdop * 100; // GPS VDOP vertical dilution of position (unitless) @@ -85,7 +85,7 @@ private: msg.cog = math::degrees(matrix::wrap_2pi(gps.cog_rad)) * 1e2f; msg.satellites_visible = gps.satellites_used; - msg.alt_ellipsoid = gps.alt_ellipsoid; + msg.alt_ellipsoid = static_cast(round(gps.altitude_ellipsoid_m * 1e3)); // convert [m] to [mm] msg.h_acc = gps.eph * 1e3f; // position uncertainty in mm msg.v_acc = gps.epv * 1e3f; // altitude uncertainty in mm msg.vel_acc = gps.s_variance_m_s * 1e3f; // speed uncertainty in mm diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 26d966a85a..c344876360 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -107,27 +107,23 @@ private: // uint8_t system_status (MAV_STATE) - System status flag. uint8_t system_status = MAV_STATE_UNINIT; - switch (vehicle_status.arming_state) { - case vehicle_status_s::ARMING_STATE_ARMED: + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE; - break; - case vehicle_status_s::ARMING_STATE_STANDBY: + } else if (vehicle_status.calibration_enabled || vehicle_status.rc_calibration_in_progress + || actuator_armed.in_esc_calibration_mode) { + system_status = MAV_STATE_CALIBRATING; + + } else if (vehicle_status.pre_flight_checks_pass) { system_status = MAV_STATE_STANDBY; - break; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: - system_status = MAV_STATE_POWEROFF; - break; } // system_status overrides - if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown + if (actuator_armed.force_failsafe || (actuator_armed.lockdown + && vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF) || actuator_armed.manual_lockdown || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { - system_status = MAV_STATE_FLIGHT_TERMINATION; - } else if (vehicle_status.calibration_enabled) { - system_status = MAV_STATE_CALIBRATING; + system_status = MAV_STATE_FLIGHT_TERMINATION; } diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp index f3586ae126..3a519c5e90 100644 --- a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp +++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp @@ -165,12 +165,12 @@ private: } if (vehicle_gps_position.fix_type >= 2) { - msg.latitude = vehicle_gps_position.lat; - msg.longitude = vehicle_gps_position.lon; + msg.latitude = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)); + msg.longitude = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)); // altitude_geodetic if (vehicle_gps_position.fix_type >= 3) { - msg.altitude_geodetic = vehicle_gps_position.alt * 1e-3f; + msg.altitude_geodetic = static_cast(round(vehicle_gps_position.altitude_msl_m)); // [m] } // horizontal_accuracy diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp index fdc8ae1ecc..b75c1d7a60 100644 --- a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp +++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp @@ -86,9 +86,9 @@ private: // Required update for dynamic message is 5 [Hz] mavlink_uavionix_adsb_out_dynamic_t dynamic_msg = { .utcTime = static_cast(vehicle_gps_position.time_utc_usec / 1000000ULL), - .gpsLat = vehicle_gps_position.lat, - .gpsLon = vehicle_gps_position.lon, - .gpsAlt = vehicle_gps_position.alt_ellipsoid, + .gpsLat = static_cast(round(vehicle_gps_position.latitude_deg * 1e7)), + .gpsLon = static_cast(round(vehicle_gps_position.longitude_deg * 1e7)), + .gpsAlt = static_cast(round(vehicle_gps_position.altitude_ellipsoid_m * 1e3)), // convert [m] to [mm] .baroAltMSL = static_cast(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar] .accuracyHor = static_cast(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm] .accuracyVert = static_cast(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm] diff --git a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp index 2cf17b69f2..b58852dbaf 100644 --- a/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp +++ b/src/modules/mavlink/streams/UTM_GLOBAL_POSITION.hpp @@ -101,8 +101,8 @@ private: msg.lon = global_pos.lon * 1e7; msg.alt = global_pos.alt_ellipsoid * 1000.f; - msg.h_acc = global_pos.eph * 1000.f; - msg.v_acc = global_pos.epv * 1000.f; + msg.h_acc = math::min(global_pos.eph * 1000.0f, (float)UINT16_MAX); + msg.v_acc = math::min(global_pos.epv * 1000.0f, (float)UINT16_MAX); msg.flags |= UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE; msg.flags |= UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE; diff --git a/src/modules/mavlink/streams/VFR_HUD.hpp b/src/modules/mavlink/streams/VFR_HUD.hpp index e09be516e1..bbdfe0d3c8 100644 --- a/src/modules/mavlink/streams/VFR_HUD.hpp +++ b/src/modules/mavlink/streams/VFR_HUD.hpp @@ -94,12 +94,18 @@ private: _vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0); _vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint_1); + const float thrust_magnitude_0 = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] + + vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] + + vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]); + + const float thrust_magnitude_1 = sqrtf(vehicle_thrust_setpoint_1.xyz[0] * vehicle_thrust_setpoint_1.xyz[0] + + vehicle_thrust_setpoint_1.xyz[1] * vehicle_thrust_setpoint_1.xyz[1] + + vehicle_thrust_setpoint_1.xyz[2] * vehicle_thrust_setpoint_1.xyz[2]); + // VFR_HUD throttle should only be used for operator feedback. // VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a // a single throttle value, but this should still be a useful heuristic for operator awareness. - msg.throttle = 100 * math::max( - -vehicle_thrust_setpoint_0.xyz[2], - vehicle_thrust_setpoint_1.xyz[0]); + msg.throttle = 100.f * math::max(thrust_magnitude_0, thrust_magnitude_1); } else { msg.throttle = 0.0f; diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index ca4482d487..31ca3eafa0 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -50,10 +50,12 @@ #include #include #include +#include #include #include #include #include +#include #include @@ -100,6 +102,7 @@ private: uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -118,12 +121,16 @@ private: float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ float _man_tilt_max; /**< maximum tilt allowed for manual flight [rad] */ + SlewRate _manual_throttle_minimum{0.f}; ///< 0 when landed and ramped to MPC_MANTHR_MIN in air + SlewRate _manual_throttle_maximum{0.f}; ///< 0 when disarmed ramped to 1 when spooled up AlphaFilter _man_roll_input_filter; AlphaFilter _man_pitch_input_filter; hrt_abstime _last_run{0}; hrt_abstime _last_attitude_setpoint{0}; + bool _spooled_up{false}; ///< used to make sure the vehicle cannot take off during the spoolup time + bool _landed{true}; bool _reset_yaw_sp{true}; bool _heading_good_for_control{true}; ///< initialized true to have heading lock when local position never published bool _vehicle_type_rotary_wing{true}; @@ -152,7 +159,9 @@ private: (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ (ParamFloat) _param_mpc_thr_max, /**< maximum throttle for stabilized */ (ParamFloat) _param_mpc_thr_hover, /**< throttle at stationary hover */ - (ParamInt) _param_mpc_thr_curve /**< throttle curve behavior */ + (ParamInt) _param_mpc_thr_curve, /**< throttle curve behavior */ + + (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 5925237219..35c65d5911 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -60,8 +60,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl(bool vtol) : _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")), _vtol(vtol) { - parameters_updated(); + // Rate of change 5% per second -> 1.6 seconds to ramp to default 8% MPC_MANTHR_MIN + _manual_throttle_minimum.setSlewRate(0.05f); + // Rate of change 50% per second -> 2 seconds to ramp to 100% + _manual_throttle_maximum.setSlewRate(0.5f); } MulticopterAttitudeControl::~MulticopterAttitudeControl() @@ -98,14 +101,21 @@ MulticopterAttitudeControl::parameters_updated() float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { - // throttle_stick_input is in range [0, 1] + float thrust = 0.f; + switch (_param_mpc_thr_curve.get()) { case 1: // no rescaling to hover throttle - return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get()); + thrust = math::interpolate(throttle_stick_input, -1.f, 1.f, + _manual_throttle_minimum.getState(), _param_mpc_thr_max.get()); + break; - default: // 0 or other: rescale to hover throttle at 0.5 stick - return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + default: // 0 or other: rescale such that a centered throttle stick corresponds to hover throttle + thrust = math::interpolateNXY(throttle_stick_input, {-1.f, 0.f, 1.f}, + {_manual_throttle_minimum.getState(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()}); + break; } + + return math::min(thrust, _manual_throttle_maximum.getState()); } void @@ -178,9 +188,9 @@ MulticopterAttitudeControl::generate_attitude_setpoint(const Quatf &q, float dt, 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 + 1.f) * .5f); - attitude_setpoint.timestamp = hrt_absolute_time(); + attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_setpoint.throttle); + attitude_setpoint.timestamp = hrt_absolute_time(); _vehicle_attitude_setpoint_pub.publish(attitude_setpoint); // update attitude controller setpoint immediately @@ -262,6 +272,16 @@ MulticopterAttitudeControl::Run() _vtol_in_transition_mode = vehicle_status.in_transition_mode; _vtol_tailsitter = vehicle_status.is_vtol_tailsitter; + const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _spooled_up = armed && hrt_elapsed_time(&vehicle_status.armed_time) > _param_com_spoolup_time.get() * 1_s; + } + } + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; } } @@ -324,6 +344,20 @@ MulticopterAttitudeControl::Run() _vehicle_rates_setpoint_pub.publish(rates_setpoint); } + if (_landed) { + _manual_throttle_minimum.update(0.f, dt); + + } else { + _manual_throttle_minimum.update(_param_mpc_manthr_min.get(), dt); + } + + if (_spooled_up) { + _manual_throttle_maximum.update(1.f, dt); + + } else { + _manual_throttle_maximum.setForcedValue(0.f); + } + // reset yaw setpoint during transitions, tailsitter.cpp generates // attitude setpoint for the transition _reset_yaw_sp = !attitude_setpoint_generated || !_heading_good_for_control || (_vtol && _vtol_in_transition_mode); diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index c540baca19..b4eba96115 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -83,7 +83,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo Vector3f body_x = y_C % body_z; // keep nose to front while inverted upside down - if (body_z(2) < 0.0f) { + if (body_z(2) < 0.f) { body_x = -body_x; } @@ -91,7 +91,7 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo // desired thrust is in XY plane, set X downside to construct correct matrix, // but yaw component will not be used actually body_x.zero(); - body_x(2) = 1.0f; + body_x(2) = 1.f; } body_x.normalize(); @@ -201,7 +201,7 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, // we have triangle CDX with known CD and CX = R, find DX float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); - if ((sphere_c - line_b) * ab_norm > 0.0f) { + if ((sphere_c - line_b) * ab_norm > 0.f) { // target waypoint is already behind us res = line_b; @@ -218,12 +218,12 @@ bool cross_sphere_line(const Vector3f &sphere_c, const float sphere_r, res = d; // go directly to line // previous waypoint is still in front of us - if ((sphere_c - line_a) * ab_norm < 0.0f) { + if ((sphere_c - line_a) * ab_norm < 0.f) { res = line_a; } // target waypoint is already behind us - if ((sphere_c - line_b) * ab_norm > 0.0f) { + if ((sphere_c - line_b) * ab_norm > 0.f) { res = line_b; } diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8e58fa66d1..955f3f4b44 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -152,8 +152,8 @@ void PositionControl::_velocityControl(const float dt) _accelerationControl(); // Integrator anti-windup in vertical direction - if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) || - (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) { + if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) || + (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.f)) { vel_error(2) = 0.f; } @@ -171,9 +171,9 @@ void PositionControl::_velocityControl(const float dt) // Determine how much horizontal thrust is left after prioritizing vertical control const float thrust_max_xy_squared = thrust_max_squared - math::sq(_thr_sp(2)); - float thrust_max_xy = 0; + float thrust_max_xy = 0.f; - if (thrust_max_xy_squared > 0) { + if (thrust_max_xy_squared > 0.f) { thrust_max_xy = sqrtf(thrust_max_xy_squared); } diff --git a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp index edfe9e8eb4..d718ef687b 100644 --- a/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp +++ b/src/modules/mc_pos_control/Takeoff/TakeoffTest.cpp @@ -46,7 +46,7 @@ TEST(TakeoffTest, RegularTakeoffRamp) { TakeoffHandling takeoff; takeoff.setSpoolupTime(1.f); - takeoff.setTakeoffRampTime(2.0); + takeoff.setTakeoffRampTime(2.f); takeoff.generateInitialRampValue(CONSTANTS_ONE_G / 0.5f); // disarmed, landed, don't want takeoff diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c deleted file mode 100644 index 181ae47e5a..0000000000 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ /dev/null @@ -1,898 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-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 mc_pos_control_params.c - * Multicopter position controller parameters. - * - * @author Anton Babushkin - */ - -/** - * Minimum collective thrust in auto thrust control - * - * It's recommended to set it > 0 to avoid free fall with zero thrust. - * Note: Without airmode zero thrust leads to zero roll/pitch control authority. (see MC_AIRMODE) - * - * @unit norm - * @min 0.05 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); - -/** - * Hover thrust - * - * Vertical thrust required to hover. - * This value is mapped to center stick for manual throttle control. - * With this value set to the thrust required to hover, transition - * from manual to Altitude or Position mode while hovering will occur with the - * throttle stick near center, which is then interpreted as (near) - * zero demand for vertical speed. - * - * This parameter is also important for the landing detection to work correctly. - * - * @unit norm - * @min 0.1 - * @max 0.8 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); - -/** - * Hover thrust source selector - * - * Set false to use the fixed parameter MPC_THR_HOVER - * Set true to use the value computed by the hover thrust estimator - * - * @boolean - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_USE_HTE, 1); - -/** - * Thrust curve in Manual Mode - * - * This parameter defines how the throttle stick input is mapped to commanded thrust - * in Manual/Stabilized flight mode. - * - * In case the default is used ('Rescale to hover thrust'), the stick input is linearly - * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). - * - * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful - * in case the hover thrust is very low and the default would lead to too much distortion - * (e.g. if hover thrust is set to 20%, 80% of the upper thrust range is squeezed into the - * upper half of the stick range). - * - * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. - * - * @value 0 Rescale to hover thrust - * @value 1 No Rescale - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); - -/** - * Horizontal thrust margin - * - * Margin that is kept for horizontal control when prioritizing vertical thrust. - * To avoid completely starving horizontal control with high vertical error. - * - * @unit norm - * @min 0.0 - * @max 0.5 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); - -/** - * Maximum thrust in auto thrust control - * - * Limit max allowed thrust - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); - -/** - * Minimum manual thrust - * - * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. - * With MC_AIRMODE set to 1, this can safely be set to 0. - * - * @unit norm - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @increment 0.01 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); - -/** - * Proportional gain for vertical position error - * - * @min 0.0 - * @max 1.5 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); - -/** - * Proportional gain for vertical velocity error - * - * defined as correction acceleration in m/s^2 per m/s velocity error - * - * @min 2.0 - * @max 15.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.0f); - -/** - * Integral gain for vertical velocity error - * - * defined as correction acceleration in m/s^2 per m velocity integral - * - * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. - * - * @min 0.2 - * @max 3.0 - * @decimal 3 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.0f); - -/** - * Differential gain for vertical velocity error - * - * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative - * - * @min 0.0 - * @max 2.0 - * @decimal 3 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.0f); - -/** - * Automatic ascent velocity - * - * Ascent velocity in auto modes. - * For manual modes and offboard, see MPC_Z_VEL_MAX_UP - * - * @unit m/s - * @min 0.5 - * @max 8.0 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); - -/** - * Maximum ascent velocity - * - * Ascent velocity in manual modes and offboard. - * For auto modes, see MPC_Z_V_AUTO_UP - * - * @unit m/s - * @min 0.5 - * @max 8.0 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); - -/** - * Automatic descent velocity - * - * Descent velocity in auto modes. - * For manual modes and offboard, see MPC_Z_VEL_MAX_DN - * - * @unit m/s - * @min 0.5 - * @max 4.0 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f); - -/** - * Maximum descent velocity - * - * Descent velocity in manual modes and offboard. - * For auto modes, see MPC_Z_V_AUTO_DN - * - * @unit m/s - * @min 0.5 - * @max 4.0 - * @increment 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f); - -/** - * Proportional gain for horizontal position error - * - * @min 0.0 - * @max 2.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); - -/** - * Proportional gain for horizontal velocity error - * - * defined as correction acceleration in m/s^2 per m/s velocity error - * - * @min 1.2 - * @max 5.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); - -/** - * Integral gain for horizontal velocity error - * - * defined as correction acceleration in m/s^2 per m velocity integral - * Non-zero value allows to eliminate steady state errors in the presence of disturbances like wind. - * - * @min 0.0 - * @max 60.0 - * @decimal 3 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); - -/** - * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. - * - * defined as correction acceleration in m/s^2 per m/s^2 velocity derivative - * - * @min 0.1 - * @max 2.0 - * @decimal 3 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); - -/** - * Default horizontal velocity in mission - * - * Horizontal velocity used when flying autonomously in e.g. Missions, RTL, Goto. - * - * @unit m/s - * @min 3.0 - * @max 20.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.0f); - -/** - * Proportional gain for horizontal trajectory position error - * - * @min 0.1 - * @max 1.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); - -/** - * Maximum horizontal error allowed by the trajectory generator - * - * The integration speed of the trajectory setpoint is linearly - * reduced with the horizontal position tracking error. When the - * error is above this parameter, the integration of the - * trajectory is stopped to wait for the drone. - * - * This value can be adjusted depending on the tracking - * capabilities of the vehicle. - * - * @min 0.1 - * @max 10.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f); - -/** - * Maximum horizontal velocity setpoint in Position mode - * - * If velocity setpoint larger than MPC_XY_VEL_MAX is set, then - * the setpoint will be capped to MPC_XY_VEL_MAX - * - * The maximum sideways and backward speed can be set differently - * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively. - * - * @unit m/s - * @min 3.0 - * @max 20.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f); - -/** - * Maximum sideways velocity in Position mode - * - * If set to a negative value or larger than - * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. - * - * @unit m/s - * @min -1.0 - * @max 20.0 - * @increment 0.1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f); - -/** - * Maximum backward velocity in Position mode - * - * If set to a negative value or larger than - * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. - * - * @unit m/s - * @min -1.0 - * @max 20.0 - * @increment 0.1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f); - -/** - * Maximum horizontal velocity - * - * Maximum horizontal velocity in AUTO mode. If higher speeds - * are commanded in a mission they will be capped to this velocity. - * - * @unit m/s - * @min 0.0 - * @max 20.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.0f); - -/** - * Maximum tilt angle in air - * - * Limits maximum tilt in AUTO and POSCTRL modes during flight. - * - * @unit deg - * @min 20.0 - * @max 89.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.0f); - -/** - * Maximum tilt during landing - * - * Limits maximum tilt angle on landing. - * - * @unit deg - * @min 10.0 - * @max 89.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); - -/** - * Landing descend rate - * - * @unit m/s - * @min 0.6 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); - -/** - * Land crawl descend rate - * - * Used below MPC_LAND_ALT3 if distance sensor data is availabe. - * - * @unit m/s - * @min 0.1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f); - -/** - * Enable user assisted descent for autonomous land routine - * - * When enabled, descent speed will be: - * stick full up - 0 - * stick centered - MPC_LAND_SPEED - * stick full down - 2 * MPC_LAND_SPEED - * - * Additionally, the vehicle can be yawed and moved laterally using the other sticks. - * Manual override during auto modes has to be disabled to use this feature (see COM_RC_OVERRIDE). - * - * @min 0 - * @max 1 - * @value 0 Fixed descent speed of MPC_LAND_SPEED - * @value 1 User assisted descent speed - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); - -/** - * User assisted landing radius - * - * When user assisted descent is enabled (see MPC_LAND_RC_HELP), - * this parameter controls the maximum position adjustment - * allowed from the original landing point. - * - * @unit m - * @min 0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); - -/** - * Takeoff climb rate - * - * @unit m/s - * @min 1 - * @max 5 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); - -/** - * Maximal tilt angle in manual or altitude mode - * - * @unit deg - * @min 0.0 - * @max 90.0 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.0f); - -/** - * Max manual yaw rate - * - * @unit deg/s - * @min 0.0 - * @max 400 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.0f); - -/** - * Manual yaw rate input filter time constant - * - * Setting this parameter to 0 disables the filter - * - * @unit s - * @min 0.0 - * @max 5.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); - -/** - * Deadzone of sticks where position hold is enabled - * - * @min 0.0 - * @max 1.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); - -/** - * Maximum horizontal velocity for which position hold is enabled (use 0 to disable check) - * - * @unit m/s - * @min 0.0 - * @max 3.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); - -/** - * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) - * - * @unit m/s - * @min 0.0 - * @max 3.0 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); - -/** - * Low pass filter cut freq. for numerical velocity derivative - * - * @unit Hz - * @min 0.0 - * @max 10 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); - -/** - * Maximum horizontal acceleration for auto mode and for manual mode - * - * MPC_POS_MODE - * 1 just deceleration - * 3 acceleration and deceleration - * 4 just acceleration - * - * @unit m/s^2 - * @min 2.0 - * @max 15.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.0f); - -/** - * Acceleration for auto and for manual - * - * Note: In manual, this parameter is only used in MPC_POS_MODE 4. - * - * @unit m/s^2 - * @min 2.0 - * @max 15.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ - -PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.0f); - -/** - * Maximum vertical acceleration in velocity controlled modes upward - * - * @unit m/s^2 - * @min 2.0 - * @max 15.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.0f); - -/** - * Maximum vertical acceleration in velocity controlled modes down - * - * @unit m/s^2 - * @min 2.0 - * @max 15.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.0f); - -/** - * Maximum jerk limit - * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother vehicle motions, but it also limits its - * agility (how fast it can change directions or break). - * - * Setting this to the maximum value essentially disables the limit. - * - * Note: This is only used when MPC_POS_MODE is set to a smoothing mode 3 or 4. - * - * @unit m/s^3 - * @min 0.5 - * @max 500.0 - * @increment 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.0f); - -/** - * Jerk limit in auto mode - * - * Limit the maximum jerk of the vehicle (how fast the acceleration can change). - * A lower value leads to smoother vehicle motions, but it also limits its - * agility. - * - * @unit m/s^3 - * @min 1.0 - * @max 80.0 - * @increment 1 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.0f); - -/** - * Altitude control mode. - * - * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in - * flight due to sensor drift. - * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down - * with terrain height variation. Requires a distance to ground sensor. The height controller will - * revert to using height above origin if the distance to ground estimate becomes invalid as indicated - * by the local_position.distance_bottom_valid message being false. - * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative - * to earth frame origin when moving horizontally. - * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. - * - * @min 0 - * @max 2 - * @value 0 Altitude following - * @value 1 Terrain following - * @value 2 Terrain hold - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); - -/** - * Manual position control stick exponential curve sensitivity - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve (default) - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); - -/** - * Manual control stick vertical exponential curve - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve (default) - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); - -/** - * Manual control stick yaw rotation exponential curve - * - * The higher the value the less sensitivity the stick has around zero - * while still reaching the maximum value with full stick deflection. - * - * 0 Purely linear input curve (default) - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); - -/** - * Max yaw rate in auto mode - * - * Limit the rate of change of the yaw setpoint in autonomous mode - * to avoid large control output and mixer saturation. - * - * @unit deg/s - * @min 0.0 - * @max 360.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Attitude Control - */ -PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.0f); - -/** - * Altitude for 1. step of slow landing (descend) - * - * Below this altitude descending velocity gets limited to a value - * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" - * Value needs to be higher than "MPC_LAND_ALT2" - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.0f); - -/** - * Altitude for 2. step of slow landing (landing) - * - * Below this altitude descending velocity gets - * limited to "MPC_LAND_SPEED" - * Value needs to be lower than "MPC_LAND_ALT1" - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.0f); - -/** - * Altitude for 3. step of slow landing - * - * Below this altitude descending velocity gets - * limited to "MPC_LAND_CRWL", if LIDAR available. - * No effect if LIDAR not available - * - * @unit m - * @min 0 - * @max 122 - * @decimal 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.0f); - -/** - * Position control smooth takeoff ramp time constant - * - * Increasing this value will make automatic and manual takeoff slower. - * If it's too slow the drone might scratch the ground and tip over. - * A time constant of 0 disables the ramp - * - * @min 0 - * @max 5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f); - -/** - * Manual-Position control sub-mode - * - * The supported sub-modes are: - * 0 Simple position control where sticks map directly to velocity setpoints - * without smoothing. Useful for velocity control tuning. - * 3 Smooth position control with maximum acceleration and jerk limits based on - * jerk optimized trajectory generator (different algorithm than 1). - * 4 Smooth position control where sticks map to acceleration and there's a virtual brake drag - * - * @value 0 Simple position control - * @value 3 Smooth position control (Jerk optimized) - * @value 4 Acceleration based input - * @group Multicopter Position Control - */ -PARAM_DEFINE_INT32(MPC_POS_MODE, 4); - -/** - * Yaw mode. - * - * Specifies the heading in Auto. - * - * @min 0 - * @max 4 - * @value 0 towards waypoint - * @value 1 towards home - * @value 2 away from home - * @value 3 along trajectory - * @value 4 towards waypoint (yaw first) - * @group Mission - */ -PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); - -/** - * Responsiveness - * - * Changes the overall responsiveness of the vehicle. - * The higher the value, the faster the vehicle will react. - * - * If set to a value greater than zero, other parameters are automatically set (such as - * the acceleration or jerk limits). - * If set to a negative value, the existing individual parameters are used. - * - * @min -1 - * @max 1 - * @decimal 2 - * @increment 0.05 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); - -/** - * Overall Horizontal Velocity Limit - * - * If set to a value greater than zero, other parameters are automatically set (such as - * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). - * If set to a negative value, the existing individual parameters are used. - * - * @min -20 - * @max 20 - * @decimal 1 - * @increment 1 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.0f); - -/** - * Overall Vertical Velocity Limit - * - * If set to a value greater than zero, other parameters are automatically set (such as - * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). - * If set to a negative value, the existing individual parameters are used. - * - * @min -3 - * @max 8 - * @decimal 1 - * @increment 0.5 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.0f); diff --git a/src/modules/mc_pos_control/multicopter_altitude_mode_params.c b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c new file mode 100644 index 0000000000..ecebaa9e01 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_altitude_mode_params.c @@ -0,0 +1,119 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Maximum upwards acceleration in climb rate controlled modes + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_UP_MAX, 4.f); + +/** + * Maximum downwards acceleration in climb rate controlled modes + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_DOWN_MAX, 3.f); + +/** + * Manual yaw rate input filter time constant + * + * Not used in Stabilized mode + * Setting this parameter to 0 disables the filter + * + * @unit s + * @min 0 + * @max 5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_TAU, 0.08f); + +/** + * Altitude reference mode + * + * Set to 0 to control height relative to the earth frame origin. This origin may move up and down in + * flight due to sensor drift. + * Set to 1 to control height relative to estimated distance to ground. The vehicle will move up and down + * with terrain height variation. Requires a distance to ground sensor. The height controller will + * revert to using height above origin if the distance to ground estimate becomes invalid as indicated + * by the local_position.distance_bottom_valid message being false. + * Set to 2 to control height relative to ground (requires a distance sensor) when stationary and relative + * to earth frame origin when moving horizontally. + * The speed threshold is controlled by the MPC_HOLD_MAX_XY parameter. + * + * @min 0 + * @max 2 + * @value 0 Altitude following + * @value 1 Terrain following + * @value 2 Terrain hold + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_ALT_MODE, 0); + +/** + * 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 + * + * @unit m/s + * @min 0 + * @max 3 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_XY, 0.8f); + +/** + * Maximum vertical velocity for which position hold is enabled (use 0 to disable check) + * + * Only used with MPC_ALT_MODE 1 + * + * @unit m/s + * @min 0 + * @max 3 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_MAX_Z, 0.6f); diff --git a/src/modules/mc_pos_control/multicopter_autonomous_params.c b/src/modules/mc_pos_control/multicopter_autonomous_params.c new file mode 100644 index 0000000000..8596cedd8b --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_autonomous_params.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Default horizontal velocity in autonomous modes + * + * e.g. in Missions, RTL, Goto if the waypoint does not specify differently + * + * @unit m/s + * @min 3 + * @max 20 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_CRUISE, 5.f); + +/** + * Ascent velocity in autonomous modes + * + * For manually controlled modes and offboard see MPC_Z_VEL_MAX_UP + * + * @unit m/s + * @min 0.5 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_UP, 3.f); + +/** + * Descent velocity in autonomous modes + * + * For manual modes and offboard, see MPC_Z_VEL_MAX_DN + * + * @unit m/s + * @min 0.5 + * @max 4 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +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. + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 3.f); + +/** + * Jerk limit in autonomous modes + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother vehicle motions but also limited agility. + * + * @unit m/s^3 + * @min 1 + * @max 80 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_AUTO, 4.f); + +/** + * Proportional gain for horizontal trajectory position error + * + * @min 0.1 + * @max 1 + * @decimal 1 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f); + +/** + * Maximum horizontal error allowed by the trajectory generator + * + * The integration speed of the trajectory setpoint is linearly + * reduced with the horizontal position tracking error. When the + * error is above this parameter, the integration of the + * trajectory is stopped to wait for the drone. + * + * This value can be adjusted depending on the tracking + * capabilities of the vehicle. + * + * @min 0.1 + * @max 10 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.f); + +/** + * Max yaw rate in autonomous modes + * + * Limits the rate of change of the yaw setpoint to avoid large + * control output and mixer saturation. + * + * @unit deg/s + * @min 0 + * @max 360 + * @decimal 0 + * @increment 5 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_MAX, 45.f); + +/** + * Heading behavior in autonomous modes + * + * @min 0 + * @max 4 + * @value 0 towards waypoint + * @value 1 towards home + * @value 2 away from home + * @value 3 along trajectory + * @value 4 towards waypoint (yaw first) + * @group Mission + */ +PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); diff --git a/src/modules/mc_pos_control/multicopter_nudging_params.c b/src/modules/mc_pos_control/multicopter_nudging_params.c new file mode 100644 index 0000000000..c431e88fd0 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_nudging_params.c @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Enable nudging based on user input during autonomous land routine + * + * Using stick input the vehicle can be moved horizontally and yawed. + * The descend speed is amended: + * stick full up - 0 + * stick centered - MPC_LAND_SPEED + * stick full down - 2 * MPC_LAND_SPEED + * + * Manual override during auto modes has to be disabled to use this feature (see COM_RC_OVERRIDE). + * + * @min 0 + * @max 1 + * @value 0 Nudging disabled + * @value 1 Nudging enabled + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0); + +/** + * User assisted landing radius + * + * When nudging is enabled (see MPC_LAND_RC_HELP), this controls + * the maximum allowed horizontal displacement from the original landing point. + * + * @unit m + * @min 0 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_RADIUS, 1000.f); diff --git a/src/modules/mc_pos_control/multicopter_position_control_gain_params.c b/src/modules/mc_pos_control/multicopter_position_control_gain_params.c new file mode 100644 index 0000000000..df36d1f733 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_position_control_gain_params.c @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Proportional gain for vertical position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0.1 + * @max 1.5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.f); + +/** + * Proportional gain for horizontal position error + * + * Defined as corrective velocity in m/s per m position error + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.95f); + +/** + * Proportional gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 2 + * @max 15 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P_ACC, 4.f); + +/** + * Proportional gain for horizontal velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s velocity error + * + * @min 1.2 + * @max 5 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P_ACC, 1.8f); + +/** + * Integral gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m velocity integral + * + * @min 0.2 + * @max 3 + * @decimal 2 + * @increment 0.1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I_ACC, 2.f); + +/** + * Integral gain for horizontal velocity error + * + * Defined as correction acceleration in m/s^2 per m velocity integral + * Allows to eliminate steady state errors in disturbances like wind. + * + * @min 0 + * @max 60 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I_ACC, 0.4f); + +/** + * Differential gain for vertical velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0 + * @max 2 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f); + +/** + * Differential gain for horizontal velocity error + * + * Defined as corrective acceleration in m/s^2 per m/s^2 velocity derivative + * + * @min 0.1 + * @max 2 + * @decimal 2 + * @increment 0.02 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f); 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 new file mode 100644 index 0000000000..a60732cfa8 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_position_control_limits_params.c @@ -0,0 +1,140 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Maximum horizontal velocity + * + * Absolute maximum for all velocity controlled modes. + * Any higher value is truncated. + * + * @unit m/s + * @min 0 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 12.f); + +/** + * Maximum ascent velocity + * + * Absolute maximum for all climb rate controlled modes. + * In manually piloted modes full stick deflection commands this velocity. + * + * For default autonomous velocity see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 8 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f); + +/** + * Maximum descent velocity + * + * Absolute maximum for all climb rate controlled modes. + * In manually piloted modes full stick deflection commands this velocity. + * + * For default autonomous velocity see MPC_Z_V_AUTO_UP + * + * @unit m/s + * @min 0.5 + * @max 4 + * @increment 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f); + +/** + * Maximum tilt angle in air + * + * Absolute maximum for all velocity or acceleration controlled modes. + * Any higher value is truncated. + * + * @unit deg + * @min 20 + * @max 89 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_AIR, 45.f); + +/** + * Maximum tilt during inital takeoff ramp + * + * Tighter tilt limit during takeoff to avoid tip over. + * + * @unit deg + * @min 5 + * @max 89 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.f); + +/** + * Minimum collective thrust in climb rate controlled modes + * + * Too low thrust leads to loss of roll/pitch/yaw torque control authority. + * With airmode enabled this parameters can be set to 0 + * while still keeping torque authority (see MC_AIRMODE). + * + * @unit norm + * @min 0.05 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); + +/** + * Maximum collective thrust in climb rate controlled modes + * + * Limit allowed thrust e.g. for indoor test of overpowered vehicle. + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.f); diff --git a/src/modules/mc_pos_control/multicopter_position_control_params.c b/src/modules/mc_pos_control/multicopter_position_control_params.c new file mode 100644 index 0000000000..a8c00fb167 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_position_control_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Vertical thrust required to hover + * + * Mapped to center throttle stick in Stabilized mode (see MPC_THR_CURVE). + * Used for initialization of the hover thrust estimator (see MPC_USE_HTE). + * The estimated hover thrust is used as base for zero vertical acceleration in altitude control. + * The hover thrust is important for land detection to work correctly. + * + * @unit norm + * @min 0.1 + * @max 0.8 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_HOVER, 0.5f); + +/** + * Hover thrust estimator + * + * Disable to use the fixed parameter MPC_THR_HOVER + * Enable to use the hover thrust estimator + * + * @boolean + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_USE_HTE, 1); + +/** + * Horizontal thrust margin + * + * Margin that is kept for horizontal control when higher priority vertical thrust is saturated. + * To avoid completely starving horizontal control with high vertical error. + * + * @unit norm + * @min 0 + * @max 0.5 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_XY_MARG, 0.3f); + +/** + * Numerical velocity derivative low pass cutoff frequency + * + * @unit Hz + * @min 0 + * @max 10 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VELD_LP, 5.0f); diff --git a/src/modules/mc_pos_control/multicopter_position_mode_params.c b/src/modules/mc_pos_control/multicopter_position_mode_params.c new file mode 100644 index 0000000000..56adad1144 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_position_mode_params.c @@ -0,0 +1,198 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Position/Altitude mode variant + * + * The supported sub-modes are: + * 0 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 + * jerk optimized trajectory generator (different algorithm than 1). + * 4 Sticks map to acceleration and there's a virtual brake drag + * + * @value 0 Direct velocity + * @value 3 Smoothed velocity + * @value 4 Acceleration based + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_POS_MODE, 4); + +/** + * Maximum horizontal velocity setpoint in Position mode + * + * Must be smaller than MPC_XY_VEL_MAX. + * + * The maximum sideways and backward speed can be set differently + * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively. + * + * @unit m/s + * @min 3 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.f); + +/** + * Maximum sideways velocity in Position mode + * + * If set to a negative value or larger than + * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. + * + * @unit m/s + * @min -1 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.f); + +/** + * Maximum backward velocity in Position mode + * + * If set to a negative value or larger than + * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used. + * + * @unit m/s + * @min -1 + * @max 20 + * @increment 1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.f); + +/** + * Maximum horizontal acceleration + * + * MPC_POS_MODE + * 1 just deceleration + * 3 acceleration and deceleration + * 4 not used, use MPC_ACC_HOR instead + * + * @unit m/s^2 + * @min 2 + * @max 15 + * @increment 1 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_MAX, 5.f); + +/** + * Maximum horizontal and vertical jerk in Position/Altitude mode + * + * Limit the maximum jerk of the vehicle (how fast the acceleration can change). + * A lower value leads to smoother motions but limits agility + * (how fast it can change directions or break). + * + * Setting this to the maximum value essentially disables the limit. + * + * Only used with smooth MPC_POS_MODE 3 and 4. + * + * @unit m/s^3 + * @min 0.5 + * @max 500 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_JERK_MAX, 8.f); + +/** + * Deadzone for sticks in manual piloted modes + * + * Does not apply to manual throttle and direct attitude piloting by stick. + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_HOLD_DZ, 0.1f); + +/** + * Manual position control stick exponential curve sensitivity + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_MAN_EXPO, 0.6f); + +/** + * Manual control stick vertical exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_MAN_EXPO, 0.6f); + +/** + * Manual control stick yaw rotation exponential curve + * + * The higher the value the less sensitivity the stick has around zero + * while still reaching the maximum value with full stick deflection. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_YAW_EXPO, 0.6f); diff --git a/src/modules/mc_pos_control/multicopter_responsiveness_params.c b/src/modules/mc_pos_control/multicopter_responsiveness_params.c new file mode 100644 index 0000000000..e0097f6ca0 --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_responsiveness_params.c @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Responsiveness + * + * Changes the overall responsiveness of the vehicle. + * The higher the value, the faster the vehicle will react. + * + * If set to a value greater than zero, other parameters are automatically set (such as + * the acceleration or jerk limits). + * If set to a negative value, the existing individual parameters are used. + * + * @min -1 + * @max 1 + * @decimal 2 + * @increment 0.05 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(SYS_VEHICLE_RESP, -0.4f); + +/** + * Overall Horizontal Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_XY_VEL_MAX or MPC_VEL_MANUAL). + * If set to a negative value, the existing individual parameters are used. + * + * @min -20 + * @max 20 + * @decimal 1 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_XY_VEL_ALL, -10.f); + +/** + * Overall Vertical Velocity Limit + * + * If set to a value greater than zero, other parameters are automatically set (such as + * MPC_Z_VEL_MAX_UP or MPC_LAND_SPEED). + * If set to a negative value, the existing individual parameters are used. + * + * @min -3 + * @max 8 + * @decimal 1 + * @increment 0.5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_Z_VEL_ALL, -3.f); diff --git a/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c new file mode 100644 index 0000000000..f478f9938e --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_stabilized_mode_params.c @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Maximal tilt angle in Stabilized or Altitude mode + * + * @unit deg + * @min 0 + * @max 90 + * @decimal 0 + * @increment 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_TILT_MAX, 35.f); + +/** + * Max manual yaw rate for Stabilized, Altitude, Position mode + * + * @unit deg/s + * @min 0 + * @max 400 + * @decimal 0 + * @increment 10 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MAN_Y_MAX, 150.f); + +/** + * Minimum collective thrust in Stabilized mode + * + * The value is mapped to the lowest throttle stick position in Stabilized mode. + * + * Too low collective thrust leads to loss of roll/pitch/yaw torque control authority. + * Airmode is used to keep torque authority with zero thrust (see MC_AIRMODE). + * + * @unit norm + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_MANTHR_MIN, 0.08f); + +/** + * Thrust curve mapping in Stabilized Mode + * + * This parameter defines how the throttle stick input is mapped to collective thrust + * in Stabilized mode. + * + * In case the default is used ('Rescale to hover thrust'), the stick input is linearly + * rescaled, such that a centered stick corresponds to the hover throttle (see MPC_THR_HOVER). + * + * Select 'No Rescale' to directly map the stick 1:1 to the output. This can be useful + * in case the hover thrust is very low and the default would lead to too much distortion + * (e.g. if hover thrust is set to 20%, then 80% of the upper thrust range is squeezed into the + * upper half of the stick range). + * + * Note: In case MPC_THR_HOVER is set to 50%, the modes 0 and 1 are the same. + * + * @value 0 Rescale to hover thrust + * @value 1 No Rescale + * @group Multicopter Position Control + */ +PARAM_DEFINE_INT32(MPC_THR_CURVE, 0); diff --git a/src/modules/mc_pos_control/multicopter_takeoff_land_params.c b/src/modules/mc_pos_control/multicopter_takeoff_land_params.c new file mode 100644 index 0000000000..328a8531ad --- /dev/null +++ b/src/modules/mc_pos_control/multicopter_takeoff_land_params.c @@ -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. + * + ****************************************************************************/ + +/** + * Smooth takeoff ramp time constant + * + * Increasing this value will make climb rate controlled takeoff slower. + * If it's too slow the drone might scratch the ground and tip over. + * A time constant of 0 disables the ramp + * + * @unit s + * @min 0 + * @max 5 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.f); + +/** + * Takeoff climb rate + * + * @unit m/s + * @min 1 + * @max 5 + * @decimal 2 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_TKO_SPEED, 1.5f); + +/** + * Altitude for 1. step of slow landing (descend) + * + * Below this altitude descending velocity gets limited to a value + * between "MPC_Z_VEL_MAX_DN" (or "MPC_Z_V_AUTO_DN") and "MPC_LAND_SPEED" + * Value needs to be higher than "MPC_LAND_ALT2" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT1, 10.f); + +/** + * Altitude for 2. step of slow landing (landing) + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_SPEED" + * Value needs to be lower than "MPC_LAND_ALT1" + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT2, 5.f); + +/** + * Altitude for 3. step of slow landing + * + * Below this altitude descending velocity gets + * limited to "MPC_LAND_CRWL", if LIDAR available. + * No effect if LIDAR not available + * + * @unit m + * @min 0 + * @max 122 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_ALT3, 1.f); + +/** + * Landing descend rate + * + * @unit m/s + * @min 0.6 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f); + +/** + * Land crawl descend rate + * + * Used below MPC_LAND_ALT3 if distance sensor data is availabe. + * + * @unit m/s + * @min 0.1 + * @decimal 1 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f); diff --git a/src/modules/mc_rate_control/mc_acro_params.c b/src/modules/mc_rate_control/mc_acro_params.c new file mode 100644 index 0000000000..ef9d315d0c --- /dev/null +++ b/src/modules/mc_rate_control/mc_acro_params.c @@ -0,0 +1,142 @@ +/**************************************************************************** + * + * 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 mc_acro_params.c + * + * Parameters for Acro mode behavior + */ + +/** + * Acro mode maximum roll rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 100.f); + +/** + * Acro mode maximum pitch rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 100.f); + +/** + * Acro mode maximum yaw rate + * + * Full stick deflection leads to this rate. + * + * @unit deg/s + * @min 0.0 + * @max 1800.0 + * @decimal 1 + * @increment 5 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 100.f); + +/** + * Acro mode roll, pitch expo factor + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.f); + +/** + * Acro mode yaw expo factor + * + * Exponential factor for tuning the input curve shape. + * + * 0 Purely linear input curve + * 1 Purely cubic input curve + * + * @min 0 + * @max 1 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.f); + +/** + * Acro mode roll, pitch super expo factor + * + * "Superexponential" factor for refining the input curve shape tuned using MC_ACRO_EXPO. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.f); + +/** + * Acro mode yaw super expo factor + * + * "Superexponential" factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. + * + * 0 Pure Expo function + * 0.7 reasonable shape enhancement for intuitive stick feel + * 0.95 very strong bent input curve only near maxima have effect + * + * @min 0 + * @max 0.95 + * @decimal 2 + * @group Multicopter Acro Mode + */ +PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.f); diff --git a/src/modules/mc_rate_control/mc_rate_control_params.c b/src/modules/mc_rate_control/mc_rate_control_params.c index 55e92f9be8..2571a70c1f 100644 --- a/src/modules/mc_rate_control/mc_rate_control_params.c +++ b/src/modules/mc_rate_control/mc_rate_control_params.c @@ -33,10 +33,8 @@ /** * @file mc_rate_control_params.c - * Parameters for multicopter attitude controller. * - * @author Lorenz Meier - * @author Anton Babushkin + * Parameters for multicopter rate controller */ /** @@ -281,110 +279,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_FF, 0.0f); */ PARAM_DEFINE_FLOAT(MC_YAWRATE_K, 1.0f); -/** - * Max acro roll rate - * - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 720.0f); - -/** - * Max acro pitch rate - * - * default: 2 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 720.0f); - -/** - * Max acro yaw rate - * - * default 1.5 turns per second - * - * @unit deg/s - * @min 0.0 - * @max 1800.0 - * @decimal 1 - * @increment 5 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 540.0f); - -/** - * Acro mode Expo factor for Roll and Pitch. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO, 0.69f); - -/** - * Acro mode Expo factor for Yaw. - * - * Exponential factor for tuning the input curve shape. - * - * 0 Purely linear input curve - * 1 Purely cubic input curve - * - * @min 0 - * @max 1 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_EXPO_Y, 0.69f); - -/** - * Acro mode SuperExpo factor for Roll and Pitch. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO. - * - * 0 Pure Expo function - * 0.7 reasonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPO, 0.7f); - -/** - * Acro mode SuperExpo factor for Yaw. - * - * SuperExpo factor for refining the input curve shape tuned using MC_ACRO_EXPO_Y. - * - * 0 Pure Expo function - * 0.7 reasonable shape enhancement for intuitive stick feel - * 0.95 very strong bent input curve only near maxima have effect - * - * @min 0 - * @max 0.95 - * @decimal 2 - * @group Multicopter Rate Control - */ -PARAM_DEFINE_FLOAT(MC_ACRO_SUPEXPOY, 0.7f); - /** * Battery power level scaler * diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 30725a05d0..b996566def 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -34,23 +34,36 @@ add_subdirectory(GeofenceBreachAvoidance) add_subdirectory(MissionFeasibility) +set(NAVIGATOR_SOURCES + navigator_main.cpp + navigator_mode.cpp + mission_base.cpp + mission_block.cpp + mission.cpp + loiter.cpp + rtl.cpp + rtl_direct.cpp + rtl_direct_mission_land.cpp + rtl_mission_fast.cpp + rtl_mission_fast_reverse.cpp + takeoff.cpp + land.cpp + precland.cpp + mission_feasibility_checker.cpp + geofence.cpp) + +if(CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF) + set(NAVIGATOR_SOURCES + ${NAVIGATOR_SOURCES} + vtol_takeoff.cpp) +endif() + px4_add_module( MODULE modules__navigator MAIN navigator - SRCS - navigator_main.cpp - navigator_mode.cpp - mission_block.cpp - mission.cpp - loiter.cpp - rtl.cpp - takeoff.cpp - land.cpp - precland.cpp - mission_feasibility_checker.cpp - geofence.cpp - vtol_takeoff.cpp + SRCS ${NAVIGATOR_SOURCES} DEPENDS + dataman_client geo adsb geofence_breach_avoidance diff --git a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp index 84e1991b34..4441a25166 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp @@ -34,7 +34,6 @@ #include #include "geofence_breach_avoidance.h" #include "fake_geofence.hpp" -#include "dataman_mocks.hpp" #include using namespace matrix; @@ -88,7 +87,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForFixedWing) Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.fence_violation = true; gf_avoidance.setHorizontalTestPointDistance(20.0f); @@ -148,7 +147,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterPointForMultirotor) value = 8; param_set(param, &value); - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.fence_violation = true; gf_avoidance.setHorizontalTestPointDistance(30.0f); @@ -198,7 +197,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForFixedWing) gf_avoidance.setVerticalTestPointDistance(vertical_test_point_dist); gf_avoidance.setCurrentPosition(0, 0, current_alt_amsl); // just care about altitude - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.max_altitude_exceeded = true; float loiter_alt = gf_avoidance.generateLoiterAltitudeForFixedWing(gf_violation); @@ -217,7 +216,7 @@ TEST_F(GeofenceBreachAvoidanceTest, generateLoiterAltitudeForMulticopter) GeofenceBreachAvoidance gf_avoidance(nullptr); const float climbrate = 10.0f; const float current_alt_amsl = 100.0f; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.max_altitude_exceeded = true; gf_avoidance.setClimbRate(climbrate); @@ -242,7 +241,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationMulticopter) FakeGeofence geo; Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.dist_to_home_exceeded = true; const float hor_vel = 8.0f; @@ -274,7 +273,7 @@ TEST_F(GeofenceBreachAvoidanceTest, maxDistToHomeViolationFixedWing) FakeGeofence geo; Vector2d home_global(42.1, 8.2); MapProjection ref{home_global(0), home_global(1)}; - geofence_violation_type_u gf_violation; + geofence_violation_type_u gf_violation{}; gf_violation.flags.dist_to_home_exceeded = true; const float test_point_distance = 30.0f; diff --git a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp b/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp deleted file mode 100644 index 5811863889..0000000000 --- a/src/modules/navigator/GeofenceBreachAvoidance/dataman_mocks.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file dataman_mocks.h - * Provides a minimal dataman implementation to compile against for testing - * - * @author Roman Bapst - * @author Julian Kent - */ -#pragma once - -#include -extern "C" { - __EXPORT ssize_t - dm_read( - dm_item_t item, /* The item type to retrieve */ - unsigned index, /* The index of the item */ - void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** write to the data manager store */ - __EXPORT ssize_t - dm_write( - dm_item_t item, /* The item type to store */ - unsigned index, /* The index of the item */ - const void *buffer, /* Pointer to caller data buffer */ - size_t buflen /* Length in bytes of data to retrieve */ - ) {return 0;}; - - /** - * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated - * atomically). - * Note that this lock is independent from dm_read & dm_write calls. - * @return 0 on success and lock taken, -1 on error (lock not taken, errno set) - */ - __EXPORT int - dm_lock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** - * Try to lock all items of a type (@see sem_trywait()). - * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly) - */ - __EXPORT int - dm_trylock( - dm_item_t item /* The item type to lock */ - ) {return 0;}; - - /** Unlock all items of a type */ - __EXPORT void - dm_unlock( - dm_item_t item /* The item type to unlock */ - ) {}; - - /** Erase all items of this type */ - __EXPORT int - dm_clear( - dm_item_t item /* The item type to clear */ - ) {return 0;}; -} - diff --git a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp index f84e24e74b..802dacaaf6 100644 --- a/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp +++ b/src/modules/navigator/GeofenceBreachAvoidance/geofence_breach_avoidance.cpp @@ -159,7 +159,7 @@ GeofenceBreachAvoidance::generateLoiterPointForMultirotor(geofence_violation_typ Vector2d test_point; // binary search for the distance from the drone to the geofence in the given direction - while (abs(current_max - current_min) > 0.5f) { + while (fabsf(current_max - current_min) > 0.5f) { test_point = waypointFromBearingAndDistance(_current_pos_lat_lon, _test_point_bearing, current_distance); if (!geofence->isInsidePolygonOrCircle(test_point(0), test_point(1), _current_alt_amsl)) { diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 422748e048..83e42cc594 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -10,3 +10,12 @@ menuconfig USER_NAVIGATOR depends on BOARD_PROTECTED && MODULES_NAVIGATOR ---help--- Put navigator in userspace memory + +menuconfig MODE_NAVIGATOR_VTOL_TAKEOFF + bool "Include VTOL takeoff mode support" + default n + depends on MODULES_NAVIGATOR + ---help--- + Add VTOL takeoff mode to enable support for MAV_CMD_NAV_VTOL_TAKEOFF. + The VTOL takes off in MC mode and transition to FW. The mode ends with + an infinite loiter diff --git a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp index f53b79edff..b31dacd9ce 100644 --- a/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp +++ b/src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp @@ -380,7 +380,6 @@ bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item) mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM && mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && - mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS && mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION); } diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index f131130b90..f6de5654f1 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,7 +43,7 @@ #include -#include +#include #include #include #include @@ -58,9 +58,8 @@ Geofence::Geofence(Navigator *navigator) : _navigator(navigator), _sub_airdata(ORB_ID(vehicle_air_data)) { - // we assume there's no concurrent fence update on startup if (_navigator != nullptr) { - _updateFence(); + updateFence(); } } @@ -71,42 +70,113 @@ Geofence::~Geofence() } } +void Geofence::run() +{ + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_fence_updated) { + _initiate_fence_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client.readAsync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client.update(); + + if (_dataman_client.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _fence_updated = false; + + _dataman_cache.invalidate(); + + if (_dataman_cache.size() != _stats.num_items) { + _dataman_cache.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache.size(); ++index) { + _dataman_cache.load(DM_KEY_FENCE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache.update(); + + if (!_dataman_cache.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _updateFence(); + _fence_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Geofence update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + + } +} + void Geofence::updateFence() { - // Note: be aware that when calling this, it can block for quite some time, the duration of a geofence transfer. - // However this is currently not used - if (dm_lock(DM_KEY_FENCE_POINTS) != 0) { - PX4_ERR("lock failed"); - return; - } - - _updateFence(); - dm_unlock(DM_KEY_FENCE_POINTS); + _initiate_fence_updated = true; } void Geofence::_updateFence() { - // initialize fence points count - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - int num_fence_items = 0; - - if (ret == sizeof(mission_stats_entry_s)) { - num_fence_items = stats.num_items; - _update_counter = stats.update_counter; - } + mission_fence_point_s mission_fence_point; + bool is_circle_area = false; // iterate over all polygons and store their starting vertices _num_polygons = 0; int current_seq = 1; - while (current_seq <= num_fence_items) { - mission_fence_point_s mission_fence_point; - bool is_circle_area = false; + while (current_seq <= _dataman_cache.size()) { - if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) != - sizeof(mission_fence_point_s)) { - PX4_ERR("dm_read failed"); + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, current_seq, + reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (!success) { + PX4_ERR("loadWait failed, seq: %i", current_seq); break; } @@ -172,9 +242,7 @@ void Geofence::_updateFence() ++current_seq; break; } - } - } bool Geofence::checkAll(const struct vehicle_global_position_s &global_position) @@ -220,7 +288,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen return checkAll(global_position); } else { - return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, gps_position.alt * 1.0e-3); + return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, gps_position.altitude_msl_m); } } else { @@ -232,7 +300,7 @@ bool Geofence::check(const vehicle_global_position_s &global_position, const sen return checkAll(global_position, baro_altitude_amsl); } else { - return checkAll(gps_position.lat * 1.0e-7, gps_position.lon * 1.0e-7, baro_altitude_amsl); + return checkAll(gps_position.latitude_deg, gps_position.longitude_deg, baro_altitude_amsl); } } } @@ -306,22 +374,7 @@ bool Geofence::isBelowMaxAltitude(float altitude) bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) { - // the following uses dm_read, so first we try to lock all items. If that fails, it (most likely) means - // the data is currently being updated (via a mavlink geofence transfer), and we do not check for a violation now - if (dm_trylock(DM_KEY_FENCE_POINTS) != 0) { - return true; - } - - // we got the lock, now check if the fence data got updated - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - - if (ret == sizeof(mission_stats_entry_s) && _update_counter != stats.update_counter) { - _updateFence(); - } - if (isEmpty()) { - dm_unlock(DM_KEY_FENCE_POINTS); /* Empty fence -> accept all points */ return true; } @@ -329,12 +382,10 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) /* Vertical check */ if (_altitude_max > _altitude_min) { // only enable vertical check if configured properly if (altitude > _altitude_max || altitude < _altitude_min) { - dm_unlock(DM_KEY_FENCE_POINTS); return false; } } - /* Horizontal check: iterate all polygons & circles */ bool outside_exclusion = true; bool inside_inclusion = false; @@ -375,8 +426,6 @@ bool Geofence::isInsidePolygonOrCircle(double lat, double lon, float altitude) } } - dm_unlock(DM_KEY_FENCE_POINTS); - return (!had_inclusion_areas || inside_inclusion) && outside_exclusion; } @@ -394,13 +443,18 @@ bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, bool c = false; for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) { - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, + reinterpret_cast(&temp_vertex_i), sizeof(mission_fence_point_s)); + + if (!success) { break; } - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, + reinterpret_cast(&temp_vertex_j), sizeof(mission_fence_point_s)); + + if (!success) { break; } @@ -426,9 +480,10 @@ bool Geofence::insideCircle(const PolygonInfo &polygon, double lat, double lon, { mission_fence_point_s circle_point{}; + bool success = _dataman_cache.loadWait(DM_KEY_FENCE_POINTS, polygon.dataman_index, + reinterpret_cast(&circle_point), sizeof(mission_fence_point_s)); - if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index, &circle_point, - sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) { + if (!success) { PX4_ERR("dm_read failed"); return false; } @@ -531,7 +586,10 @@ Geofence::loadFromFile(const char *filename) } } - if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, &vertex, sizeof(vertex)) != sizeof(vertex)) { + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, pointCounter + 1, reinterpret_cast(&vertex), + sizeof(vertex)); + + if (!success) { goto error; } @@ -555,22 +613,32 @@ Geofence::loadFromFile(const char *filename) if (gotVertical && pointCounter > 2) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported\t"); events::send(events::ID("navigator_geofence_imported"), events::Log::Info, "Geofence imported"); - ret_val = PX4_OK; + ret_val = PX4_ERROR; /* do a second pass, now that we know the number of vertices */ for (int seq = 1; seq <= pointCounter; ++seq) { mission_fence_point_s mission_fence_point; - if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) == - sizeof(mission_fence_point_s)) { + bool success = _dataman_client.readSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); + + if (success) { mission_fence_point.vertex_count = pointCounter; - dm_write(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)); + _dataman_client.writeSync(DM_KEY_FENCE_POINTS, seq, reinterpret_cast(&mission_fence_point), + sizeof(mission_fence_point_s)); } } mission_stats_entry_s stats; stats.num_items = pointCounter; - ret_val = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); + stats.update_counter = _update_counter + 1; + + bool success = _dataman_client.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (success) { + ret_val = PX4_OK; + } } else { mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence: import error\t"); @@ -586,7 +654,7 @@ error: int Geofence::clearDm() { - dm_clear(DM_KEY_FENCE_POINTS); + _dataman_client.clearSync(DM_KEY_FENCE_POINTS); updateFence(); return PX4_OK; } diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index b7386db830..1804d151d5 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,7 @@ #include +#include #include #include #include @@ -77,9 +78,13 @@ public: GF_SOURCE_GPS = 1 }; + /** + * @brief function to call regularly to do background work + */ + void run(); + /** * update the geofence from dataman. - * It's generally not necessary to call this as it will automatically update when the data is changed. */ void updateFence(); @@ -136,7 +141,7 @@ public: */ int loadFromFile(const char *filename); - bool isEmpty() { return _num_polygons == 0; } + bool isEmpty() { return (!_fence_updated || (_num_polygons == 0)); } int getSource() { return _param_gf_source.get(); } int getGeofenceAction() { return _param_gf_action.get(); } @@ -154,6 +159,14 @@ public: private: + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error + }; + struct PolygonInfo { uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_* (can also be a circular region) uint16_t dataman_index; @@ -166,6 +179,12 @@ private: Navigator *_navigator{nullptr}; PolygonInfo *_polygons{nullptr}; + mission_stats_entry_s _stats; + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + DatamanCache _dataman_cache{"geofence_dm_cache_miss", 4}; + DatamanClient &_dataman_client = _dataman_cache.client(); + hrt_abstime _last_horizontal_range_warning{0}; hrt_abstime _last_vertical_range_warning{0}; @@ -179,10 +198,12 @@ private: uORB::SubscriptionData _sub_airdata; int _outside_counter{0}; - uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, we polygon data was updated + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, polygon data was updated + bool _fence_updated{true}; ///< flag indicating if fence are updated to dataman cache + bool _initiate_fence_updated{true}; ///< flag indicating if fence updated is needed /** - * implementation of updateFence(), but without locking + * implementation of updateFence() */ void _updateFence(); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 129aeb2774..053a0aa36c 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -128,7 +128,9 @@ PARAM_DEFINE_FLOAT(GF_MAX_HOR_DIST, 0); PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0); /** - * Use Pre-emptive geofence triggering + * [EXPERIMENTAL] Use Pre-emptive geofence triggering + * + * WARNING: This experimental feature may cause flyaways. Use at your own risk. * * Predict the motion of the vehicle and trigger the breach if it is determined that the current trajectory * would result in a breach happening before the vehicle can make evasive maneuvers. @@ -137,4 +139,4 @@ PARAM_DEFINE_FLOAT(GF_MAX_VER_DIST, 0); * @boolean * @group Geofence */ -PARAM_DEFINE_INT32(GF_PREDICT, 1); +PARAM_DEFINE_INT32(GF_PREDICT, 0); diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp index d280c942c6..08513bfa9f 100644 --- a/src/modules/navigator/land.cpp +++ b/src/modules/navigator/land.cpp @@ -62,8 +62,6 @@ Land::on_activation() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(false); - _navigator->set_position_setpoint_triplet_updated(); // reset cruising speed to default diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index 655db702fa..66fadb10c4 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -48,19 +48,15 @@ Loiter::Loiter(Navigator *navigator) : { } -void -Loiter::on_inactive() -{ - _loiter_pos_set = false; -} - void Loiter::on_activation() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_navigator->get_reposition_triplet()->current.valid + && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) { reposition(); } else { + // this is executed when the flight mode is switched to Hold manually, not through a reposition set_loiter_position(); } @@ -71,14 +67,10 @@ Loiter::on_activation() void Loiter::on_active() { - if (_navigator->get_reposition_triplet()->current.valid) { + if (_navigator->get_reposition_triplet()->current.valid + && hrt_elapsed_time(&_navigator->get_reposition_triplet()->current.timestamp) < 500_ms) { reposition(); } - - // reset the loiter position if we get disarmed - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) { - _loiter_pos_set = false; - } } void @@ -90,35 +82,23 @@ Loiter::set_loiter_position() // Not setting loiter position if disarmed and landed, instead mark the current // setpoint as invalid and idle (both, just to be sure). - _navigator->set_can_loiter_at_sp(false); _navigator->get_position_setpoint_triplet()->current.type = position_setpoint_s::SETPOINT_TYPE_IDLE; _navigator->set_position_setpoint_triplet_updated(); - _loiter_pos_set = false; return; - } else if (_loiter_pos_set) { - // Already set, nothing to do. - return; } - _loiter_pos_set = true; - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); if (_navigator->get_land_detected()->landed) { _mission_item.nav_cmd = NAV_CMD_IDLE; } else { - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + setLoiterItemFromCurrentPositionWithBreaking(&_mission_item); } else { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - setLoiterItemFromCurrentPositionWithBreaking(&_mission_item); - - } else { - setLoiterItemFromCurrentPosition(&_mission_item); - } + setLoiterItemFromCurrentPosition(&_mission_item); } } @@ -129,7 +109,6 @@ Loiter::set_loiter_position() mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); } @@ -155,7 +134,6 @@ Loiter::reposition() memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current)); pos_sp_triplet->next.valid = false; - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); _navigator->set_position_setpoint_triplet_updated(); // mark this as done diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 9d2e84cda4..3af6265e1c 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -51,7 +51,6 @@ public: Loiter(Navigator *navigator); ~Loiter() = default; - void on_inactive() override; void on_activation() override; void on_active() override; @@ -67,5 +66,4 @@ private: */ void set_loiter_position(); - bool _loiter_pos_set{false}; }; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index e9270246f4..3e97896d02 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -50,7 +50,6 @@ #include #include -#include #include #include #include @@ -63,442 +62,43 @@ using namespace time_literals; +static constexpr int32_t DEFAULT_MISSION_CACHE_SIZE = 10; + Mission::Mission(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + MissionBase(navigator, DEFAULT_MISSION_CACHE_SIZE) { - mission_init(); -} - -void Mission::mission_init() -{ - // init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start - mission_s mission{}; - - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { - if ((mission.timestamp != 0) - && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) { - if (mission.count > 0) { - PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count); - } - - } else { - PX4_ERR("reading mission state failed"); - - // initialize mission state in dataman - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.timestamp = hrt_absolute_time(); - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); - } - } } void Mission::on_inactive() { - /* Without home a mission can't be valid yet anyway, let's wait. */ - if (!_navigator->home_global_position_valid()) { - return; + _vehicle_status_sub.update(); + + if (_need_mission_save && _vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + save_mission_state(); } - if (_inited) { - if (_mission_sub.updated()) { - update_mission(); - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - } - - /* reset the current mission if needed */ - if (need_to_reset_mission()) { - reset_mission(_mission); - _navigator->reset_cruising_speed(); - _current_mission_index = 0; - _navigator->reset_vroi(); - set_current_mission_item(); - } - - } else { - - /* load missions from storage */ - mission_s mission_state = {}; - - dm_lock(DM_KEY_MISSION_STATE); - - /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - - dm_unlock(DM_KEY_MISSION_STATE); - - if (read_res == sizeof(mission_s)) { - _mission.dataman_id = mission_state.dataman_id; - _mission.count = mission_state.count; - _current_mission_index = mission_state.current_seq; - - // find and store landing start marker (if available) - find_mission_land_start(); - } - - /* On init let's check the mission, maybe there is already one available. */ - check_mission_valid(false); - - _inited = true; - } - - /* require takeoff after non-loiter or landing */ - if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) { - _need_takeoff = true; - } - - /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* reset so MISSION_ITEM_REACHED isn't published */ - _navigator->get_mission_result()->seq_reached = -1; -} - -void -Mission::on_inactivation() -{ - // Disable camera trigger - vehicle_command_s cmd {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // Pause trigger - cmd.param1 = -1.0f; - cmd.param3 = 1.0f; - _navigator->publish_vehicle_cmd(&cmd); - - _navigator->stop_capturing_images(); - _navigator->release_gimbal_control(); - - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - /* reset so current mission item gets restarted if mission was paused */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; + MissionBase::on_inactive(); } void Mission::on_activation() { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } + _need_mission_save = true; - _mission_waypoints_changed = false; - } - - // we already reset the mission items - _execution_mode_changed = false; - - set_mission_items(); - - // unpause triggering if it was paused - vehicle_command_s cmd = {}; - cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; - // unpause trigger - cmd.param1 = -1.0f; - cmd.param3 = 0.0f; - _navigator->publish_vehicle_cmd(&cmd); - - // reset cruise speed - _navigator->reset_cruising_speed(); -} - -void -Mission::on_active() -{ - check_mission_valid(false); - - /* Check if stored mission plan has changed */ - const bool mission_sub_updated = _mission_sub.updated(); - - if (mission_sub_updated) { - _navigator->reset_triplets(); - update_mission(); - } - - /* mission is running (and we are armed), need reset after disarm */ - _need_mission_reset = true; - - _mission_changed = false; - - /* reset mission items if needed */ - if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) { - if (_mission_waypoints_changed) { - // do not set the closest mission item in the normal mission mode - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) { - _current_mission_index = index_closest_mission_item(); - } - - _mission_waypoints_changed = false; - } - - _execution_mode_changed = false; - set_mission_items(); - } - - /* lets check if we reached the current mission item */ - if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { - /* If we just completed a takeoff which was inserted before the right waypoint, - there is no need to report that we reached it because we didn't. */ - if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) { - set_mission_item_reached(); - } - - if (_mission_item.autocontinue) { - /* switch to next waypoint if 'autocontinue' flag set */ - advance_mission(); - set_mission_items(); - } - - } else { - /* if waypoint position reached allow loiter on the setpoint */ - if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { - _navigator->set_can_loiter_at_sp(true); - } - } - - /* check if a cruise speed change has been commanded */ - if (_mission_type != MISSION_TYPE_NONE) { - cruising_speed_sp_update(); - } - - /* see if we need to update the current yaw heading */ - if (!_param_mis_mnt_yaw_ctl.get() - && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) - && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) - && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - || _mission_item.nav_cmd == NAV_CMD_LAND - || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND - || _work_item_type == WORK_ITEM_TYPE_ALIGN)) { - // Mount control is disabled If the vehicle is in ROI-mode, the vehicle - // needs to rotate such that ROI is in the field of view. - // ROI only makes sense for multicopters. - heading_sp_update(); - } - - // TODO: Add vtol heading update method if required. - // Question: Why does vtol ever have to update heading? - - /* check if landing needs to be aborted */ - if ((_mission_item.nav_cmd == NAV_CMD_LAND) - && (_navigator->abort_landing())) { - - do_abort_landing(); - } - - if (_work_item_type == WORK_ITEM_TYPE_PRECISION_LAND) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } + MissionBase::on_activation(); } bool -Mission::set_current_mission_index(uint16_t index) -{ - if (index == _current_mission_index) { - return true; // nothing to do, so return true - - } else if (_navigator->get_mission_result()->valid && (index < _mission.count)) { - - _current_mission_index = index; - - // a mission index is set manually which has the higher priority than the closest mission item - // as it is set by the user - _mission_waypoints_changed = false; - - // update mission items if already in active mission - if (_navigator->is_planned_mission()) { - // prevent following "previous - current" line - _navigator->get_position_setpoint_triplet()->previous.valid = false; - _navigator->get_position_setpoint_triplet()->current.valid = false; - _navigator->get_position_setpoint_triplet()->next.valid = false; - set_mission_items(); - } - - return true; - } - - return false; -} - -void -Mission::set_closest_item_as_current() -{ - _current_mission_index = index_closest_mission_item(); -} - -void -Mission::set_execution_mode(const uint8_t mode) -{ - if (_mission_execution_mode != mode) { - _execution_mode_changed = true; - _navigator->get_mission_result()->execution_mode = mode; - - - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: - if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - // command a transition if in vtol mc mode - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - _navigator->get_vstatus()->is_vtol && - !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - issue_command(_mission_item); - } - - if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) { - _mission_type = MISSION_TYPE_MISSION; - } - - if (_current_mission_index > _mission.count - 1) { - _current_mission_index = _mission.count - 1; - - } else if (_current_mission_index > 0) { - --_current_mission_index; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: - if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) || - (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) { - // handle switch from reverse to forward mission - if (_current_mission_index < 0) { - _current_mission_index = 0; - - } else if (_current_mission_index < _mission.count - 1) { - ++_current_mission_index; - } - - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - } - - break; - - } - - _mission_execution_mode = mode; - } -} - -bool -Mission::find_mission_land_start() -{ - /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index - * return false if not found - */ - - const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; - struct mission_item_s missionitem = {}; - struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START - - _land_start_available = false; - - bool found_land_start_marker = false; - - for (size_t i = 1; i < _mission.count; i++) { - const ssize_t len = sizeof(missionitem); - missionitem_prev = missionitem; // store the last mission item before reading a new one - - if (dm_read(dm_current, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - found_land_start_marker = true; - } - - if (found_land_start_marker && !_land_start_available && item_contains_position(missionitem)) { - // use the position of any waypoint after the land start marker which specifies a position. - _landing_start_lat = missionitem.lat; - _landing_start_lon = missionitem.lon; - _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; - _landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius) - && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) : - _navigator->get_loiter_radius(); - _land_start_available = true; - _land_start_index = i; // set it to the first item containing a position after the land start marker was found - } - - if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || - (missionitem.nav_cmd == NAV_CMD_LAND)) { - - _landing_lat = missionitem.lat; - _landing_lon = missionitem.lon; - _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : - missionitem.altitude; - - // don't have a valid land start yet, use the landing item itself then - if (!_land_start_available) { - _land_start_index = i; - _landing_start_lat = _landing_lat; - _landing_start_lon = _landing_lon; - _landing_start_alt = _landing_alt; - _land_start_available = true; - } - - } - } - - return _land_start_available; -} - -bool -Mission::land_start() -{ - // if not currently landing, jump to do_land_start - if (_land_start_available) { - // check if we're currently already in mission mode and on landing part, then simply return true. - // note: it's not enough to check landing(), as that is not reset until set_current_mission_index(get_land_start_index()) - if (_navigator->on_mission_landing()) { - return true; - - } else { - set_current_mission_index(get_land_start_index()); - return landing(); - } - } - - return false; -} - -bool -Mission::landing() +Mission::isLanding() { // vehicle is currently landing if // mission valid, still flying, and in the landing portion of mission (past land start marker) - - const bool mission_valid = _navigator->get_mission_result()->valid; - bool on_landing_stage = _land_start_available && _current_mission_index > get_land_start_index(); + bool on_landing_stage = get_land_start_available() && _mission.current_seq > get_land_start_index(); // special case: if the land start index is at a LOITER_TO_ALT WP, then we're in the landing sequence already when the // distance to the WP is below the loiter radius + acceptance. - if (_current_mission_index == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { + if (_mission.current_seq == get_land_start_index() && _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) { const float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); @@ -510,777 +110,56 @@ Mission::landing() on_landing_stage = d_current <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs); } - return mission_valid && on_landing_stage; -} - -void -Mission::update_mission() -{ - - bool failed = true; - - /* Reset vehicle_roi - * Missions that do not explicitly configure ROI would not override - * an existing ROI setting from previous missions */ - _navigator->reset_vroi(); - - const mission_s old_mission = _mission; - - if (_mission_sub.copy(&_mission)) { - /* determine current index */ - if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) { - _current_mission_index = _mission.current_seq; - - } else { - /* if less items available, reset to first item */ - if (_current_mission_index >= (int)_mission.count) { - _current_mission_index = 0; - - } else if (_current_mission_index < 0) { - /* if not initialized, set it to 0 */ - _current_mission_index = 0; - } - - /* otherwise, just leave it */ - } - - check_mission_valid(true); - - failed = !_navigator->get_mission_result()->valid; - - if (!failed) { - /* reset mission failure if we have an updated valid mission */ - _navigator->get_mission_result()->failure = false; - - /* reset sequence info as well */ - _navigator->get_mission_result()->seq_reached = -1; - _navigator->get_mission_result()->seq_total = _mission.count; - - /* reset work item if new mission has been accepted */ - _work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_changed = true; - } - - /* check if the mission waypoints changed while the vehicle is in air - * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */ - if (((_mission.count != old_mission.count) || - (_mission.dataman_id != old_mission.dataman_id)) && - !_navigator->get_land_detected()->landed) { - _mission_waypoints_changed = true; - } - - } else { - PX4_ERR("mission update failed"); - } - - if (failed) { - // only warn if the check failed on merit - if ((int)_mission.count > 0) { - PX4_WARN("mission check failed"); - } - - // reset the mission - _mission.count = 0; - _mission.current_seq = 0; - _current_mission_index = 0; - } - - // find and store landing start marker (if available) - find_mission_land_start(); - - set_current_mission_item(); -} - - -void -Mission::advance_mission() -{ - /* do not advance mission item if we're processing sub mission work items */ - if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { - return; - } - - switch (_mission_type) { - case MISSION_TYPE_MISSION: - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - _current_mission_index++; - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // find next position item in reverse order - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (int32_t i = _current_mission_index - 1; i >= 0; i--) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - - if (dm_read(dm_current, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - _current_mission_index = i; - return; - } - } - - // finished flying back the mission - _current_mission_index = -1; - break; - } - - default: - _current_mission_index++; - } - - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::set_mission_items() -{ - /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = false; - - /* mission item that comes after current if available */ - struct mission_item_s mission_item_next_position; - struct mission_item_s mission_item_after_next_position; - bool has_next_position_item = false; - bool has_after_next_position_item = false; - - work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item, - &mission_item_after_next_position, &has_after_next_position_item)) { - /* if mission type changed, notify */ - if (_mission_type != MISSION_TYPE_MISSION) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Executing Reverse Mission\t" : - "Executing Mission\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_execute_rev"), events::Log::Info, "Executing Reverse Mission"); - - } else { - events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_MISSION; - - } else { - if (_mission_type != MISSION_TYPE_NONE) { - - if (_navigator->get_land_detected()->landed) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed\t" : - "Mission finished, landed\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev"), events::Log::Info, "Reverse Mission finished, landed"); - - } else { - events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); - } - - } else { - /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ - mavlink_log_info(_navigator->get_mavlink_log_pub(), - _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, loitering\t" : - "Mission finished, loitering\t"); - - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - events::send(events::ID("mission_finished_rev_loiter"), events::Log::Info, "Reverse Mission finished, loitering"); - - } else { - events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); - } - - /* use last setpoint for loiter */ - _navigator->set_can_loiter_at_sp(true); - } - - user_feedback_done = true; - } - - _mission_type = MISSION_TYPE_NONE; - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - if (_navigator->get_land_detected()->landed) { - _mission_item.nav_cmd = NAV_CMD_IDLE; - - } else { - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { - setLoiterItemFromCurrentPositionSetpoint(&_mission_item); - - } else { - setLoiterItemFromCurrentPosition(&_mission_item); - } - - } - - /* update position setpoint triplet */ - pos_sp_triplet->previous.valid = false; - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - - /* reuse setpoint for LOITER only if it's not IDLE */ - _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER); - - // set mission finished - _navigator->get_mission_result()->finished = true; - _navigator->set_mission_result_updated(); - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); - - if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - - if (_navigator->get_land_detected()->landed) { - /* landed, refusing to take off without a mission */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff\t"); - events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, refusing takeoff"); - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); - events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, - "No valid mission available, loitering"); - } - - user_feedback_done = true; - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); - - return; - } - - /*********************************** handle mission item *********************************************/ - - /* handle mission items depending on the mode */ - - const position_setpoint_s current_setpoint_copy = _navigator->get_position_setpoint_triplet()->current; - - if (item_contains_position(_mission_item)) { - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - /* force vtol land */ - if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { - _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; - } - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* do takeoff before going to setpoint if needed and not already in takeoff */ - /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ - if (do_need_vertical_takeoff() && - _work_item_type == WORK_ITEM_TYPE_DEFAULT && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_TAKEOFF; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; - has_next_position_item = true; - - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home\t", - (double)(takeoff_alt - _navigator->get_home_position()->alt)); - events::send(events::ID("mission_takeoff_to"), events::Log::Info, - "Takeoff to {1:.1m_v} above home", takeoff_alt - _navigator->get_home_position()->alt); - - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - /* hold heading for takeoff items */ - _mission_item.yaw = _navigator->get_local_position()->heading; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - - /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - - } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - // if the vehicle is already in fixed wing mode then the current mission item - // will be accepted immediately and the work items will be skipped - _work_item_type = WORK_ITEM_TYPE_TAKEOFF; - - - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a normal takeoff navigate to the actual waypoint now */ - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ - _mission_item.yaw = NAN; - } - - /* if we just did a VTOL takeoff, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_TAKEOFF && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - - _mission_item.force_heading = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN; - - /* set position setpoint to current while aligning */ - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - } - - /* heading is aligned now, prepare transition */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && - _work_item_type == WORK_ITEM_TYPE_ALIGN && - _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && - !_navigator->get_land_detected()->landed) { - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - /* check if the vtol_takeoff waypoint is on top of us */ - if (do_need_move_to_takeoff()) { - new_work_item_type = WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; - } - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); - _mission_item.yaw = _navigator->get_local_position()->heading; - - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF - && _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - /* move to land wp as fixed wing */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && !_navigator->get_land_detected()->landed) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - _mission_item.vtol_back_transition = true; - } - - /* transition to MC */ - if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND - && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING - && !_navigator->get_land_detected()->landed) { - - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - _mission_item.altitude = _navigator->get_global_position()->alt; - _mission_item.altitude_is_relative = false; - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; - - // make previous setpoint invalid, such that there will be no prev-current line following - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - } - - /* move to landing waypoint before descent if necessary */ - if (do_need_move_to_land() && - (_work_item_type == WORK_ITEM_TYPE_DEFAULT || - _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION) && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND; - - /* use current mission item as next position item */ - mission_item_next_position = _mission_item; - has_next_position_item = true; - - /* - * Ignoring waypoint altitude: - * Set altitude to the same as we have now to prevent descending too fast into - * the ground. Actual landing will descend anyway until it touches down. - * XXX: We might want to change that at some point if it is clear to the user - * what the altitude means on this waypoint type. - */ - float altitude = _navigator->get_global_position()->alt; - - if (pos_sp_triplet->current.valid - && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - altitude = pos_sp_triplet->current.alt; - } - - _mission_item.altitude = altitude; - _mission_item.altitude_is_relative = false; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - // make previous setpoint invalid, such that there will be no prev-current line following. - // if the vehicle drifted off the path during back-transition it should just go straight to the landing point - pos_sp_triplet->previous.valid = false; - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WORK_ITEM_TYPE_DEFAULT) { - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - } - - /* we just moved to the landing waypoint, now descend */ - if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { - new_work_item_type = WORK_ITEM_TYPE_PRECISION_LAND; - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - - } else { //_mission_item.land_precision == 2 - _navigator->get_precland()->set_mode(PrecLandMode::Required); - } - - _navigator->get_precland()->on_activation(); - - } - - } - - /* ignore yaw for landing items */ - /* XXX: if specified heading for landing is desired we could add another step before the descent - * that aligns the vehicle first */ - if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { - _mission_item.yaw = NAN; - } - - - // for fast forward convert certain types to simple waypoint - // XXX: add other types which should be ignored in fast forward - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD && - ((_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) || - (_mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT))) { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - if (item_contains_position(_mission_item)) { - // convert mission item to a simple waypoint - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "MissionReverse: Got a non-position mission item, ignoring it\t"); - events::send(events::ID("mission_ignore_non_position_item"), events::Log::Info, - "MissionReverse: Got a non-position mission item, ignoring it"); - } - - break; - } - } - - } else { - /* handle non-position mission items such as commands */ - switch (_mission_execution_mode) { - case mission_result_s::MISSION_EXECUTION_MODE_NORMAL: - case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: { - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - /* turn towards next waypoint before MC to FW transition */ - if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION - && _work_item_type == WORK_ITEM_TYPE_DEFAULT - && new_work_item_type == WORK_ITEM_TYPE_DEFAULT - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_navigator->get_land_detected()->landed - && has_next_position_item) { - - /* disable weathervane before front transition for allowing yaw to align */ - pos_sp_triplet->current.disable_weather_vane = true; - - new_work_item_type = WORK_ITEM_TYPE_ALIGN; - - set_align_mission_item(&_mission_item, &mission_item_next_position); - - /* set position setpoint to target during the transition */ - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - /* yaw is aligned now */ - if (_work_item_type == WORK_ITEM_TYPE_ALIGN && - new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { - - new_work_item_type = WORK_ITEM_TYPE_DEFAULT; - - /* re-enable weather vane again after alignment */ - pos_sp_triplet->current.disable_weather_vane = false; - - pos_sp_triplet->previous = pos_sp_triplet->current; - // keep current setpoints (FW position controller generates wp to track during transition) - pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } - - // ignore certain commands in mission fast forward - if ((_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD) && - (_mission_item.nav_cmd == NAV_CMD_DELAY)) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0.0f; - } - - break; - } - - case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: { - // nothing to do, all commands are ignored - break; - } - } - - if (_mission_item.nav_cmd == NAV_CMD_CONDITION_GATE) { - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; - } - } - - /*********************************** set setpoints and check next *********************************************/ - // The logic in this section establishes the tracking between the current waypoint - // which we are approaching and the next waypoint, which will tell us in which direction - // we will change our trajectory right after reaching it. - - // Because actions, gates and jump labels can be interleaved with waypoints, - // we are searching around the current mission item in the list to find the closest - // gate and the closest waypoint. We then store them separately. - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - // Check if the mission item is a gate along the current trajectory - if (item_contains_gate(_mission_item)) { - - // The mission item is a gate, let's check if the next item in the list provides - // a position to go towards. - - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (has_next_position_item) { - // We have a position, convert it to the setpoint and update setpoint triplet - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - - } else { - // The mission item is not a gate, set the current position setpoint from mission item (is protected against non-position items) - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) { - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); - } - - // ELSE: The current position setpoint stays unchanged. - } - - // Only set the previous position item if the current one really changed - // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 - if ((_work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) && - !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { - pos_sp_triplet->previous = current_setpoint_copy; - } - - /* issue command if ready (will do nothing for position mission items) */ - issue_command(_mission_item); - - /* set current work item type */ - _work_item_type = new_work_item_type; - - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND - || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { - - _need_takeoff = true; - } - - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); - - if (_mission_type == MISSION_TYPE_MISSION) { - set_current_mission_item(); - } - - // If the mission item under evaluation contains a gate, we need to check if we have a next position item so - // the controller can fly the correct line between the current and next setpoint - if (item_contains_gate(_mission_item)) { - if (has_after_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_apply_limitation(mission_item_next_position); - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - pos_sp_triplet->next.valid = false; - } - - } else { - // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout - // This is done by setting the position triplet's next position's valid flag to false, - // which makes the FlightTask disregard the next position - // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior - // seems hacky, handle this more properly. - const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); - - if (_mission_item.autocontinue && !brake_for_hold) { - /* try to process next mission item */ - if (has_next_position_item) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next); - - } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; - } - - } else { - /* vehicle will be paused on current waypoint, don't set next item */ - pos_sp_triplet->next.valid = false; - } - } - - publish_navigator_mission_item(); // for logging - _navigator->set_position_setpoint_triplet_updated(); + return _navigator->get_mission_result()->valid && on_landing_stage; } bool -Mission::do_need_vertical_takeoff() +Mission::set_current_mission_index(uint16_t index) { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (index == _mission.current_seq) { + return true; + } - float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - - if (_navigator->get_land_detected()->landed) { - /* force takeoff if landed (additional protection) */ - _need_takeoff = true; - - } else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) { - /* if in-air and already above takeoff height, don't do takeoff */ - _need_takeoff = false; - - } else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius() - && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF - || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) { - /* if in-air but below takeoff height and we have a takeoff item */ - _need_takeoff = true; + if (_navigator->get_mission_result()->valid && (index < _mission.count)) { + if (goToItem(index, true) != PX4_OK) { + // Keep the old mission index (it was not updated by the interface) and report back. + return false; } - /* check if current mission item is one that requires takeoff before */ - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + _is_current_planned_mission_item_valid = true; - _need_takeoff = false; - return true; + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); } + + // update mission items if already in active mission + if (isActive()) { + // prevent following "previous - current" line + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + } + + return true; } return false; } +bool Mission::setNextMissionItem() +{ + return (goToNextItem(true) == PX4_OK); +} + bool Mission::do_need_move_to_land() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1291,11 +170,11 @@ Mission::do_need_move_to_land() bool Mission::do_need_move_to_takeoff() { - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) { float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + _global_pos_sub.get().lat, _global_pos_sub.get().lon); return d_current > _navigator->get_acceptance_radius(); } @@ -1303,386 +182,440 @@ Mission::do_need_move_to_takeoff() return false; } -void -Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint) +void Mission::setActiveMissionItems() { - if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { - mission_item->lat = setpoint->lat; - mission_item->lon = setpoint->lon; - mission_item->altitude = setpoint->alt; + /* Get mission item that comes after current if available */ + constexpr static size_t max_num_next_items{2u}; + int32_t next_mission_items_index[max_num_next_items]; + size_t num_found_items; - } else { - mission_item->lat = _navigator->get_global_position()->lat; - mission_item->lon = _navigator->get_global_position()->lon; - mission_item->altitude = _navigator->get_global_position()->alt; - } + getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items); - mission_item->altitude_is_relative = false; -} + mission_item_s next_mission_items[max_num_next_items]; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); -void -Mission::set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next) -{ - mission_item->nav_cmd = NAV_CMD_WAYPOINT; - copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); - mission_item->altitude_is_relative = false; - mission_item->autocontinue = true; - mission_item->time_inside = 0.0f; - mission_item->yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - mission_item_next->lat, mission_item_next->lon); - mission_item->force_heading = true; -} + for (size_t i = 0U; i < num_found_items; i++) { + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i], + reinterpret_cast(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT); -float -Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) -{ - /* calculate takeoff altitude */ - float takeoff_alt = get_absolute_altitude_for_item(*mission_item); - - /* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ - if (_navigator->get_land_detected()->landed) { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt()); - - } else { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt()); - } - - return takeoff_alt; -} - -void -Mission::heading_sp_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = - _navigator->get_position_setpoint_triplet(); - - // Only update if current triplet is valid - if (pos_sp_triplet->current.valid) { - - double point_from_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - double point_to_latlon[2] = { _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon - }; - float yaw_offset = 0.0f; - - // Depending on ROI-mode, update heading - switch (_navigator->get_vroi().mode) { - case vehicle_roi_s::ROI_LOCATION: { - // ROI is a fixed location. Vehicle needs to point towards that location - point_to_latlon[0] = _navigator->get_vroi().lat; - point_to_latlon[1] = _navigator->get_vroi().lon; - // No yaw offset required - yaw_offset = 0.0f; - break; - } - - case vehicle_roi_s::ROI_WPNEXT: { - // ROI is current waypoint. Vehcile needs to point towards current waypoint - point_to_latlon[0] = pos_sp_triplet->current.lat; - point_to_latlon[1] = pos_sp_triplet->current.lon; - // Add the gimbal's yaw offset - yaw_offset = _navigator->get_vroi().yaw_offset; - break; - } - - case vehicle_roi_s::ROI_NONE: - case vehicle_roi_s::ROI_WPINDEX: - case vehicle_roi_s::ROI_TARGET: - case vehicle_roi_s::ROI_ENUM_END: - default: { - return; - } - } - - // Get desired heading and update it. - // However, only update if distance to desired heading is - // larger than acceptance radius to prevent excessive yawing - float d_current = get_distance_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); - - if (d_current > _navigator->get_acceptance_radius()) { - float yaw = matrix::wrap_pi( - get_bearing_to_next_waypoint(point_from_latlon[0], - point_from_latlon[1], point_to_latlon[0], - point_to_latlon[1]) + yaw_offset); - - _mission_item.yaw = yaw; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; + if (success) { + next_mission_items[i] = next_mission_item; } else { - if (!pos_sp_triplet->current.yaw_valid) { - _mission_item.yaw = _navigator->get_local_position()->heading; - pos_sp_triplet->current.yaw = _mission_item.yaw; - pos_sp_triplet->current.yaw_valid = true; - } + num_found_items = i; + break; + } + } + + /*********************************** handle mission item *********************************************/ + WorkItemType new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; + + if (item_contains_position(_mission_item)) { + /* force vtol land */ + if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) { + _mission_item.nav_cmd = NAV_CMD_VTOL_LAND; } - // we set yaw directly so we can run this in parallel to the FOH update - publish_navigator_mission_item(); - _navigator->set_position_setpoint_triplet_updated(); - } -} + handleTakeoff(new_work_item_type, next_mission_items, num_found_items); -void -Mission::cruising_speed_sp_update() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + handleLanding(new_work_item_type, next_mission_items, num_found_items); - const float cruising_speed = _navigator->get_cruising_speed(); + // TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320 + if (new_work_item_type != WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } - /* Don't change setpoint if the current waypoint is not valid */ - if (!pos_sp_triplet->current.valid || - fabsf(pos_sp_triplet->current.cruising_speed - cruising_speed) < FLT_EPSILON) { - return; + // Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout + // This is done by setting the position triplet's next position's valid flag to false, + // which makes the FlightTask disregard the next position + // TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior + // seems hacky, handle this more properly. + const bool brake_for_hold = _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item)); + + if (_mission_item.autocontinue && !brake_for_hold) { + /* try to process next mission item */ + if (num_found_items >= 1u) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + + } else { + /* vehicle will be paused on current waypoint, don't set next item */ + pos_sp_triplet->next.valid = false; + } + + } else if (item_contains_gate(_mission_item)) { + // The mission item is a gate, let's check if the next item in the list provides + // a position to go towards. + + if (num_found_items > 0u) { + // We have a position, convert it to the setpoint and update setpoint triplet + mission_apply_limitation(next_mission_items[0u]); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + if (num_found_items >= 2u) { + /* got next mission item, update setpoint triplet */ + mission_apply_limitation(next_mission_items[1u]); + mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next); + + } else { + pos_sp_triplet->next.valid = false; + } + + } else { + handleVtolTransition(new_work_item_type, next_mission_items, num_found_items); } - pos_sp_triplet->current.cruising_speed = cruising_speed; - - publish_navigator_mission_item(); - _navigator->set_position_setpoint_triplet_updated(); -} - -void -Mission::do_abort_landing() -{ - // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT - - if (_mission_item.nav_cmd != NAV_CMD_LAND) { - return; + // Only set the previous position item if the current one really changed + if ((_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) && + !position_setpoint_equal(&pos_sp_triplet->current, ¤t_setpoint_copy)) { + pos_sp_triplet->previous = current_setpoint_copy; } - const float alt_landing = get_absolute_altitude_for_item(_mission_item); - const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), - _navigator->get_global_position()->alt); + issue_command(_mission_item); - // turn current landing waypoint into an indefinite loiter - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = alt_sp; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; + /* set current work item type */ + _work_item_type = new_work_item_type; - mission_apply_limitation(_mission_item); - mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + reset_mission_item_reached(); - // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during - // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger - // the early landing configuration (flaps and landing airspeed) during the hold. - _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; - _navigator->get_position_setpoint_triplet()->next.alt = NAN; + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } publish_navigator_mission_item(); // for logging _navigator->set_position_setpoint_triplet_updated(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", - (int)(alt_sp - alt_landing)); - events::send(events::ID("mission_holding_above_landing"), events::Log::Info, - "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); - - // reset mission index to start of landing - if (_land_start_available) { - _current_mission_index = get_land_start_index(); - - } else { - // move mission index back (landing approach point) - _current_mission_index -= 1; - } - - // send reposition cmd to get out of mission - vehicle_command_s vcmd = {}; - - vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; - vcmd.param1 = -1; - vcmd.param2 = 1; - vcmd.param5 = _mission_item.lat; - vcmd.param6 = _mission_item.lon; - vcmd.param7 = alt_sp; - - _navigator->publish_vehicle_cmd(&vcmd); } -bool -Mission::prepare_mission_items(struct mission_item_s *mission_item, - struct mission_item_s *next_position_mission_item, bool *has_next_position_item, - struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item) +void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) { - *has_next_position_item = false; - bool first_res = false; - int offset = 1; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset = -1; - } + /* do climb before going to setpoint if needed and not already executing climb */ + /* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */ + if (PX4_ISFINITE(_mission_init_climb_altitude_amsl) && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { - if (read_mission_item(0, mission_item)) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; - first_res = true; + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + next_mission_items[0u].nav_cmd = NAV_CMD_WAYPOINT; - /* trying to find next position mission item */ - while (read_mission_item(offset, next_position_mission_item)) { - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - offset--; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Climb to %.1f meters above home\t", + (double)(_mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt)); + events::send(events::ID("mission_climb_before_start"), events::Log::Info, + "Climb to {1:.1m_v} above home", _mission_init_climb_altitude_amsl - _navigator->get_home_position()->alt); - } else { - offset++; - } - - if (item_contains_position(*next_position_mission_item)) { - *has_next_position_item = true; - break; - } - } - - if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE && - after_next_position_mission_item && has_after_next_position_item) { - /* trying to find next next position mission item */ - while (read_mission_item(offset, after_next_position_mission_item)) { - offset++; - - if (item_contains_position(*after_next_position_mission_item)) { - *has_after_next_position_item = true; - break; - } - } - } - } - - return first_res; -} - -bool -Mission::read_mission_item(int offset, struct mission_item_s *mission_item) -{ - /* select mission */ - const int current_index = _current_mission_index; - int index_to_read = current_index + offset; - - int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read; - const dm_item_t dm_item = (dm_item_t)_mission.dataman_id; - - /* do not work on empty missions */ - if (_mission.count == 0) { - return false; - } - - /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after - * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */ - for (int i = 0; i < 10; i++) { - if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) { - /* mission item index out of bounds - if they are equal, we just reached the end */ - if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Mission item index out of bound, index: %d, max: %" PRIu16 ".\t", - *mission_index_ptr, _mission.count); - events::send(events::ID("mission_index_out_of_bound"), events::Log::Error, - "Mission item index out of bound, index: {1}, max: {2}", *mission_index_ptr, _mission.count); - } - - return false; - } - - const ssize_t len = sizeof(struct mission_item_s); - - /* read mission item to temp storage first to not overwrite current mission item if data damaged */ - struct mission_item_s mission_item_tmp; - - /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); - events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, - "Waypoint {1} could not be read from storage", *mission_index_ptr); - return false; - } - - /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ - if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) { - const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL; - - /* do DO_JUMP as many times as requested if not in reverse mode */ - if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) { - - /* only raise the repeat count if this is for the current mission item - * but not for the read ahead mission item */ - if (offset == 0) { - (mission_item_tmp.do_jump_current_count)++; - - /* save repeat count */ - if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) { - /* not supposed to happen unless the datamanager can't access the dataman */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); - events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, - "DO JUMP waypoint could not be written"); - return false; - } - - report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count); - } - - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index_ptr = mission_item_tmp.do_jump_mission_index; - - } else { - if (offset == 0 && execute_jumps) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.\t"); - events::send(events::ID("mission_do_jump_rep_completed"), events::Log::Info, - "DO JUMP repetitions completed"); - } - - /* no more DO_JUMPS, therefore just try to continue with next mission item */ - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - (*mission_index_ptr)--; - - } else { - (*mission_index_ptr)++; - } - } + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; } else { - /* if it's not a DO_JUMP, then we were successful */ - memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s)); - return true; + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + /* hold heading for takeoff items */ + _mission_item.yaw = _navigator->get_local_position()->heading; + _mission_item.altitude = _mission_init_climb_altitude_amsl; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + _mission_init_climb_altitude_amsl = NAN; + + } else if (_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_ROTARY_WING) { + + /* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */ + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + + } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // if the vehicle is already in fixed wing mode then the current mission item + // will be accepted immediately and the work items will be skipped + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a normal takeoff navigate to the actual waypoint now */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ + _mission_item.yaw = NAN; + } + + /* if we just did a VTOL takeoff, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + /* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + _mission_item.force_heading = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + /* set position setpoint to current while aligning */ + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + } + + /* heading is aligned now, prepare transition */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && + _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + !_land_detected_sub.get().landed) { + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + /* check if the vtol_takeoff waypoint is on top of us */ + if (do_need_move_to_takeoff()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + } + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } + + /* takeoff completed and transitioned, move to takeoff wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + } +} + +void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* move to land wp as fixed wing */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF) + && !_land_detected_sub.get().landed) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + } + + /* transition to MC */ + if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING + && !_land_detected_sub.get().landed) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + } + + /* move to landing waypoint before descent if necessary */ + if (do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + /* use current mission item as next position item */ + num_found_items = 1u; + next_mission_items[0u] = _mission_item; + + /* + * Ignoring waypoint altitude: + * Set altitude to the same as we have now to prevent descending too fast into + * the ground. Actual landing will descend anyway until it touches down. + * XXX: We might want to change that at some point if it is clear to the user + * what the altitude means on this waypoint type. + */ + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid + && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + pos_sp_triplet->previous.valid = false; + + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); } } - /* we have given up, we don't want to cycle forever */ - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.\t"); - events::send(events::ID("mission_do_jump_cycle"), events::Log::Error, "DO JUMP is cycling, giving up"); - return false; + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + + } else { //_mission_item.land_precision == 2 + _navigator->get_precland()->set_mode(PrecLandMode::Required); + } + + _navigator->get_precland()->on_activation(); + } + } + + /* ignore yaw for landing items */ + /* XXX: if specified heading for landing is desired we could add another step before the descent + * that aligns the vehicle first */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + _mission_item.yaw = NAN; + } +} + +void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* turn towards next waypoint before MC to FW transition */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_land_detected_sub.get().landed + && (num_found_items > 0u)) { + + /* disable weathervane before front transition for allowing yaw to align */ + pos_sp_triplet->current.disable_weather_vane = true; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING; + + set_align_mission_item(&_mission_item, &next_mission_items[0u]); + + /* set position setpoint to target during the transition */ + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current); + } + + /* yaw is aligned now */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING && + new_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* re-enable weather vane again after alignment */ + pos_sp_triplet->current.disable_weather_vane = false; + + pos_sp_triplet->previous = pos_sp_triplet->current; + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } } void Mission::save_mission_state() { - mission_s mission_state = {}; - - /* lock MISSION_STATE item */ - int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE); - - if (dm_lock_ret != 0) { - PX4_ERR("DM_KEY_MISSION_STATE lock failed"); + if (_vehicle_status_sub.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + // Save only while disarmed, as this is a blocking operation + _need_mission_save = true; + return; } + _need_mission_save = false; + mission_s mission_state = {}; + /* read current state */ - int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); - if (read_res == sizeof(mission_s)) { + if (success) { /* data read successfully, check dataman ID and items count */ - if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) { + if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count + && mission_state.mission_update_counter && _mission.mission_update_counter) { /* navigator may modify only sequence, write modified state only if it changed */ - if (mission_state.current_seq != _current_mission_index) { - mission_state.current_seq = _current_mission_index; - mission_state.timestamp = hrt_absolute_time(); + if (mission_state.current_seq != _mission.current_seq) { + mission_state = _mission; - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } @@ -1691,10 +624,7 @@ Mission::save_mission_state() } else { /* invalid data, this must not happen and indicates error in mission publisher */ - mission_state.timestamp = hrt_absolute_time(); - mission_state.dataman_id = _mission.dataman_id; - mission_state.count = _mission.count; - mission_state.current_seq = _current_mission_index; + mission_state = _mission; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state.\t"); /* EVENT @@ -1703,238 +633,12 @@ Mission::save_mission_state() events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state"); /* write modified state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission_state), + sizeof(mission_s)); + + if (!success) { PX4_ERR("Can't save mission state"); } } - - /* unlock MISSION_STATE item */ - if (dm_lock_ret == 0) { - dm_unlock(DM_KEY_MISSION_STATE); - } -} - -void -Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining) -{ - /* inform about the change */ - _navigator->get_mission_result()->item_do_jump_changed = true; - _navigator->get_mission_result()->item_changed_index = index; - _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; - - _navigator->set_mission_result_updated(); -} - -void -Mission::set_mission_item_reached() -{ - _navigator->get_mission_result()->seq_reached = _current_mission_index; - _navigator->set_mission_result_updated(); - reset_mission_item_reached(); -} - -void -Mission::set_current_mission_item() -{ - _navigator->get_mission_result()->finished = false; - _navigator->get_mission_result()->seq_current = _current_mission_index; - - _navigator->set_mission_result_updated(); - - save_mission_state(); -} - -void -Mission::check_mission_valid(bool force) -{ - if ((!_home_inited && _navigator->home_global_position_valid()) || force) { - - MissionFeasibilityChecker _missionFeasibilityChecker(_navigator); - - _navigator->get_mission_result()->valid = - _missionFeasibilityChecker.checkMissionFeasible(_mission); - - _navigator->get_mission_result()->seq_total = _mission.count; - _navigator->increment_mission_instance_count(); - _navigator->set_mission_result_updated(); - _home_inited = _navigator->home_global_position_valid(); - - // find and store landing start marker (if available) - find_mission_land_start(); - } -} - -void -Mission::reset_mission(struct mission_s &mission) -{ - dm_lock(DM_KEY_MISSION_STATE); - - if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { - if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) { - /* set current item to 0 */ - mission.current_seq = 0; - - /* reset jump counters */ - if (mission.count > 0) { - const dm_item_t dm_current = (dm_item_t)mission.dataman_id; - - for (unsigned index = 0; index < mission.count; index++) { - struct mission_item_s item; - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, index, &item, len) != len) { - PX4_WARN("could not read mission item during reset"); - break; - } - - if (item.nav_cmd == NAV_CMD_DO_JUMP) { - item.do_jump_current_count = 0; - - if (dm_write(dm_current, index, &item, len) != len) { - PX4_WARN("could not save mission item during reset"); - break; - } - } - } - } - - } else { - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t"); - events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission"); - - /* initialize mission state in dataman */ - mission.timestamp = hrt_absolute_time(); - mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; - mission.count = 0; - mission.current_seq = 0; - } - - dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)); - } - - dm_unlock(DM_KEY_MISSION_STATE); -} - -bool -Mission::need_to_reset_mission() -{ - /* reset mission state when disarmed */ - if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) { - _need_mission_reset = false; - return true; - } - - return false; -} - -int32_t -Mission::index_closest_mission_item() const -{ - int32_t min_dist_index(0); - float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); - - dm_item_t dm_current = (dm_item_t)(_mission.dataman_id); - - for (size_t i = 0; i < _mission.count; i++) { - struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - - if (dm_read(dm_current, i, &missionitem, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - PX4_ERR("dataman read failure"); - break; - } - - if (item_contains_position(missionitem)) { - // do not consider land waypoints for a fw - if (!((missionitem.nav_cmd == NAV_CMD_LAND) && - (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && - (!_navigator->get_vstatus()->is_vtol))) { - float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon, - get_absolute_altitude_for_item(missionitem), - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = i; - } - } - } - } - - // for mission reverse also consider the home position - if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) { - float dist = get_distance_to_point_global_wgs84( - _navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, - _navigator->get_home_position()->alt, - _navigator->get_global_position()->lat, - _navigator->get_global_position()->lon, - _navigator->get_global_position()->alt, - &dist_xy, &dist_z); - - if (dist < min_dist) { - min_dist = dist; - min_dist_index = -1; - } - } - - return min_dist_index; -} - -bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const -{ - return ((p1->valid == p2->valid) && - (p1->type == p2->type) && - (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && - (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && - (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && - (fabs(p1->lat - p2->lat) < DBL_EPSILON) && - (fabs(p1->lon - p2->lon) < DBL_EPSILON) && - (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && - ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && - (p1->yaw_valid == p2->yaw_valid) && - (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && - (p1->yawspeed_valid == p2->yawspeed_valid) && - (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && - (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && - (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && - (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && - ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) - && !PX4_ISFINITE(p2->cruising_throttle)))); - -} - -void Mission::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 = _current_mission_index; - 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/mission.h b/src/modules/navigator/mission.h index 0cdddae4fb..e8c33e1f0e 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -46,92 +46,32 @@ #pragma once -#include "mission_block.h" -#include "mission_feasibility_checker.h" -#include "navigator_mode.h" +#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "mission_base.h" +#include "navigation.h" class Navigator; -class Mission : public MissionBlock, public ModuleParams +class Mission : public MissionBase { public: Mission(Navigator *navigator); - ~Mission() override = default; + ~Mission() = default; - void on_inactive() override; - void on_inactivation() override; - void on_activation() override; - void on_active() override; + virtual void on_inactive() override; + virtual void on_activation() override; bool set_current_mission_index(uint16_t index); - bool land_start(); - bool landing(); + uint16_t get_land_start_index() const { return _mission.land_start_index; } + bool get_land_start_available() const { return hasMissionLandStart(); } - uint16_t get_land_start_index() const { return _land_start_index; } - bool get_land_start_available() const { return _land_start_available; } - bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } - bool get_mission_changed() const { return _mission_changed ; } - bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } - double get_landing_start_lat() { return _landing_start_lat; } - double get_landing_start_lon() { return _landing_start_lon; } - float get_landing_start_alt() { return _landing_start_alt; } + bool isLanding(); - double get_landing_lat() { return _landing_lat; } - double get_landing_lon() { return _landing_lon; } - float get_landing_alt() { return _landing_alt; } - float get_landing_loiter_rad() { return _landing_loiter_radius; } - - void set_closest_item_as_current(); - - /** - * Set a new mission mode and handle the switching between the different modes - * - * For a list of the different modes refer to mission_result.msg - */ - void set_execution_mode(const uint8_t mode); private: - void mission_init(); - - /** - * Update mission topic - */ - void update_mission(); - - /** - * Move on to next mission item or switch to loiter - */ - void advance_mission(); - - /** - * @brief Configures mission items in current setting - * - * Configure the mission items depending on current mission item index and settings such - * as terrain following, etc. - */ - void set_mission_items(); - - /** - * Returns true if we need to do a takeoff at the current state - */ - bool do_need_vertical_takeoff(); + bool setNextMissionItem() override; /** * Returns true if we need to move to waypoint location before starting descent @@ -143,152 +83,24 @@ private: */ bool do_need_move_to_takeoff(); - /** - * Copies position from setpoint if valid, otherwise copies current position - */ - void copy_position_if_valid(struct mission_item_s *mission_item, struct position_setpoint_s *setpoint); - - /** - * Create mission item to align towards next waypoint - */ - void set_align_mission_item(struct mission_item_s *mission_item, struct mission_item_s *mission_item_next); - /** * Calculate takeoff height for mission item considering ground clearance */ float calculate_takeoff_altitude(struct mission_item_s *mission_item); - /** - * Updates the heading of the vehicle. Rotary wings only. - */ - void heading_sp_update(); - - /** - * Update the cruising speed setpoint. - */ - void cruising_speed_sp_update(); - - /** - * Abort landing - */ - void do_abort_landing(); - - /** - * Read the current and the next mission item. The next mission item read is the - * next mission item that contains a position. - * - * @return true if current mission item available - */ - bool prepare_mission_items(mission_item_s *mission_item, - mission_item_s *next_position_mission_item, bool *has_next_position_item, - mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr); - - /** - * Read current (offset == 0) or a specific (offset > 0) mission item - * from the dataman and watch out for DO_JUMPS - * - * @return true if successful - */ - bool read_mission_item(int offset, struct mission_item_s *mission_item); - /** * Save current mission state to dataman */ void save_mission_state(); - /** - * Inform about a changed mission item after a DO_JUMP - */ - void report_do_jump_mission_changed(int index, int do_jumps_remaining); + void setActiveMissionItems() override; - /** - * Set a mission item as reached - */ - void set_mission_item_reached(); + void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Set the current mission item - */ - void set_current_mission_item(); + void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items); - /** - * Check whether a mission is ready to go - */ - void check_mission_valid(bool force); + void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], + size_t &num_found_items); - /** - * Reset mission - */ - void reset_mission(struct mission_s &mission); - - /** - * Returns true if we need to reset the mission (call this only when inactive) - */ - bool need_to_reset_mission(); - - /** - * Find and store the index of the landing sequence (DO_LAND_START) - */ - bool find_mission_land_start(); - - /** - * Return the index of the closest mission item to the current global position. - */ - int32_t index_closest_mission_item() const; - - bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; - - void publish_navigator_mission_item(); - - DEFINE_PARAMETERS( - (ParamFloat) _param_mis_dist_1wp, - (ParamInt) _param_mis_mnt_yaw_ctl - ) - - uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; - - uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription */ - mission_s _mission {}; - - int32_t _current_mission_index{-1}; - - // track location of planned mission landing - bool _land_start_available{false}; - uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ - double _landing_start_lat{0.0}; - double _landing_start_lon{0.0}; - float _landing_start_alt{0.0f}; - - double _landing_lat{0.0}; - double _landing_lon{0.0}; - float _landing_alt{0.0f}; - - float _landing_loiter_radius{0.f}; - - bool _need_takeoff{true}; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_MISSION - } _mission_type{MISSION_TYPE_NONE}; - - bool _inited{false}; - bool _home_inited{false}; - bool _need_mission_reset{false}; - bool _mission_waypoints_changed{false}; - bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */ - - // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message - enum work_item_type { - WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ - WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ - WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ - WORK_ITEM_TYPE_ALIGN, /**< align for next waypoint */ - WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, - WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, - WORK_ITEM_TYPE_PRECISION_LAND - } _work_item_type{WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ - - uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL}; /**< the current mode of how the mission is executed,look at mission_result.msg for the definition */ - bool _execution_mode_changed{false}; + bool _need_mission_save{false}; }; diff --git a/src/modules/navigator/mission_base.cpp b/src/modules/navigator/mission_base.cpp new file mode 100644 index 0000000000..5e9a77802e --- /dev/null +++ b/src/modules/navigator/mission_base.cpp @@ -0,0 +1,1239 @@ +/**************************************************************************** + * + * 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 mission_base.cpp + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#include + +#include "mission_base.h" + +#include "px4_platform_common/defines.h" + +#include "mission_feasibility_checker.h" +#include "navigator.h" + +MissionBase::MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed) : + MissionBlock(navigator), + ModuleParams(navigator), + _dataman_cache_size_signed(dataman_cache_size_signed) +{ + _dataman_cache.resize(abs(dataman_cache_size_signed)); + _is_current_planned_mission_item_valid = (initMission() == PX4_OK); + + updateDatamanCache(); + + _mission_pub.advertise(); +} + +int MissionBase::initMission() +{ + mission_s mission; + int ret_val{PX4_ERROR}; + + bool success = _dataman_client.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), + sizeof(mission_s)); + + if (success) { + if (isMissionValid(mission)) { + _mission = mission; + ret_val = PX4_OK; + + } else { + resetMission(); + } + + } else { + PX4_ERR("Could not initialize Mission: Dataman read failed"); + resetMission(); + } + + return ret_val; +} + +void +MissionBase::updateDatamanCache() +{ + if ((_mission.count > 0) && (_mission.current_seq != _load_mission_index)) { + + int32_t start_index = _mission.current_seq; + int32_t end_index = start_index + _dataman_cache_size_signed; + + end_index = math::max(math::min(end_index, static_cast(_mission.count)), INT32_C(0)); + + for (int32_t index = start_index; index != end_index; index += math::signNoZero(_dataman_cache_size_signed)) { + + _dataman_cache.load(static_cast(_mission.dataman_id), index); + } + + _load_mission_index = _mission.current_seq; + } + + _dataman_cache.update(); +} + +void MissionBase::updateMavlinkMission() +{ + if (_mission_sub.updated()) { + mission_s new_mission; + _mission_sub.update(&new_mission); + + if (isMissionValid(new_mission)) { + /* Relevant mission items updated externally*/ + if (checkMissionDataChanged(new_mission)) { + bool mission_items_changed = (new_mission.mission_update_counter != _mission.mission_update_counter); + + if (new_mission.current_seq < 0) { + new_mission.current_seq = math::max(math::min(_mission.current_seq, static_cast(new_mission.count) - 1), + INT32_C(0)); + } + + _mission = new_mission; + + onMissionUpdate(mission_items_changed); + } + } + } +} + +void MissionBase::onMissionUpdate(bool has_mission_items_changed) +{ + _is_current_planned_mission_item_valid = _mission.count > 0; + + if (has_mission_items_changed) { + _dataman_cache.invalidate(); + _load_mission_index = -1; + + check_mission_valid(); + + // only warn if the check failed on merit + if ((!_navigator->get_mission_result()->valid) && _mission.count > 0U) { + PX4_WARN("mission check failed"); + resetMission(); + } + } + + if (isActive()) { + _mission_has_been_activated = true; + _navigator->reset_triplets(); + update_mission(); + set_mission_items(); + + } else { + if (has_mission_items_changed) { + _mission_has_been_activated = false; + } + } + + // reset as when we update mission we don't want to proceed at previous index + _inactivation_index = -1; +} + +void +MissionBase::on_inactive() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + + /* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */ + if (_navigator->home_global_position_valid() && !_initialized_mission_checked) { + check_mission_valid(); + _initialized_mission_checked = true; + } + + if (_vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + _system_disarmed_while_inactive = true; + } +} + +void +MissionBase::on_inactivation() +{ + _navigator->disable_camera_trigger(); + + _navigator->stop_capturing_images(); + _navigator->release_gimbal_control(); + + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } + + /* reset so current mission item gets restarted if mission was paused */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset so MISSION_ITEM_REACHED isn't published */ + _navigator->get_mission_result()->seq_reached = -1; + + _mission_type = MissionType::MISSION_TYPE_NONE; + + _inactivation_index = _mission.current_seq; +} + +void +MissionBase::on_activation() +{ + /* reset the current mission to the start sequence if needed.*/ + checkMissionRestart(); + + _mission_has_been_activated = true; + _system_disarmed_while_inactive = false; + + check_mission_valid(); + + update_mission(); + + // reset the cache and fill it with the items up to the previous item. The cache contains + // commands that are valid for the whole mission, not just a single waypoint. + if (_mission.current_seq > 0) { + resetItemCache(); + updateCachedItemsUpToIndex(_mission.current_seq - 1); + } + + int32_t resume_index = _inactivation_index > 0 ? _inactivation_index : 0; + + if (_inactivation_index > 0 && cameraWasTriggering()) { + size_t num_found_items{0U}; + getPreviousPositionItems(_inactivation_index - 1, &resume_index, num_found_items, 1U); + + if (num_found_items == 1U) { + // The mission we are resuming had camera triggering enabled. In order to not lose any images + // we restart the mission at the previous position item. + // We will replay the cached commands once we reach the previous position item and have yaw aligned. + setMissionIndex(resume_index); + + _align_heading_necessary = true; + } + } + + checkClimbRequired(resume_index); + set_mission_items(); + + _inactivation_index = -1; // reset + + // reset cruise speed + _navigator->reset_cruising_speed(); +} + +void +MissionBase::on_active() +{ + _land_detected_sub.update(); + _vehicle_status_sub.update(); + _global_pos_sub.update(); + + parameters_update(); + + updateMavlinkMission(); + updateDatamanCache(); + + // check if heading alignment is necessary, and add it to the current mission item if necessary + if (_align_heading_necessary && is_mission_item_reached_or_completed()) { + + // add yaw alignment requirement on the current mission item + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items == 1U && !PX4_ISFINITE(_mission_item.yaw)) { + mission_item_s next_position_mission_item; + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_position_mission_item), sizeof(next_position_mission_item), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + _mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon, + next_position_mission_item.lat, next_position_mission_item.lon)); + _mission_item.force_heading = true; // note: doesn't have effect in fixed-wing mode + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + reset_mission_item_reached(); + + _navigator->set_position_setpoint_triplet_updated(); + _align_heading_necessary = false; + } + + // replay gimbal and camera commands immediately after resuming mission + if (haveCachedGimbalOrCameraItems()) { + replayCachedGimbalCameraItems(); + } + + // replay trigger commands upon raching the resume waypoint if the trigger relay flag is set + if (cameraWasTriggering() && is_mission_item_reached_or_completed()) { + replayCachedTriggerItems(); + } + + replayCachedSpeedChangeItems(); + + /* lets check if we reached the current mission item */ + if (_mission_type != MissionType::MISSION_TYPE_NONE && is_mission_item_reached_or_completed()) { + /* If we just completed a takeoff which was inserted before the right waypoint, + there is no need to report that we reached it because we didn't. */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_CLIMB) { + set_mission_item_reached(); + } + + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + } + } + + /* see if we need to update the current yaw heading */ + if (!_param_mis_mnt_yaw_ctl.get() + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) + && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE) + && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF + || _mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + || _mission_item.nav_cmd == NAV_CMD_LAND + || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND + || _work_item_type == WorkItemType::WORK_ITEM_TYPE_ALIGN_HEADING)) { + // Mount control is disabled If the vehicle is in ROI-mode, the vehicle + // needs to rotate such that ROI is in the field of view. + // ROI only makes sense for multicopters. + heading_sp_update(); + } + + // TODO: Add vtol heading update method if required. + // Question: Why does vtol ever have to update heading? + + /* check if landing needs to be aborted */ + if ((_mission_item.nav_cmd == NAV_CMD_LAND) + && (_navigator->abort_landing())) { + + do_abort_landing(); + } + + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND) { + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void MissionBase::update_mission() +{ + if (_mission.count == 0u || !_is_current_planned_mission_item_valid || !_navigator->get_mission_result()->valid) { + 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"); + events::send(events::ID("mission_not_valid_refuse"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, refusing takeoff"); + + } else { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, loitering\t"); + events::send(events::ID("mission_not_valid_loiter"), {events::Log::Error, events::LogInternal::Disabled}, + "No valid mission available, loitering"); + } + + _mission_type = MissionType::MISSION_TYPE_NONE; + + } else { + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing Mission\t"); + events::send(events::ID("mission_execute"), events::Log::Info, "Executing Mission"); + } + + _mission_type = MissionType::MISSION_TYPE_MISSION; + } + + /* Reset vehicle_roi + * Missions that do not explicitly configure ROI would not override + * an existing ROI setting from previous missions */ + _navigator->reset_vroi(); + + if (_navigator->get_mission_result()->valid) { + /* reset work item if new mission has been accepted */ + _work_item_type = WorkItemType::WORK_ITEM_TYPE_DEFAULT; + + /* reset mission failure if we have an updated valid mission */ + _navigator->get_mission_result()->failure = false; + + /* reset sequence info as well */ + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->seq_total = _mission.count; + } + + // we start from the first item so can reset the cache + if (_mission.current_seq == 0) { + resetItemCache(); + } + + set_mission_result(); +} + +void +MissionBase::advance_mission() +{ + /* do not advance mission item if we're processing sub mission work items */ + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + return; + } + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + _is_current_planned_mission_item_valid = setNextMissionItem(); + + if (!_is_current_planned_mission_item_valid) { + // Mission ended + if (_land_detected_sub.get().landed) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, landed\t"); + + events::send(events::ID("mission_finished"), events::Log::Info, "Mission finished, landed"); + + } else { + /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Mission finished, loitering\t"); + + events::send(events::ID("mission_finished_loiter"), events::Log::Info, "Mission finished, loitering"); + } + + // Reset jump counter if the mission was completed + if ((_mission.current_seq + 1) == _mission.count) { + resetMissionJumpCounter(); + } + } + } +} + +void +MissionBase::set_mission_items() +{ + if (_is_current_planned_mission_item_valid) { + /* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */ + loadCurrentMissionItem(); + + setActiveMissionItems(); + + } else { + setEndOfMissionItems(); + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION); + } +} + +void MissionBase::loadCurrentMissionItem() +{ + const dm_item_t dm_item = static_cast(_mission.dataman_id); + bool success = _dataman_cache.loadWait(dm_item, _mission.current_seq, reinterpret_cast(&_mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item could not be set.\t"); + events::send(events::ID("mission_item_set_failed"), events::Log::Error, + "Mission item could not be set"); + } +} + +void MissionBase::setEndOfMissionItems() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + if (_land_detected_sub.get().landed) { + _mission_item.nav_cmd = NAV_CMD_IDLE; + + } else { + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) { + setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + + } else { + setLoiterItemFromCurrentPosition(&_mission_item); + } + } + + /* update position setpoint triplet */ + pos_sp_triplet->previous.valid = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + // set mission finished + _navigator->get_mission_result()->finished = true; + _navigator->set_mission_result_updated(); + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + _mission_type = MissionType::MISSION_TYPE_NONE; +} + +void +MissionBase::set_mission_item_reached() +{ + _navigator->get_mission_result()->seq_reached = _mission.current_seq; + _navigator->set_mission_result_updated(); + + reset_mission_item_reached(); +} + +void +MissionBase::set_mission_result() +{ + _navigator->get_mission_result()->finished = false; + _navigator->get_mission_result()->seq_current = _mission.current_seq > 0 ? _mission.current_seq : 0; + + _navigator->set_mission_result_updated(); +} + +bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const +{ + return ((p1->valid == p2->valid) && + (p1->type == p2->type) && + (fabsf(p1->vx - p2->vx) < FLT_EPSILON) && + (fabsf(p1->vy - p2->vy) < FLT_EPSILON) && + (fabsf(p1->vz - p2->vz) < FLT_EPSILON) && + (fabs(p1->lat - p2->lat) < DBL_EPSILON) && + (fabs(p1->lon - p2->lon) < DBL_EPSILON) && + (fabsf(p1->alt - p2->alt) < FLT_EPSILON) && + ((fabsf(p1->yaw - p2->yaw) < FLT_EPSILON) || (!PX4_ISFINITE(p1->yaw) && !PX4_ISFINITE(p2->yaw))) && + (p1->yaw_valid == p2->yaw_valid) && + (fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) && + (p1->yawspeed_valid == p2->yawspeed_valid) && + (fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) && + (p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) && + (fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) && + (fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) && + ((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle) + && !PX4_ISFINITE(p2->cruising_throttle)))); + +} + +void +MissionBase::report_do_jump_mission_changed(int index, int do_jumps_remaining) +{ + /* inform about the change */ + _navigator->get_mission_result()->item_do_jump_changed = true; + _navigator->get_mission_result()->item_changed_index = index; + _navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining; + + _navigator->set_mission_result_updated(); +} + +void +MissionBase::checkMissionRestart() +{ + if (_system_disarmed_while_inactive && _mission_has_been_activated && (_mission.count > 0U) + && ((_mission.current_seq + 1) == _mission.count)) { + setMissionIndex(0); + _is_current_planned_mission_item_valid = isMissionValid(_mission); + resetMissionJumpCounter(); + _navigator->reset_cruising_speed(); + _navigator->reset_vroi(); + set_mission_result(); + } +} + +void +MissionBase::check_mission_valid() +{ + if (_navigator->get_mission_result()->instance_count != _mission.mission_update_counter) { + MissionFeasibilityChecker missionFeasibilityChecker(_navigator, _dataman_client); + + bool is_mission_valid = + missionFeasibilityChecker.checkMissionFeasible(_mission); + + _navigator->get_mission_result()->valid = is_mission_valid; + _navigator->get_mission_result()->instance_count = _mission.mission_update_counter; + _navigator->get_mission_result()->seq_total = _mission.count; + _navigator->get_mission_result()->seq_reached = -1; + _navigator->get_mission_result()->failure = false; + set_mission_result(); + } +} + +void +MissionBase::heading_sp_update() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = + _navigator->get_position_setpoint_triplet(); + + // Only update if current triplet is valid + if (pos_sp_triplet->current.valid) { + + double point_from_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + double point_to_latlon[2] = { _global_pos_sub.get().lat, + _global_pos_sub.get().lon + }; + float yaw_offset = 0.0f; + + // Depending on ROI-mode, update heading + switch (_navigator->get_vroi().mode) { + case vehicle_roi_s::ROI_LOCATION: { + // ROI is a fixed location. Vehicle needs to point towards that location + point_to_latlon[0] = _navigator->get_vroi().lat; + point_to_latlon[1] = _navigator->get_vroi().lon; + // No yaw offset required + yaw_offset = 0.0f; + break; + } + + case vehicle_roi_s::ROI_WPNEXT: { + // ROI is current waypoint. Vehcile needs to point towards current waypoint + point_to_latlon[0] = pos_sp_triplet->current.lat; + point_to_latlon[1] = pos_sp_triplet->current.lon; + // Add the gimbal's yaw offset + yaw_offset = _navigator->get_vroi().yaw_offset; + break; + } + + case vehicle_roi_s::ROI_NONE: + case vehicle_roi_s::ROI_WPINDEX: + case vehicle_roi_s::ROI_TARGET: + case vehicle_roi_s::ROI_ENUM_END: + default: { + return; + } + } + + // Get desired heading and update it. + // However, only update if distance to desired heading is + // larger than acceptance radius to prevent excessive yawing + float d_current = get_distance_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]); + + if (d_current > _navigator->get_acceptance_radius()) { + float yaw = matrix::wrap_pi( + get_bearing_to_next_waypoint(point_from_latlon[0], + point_from_latlon[1], point_to_latlon[0], + point_to_latlon[1]) + yaw_offset); + + _mission_item.yaw = yaw; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + + } else { + if (!pos_sp_triplet->current.yaw_valid) { + _mission_item.yaw = _navigator->get_local_position()->heading; + pos_sp_triplet->current.yaw = _mission_item.yaw; + pos_sp_triplet->current.yaw_valid = true; + } + } + + // we set yaw directly so we can run this in parallel to the FOH update + publish_navigator_mission_item(); + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void +MissionBase::do_abort_landing() +{ + // Abort FW landing, loiter above landing site in at least MIS_LND_ABRT_ALT + if (_mission_type == MissionType::MISSION_TYPE_NONE) { + return; + } + + if (_mission_item.nav_cmd != NAV_CMD_LAND) { + return; + } + + const float alt_landing = get_absolute_altitude_for_item(_mission_item); + const float alt_sp = math::max(alt_landing + _navigator->get_landing_abort_min_alt(), + _global_pos_sub.get().alt); + + // turn current landing waypoint into an indefinite loiter + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = alt_sp; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current); + + // XXX: this is a hack to invalidate the "next" position setpoint for the fixed-wing position controller during + // the landing abort hold. otherwise, the "next" setpoint would still register as a "LAND" point, and trigger + // the early landing configuration (flaps and landing airspeed) during the hold. + _navigator->get_position_setpoint_triplet()->next.lat = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.lon = (double)NAN; + _navigator->get_position_setpoint_triplet()->next.alt = NAN; + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing waypoint.\t", + (int)(alt_sp - alt_landing)); + events::send(events::ID("mission_holding_above_landing"), events::Log::Info, + "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing); + + // reset mission index to start of landing + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = true; + setMissionIndex(_mission.land_start_index); + + } else { + // move mission index back (landing approach point) + _is_current_planned_mission_item_valid = goToPreviousItem(false); + } + + // send reposition cmd to get out of mission + vehicle_command_s vcmd = {}; + + vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION; + vcmd.param1 = -1; + vcmd.param2 = 1; + vcmd.param5 = _mission_item.lat; + vcmd.param6 = _mission_item.lon; + vcmd.param7 = alt_sp; + + _navigator->publish_vehicle_cmd(&vcmd); +} + +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; + 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); +} + +bool MissionBase::isMissionValid(mission_s &mission) const +{ + bool ret_val{false}; + + if (((mission.current_seq < mission.count) || (mission.count == 0U && mission.current_seq <= 0)) && + (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) && + (mission.timestamp != 0u)) { + ret_val = true; + + } + + return ret_val; +} + +int MissionBase::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, + bool write_jumps, bool mission_direction_backward) +{ + if (mission_index >= _mission.count || mission_index < 0) { + return PX4_ERROR; + } + + const dm_item_t dataman_id = (dm_item_t)_mission.dataman_id; + int32_t new_mission_index{mission_index}; + mission_item_s new_mission; + + for (uint16_t jump_count = 0u; jump_count < MAX_JUMP_ITERATION; jump_count++) { + /* read mission item from datamanager */ + bool success = _dataman_cache.loadWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t"); + events::send(events::ID("mission_failed_to_read_wp"), events::Log::Error, + "Waypoint {1} could not be read from storage", new_mission_index); + return PX4_ERROR; + } + + if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) { + if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) { + PX4_ERR("Do Jump mission index is out of bounds."); + return PX4_ERROR; + } + + if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) { + if (write_jumps) { + new_mission.do_jump_current_count++; + success = _dataman_cache.writeWait(dataman_id, new_mission_index, reinterpret_cast(&new_mission), + sizeof(struct mission_item_s)); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t"); + events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error, + "DO JUMP waypoint could not be written"); + // Still continue searching for next non jump item. + } + + report_do_jump_mission_changed(new_mission_index, new_mission.do_jump_repeat_count - new_mission.do_jump_current_count); + } + + new_mission_index = new_mission.do_jump_mission_index; + + } else { + if (mission_direction_backward) { + new_mission_index--; + + } else { + new_mission_index++; + } + } + + } else { + break; + } + } + + mission_index = new_mission_index; + mission = new_mission; + + return PX4_OK; +} + +int MissionBase::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward) +{ + mission_item_s mission_item; + + if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == PX4_OK) { + setMissionIndex(index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +void MissionBase::setMissionIndex(int32_t index) +{ + if (index != _mission.current_seq) { + _mission.current_seq = index; + _mission.timestamp = hrt_absolute_time(); + _mission_pub.publish(_mission); + } +} + +void MissionBase::getPreviousPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index < 0) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + next_mission_index--; + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == PX4_OK; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = next_mission_index; + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +void MissionBase::getNextPositionItems(int32_t start_index, int32_t items_index[], + size_t &num_found_items, uint8_t max_num_items) +{ + // Make sure vector does not contain any preexisting elements. + num_found_items = 0u; + + int32_t next_mission_index{start_index}; + + for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) { + if (next_mission_index >= _mission.count) { + break; + } + + mission_item_s next_mission_item; + bool found_next_item{false}; + + do { + found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == PX4_OK; + next_mission_index++; + } while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item); + + if (found_next_item) { + items_index[item_idx] = math::max(next_mission_index - 1, + static_cast(0)); // subtract 1 to get the index of the first position item + num_found_items = item_idx + 1; + + } else { + break; + } + } +} + +int MissionBase::goToNextItem(bool execute_jump) +{ + if (_mission.current_seq + 1 >= (_mission.count)) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq + 1, execute_jump); +} + +int MissionBase::goToPreviousItem(bool execute_jump) +{ + if (_mission.current_seq <= 0) { + return PX4_ERROR; + } + + return goToItem(_mission.current_seq - 1, execute_jump, true); +} + +int MissionBase::goToPreviousPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t previous_position_item_index; + getPreviousPositionItems(_mission.current_seq, &previous_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(previous_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::goToNextPositionItem(bool execute_jump) +{ + size_t num_found_items{0U}; + int32_t next_position_item_index; + getNextPositionItems(_mission.current_seq + 1, &next_position_item_index, num_found_items, 1); + + if (num_found_items == 1U) { + setMissionIndex(next_position_item_index); + return PX4_OK; + + } else { + return PX4_ERROR; + } +} + +int MissionBase::setMissionToClosestItem(double lat, double lon, float alt, float home_alt, + const vehicle_status_s &vehicle_status) +{ + int32_t min_dist_index(-1); + float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX); + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) { + mission_item_s mission; + + bool success = _dataman_cache.loadWait(dataman_id, mission_item_index, reinterpret_cast(&mission), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not set mission closest to position.\t"); + events::send(events::ID("mission_failed_set_closest"), events::Log::Error, + "Could not set mission closest to position"); + return PX4_ERROR; + } + + if (MissionBlock::item_contains_position(mission)) { + // do not consider land waypoints for a fw + if (!((mission.nav_cmd == NAV_CMD_LAND) && + (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + (!vehicle_status.is_vtol))) { + float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon, + MissionBlock::get_absolute_altitude_for_item(mission, home_alt), + lat, + lon, + alt, + &dist_xy, &dist_z); + + if (dist < min_dist) { + min_dist = dist; + min_dist_index = mission_item_index; + } + } + } + } + + setMissionIndex(min_dist_index); + + return PX4_OK; +} + +void MissionBase::resetMission() +{ + /* we do not need to reset mission if is already.*/ + if (_mission.count == 0u && isMissionValid(_mission)) { + return; + } + + /* Set a new mission*/ + mission_s new_mission{_mission}; + new_mission.timestamp = hrt_absolute_time(); + new_mission.current_seq = 0; + new_mission.land_start_index = -1; + new_mission.land_index = -1; + new_mission.count = 0u; + new_mission.mission_update_counter = _mission.mission_update_counter + 1; + new_mission.dataman_id = _mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 : + DM_KEY_WAYPOINTS_OFFBOARD_0; + + bool success = _dataman_client.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&new_mission), + sizeof(mission_s)); + + if (success) { + _mission = new_mission; + _mission_pub.publish(_mission); + + } else { + PX4_ERR("Mission Initialization failed."); + } +} + +void MissionBase::resetMissionJumpCounter() +{ + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + + for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) { + mission_item_s mission_item; + + bool success = _dataman_client.readSync(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission could not reset jump count.\t"); + events::send(events::ID("mission_failed_set_jump_count"), events::Log::Error, + "Mission could not reset jump count"); + break; + } + + if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) { + mission_item.do_jump_current_count = 0u; + + bool write_success = _dataman_cache.writeWait(dataman_id, mission_index, reinterpret_cast(&mission_item), + sizeof(struct mission_item_s)); + + if (!write_success) { + PX4_ERR("Could not write mission item for jump count reset."); + break; + } + } + } +} + +void MissionBase::cacheItem(const mission_item_s &mission_item) +{ + switch (mission_item.nav_cmd) { + case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: + _last_gimbal_configure_item = mission_item; + break; + + case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW: + _last_gimbal_control_item = mission_item; + break; + + case NAV_CMD_SET_CAMERA_MODE: + _last_camera_mode_item = mission_item; + break; + + case NAV_CMD_DO_SET_CAM_TRIGG_DIST: + case NAV_CMD_DO_TRIGGER_CONTROL: + case NAV_CMD_IMAGE_START_CAPTURE: + case NAV_CMD_IMAGE_STOP_CAPTURE: + _last_camera_trigger_item = mission_item; + break; + + case NAV_CMD_DO_CHANGE_SPEED: + _last_speed_change_item = mission_item; + break; + + case NAV_CMD_DO_VTOL_TRANSITION: + // delete speed changes after a VTOL transition + _last_speed_change_item = {}; + break; + + default: + break; + } +} + +void MissionBase::replayCachedGimbalCameraItems() +{ + if (_last_gimbal_configure_item.nav_cmd > 0) { + issue_command(_last_gimbal_configure_item); + _last_gimbal_configure_item = {}; // delete cached item + } + + if (_last_gimbal_control_item.nav_cmd > 0) { + issue_command(_last_gimbal_control_item); + _last_gimbal_control_item = {}; // delete cached item + } + + if (_last_camera_mode_item.nav_cmd > 0) { + issue_command(_last_camera_mode_item); + _last_camera_mode_item = {}; // delete cached item + } +} + +void MissionBase::replayCachedTriggerItems() +{ + if (_last_camera_trigger_item.nav_cmd > 0) { + issue_command(_last_camera_trigger_item); + _last_camera_trigger_item = {}; // delete cached item + } +} + +void MissionBase::replayCachedSpeedChangeItems() +{ + if (_last_speed_change_item.nav_cmd == NAV_CMD_DO_CHANGE_SPEED) { + issue_command(_last_speed_change_item); + _last_speed_change_item = {}; // delete cached item + } +} + +void MissionBase::resetItemCache() +{ + _last_gimbal_configure_item = {}; + _last_gimbal_control_item = {}; + _last_camera_mode_item = {}; + _last_camera_trigger_item = {}; +} + +bool MissionBase::haveCachedGimbalOrCameraItems() +{ + return _last_gimbal_configure_item.nav_cmd > 0 || + _last_gimbal_control_item.nav_cmd > 0 || + _last_camera_mode_item.nav_cmd > 0; +} + +bool MissionBase::cameraWasTriggering() +{ + return (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL + && (int)(_last_camera_trigger_item.params[0] + 0.5f) == 1) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) || + (_last_camera_trigger_item.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST + && _last_camera_trigger_item.params[0] > FLT_EPSILON); +} + +void MissionBase::updateCachedItemsUpToIndex(const int end_index) +{ + for (int i = 0; i <= end_index; i++) { + mission_item_s mission_item; + const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; + bool success = _dataman_client.readSync(dm_current, i, reinterpret_cast(&mission_item), + sizeof(mission_item), 500_ms); + + if (success) { + cacheItem(mission_item); + } + } +} + +void MissionBase::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} + +void MissionBase::checkClimbRequired(int32_t mission_item_index) +{ + int32_t next_mission_item_index; + size_t num_found_items{0U}; + getNextPositionItems(mission_item_index, &next_mission_item_index, num_found_items, 1U); + + if (num_found_items > 0U) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s mission; + _mission_init_climb_altitude_amsl = NAN; // default to NAN, overwrite below if applicable + + const bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, reinterpret_cast(&mission), + sizeof(mission), MAX_DATAMAN_LOAD_WAIT); + + const bool is_fw_and_takeoff = mission.nav_cmd == NAV_CMD_TAKEOFF + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + // for FW when on a Takeoff item do not require climb before mission, as we need to keep course to takeoff item straight + if (success && !is_fw_and_takeoff) { + const float altitude_amsl_next_position_item = MissionBlock::get_absolute_altitude_for_item(mission); + const float error_below_setpoint = altitude_amsl_next_position_item - + _navigator->get_global_position()->alt; + + if (error_below_setpoint > _navigator->get_altitude_acceptance_radius()) { + + _mission_init_climb_altitude_amsl = altitude_amsl_next_position_item; + } + } + } +} + +bool MissionBase::checkMissionDataChanged(mission_s new_mission) +{ + /* count and land_index are the same if the mission_counter did not change. We do not care about changes in geofence or rally counters.*/ + return ((new_mission.dataman_id != _mission.dataman_id) || + (new_mission.mission_update_counter != _mission.mission_update_counter) || + (new_mission.current_seq != _mission.current_seq)); +} diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h new file mode 100644 index 0000000000..54a24fefa1 --- /dev/null +++ b/src/modules/navigator/mission_base.h @@ -0,0 +1,446 @@ +/**************************************************************************** + * + * 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 mission_base.h + * + * Mission base mode class that can be used for modes interacting with a mission. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mission_block.h" +#include "navigation.h" + +using namespace time_literals; + +class Navigator; + +class MissionBase : public MissionBlock, public ModuleParams +{ +public: + MissionBase(Navigator *navigator, int32_t dataman_cache_size_signed); + ~MissionBase() override = default; + + virtual void on_inactive() override; + virtual void on_inactivation() override; + virtual void on_activation() override; + virtual void on_active() override; + +protected: + + /** + * @brief Maximum time to wait for dataman loading + * + */ + static constexpr hrt_abstime MAX_DATAMAN_LOAD_WAIT{500_ms}; + + // Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message + enum class WorkItemType { + WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ + WORK_ITEM_TYPE_CLIMB, /**< takeoff before moving to waypoint */ + WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ + WORK_ITEM_TYPE_ALIGN_HEADING, /**< align for next waypoint */ + WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF, + WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION, + WORK_ITEM_TYPE_PRECISION_LAND + } _work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; /**< current type of work to do (sub mission item) */ + + enum class MissionType { + MISSION_TYPE_NONE, + MISSION_TYPE_MISSION + } _mission_type{MissionType::MISSION_TYPE_NONE}; + + /** + * @brief Get the Previous Mission Position Items + * + * @param[in] start_index is the index from where to start searching the previous mission position items + * @param[out] items_index is an array of indexes indicating the previous mission position items found + * @param[out] num_found_items are the amount of previous position items found + * @param[in] max_num_items are the maximum amount of previous position items to be searched + */ + void getPreviousPositionItems(int32_t start_index, int32_t items_index[], size_t &num_found_items, + uint8_t max_num_items); + /** + * @brief Get the next mission item containing a position setpoint + * + * @param[in] start_index is the index from where to start searching (first possible return index) + * @param[out] items_index is an array of indexes indicating the next mission position items found + * @param[out] num_found_items are the amount of next position items found + * @param[in] max_num_items are the maximum amount of next position items to be searched + */ + 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 + * + * @return true If mission has a land start of land item + * @return false otherwise + */ + bool hasMissionLandStart() const { return _mission.land_start_index > 0;}; + /** + * @brief Go to next Mission Item + * Go to next non jump mission item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextItem(bool execute_jump); + /** + * @brief Go to previous Mission Item + * Go to previous non jump mission item + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousItem(bool execute_jump); + /** + * @brief Go to Mission Item + * + * @param[in] index Index of the mission item to go to + * @param[in] execute_jump Flag indicating if a jump should be executed of ignored + * @param[in] mission_direction_backward Flag indicating if a mission is flown backward + * @return PX4_OK if the mission item exists, PX4_ERR otherwise + */ + int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false); + /** + * @brief Go To Previous Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if previous mission item exists, PX4_ERR otherwise + */ + int goToPreviousPositionItem(bool execute_jump); + /** + * @brief Go To Next Mission Position Item + * + * @param[in] execute_jump Flag indicating if a jump should be executed or ignored + * @return PX4_OK if next mission item exists, PX4_ERR otherwise + */ + int goToNextPositionItem(bool execute_jump); + /** + * @brief Go to Mission Land Start Item + * + * @return PX4_OK if land start item exists and is loaded, PX4_ERR otherwise + */ + int goToMissionLandStart(); + /** + * @brief Set the Mission to closest mission position item from current position + * + * @param[in] lat latitude of the current position + * @param[in] lon longitude of the current position + * @param[in] alt altitude of the current position + * @param[in] home_alt altitude of the home position + * @param[in] vehicle_status vehicle status struct + * @return PX4_OK if closest item is found and loaded, PX4_ERR otherwise + */ + int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status); + /** + * @brief Initialize Mission + * + * @return PX4_OK if mission could be loaded, PX4_ERR otherwise + */ + int initMission(); + /** + * @brief Reset Mission + * + */ + void resetMission(); + /** + * @brief Reset Mission Jump Counter of Mission Jump Items + * + */ + void resetMissionJumpCounter(); + /** + * @brief Get the Non Jump Mission Item + * + * @param[out] mission_index Index of the mission item + * @param[out] mission The return mission item + * @param execute_jump Flag indicating if a jump item should be executed or ignored + * @param write_jumps Flag indicating if the jump counter should be updated + * @param mission_direction_backward Flag indicating if the mission is flown backwards + * @return PX4_OK if mission item could be loaded, PX4_ERR otherwise + */ + int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps, + bool mission_direction_backward = false); + /** + * @brief Is Mission Parameters Valid + * + * @param mission Mission struct + * @return true is mission parameters are valid + * @return false otherwise + */ + bool isMissionValid(mission_s &mission) const; + + /** + * On mission update + * Change behaviour after external mission update. + * @param[in] has_mission_items_changed flag if the mission items have been changed. + */ + void onMissionUpdate(bool has_mission_items_changed); + + /** + * Update mission topic + */ + void update_mission(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * @brief Configures mission items in current setting + * + * Configure the mission items depending on current mission item index and settings such + * as terrain following, etc. + */ + void set_mission_items(); + + /** + * @brief Load current mission item + * + * Load current mission item from dataman cache. + */ + void loadCurrentMissionItem(); + + /** + * Set the mission result + */ + void set_mission_result(); + + /** + * @brief Reset the item cache + */ + void resetItemCache(); + + /** + * @brief Set the actions to be performed on Active Mission Item + * + */ + virtual void setActiveMissionItems() = 0; + /** + * @brief Set the Next Mission Item after old mission item has been completed + * + * @return true if the next mission item could be set + * @return false otherwise + */ + virtual bool setNextMissionItem() = 0; + /** + * @brief Set action at the end of the mission + * + */ + void setEndOfMissionItems(); + /** + * @brief Publish navigator mission item + * + */ + void publish_navigator_mission_item(); + /** + * @brief I position setpoint equal + * + * @param p1 First position setpoint to compare + * @param p2 Second position setpoint to compare + * @return true if both setpoints are equal + * @return false otherwise + */ + bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const; + + 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 _initialized_mission_checked{false}; /**< Flag indicating if the initialized mission has been checked by the mission validator*/ + bool _system_disarmed_while_inactive{false}; /**< Flag indicating if the system has been disarmed while mission is inactive*/ + mission_s _mission; /**< Currently active mission*/ + float _mission_init_climb_altitude_amsl{NAN}; /**< altitude AMSL the vehicle will climb to when mission starts */ + + DatamanCache _dataman_cache{"mission_dm_cache_miss", 10}; /**< Dataman cache of mission items*/ + DatamanClient &_dataman_client = _dataman_cache.client(); /**< Dataman client*/ + + uORB::Subscription _mission_sub{ORB_ID(mission)}; /**< mission subscription*/ + 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 _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::Publication _navigator_mission_item_pub{ORB_ID::navigator_mission_item}; /**< Navigator mission item publication*/ + uORB::Publication _mission_pub{ORB_ID(mission)}; /**< Mission publication*/ +private: + /** + * @brief Maximum number of jump mission items iterations + * + */ + static constexpr uint16_t MAX_JUMP_ITERATION{10u}; + /** + * @brief Update Dataman cache + * + */ + void updateDatamanCache(); + /** + * @brief Update mission subscription + * + */ + void updateMavlinkMission(); + + /** + * Check whether a mission is ready to go + */ + void check_mission_valid(); + + /** + * Reset mission + */ + void checkMissionRestart(); + + /** + * Set a mission item as reached + */ + void set_mission_item_reached(); + + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + + /** + * Abort landing + */ + void do_abort_landing(); + + /** + * Inform about a changed mission item after a DO_JUMP + */ + void report_do_jump_mission_changed(int index, int do_jumps_remaining); + + /** + * @brief Cache the mission items containing gimbal, camera mode and trigger commands + * + * @param mission_item The mission item to cache if applicable + */ + void cacheItem(const mission_item_s &mission_item); + + /** + * @brief Update the cached items up to the given index + * + * @param end_index The index to update up to + */ + void updateCachedItemsUpToIndex(int end_index); + + /** + * @brief Replay the cached gimbal and camera mode items + */ + void replayCachedGimbalCameraItems(); + + /** + * @brief Replay the cached trigger items + * + */ + void replayCachedTriggerItems(); + + /** + * @brief Replay the cached speed change items and delete them afterwards + * + */ + void replayCachedSpeedChangeItems(); + + /** + * @brief Check if there are cached gimbal or camera mode items to be replayed + * + * @return true if there are cached items + */ + bool haveCachedGimbalOrCameraItems(); + + /** + * @brief Check if the camera was triggering + * + * @return true if there was a camera trigger command in the cached items that didn't disable triggering + */ + bool cameraWasTriggering(); + + /** + * @brief Set the Mission Index + * + * @param[in] index Index of the mission item + */ + void setMissionIndex(int32_t index); + + /** + * @brief Parameters update + * + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** + * @brief Check if a climb is necessary to align with mission altitude prior to starting the mission + * + * @param mission_item_index The index of the mission item to check if a climb is necessary + */ + void checkClimbRequired(int32_t mission_item_index); + + /** + * @brief check if relevant data in the new mission have changed. + * @param[in] new_mission new mission received over uorb + * @return true if the relevant mission data has changed, false otherwise + */ + bool checkMissionDataChanged(mission_s new_mission); + + 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*/ + + int _inactivation_index{-1}; // index of mission item at which the mission was paused. Used to resume survey missions at previous waypoint to not lose images. + bool _align_heading_necessary{false}; // if true, heading of vehicle needs to be aligned with heading of next waypoint. Used to create new mission items for heading alignment. + + mission_item_s _last_gimbal_configure_item {}; + mission_item_s _last_gimbal_control_item {}; + mission_item_s _last_camera_mode_item {}; + mission_item_s _last_camera_trigger_item {}; + mission_item_s _last_speed_change_item {}; + + DEFINE_PARAMETERS( + (ParamFloat) _param_mis_dist_1wp, + (ParamInt) _param_mis_mnt_yaw_ctl + ) + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 565b2f2150..71fcb756a5 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -901,15 +901,54 @@ MissionBlock::mission_apply_limitation(mission_item_s &item) float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const +{ + return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt); +} + +float +MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt) { if (mission_item.altitude_is_relative) { - return mission_item.altitude + _navigator->get_home_position()->alt; + return mission_item.altitude + home_alt; } else { return mission_item.altitude; } } +void +MissionBlock::copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const +{ + if (setpoint->valid && setpoint->type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + mission_item->lat = setpoint->lat; + mission_item->lon = setpoint->lon; + mission_item->altitude = setpoint->alt; + + } else { + mission_item->lat = _navigator->get_global_position()->lat; + mission_item->lon = _navigator->get_global_position()->lon; + mission_item->altitude = _navigator->get_global_position()->alt; + } + + mission_item->altitude_is_relative = false; +} + +void +MissionBlock::set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const +{ + mission_item->nav_cmd = NAV_CMD_WAYPOINT; + copy_position_if_valid(mission_item, &(_navigator->get_position_setpoint_triplet()->current)); + mission_item->altitude_is_relative = false; + mission_item->autocontinue = true; + mission_item->time_inside = 0.0f; + mission_item->yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + mission_item_next->lat, mission_item_next->lon); + mission_item->force_heading = true; +} + void MissionBlock::initialize() { diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index 450b57ba7b..1a669512af 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,6 +91,15 @@ public: */ static bool item_contains_gate(const mission_item_s &item); + /** + * Get the absolute altitude for mission item + * + * @param mission_item the mission item of interest + * @param home_alt the home altitude in [m AMSL]. + * @return Mission item altitude in [m AMSL] + */ + static float get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt); + /** * Check if the mission item contains a marker * @@ -124,6 +133,18 @@ public: _payload_deploy_timeout_s = timeout_s; } + /** + * Copies position from setpoint if valid, otherwise copies current position + */ + void copy_position_if_valid(struct mission_item_s *const mission_item, + const struct position_setpoint_s *const setpoint) const; + + /** + * Create mission item to align towards next waypoint + */ + void set_align_mission_item(struct mission_item_s *const mission_item, + const struct mission_item_s *const mission_item_next) const; + protected: /** * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index ba21850c6c..afe8f3ba00 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -78,9 +78,11 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission) for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(struct mission_item_s); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { _navigator->get_mission_result()->warning = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; @@ -116,9 +118,11 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al if (_navigator->get_geofence().valid()) { for (size_t i = 0; i < mission.count; i++) { struct mission_item_s missionitem = {}; - const ssize_t len = sizeof(missionitem); - if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) { + bool success = _dataman_client.readSync((dm_item_t)mission.dataman_id, i, reinterpret_cast(&missionitem), + sizeof(mission_item_s)); + + if (!success) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return false; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 902d803eda..0eedaf7366 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -42,7 +42,7 @@ #pragma once -#include +#include #include #include #include "MissionFeasibility/FeasibilityChecker.hpp" @@ -54,14 +54,16 @@ class MissionFeasibilityChecker: public ModuleParams { private: Navigator *_navigator{nullptr}; + DatamanClient &_dataman_client; FeasibilityChecker _feasibility_checker; bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid); public: - MissionFeasibilityChecker(Navigator *navigator) : + MissionFeasibilityChecker(Navigator *navigator, DatamanClient &dataman_client) : ModuleParams(nullptr), _navigator(navigator), + _dataman_client(dataman_client), _feasibility_checker() { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 9b411355bc..edfbac54bd 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -44,13 +44,13 @@ */ /** - * Take-off altitude + * Default take-off altitude * - * This is the minimum altitude the system will take off to. + * This is the relative altitude the system will take off to + * if not otherwise specified. * * @unit m * @min 0 - * @max 80 * @decimal 1 * @increment 0.5 * @group Mission diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b62bffe334..1ddb1e8891 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -49,7 +49,9 @@ #include "navigator_mode.h" #include "rtl.h" #include "takeoff.h" +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF #include "vtol_takeoff.h" +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF #include "navigation.h" @@ -77,6 +79,7 @@ #include #include #include +#include #include #include #include @@ -142,7 +145,6 @@ public: /** * Setters */ - void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } void set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; } void set_mission_result_updated() { _mission_result_updated = true; } @@ -171,8 +173,6 @@ public: Geofence &get_geofence() { return _geofence; } - bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } - float get_loiter_radius() { return _param_nav_loiter_rad.get(); } /** @@ -199,25 +199,21 @@ public: * * @return the desired cruising speed for this mission */ - float get_cruising_speed(); + float get_cruising_speed() { return _cruising_speed_current_mode; } /** * Set the cruising speed * - * Passing a negative value or leaving the parameter away will reset the cruising speed - * to its default value. - * - * For VTOL: sets cruising speed for current mode only (multirotor or fixed-wing). - * + * Passing a negative value will reset the cruising speed + * to its default value. Will automatically be reset to default + * on mode switch. */ - void set_cruising_speed(float speed = -1.0f); + void set_cruising_speed(float desired_speed) { _cruising_speed_current_mode = desired_speed; } /** * Reset cruising speed to default values - * - * For VTOL: resets both cruising speeds. */ - void reset_cruising_speed(); + void reset_cruising_speed() { _cruising_speed_current_mode = -1.f; } /** * Set triplets to invalid @@ -253,32 +249,12 @@ public: orb_advert_t *get_mavlink_log_pub() { return &_mavlink_log_pub; } - void increment_mission_instance_count() { _mission_result.instance_count++; } - int mission_instance_count() const { return _mission_result.instance_count; } void set_mission_failure_heading_timeout(); - bool is_planned_mission() const { return _navigation_mode == &_mission; } - - bool on_mission_landing() { return (_mission.landing() && _navigation_mode == &_mission); } - - bool start_mission_landing() { return _mission.land_start(); } - bool get_mission_start_land_available() { return _mission.get_land_start_available(); } - int get_mission_landing_index() { return _mission.get_land_start_index(); } - - double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); } - double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); } - float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); } - - double get_mission_landing_lat() { return _mission.get_landing_lat(); } - double get_mission_landing_lon() { return _mission.get_landing_lon(); } - float get_mission_landing_alt() { return _mission.get_landing_alt(); } - - float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); } - // RTL bool in_rtl_state() const { return _vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; } @@ -289,7 +265,7 @@ public: // Param access 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_takeoff_min_alt() const { return _param_mis_takeoff_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()); } @@ -302,8 +278,10 @@ public: void acquire_gimbal_control(); void release_gimbal_control(); - void calculate_breaking_stop(double &lat, double &lon, float &yaw); - void stop_capturing_images(); + void calculate_breaking_stop(double &lat, double &lon, float &yaw); + + void stop_capturing_images(); + void disable_camera_trigger(); void mode_completed(uint8_t nav_state, uint8_t result = mode_completed_s::RESULT_SUCCESS); @@ -344,8 +322,6 @@ private: vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ - bool _rtl_activated{false}; - // Publications geofence_result_s _geofence_result{}; position_setpoint_triplet_s _pos_sp_triplet{}; /**< triplet of position setpoints */ @@ -360,17 +336,16 @@ private: hrt_abstime _last_geofence_check = 0; bool _geofence_violation_warning_sent{false}; /**< prevents spaming to mavlink */ - bool _can_loiter_at_sp{false}; /**< flags if current position SP can be used to loiter */ bool _pos_sp_triplet_updated{false}; /**< flags if position SP triplet needs to be published */ bool _pos_sp_triplet_published_invalid_once{false}; /**< flags if position SP triplet has been published once to UORB */ bool _mission_result_updated{false}; /**< flags if mission result has seen an update */ - bool _shouldEngageMissionForLanding{false}; - Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ Takeoff _takeoff; /**< class for handling takeoff commands */ +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF VtolTakeoff _vtol_takeoff; /**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */ +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ @@ -387,8 +362,7 @@ private: float _param_mpc_jerk_auto{4.f}; /**< initialized with the default jerk auto value to prevent division by 0 if the parameter is accidentally set to 0 */ float _param_mpc_acc_hor{3.f}; /**< initialized with the default horizontal acc value to prevent division by 0 if the parameter is accidentally set to 0 */ - float _mission_cruising_speed_mc{-1.0f}; - float _mission_cruising_speed_fw{-1.0f}; + float _cruising_speed_current_mode{-1.0f}; float _mission_throttle{NAN}; traffic_buffer_s _traffic_buffer{}; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cbd0bd8fa2..fd0f076569 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -48,7 +48,7 @@ #include #include -#include +#include #include #include #include @@ -75,7 +75,9 @@ Navigator::Navigator() : _mission(this), _loiter(this), _takeoff(this), +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _vtol_takeoff(this), +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _land(this), _precland(this), _rtl(this) @@ -87,7 +89,9 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_takeoff; _navigation_mode_array[4] = &_land; _navigation_mode_array[5] = &_precland; +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF _navigation_mode_array[6] = &_vtol_takeoff; +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF /* iterate through navigation modes and initialize _mission_item for each */ for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { @@ -168,6 +172,9 @@ void Navigator::run() fds[2].fd = _mission_sub; fds[2].events = POLLIN; + uint16_t geofence_update_counter{0}; + uint16_t safe_points_update_counter{0}; + /* rate-limit position subscription to 20 Hz / 50 ms */ orb_set_interval(_local_pos_sub, 50); @@ -192,9 +199,18 @@ void Navigator::run() orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus); if (fds[2].revents & POLLIN) { - // copy mission to clear any update mission_s mission; orb_copy(ORB_ID(mission), _mission_sub, &mission); + + if (mission.geofence_update_counter != geofence_update_counter) { + geofence_update_counter = mission.geofence_update_counter; + _geofence.updateFence(); + } + + if (mission.safe_points_update_counter != safe_points_update_counter) { + safe_points_update_counter = mission.safe_points_update_counter; + _rtl.updateSafePoints(); + } } /* gps updated */ @@ -257,8 +273,16 @@ void Navigator::run() bool reposition_valid = true; vehicle_global_position_s position_setpoint{}; - position_setpoint.lat = cmd.param5; - position_setpoint.lon = cmd.param6; + + if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) { + position_setpoint.lat = cmd.param5; + position_setpoint.lon = cmd.param6; + + } else { + position_setpoint.lat = get_global_position()->lat; + position_setpoint.lon = get_global_position()->lon; + } + position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt; if (have_geofence_position_data) { @@ -555,6 +579,8 @@ void Navigator::run() // CMD_NAV_TAKEOFF is acknowledged by commander +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF) { _vtol_takeoff.setTransitionAltitudeAbsolute(cmd.param7); @@ -564,13 +590,15 @@ void Navigator::run() // loiter height is the height above takeoff altitude at which the vehicle will establish on a loiter circle _vtol_takeoff.setLoiterHeight(cmd.param1); +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) { // find NAV_CMD_DO_LAND_START in the mission and // use MAV_CMD_MISSION_START to start the mission from the next item containing a position setpoint + uint8_t result{vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED}; - if (_mission.land_start()) { + if (_mission.get_land_start_available()) { vehicle_command_s vcmd = {}; vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START; vcmd.param1 = _mission.get_land_start_index(); @@ -578,9 +606,10 @@ void Navigator::run() } else { PX4_WARN("planned mission landing not available"); + result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED; } - publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); + publish_vehicle_command_ack(cmd, result); } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) { if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) { @@ -597,7 +626,7 @@ void Navigator::run() set_cruising_speed(cmd.param2); } else { - set_cruising_speed(); + reset_cruising_speed(); /* if no speed target was given try to set throttle */ if (cmd.param3 > FLT_EPSILON) { @@ -674,7 +703,6 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: _pos_sp_triplet_published_invalid_once = false; - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_NORMAL); navigation_mode_new = &_mission; break; @@ -684,131 +712,31 @@ void Navigator::run() navigation_mode_new = &_loiter; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: { + case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: + + // If we are already in mission landing, do not switch. + if (_navigation_mode == &_mission && _mission.isLanding()) { + navigation_mode_new = &_mission; + + } else { _pos_sp_triplet_published_invalid_once = false; - const bool rtl_activated_now = !_rtl_activated; - - switch (_rtl.get_rtl_type()) { - case RTL::RTL_TYPE_MISSION_LANDING: - case RTL::RTL_TYPE_CLOSEST: { - // If a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode. - // In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission. - if (rtl_activated_now) { - _shouldEngageMissionForLanding = _rtl.getRTLDestinationTypeMission() - && _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - } - - if (_shouldEngageMissionForLanding && (on_mission_landing() || _rtl.getRTLState() > RTL::RTL_STATE_CLIMB)) { - - // already in a mission landing, we just need to inform the user and stay in mission - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL to Mission landing, continue landing\t"); - events::send(events::ID("rtl_land_at_mission_continue_landing"), events::Log::Info, - "RTL to Mission landing, continue landing"); - } - - if (_navigation_mode != &_mission) { - // the first time we're here start the mission landig - start_mission_landing(); - } - - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - navigation_mode_new = &_mission; - - } else { - navigation_mode_new = &_rtl; - } - - break; - } - - case RTL::RTL_TYPE_MISSION_LANDING_REVERSED: - if (_mission.get_land_start_available() && !get_land_detected()->landed) { - // the mission contains a landing spot - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); - - if (_navigation_mode != &_mission) { - if (_navigation_mode == nullptr) { - // switching from an manual mode, go to landing if not already landing - if (!on_mission_landing()) { - start_mission_landing(); - } - - } else { - // switching from an auto mode, continue the mission from the closest item - _mission.set_closest_item_as_current(); - } - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t"); - events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info, - "RTL Mission activated, continue mission"); - } - - navigation_mode_new = &_mission; - - } else { - // fly the mission in reverse if switching from a non-manual mode - _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE); - - if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) && - (! _mission.get_mission_finished()) && - (!get_land_detected()->landed)) { - // determine the closest mission item if switching from a non-mission mode, and we are either not already - // mission mode or the mission waypoints changed. - // The seconds condition is required so that when no mission was uploaded and one is available the closest - // mission item is determined and also that if the user changes the active mission index while rtl is active - // always that waypoint is tracked first. - if ((_navigation_mode != &_mission) && (rtl_activated_now || _mission.get_mission_waypoints_changed())) { - _mission.set_closest_item_as_current(); - } - - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t"); - events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info, - "RTL Mission activated, fly mission in reverse"); - } - - navigation_mode_new = &_mission; - - } else { - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t"); - events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info, - "RTL Mission activated, fly to home"); - } - - navigation_mode_new = &_rtl; - } - } - - break; - - default: - if (rtl_activated_now) { - mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t"); - events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated"); - } - - navigation_mode_new = &_rtl; - break; - - } - - _rtl_activated = true; - break; + navigation_mode_new = &_rtl; } + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_takeoff; break; +#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF + case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: _pos_sp_triplet_published_invalid_once = false; navigation_mode_new = &_vtol_takeoff; break; +#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: _pos_sp_triplet_published_invalid_once = false; @@ -831,15 +759,9 @@ void Navigator::run() case vehicle_status_s::NAVIGATION_STATE_STAB: default: navigation_mode_new = nullptr; - _can_loiter_at_sp = false; break; } - if (_vstatus.nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { - _rtl_activated = false; - _rtl.resetRtlState(); - } - // Do not execute any state machine while we are disarmed if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { navigation_mode_new = nullptr; @@ -909,6 +831,8 @@ void Navigator::run() publish_mission_result(); } + _geofence.run(); + perf_end(_loop_perf); } } @@ -1063,7 +987,7 @@ int Navigator::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_NAVIGATION, - PX4_STACK_ADJUSTED(1952), + PX4_STACK_ADJUSTED(2160), (px4_main_t)&run_trampoline, (char *const *)argv); @@ -1136,43 +1060,6 @@ float Navigator::get_altitude_acceptance_radius() } } -float Navigator::get_cruising_speed() -{ - /* there are three options: The mission-requested cruise speed, or the current hover / plane speed */ - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_mission_cruising_speed_mc > 0.0f) { - return _mission_cruising_speed_mc; - - } else { - return -1.0f; - } - - } else { - if (_mission_cruising_speed_fw > 0.0f) { - return _mission_cruising_speed_fw; - - } else { - return -1.0f; - } - } -} - -void Navigator::set_cruising_speed(float speed) -{ - if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _mission_cruising_speed_mc = speed; - - } else { - _mission_cruising_speed_fw = speed; - } -} - -void Navigator::reset_cruising_speed() -{ - _mission_cruising_speed_mc = -1.0f; - _mission_cruising_speed_fw = -1.0f; -} - void Navigator::reset_triplets() { reset_position_setpoint(_pos_sp_triplet.previous); @@ -1428,7 +1315,7 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd) break; default: - vcmd->target_component = _vstatus.component_id; + vcmd->target_component = 0; break; } @@ -1527,6 +1414,18 @@ void Navigator::mode_completed(uint8_t nav_state, uint8_t result) _mode_completed_pub.publish(mode_completed); } + +void Navigator::disable_camera_trigger() +{ + // Disable camera trigger + vehicle_command_s cmd {}; + cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL; + // Pause trigger + cmd.param1 = -1.0f; + cmd.param3 = 1.0f; + publish_vehicle_cmd(&cmd); +} + int Navigator::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 5adb0b1bdb..b0ced9006c 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -55,7 +55,6 @@ NavigatorMode::run(bool active) { if (active) { if (!_active) { - _navigator->set_mission_result_updated(); on_activation(); } else { diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h index 3aa6f255f6..60abdcd165 100644 --- a/src/modules/navigator/navigator_mode.h +++ b/src/modules/navigator/navigator_mode.h @@ -54,6 +54,8 @@ public: void run(bool active); + bool isActive() {return _active;}; + /** * This function is called while the mode is inactive */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 7ef16df6f4..c300c8e3c8 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -43,7 +43,7 @@ /** * Loiter radius (FW only) * - * Default value of loiter radius for missions, Hold mode, Return mode, etc. (fixedwing only). + * Default value of loiter radius in FW mode (e.g. for Loiter mode). * * @unit m * @min 25 diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 2a94c90619..d28952c9f5 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -42,650 +42,240 @@ #include "rtl.h" #include "navigator.h" -#include + +#include #include -#include - - -static constexpr float DELAY_SIGMA = 0.01f; - using namespace time_literals; using namespace math; +static constexpr float MIN_DIST_THRESHOLD = 2.f; + RTL::RTL(Navigator *navigator) : - MissionBlock(navigator), - ModuleParams(navigator) + NavigatorMode(navigator), + ModuleParams(navigator), + _rtl_direct(navigator) { - _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); - _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); - _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); - _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); - _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); - _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); - _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); - _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); +} + +void RTL::updateDatamanCache() +{ + bool success; + + switch (_dataman_state) { + + case DatamanState::UpdateRequestWait: + + if (_initiate_safe_points_updated) { + _initiate_safe_points_updated = false; + _dataman_state = DatamanState::Read; + } + + break; + + case DatamanState::Read: + + _dataman_state = DatamanState::ReadWait; + success = _dataman_client_geofence.readAsync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&_stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + _error_state = DatamanState::Read; + _dataman_state = DatamanState::Error; + } + + break; + + case DatamanState::ReadWait: + + _dataman_client_geofence.update(); + + if (_dataman_client_geofence.lastOperationCompleted(success)) { + + if (!success) { + _error_state = DatamanState::ReadWait; + _dataman_state = DatamanState::Error; + + } else if (_update_counter != _stats.update_counter) { + + _update_counter = _stats.update_counter; + _safe_points_updated = false; + + _dataman_cache_geofence.invalidate(); + + if (_dataman_cache_geofence.size() != _stats.num_items) { + _dataman_cache_geofence.resize(_stats.num_items); + } + + for (int index = 1; index <= _dataman_cache_geofence.size(); ++index) { + _dataman_cache_geofence.load(DM_KEY_SAFE_POINTS, index); + } + + _dataman_state = DatamanState::Load; + + } else { + _dataman_state = DatamanState::UpdateRequestWait; + } + } + + break; + + case DatamanState::Load: + + _dataman_cache_geofence.update(); + + if (!_dataman_cache_geofence.isLoading()) { + _dataman_state = DatamanState::UpdateRequestWait; + _safe_points_updated = true; + } + + break; + + case DatamanState::Error: + PX4_ERR("Safe points update failed! state: %" PRIu8, static_cast(_error_state)); + _dataman_state = DatamanState::UpdateRequestWait; + break; + + default: + break; + + } + + if (_mission_counter != _mission_sub.get().mission_update_counter) { + _mission_counter = _mission_sub.get().mission_update_counter; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + _dataman_cache_landItem.invalidate(); + + if (_mission_sub.get().land_start_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_start_index); + } + + if (_mission_sub.get().land_index > 0) { + _dataman_cache_landItem.load(dm_item, _mission_sub.get().land_index); + } + } + + _dataman_cache_landItem.update(); } void RTL::on_inactivation() { - if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: // Fall through + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactivation(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactivation(); + break; + + default: + break; } } void RTL::on_inactive() { + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + parameters_update(); + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_inactive(); + break; + + case RtlType::RTL_DIRECT: + _rtl_direct.on_inactive(); + break; + + default: + break; + } + // Limit inactive calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); + hrt_abstime now{hrt_absolute_time()}; - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + if ((now - _destination_check_time) > 1_s) { + _destination_check_time = now; + setRtlTypeAndDestination(); - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; + const bool global_position_recently_updated = _global_pos_sub.get().timestamp > 0 + && hrt_elapsed_time(&_global_pos_sub.get().timestamp) < 10_s; - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; + rtl_time_estimate_s estimated_time{}; + estimated_time.valid = false; - // Calculate RTL destination and time estimate only when there is a valid home and global position if (_navigator->home_global_position_valid() && global_position_recently_updated) { - find_RTL_destination(); - calcRtlTimeEstimate(RTLState::RTL_STATE_NONE, rtl_time_estimate); - rtl_time_estimate.valid = true; - } + switch (_rtl_type) { + case RtlType::RTL_DIRECT: + estimated_time = _rtl_direct.calc_rtl_time_estimate(); + break; - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); - } -} + case RtlType::RTL_DIRECT_MISSION_LAND: + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + estimated_time = _rtl_mission_type_handle->calc_rtl_time_estimate(); + break; -void RTL::find_RTL_destination() -{ - // get home position: - home_position_s &home_landing_position = *_navigator->get_home_position(); - - // get global position - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - // set destination to home per default, then check if other valid landing spot is closer - _destination.set(home_landing_position); - - // get distance to home position - double dlat = home_landing_position.lat - global_position.lat; - double dlon = home_landing_position.lon - global_position.lon; - - double lon_scale = cos(radians(global_position.lat)); - - auto coord_dist_sq = [lon_scale](double lat_diff, double lon_diff) -> double { - double lon_diff_scaled = lon_scale * matrix::wrap(lon_diff, -180., 180.); - return lat_diff * lat_diff + lon_diff_scaled * lon_diff_scaled; - }; - - double min_dist_squared = coord_dist_sq(dlat, dlon); - - _destination.type = RTL_DESTINATION_HOME; - - const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; - - - // consider the mission landing if not RTL_TYPE_HOME_OR_RALLY type set - if (_param_rtl_type.get() != RTL_TYPE_HOME_OR_RALLY && _navigator->get_mission_start_land_available()) { - double mission_landing_lat; - double mission_landing_lon; - float mission_landing_alt; - - if (vtol_in_rw_mode) { - mission_landing_lat = _navigator->get_mission_landing_lat(); - mission_landing_lon = _navigator->get_mission_landing_lon(); - mission_landing_alt = _navigator->get_mission_landing_alt(); - - } else { - mission_landing_lat = _navigator->get_mission_landing_start_lat(); - mission_landing_lon = _navigator->get_mission_landing_start_lon(); - mission_landing_alt = _navigator->get_mission_landing_start_alt(); - } - - dlat = mission_landing_lat - global_position.lat; - dlon = mission_landing_lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); - - // always find closest destination if in hover and VTOL - if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->on_mission_landing())) { - - // compare home position to landing position to decide which is closer - if (dist_squared < min_dist_squared) { - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; + default: break; } - - } else { - // it has to be the mission landing - _destination.type = RTL_DESTINATION_MISSION_LANDING; - min_dist_squared = dist_squared; - _destination.lat = mission_landing_lat; - _destination.lon = mission_landing_lon; - _destination.alt = mission_landing_alt; - } - } - - // do not consider rally point if RTL type is set to RTL_TYPE_MISSION_LANDING_REVERSED, so exit function and use either home or mission landing - if (_param_rtl_type.get() == RTL_TYPE_MISSION_LANDING_REVERSED) { - return; - } - - // compare to safe landing positions - mission_safe_point_s closest_safe_point {}; - mission_stats_entry_s stats; - int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s)); - int num_safe_points = 0; - - if (ret == sizeof(mission_stats_entry_s)) { - num_safe_points = stats.num_items; - } - - // check if a safe point is closer than home or landing - int closest_index = 0; - - for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) { - mission_safe_point_s mission_safe_point; - - if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) != - sizeof(mission_safe_point_s)) { - PX4_ERR("dm_read failed"); - continue; } - // TODO: take altitude into account for distance measurement - dlat = mission_safe_point.lat - global_position.lat; - dlon = mission_safe_point.lon - global_position.lon; - double dist_squared = coord_dist_sq(dlat, dlon); - - if (dist_squared < min_dist_squared) { - closest_index = current_seq; - min_dist_squared = dist_squared; - closest_safe_point = mission_safe_point; - } - } - - if (closest_index > 0) { - _destination.type = RTL_DESTINATION_SAFE_POINT; - - // There is a safe point closer than home/mission landing - // TODO: handle all possible mission_safe_point.frame cases - switch (closest_safe_point.frame) { - case 0: // MAV_FRAME_GLOBAL - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt; - _destination.yaw = home_landing_position.yaw; - break; - - case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT - _destination.lat = closest_safe_point.lat; - _destination.lon = closest_safe_point.lon; - _destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home - _destination.yaw = home_landing_position.yaw; - break; - - default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); - events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", - closest_safe_point.frame); - break; - } - } - - if (_param_rtl_cone_half_angle_deg.get() > 0 - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); - - } else { - _rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); + _rtl_time_estimate_pub.publish(estimated_time); } } void RTL::on_activation() { - _rtl_state = RTL_STATE_NONE; + setRtlTypeAndDestination(); - // output the correct message, depending on where the RTL destination is - switch (_destination.type) { - case RTL_DESTINATION_HOME: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.\t"); - events::send(events::ID("rtl_land_at_home"), events::Log::Info, "RTL: landing at home position"); + switch (_rtl_type) { + case RtlType::RTL_DIRECT_MISSION_LAND: // Fall through + case RtlType::RTL_MISSION_FAST: // Fall through + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_type_handle->setReturnAltMin(_enforce_rtl_alt); + _rtl_mission_type_handle->on_activation(); break; - case RTL_DESTINATION_MISSION_LANDING: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.\t"); - events::send(events::ID("rtl_land_at_mission"), events::Log::Info, "RTL: landing at mission landing"); + case RtlType::RTL_DIRECT: + _rtl_direct.setReturnAltMin(_enforce_rtl_alt); + _rtl_direct.on_activation(); break; - case RTL_DESTINATION_SAFE_POINT: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.\t"); - events::send(events::ID("rtl_land_at_safe_point"), events::Log::Info, "RTL: landing at safe landing point"); + + default: break; } - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - if (_navigator->get_land_detected()->landed) { - // For safety reasons don't go into RTL if landed. - _rtl_state = RTL_STATE_LANDED; - - } else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) { - - // If lower than return altitude, climb up first. - // If rtl_alt_min is true then forcing altitude change even if above. - _rtl_state = RTL_STATE_CLIMB; - - } else { - // Otherwise go straight to return - _rtl_state = RTL_STATE_RETURN; - } - - // reset cruising speed and throttle to default for RTL - _navigator->reset_cruising_speed(); - _navigator->set_cruising_throttle(); - - set_rtl_item(); - } void RTL::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { - advance_rtl(); - set_rtl_item(); - } - - if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { - _navigator->get_precland()->on_active(); - - } else if (_navigator->get_precland()->is_activated()) { - _navigator->get_precland()->on_inactivation(); - } - - // Limit rtl time calculation to 1Hz - if ((hrt_absolute_time() - _destination_check_time) > 1_s) { - _destination_check_time = hrt_absolute_time(); - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - - const bool global_position_recently_updated = global_position.timestamp > 0 - && hrt_elapsed_time(&global_position.timestamp) < 10_s; - - rtl_time_estimate_s rtl_time_estimate{}; - rtl_time_estimate.valid = false; - - // Calculate RTL destination and time estimate only when there is a valid home and global position - if (_navigator->home_global_position_valid() && global_position_recently_updated) { - find_RTL_destination(); - calcRtlTimeEstimate(_rtl_state, rtl_time_estimate); - rtl_time_estimate.valid = true; - } - - rtl_time_estimate.timestamp = hrt_absolute_time(); - _rtl_time_estimate_pub.publish(rtl_time_estimate); - } -} - -void RTL::set_rtl_item() -{ - _navigator->set_can_loiter_at_sp(false); - - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); - - position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - - // if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission - const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ? - _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get(); - - const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - - _mission_item.lat = gpos.lat; - _mission_item.lon = gpos.lon; - _mission_item.altitude = _rtl_alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", - (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); - events::send(events::ID("rtl_climb_to"), events::Log::Info, - "RTL: climb to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); - break; - } - - case RTL_STATE_RETURN: { - - // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status - // can be displayed on groundstation and the WP is accepted once within loiter radius - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - - - } else { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _rtl_alt; // Don't change altitude - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && - destination_dist > _param_rtl_min_dist.get()) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || - destination_dist < _param_rtl_min_dist.get()) { - // Use destination yaw if close to _destination. - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_return_at"), events::Log::Info, - "RTL: return at {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // Except for vtol which might be still off here and should point towards this location. - const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", - (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); - events::send(events::ID("rtl_descend_to"), events::Log::Info, - "RTL: descend to {1m_v} ({2m_v} above destination)", - (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); - break; - } - - case RTL_STATE_LOITER: { - const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); - - if (autocontinue) { - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", - (double)_param_rtl_land_delay.get()); - events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); - - } else { - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); - events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); - } - - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; // Don't change altitude. - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); - _mission_item.autocontinue = autocontinue; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.loiter_radius = landing_loiter_radius; - - _navigator->set_can_loiter_at_sp(true); - - break; - } - - case RTL_STATE_HEAD_TO_CENTER: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.vtol_back_transition = true; - // acceptance_radius will be overwritten since vtol_back_transition is set, - // set as a default value only - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - // Disable previous setpoint to prevent drift. - pos_sp_triplet->previous.valid = false; - break; - } - - case RTL_STATE_TRANSITION_TO_MC: { - set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); - break; - } - - case RTL_MOVE_TO_LAND_HOVER_VTOL: { - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = loiter_altitude; - _mission_item.altitude_is_relative = false; - - // have to reset here because these field were used in set_vtol_transition_item - _mission_item.time_inside = 0.f; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - - if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { - _mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon); - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { - _mission_item.yaw = _destination.yaw; - - } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - } - - _mission_item.origin = ORIGIN_ONBOARD; - break; - } - - case RTL_STATE_LAND: { - // Land at destination. - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.lat = _destination.lat; - _mission_item.lon = _destination.lon; - _mission_item.altitude = _destination.alt; - _mission_item.altitude_is_relative = false; - - if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { - _mission_item.yaw = _navigator->get_local_position()->heading; - - } else { - _mission_item.yaw = _destination.yaw; - } - - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - _mission_item.land_precision = _param_rtl_pld_md.get(); - - if (_mission_item.land_precision == 1) { - _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); - _navigator->get_precland()->on_activation(); - - } else if (_mission_item.land_precision == 2) { - _navigator->get_precland()->set_mode(PrecLandMode::Required); - _navigator->get_precland()->on_activation(); - } - - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); - events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); - break; - } - - case RTL_STATE_LANDED: { - set_idle_item(&_mission_item); - set_return_alt_min(false); - break; - } - - default: - break; - } - - reset_mission_item_reached(); - - // Execute command if set. This is required for commands like VTOL transition. - if (!item_contains_position(_mission_item)) { - issue_command(_mission_item); - } - - // Convert mission item to current position setpoint and make it valid. - mission_apply_limitation(_mission_item); - - if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { - _navigator->set_position_setpoint_triplet_updated(); - } -} - -void RTL::advance_rtl() -{ - // determines if the vehicle should loiter above land - const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; - - // vehicle is a vtol and currently in fixed wing mode - const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol - && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; - - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + _global_pos_sub.update(); + _vehicle_status_sub.update(); + _mission_sub.update(); + _home_pos_sub.update(); + + updateDatamanCache(); + + switch (_rtl_type) { + case RtlType::RTL_MISSION_FAST: + case RtlType::RTL_MISSION_FAST_REVERSE: + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle->on_active(); break; - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; - break; - - case RTL_STATE_DESCEND: - - if (descend_and_loiter) { - _rtl_state = RTL_STATE_LOITER; - - } else if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_LOITER: - - if (vtol_in_fw_mode) { - _rtl_state = RTL_STATE_HEAD_TO_CENTER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - - break; - - case RTL_STATE_HEAD_TO_CENTER: - - _rtl_state = RTL_STATE_TRANSITION_TO_MC; - - break; - - case RTL_STATE_TRANSITION_TO_MC: - - _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; - - break; - - case RTL_MOVE_TO_LAND_HOVER_VTOL: - - _rtl_state = RTL_STATE_LAND; - - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + case RtlType::RTL_DIRECT: + _rtl_direct.on_active(); break; default: @@ -693,23 +283,163 @@ void RTL::advance_rtl() } } -float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) +void RTL::setRtlTypeAndDestination() { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + init_rtl_mission_type(); + + if (_param_rtl_type.get() != 2) { + // check the closest allowed destination. + bool isMissionLanding{false}; + RtlDirect::RtlPosition rtl_position; + float rtl_alt; + findRtlDestination(isMissionLanding, rtl_position, rtl_alt); + + if (isMissionLanding) { + _rtl_type = RtlType::RTL_DIRECT_MISSION_LAND; + _rtl_mission_type_handle->setRtlAlt(rtl_alt); + + } else { + _rtl_type = RtlType::RTL_DIRECT; + _rtl_direct.setRtlAlt(rtl_alt); + _rtl_direct.setRtlPosition(rtl_position); + } + } +} + +void RTL::findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt) +{ + // set destination to home per default, then check if other valid landing spot is closer + rtl_position.alt = _home_pos_sub.get().alt; + rtl_position.lat = _home_pos_sub.get().lat; + rtl_position.lon = _home_pos_sub.get().lon; + rtl_position.yaw = _home_pos_sub.get().yaw; + isMissionLanding = false; + + const bool vtol_in_rw_mode = _vehicle_status_sub.get().is_vtol + && (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + + // get distance to home position + float home_dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, rtl_position.lat, rtl_position.lon)}; + float min_dist; + + if ((_param_rtl_type.get() == 1) && !vtol_in_rw_mode) { + // Set minimum distance to maximum value when RTL_TYPE is set to 1 and we are not in RW mode. + min_dist = FLT_MAX; + + } else { + min_dist = home_dist; + } + + // consider the mission landing if available and allowed + if (((_param_rtl_type.get() == 1) || (_param_rtl_type.get() == 3)) && hasMissionLandStart()) { + mission_item_s land_mission_item; + const dm_item_t dm_item = static_cast(_mission_sub.get().dataman_id); + bool success = _dataman_cache_landItem.loadWait(dm_item, _mission_sub.get().land_index, + reinterpret_cast(&land_mission_item), sizeof(mission_item_s), 500_ms); + + if (!success) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission land item could not be read.\t"); + events::send(events::ID("rtl_failed_to_read_land_item"), events::Log::Error, + "Mission land item could not be read"); + } + + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, land_mission_item.lat, land_mission_item.lon)}; + + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { + min_dist = dist; + setLandPosAsDestination(rtl_position, land_mission_item); + isMissionLanding = true; + } + } + + if (_safe_points_updated) { + + for (int current_seq = 1; current_seq <= _dataman_cache_geofence.size(); ++current_seq) { + mission_safe_point_s mission_safe_point; + + bool success = _dataman_cache_geofence.loadWait(DM_KEY_SAFE_POINTS, current_seq, + reinterpret_cast(&mission_safe_point), + sizeof(mission_safe_point_s), 500_ms); + + if (!success) { + PX4_ERR("dm_read failed"); + continue; + } + + float dist{get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, mission_safe_point.lat, mission_safe_point.lon)}; + + if ((dist + MIN_DIST_THRESHOLD) < min_dist) { + min_dist = dist; + setSafepointAsDestination(rtl_position, mission_safe_point); + isMissionLanding = false; + } + } + } + + if (_param_rtl_cone_half_angle_deg.get() > 0 + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + rtl_alt = calculate_return_alt_from_cone_half_angle(rtl_position, (float)_param_rtl_cone_half_angle_deg.get()); + + } else { + rtl_alt = max(_global_pos_sub.get().alt, rtl_position.alt + _param_rtl_return_alt.get()); + } +} + +void RTL::setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item) +{ + rtl_position.alt = land_mission_item.altitude_is_relative ? land_mission_item.altitude + + _home_pos_sub.get().alt : land_mission_item.altitude; + rtl_position.lat = land_mission_item.lat; + rtl_position.lon = land_mission_item.lon; + rtl_position.yaw = _home_pos_sub.get().yaw; +} + +void RTL::setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, + const mission_safe_point_s &mission_safe_point) +{ + // There is a safe point closer than home/mission landing + // TODO: handle all possible mission_safe_point.frame cases + switch (mission_safe_point.frame) { + case 0: // MAV_FRAME_GLOBAL + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt; + rtl_position.yaw = _home_pos_sub.get().yaw;; + break; + + case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT + rtl_position.lat = mission_safe_point.lat; + rtl_position.lon = mission_safe_point.lon; + rtl_position.alt = mission_safe_point.alt + _home_pos_sub.get().alt; // alt of safe point is rel to home + rtl_position.yaw = _home_pos_sub.get().yaw;; + break; + + default: + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); + events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", + mission_safe_point.frame); + break; + } +} + +float RTL::calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, + float cone_half_angle_deg) +{ // horizontal distance to destination - const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon); + const float destination_dist = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + rtl_position.lat, rtl_position.lon); // minium rtl altitude to use when outside of horizontal acceptance radius of target position. // We choose the minimum height to be two times the distance from the land position in order to // avoid the vehicle touching the ground while still moving horizontally. - const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f * - _navigator->get_acceptance_radius(); + const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get(); + float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); - if (destination_dist <= _navigator->get_acceptance_radius()) { - return_altitude_amsl = _destination.alt + 2.0f * destination_dist; + if (destination_dist <= _param_nav_acc_rad.get()) { + return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { @@ -719,7 +449,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) const float cone_half_angle_rad = radians(constrain(cone_half_angle_deg, 1.0f, 89.0f)); // minimum altitude we need in order to be within the user defined cone - const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + _destination.alt; + const float cone_intersection_altitude_amsl = destination_dist / tanf(cone_half_angle_rad) + rtl_position.alt; return_altitude_amsl = min(cone_intersection_altitude_amsl, return_altitude_amsl); } @@ -727,211 +457,73 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, gpos.alt); + return max(return_altitude_amsl, _global_pos_sub.get().alt); } -void RTL::calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate) +void RTL::init_rtl_mission_type() { - const vehicle_global_position_s &gpos = *_navigator->get_global_position(); + RtlType new_rtl_mission_type{RtlType::RTL_DIRECT_MISSION_LAND}; - // Sum up time estimate for various segments of the landing procedure - switch (rtl_state) { - case RTL_STATE_NONE: - case RTL_STATE_CLIMB: { - // Climb segment is only relevant if the drone is below return altitude - const float climb_dist = gpos.alt < _rtl_alt ? (_rtl_alt - gpos.alt) : 0; + if (_param_rtl_type.get() == 2) { + if (hasMissionLandStart()) { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST; - if (climb_dist > FLT_EPSILON) { - rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); - } + } else { + new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; } + } - // FALLTHROUGH - case RTL_STATE_RETURN: + if (_set_rtl_mission_type == new_rtl_mission_type) { + return; + } - // Add cruise segment to home - rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( - _destination.lat, _destination.lon, gpos.lat, gpos.lon) / getCruiseGroundSpeed(); - - // FALLTHROUGH - case RTL_STATE_HEAD_TO_CENTER: - case RTL_STATE_TRANSITION_TO_MC: - case RTL_STATE_DESCEND: { - // when descending, the target altitude is stored in the current mission item - float initial_altitude = 0.f; - float loiter_altitude = 0.f; - - if (rtl_state == RTL_STATE_DESCEND) { - // Take current vehicle altitude as the starting point for calculation - initial_altitude = gpos.alt; // TODO: Check if this is in the right frame - loiter_altitude = _mission_item.altitude; // Next waypoint = loiter - - - } else { - // Take the return altitude as the starting point for the calculation - initial_altitude = _rtl_alt; // CLIMB and RETURN - loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - } - - // Add descend segment (first landing phase: return alt to loiter alt) - rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); - } - - // FALLTHROUGH - case RTL_STATE_LOITER: - // Add land delay (the short pause for deploying landing gear) - // TODO: Check if landing gear is deployed or not - rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); - - // FALLTHROUGH - case RTL_MOVE_TO_LAND_HOVER_VTOL: - case RTL_STATE_LAND: { - float initial_altitude; - - // Add land segment (second landing phase) which comes after LOITER - if (rtl_state == RTL_STATE_LAND) { - // If we are in this phase, use the current vehicle altitude instead - // of the altitude paramteter to get a continous time estimate - initial_altitude = gpos.alt; - - - } else { - // If this phase is not active yet, simply use the loiter altitude, - // which is where the LAND phase will start - const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); - initial_altitude = loiter_altitude; - } - - // Prevent negative times when close to the ground - if (initial_altitude > _destination.alt) { - rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); - } - } + if (_rtl_mission_type_handle) { + delete _rtl_mission_type_handle; + _rtl_mission_type_handle = nullptr; + _set_rtl_mission_type = RtlType::NONE; + } + switch (new_rtl_mission_type) { + case RtlType::RTL_DIRECT_MISSION_LAND: + _rtl_mission_type_handle = new RtlDirectMissionLand(_navigator); + _set_rtl_mission_type = RtlType::RTL_DIRECT_MISSION_LAND; + // RTL type is either direct or mission land have to set it later. break; - case RTL_STATE_LANDED: - // Remaining time is 0 + case RtlType::RTL_MISSION_FAST: + _rtl_mission_type_handle = new RtlMissionFast(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST; + _rtl_type = RtlType::RTL_MISSION_FAST; + break; + + case RtlType::RTL_MISSION_FAST_REVERSE: + _rtl_mission_type_handle = new RtlMissionFastReverse(_navigator); + _set_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE; + _rtl_type = RtlType::RTL_MISSION_FAST_REVERSE;; + break; + + default: break; } - - // Prevent negative durations as phyiscally they make no sense. These can - // occur during the last phase of landing when close to the ground. - rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); - - // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin - rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate - + _param_rtl_time_margin.get(); } -float RTL::getCruiseSpeed() +void RTL::parameters_update() { - float ret = 1e6f; + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { - ret = 1e6f; - } + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { - if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { - ret = 1e6f; + if (!isActive()) { + setRtlTypeAndDestination(); } } - - return ret; } -float RTL::getHoverLandSpeed() +bool RTL::hasMissionLandStart() { - float ret = 1e6f; - - if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { - ret = 1e6f; - } - - return ret; -} - -matrix::Vector2f RTL::get_wind() -{ - _wind_sub.update(); - matrix::Vector2f wind; - - if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { - wind(0) = _wind_sub.get().windspeed_north; - wind(1) = _wind_sub.get().windspeed_east; - } - - return wind; -} - -float RTL::getClimbRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - - if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getDescendRate() -{ - float ret = 1e6f; - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { - ret = 1e6f; - } - - } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { - ret = 1e6f; - } - } - - return ret; -} - -float RTL::getCruiseGroundSpeed() -{ - float cruise_speed = getCruiseSpeed(); - - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); - matrix::Vector2f wind = get_wind(); - - matrix::Vector2f to_destination_vec; - get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, - &to_destination_vec(0), &to_destination_vec(1)); - - const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); - - const float wind_towards_home = wind.dot(to_home_dir); - const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); - - - // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient - const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( - 0.f, wind_towards_home); - - cruise_speed = ground_speed; - } - - return cruise_speed; + return _mission_sub.get().land_start_index > 0; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3c6dea1a2b..fab59e65d6 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -44,55 +44,36 @@ #include #include "navigator_mode.h" -#include "mission_block.h" +#include +#include "rtl_base.h" +#include "rtl_direct.h" +#include "rtl_direct_mission_land.h" +#include "rtl_mission_fast.h" +#include "rtl_mission_fast_reverse.h" +#include #include +#include #include +#include +#include #include -#include -#include -#include -#include class Navigator; -class RTL : public MissionBlock, public ModuleParams +class RTL : public NavigatorMode, public ModuleParams { public: RTL(Navigator *navigator); ~RTL() = default; - enum RTLType { - RTL_TYPE_HOME_OR_RALLY = 0, - RTL_TYPE_MISSION_LANDING, - RTL_TYPE_MISSION_LANDING_REVERSED, - RTL_TYPE_CLOSEST, - }; - - enum RTLDestinationType { - RTL_DESTINATION_HOME = 0, - RTL_DESTINATION_MISSION_LANDING, - RTL_DESTINATION_SAFE_POINT, - }; - - enum RTLHeadingMode { - RTL_NAVIGATION_HEADING = 0, - RTL_DESTINATION_HEADING, - RTL_CURRENT_HEADING, - }; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_TRANSITION_TO_MC, - RTL_MOVE_TO_LAND_HOVER_VTOL, - RTL_STATE_LAND, - RTL_STATE_LANDED, - RTL_STATE_HEAD_TO_CENTER, + enum class RtlType { + NONE, + RTL_DIRECT, + RTL_DIRECT_MISSION_LAND, + RTL_MISSION_FAST, + RTL_MISSION_FAST_REVERSE, }; void on_inactivation() override; @@ -100,98 +81,107 @@ public: void on_activation() override; void on_active() override; - void find_RTL_destination(); + void initialize() override {}; - void set_return_alt_min(bool min) { _rtl_alt_min = min; } + void set_return_alt_min(bool min) { _enforce_rtl_alt = min; } - int get_rtl_type() const { return _param_rtl_type.get(); } - - void get_rtl_xy_z_speed(float &xy, float &z); - - matrix::Vector2f get_wind(); - - RTLState getRTLState() { return _rtl_state; } - - bool getRTLDestinationTypeMission() { return _destination.type == RTLDestinationType::RTL_DESTINATION_MISSION_LANDING; } - - void resetRtlState() { _rtl_state = RTL_STATE_NONE; } + void updateSafePoints() { _initiate_safe_points_updated = true; } private: + bool hasMissionLandStart(); - void set_rtl_item(); + /** + * @brief function to call regularly to do background work + */ + void updateDatamanCache(); - void advance_rtl(); + void setRtlTypeAndDestination(); - float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg); - void calcRtlTimeEstimate(const RTLState rtl_state, rtl_time_estimate_s &rtl_time_estimate); + /** + * @brief Find RTL destination. + * + */ + void findRtlDestination(bool &isMissionLanding, RtlDirect::RtlPosition &rtl_position, float &rtl_alt); - float getCruiseGroundSpeed(); + /** + * @brief Set the position of the land start marker in the planned mission as destination. + * + */ + void setLandPosAsDestination(RtlDirect::RtlPosition &rtl_position, mission_item_s &land_mission_item); - float getClimbRate(); + /** + * @brief Set the safepoint as destination. + * + * @param mission_safe_point is the mission safe point/rally point to set as destination. + */ + void setSafepointAsDestination(RtlDirect::RtlPosition &rtl_position, const mission_safe_point_s &mission_safe_point); - float getDescendRate(); + /** + * @brief calculate return altitude from cone half angle + * + * @param[in] rtl_position landing position of the rtl + * @param[in] cone_half_angle_deg half angle of the cone [deg] + * @return return altitude + */ + float calculate_return_alt_from_cone_half_angle(const RtlDirect::RtlPosition &rtl_position, float cone_half_angle_deg); - float getCruiseSpeed(); + /** + * @brief initialize RTL mission type + * + */ + void init_rtl_mission_type(); - float getHoverLandSpeed(); + /** + * @brief Update parameters + * + */ + void parameters_update(); - RTLState _rtl_state{RTL_STATE_NONE}; - - struct RTLPosition { - double lat; - double lon; - float alt; - float yaw; - uint8_t safe_point_index; ///< 0 = home position, 1 = mission landing, >1 = safe landing points (rally points) - RTLDestinationType type{RTL_DESTINATION_HOME}; - - void set(const home_position_s &home_position) - { - lat = home_position.lat; - lon = home_position.lon; - alt = home_position.alt; - yaw = home_position.yaw; - safe_point_index = 0; - type = RTL_DESTINATION_HOME; - } + enum class DatamanState { + UpdateRequestWait, + Read, + ReadWait, + Load, + Error }; - RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point) - hrt_abstime _destination_check_time{0}; - float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position + RtlBase *_rtl_mission_type_handle{nullptr}; + RtlType _set_rtl_mission_type{RtlType::NONE}; - bool _rtl_alt_min{false}; + RtlType _rtl_type{RtlType::RTL_DIRECT}; + + DatamanState _dataman_state{DatamanState::UpdateRequestWait}; + DatamanState _error_state{DatamanState::UpdateRequestWait}; + uint16_t _update_counter{0}; ///< dataman update counter: if it does not match, safe points data was updated + bool _safe_points_updated{false}; ///< flag indicating if safe points are updated to dataman cache + DatamanCache _dataman_cache_geofence{"rtl_dm_cache_miss_geo", 4}; + DatamanClient &_dataman_client_geofence = _dataman_cache_geofence.client(); + bool _initiate_safe_points_updated{true}; ///< flag indicating if safe points update is needed + DatamanCache _dataman_cache_landItem{"rtl_dm_cache_miss_land", 2}; + int16_t _mission_counter = -1; + + mission_stats_entry_s _stats; + + RtlDirect _rtl_direct; + + bool _enforce_rtl_alt{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rtl_return_alt, - (ParamFloat) _param_rtl_descend_alt, - (ParamFloat) _param_rtl_land_delay, - (ParamFloat) _param_rtl_min_dist, (ParamInt) _param_rtl_type, (ParamInt) _param_rtl_cone_half_angle_deg, - (ParamInt) _param_rtl_pld_md, - (ParamFloat) _param_rtl_loiter_rad, - (ParamInt) _param_rtl_hdg_md, - (ParamFloat) _param_rtl_time_factor, - (ParamInt) _param_rtl_time_margin + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_min_dist, + (ParamFloat) _param_nav_acc_rad ) - param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; - param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; - param_t _param_mpc_land_speed{PARAM_INVALID}; - param_t _param_fw_climb_rate{PARAM_INVALID}; - param_t _param_fw_sink_rate{PARAM_INVALID}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - param_t _param_fw_airspeed_trim{PARAM_INVALID}; - param_t _param_mpc_xy_cruise{PARAM_INVALID}; - param_t _param_rover_cruise_speed{PARAM_INVALID}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::SubscriptionData _mission_sub{ORB_ID(mission)}; + uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; - uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_time_estimate_pub{ORB_ID(rtl_time_estimate)}; }; - -float time_to_home(const matrix::Vector3f &to_home_vec, - const matrix::Vector2f &wind_velocity, float vehicle_speed_m_s, - float vehicle_descent_speed_m_s); diff --git a/src/modules/navigator/rtl_base.h b/src/modules/navigator/rtl_base.h new file mode 100644 index 0000000000..dc698687fc --- /dev/null +++ b/src/modules/navigator/rtl_base.h @@ -0,0 +1,57 @@ +/*************************************************************************** + * + * 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 rtl_base.h + * + * Helper class for RTL modes using the mission + * + */ + +#pragma once + +#include "mission_base.h" +#include + +class RtlBase : public MissionBase +{ +public: + RtlBase(Navigator *navigator, int32_t dataman_cache_size_signed): + MissionBase(navigator, dataman_cache_size_signed) {}; + virtual ~RtlBase() = default; + + virtual rtl_time_estimate_s calc_rtl_time_estimate() = 0; + + virtual void setReturnAltMin(bool min) { (void)min;}; + + virtual void setRtlAlt(float alt) { (void)alt;}; +}; diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp new file mode 100644 index 0000000000..22c79f7161 --- /dev/null +++ b/src/modules/navigator/rtl_direct.cpp @@ -0,0 +1,723 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 rtl.cpp + * + * Helper class to access RTL + * + * @author Julian Oes + * @author Anton Babushkin + * @author Julian Kent + */ + +#include "rtl_direct.h" +#include "navigator.h" +#include +#include + +#include + + +static constexpr float DELAY_SIGMA = 0.01f; + +using namespace time_literals; +using namespace math; + +RtlDirect::RtlDirect(Navigator *navigator) : + MissionBlock(navigator), + ModuleParams(navigator) +{ + _param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP"); + _param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN"); + _param_mpc_land_speed = param_find("MPC_LAND_SPEED"); + _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); + _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); + _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); + _param_rover_cruise_speed = param_find("GND_SPEED_THR_SC"); +} + +void RtlDirect::on_inactivation() +{ + if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::on_activation() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_land_detected_sub.get().landed) { + // For safety reasons don't go into RTL if landed. + _rtl_state = RTL_STATE_LANDED; + + } else if ((_global_pos_sub.get().alt < _destination.alt + _param_rtl_return_alt.get()) || _enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If rtl_alt_min is true then forcing altitude change even if above. + _rtl_state = RTL_STATE_CLIMB; + + } else { + // Otherwise go start with climb + _rtl_state = RTL_STATE_RETURN; + } + + // reset cruising speed and throttle to default for RTL + _navigator->reset_cruising_speed(); + _navigator->set_cruising_throttle(); + + set_rtl_item(); +} + +void RtlDirect::on_active() +{ + _global_pos_sub.update(); + _local_pos_sub.update(); + _land_detected_sub.update(); + _vehicle_status_sub.update(); + + parameters_update(); + + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached_or_completed()) { + advance_rtl(); + set_rtl_item(); + } + + if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) { + // Need to update the position and type on the current setpoint triplet. + _navigator->get_precland()->on_active(); + + } else if (_navigator->get_precland()->is_activated()) { + _navigator->get_precland()->on_inactivation(); + } +} + +void RtlDirect::set_rtl_item() +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + + const RTLHeadingMode rtl_heading_mode = static_cast(_param_rtl_hdg_md.get()); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode != RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", + (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("rtl_climb_to"), events::Log::Info, + "RTL: climb to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + break; + } + + case RTL_STATE_RETURN: { + + // For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status + // can be displayed on groundstation and the WP is accepted once within loiter radius + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + + + } else { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _rtl_alt; // Don't change altitude + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING && + destination_dist > _param_rtl_min_dist.get()) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING || + destination_dist < _param_rtl_min_dist.get()) { + // Use destination yaw if close to _destination. + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_return_at"), events::Log::Info, + "RTL: return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // Except for vtol which might be still off here and should point towards this location. + const float d_current = get_distance_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + if (_vehicle_status_sub.get().is_vtol && (d_current > _navigator->get_acceptance_radius())) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, + _mission_item.lat, _mission_item.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", + (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_descend_to"), events::Log::Info, + "RTL: descend to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); + break; + } + + case RTL_STATE_LOITER: { + const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON); + + if (autocontinue) { + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", + (double)_param_rtl_land_delay.get()); + events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get()); + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); + events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); + } + + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; // Don't change altitude. + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f); + _mission_item.autocontinue = autocontinue; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _param_rtl_loiter_rad.get(); + + break; + } + + case RTL_STATE_HEAD_TO_CENTER: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.vtol_back_transition = true; + // acceptance_radius will be overwritten since vtol_back_transition is set, + // set as a default value only + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + // Disable previous setpoint to prevent drift. + pos_sp_triplet->previous.valid = false; + break; + } + + case RTL_STATE_TRANSITION_TO_MC: { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + break; + } + + case RTL_MOVE_TO_LAND_HOVER_VTOL: { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = loiter_altitude; + _mission_item.altitude_is_relative = false; + + // have to reset here because these field were used in set_vtol_transition_item + _mission_item.time_inside = 0.f; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + + if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING) { + _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos_sub.get().lat, _global_pos_sub.get().lon, _destination.lat, + _destination.lon); + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_DESTINATION_HEADING) { + _mission_item.yaw = _destination.yaw; + + } else if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + } + + _mission_item.origin = ORIGIN_ONBOARD; + break; + } + + case RTL_STATE_LAND: { + // Land at destination. + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _destination.lat; + _mission_item.lon = _destination.lon; + _mission_item.altitude = _destination.alt; + _mission_item.altitude_is_relative = false; + + if (rtl_heading_mode == RTLHeadingMode::RTL_CURRENT_HEADING) { + _mission_item.yaw = _local_pos_sub.get().heading; + + } else { + _mission_item.yaw = _destination.yaw; + } + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision == 1) { + _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); + _navigator->get_precland()->on_activation(); + + } else if (_mission_item.land_precision == 2) { + _navigator->get_precland()->set_mode(PrecLandMode::Required); + _navigator->get_precland()->on_activation(); + } + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); + events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); + break; + } + + case RTL_STATE_LANDED: { + set_idle_item(&_mission_item); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + // Execute command if set. This is required for commands like VTOL transition. + if (!MissionBlock::item_contains_position(_mission_item)) { + issue_command(_mission_item); + } + + // Convert mission item to current position setpoint and make it valid. + mission_apply_limitation(_mission_item); + + if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + _navigator->set_position_setpoint_triplet_updated(); + } +} + +void RtlDirect::advance_rtl() +{ + // determines if the vehicle should loiter above land + const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA; + + // vehicle is a vtol and currently in fixed wing mode + const bool vtol_in_fw_mode = _vehicle_status_sub.get().is_vtol + && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; + + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + + if (descend_and_loiter) { + _rtl_state = RTL_STATE_LOITER; + + } else if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_LOITER: + + if (vtol_in_fw_mode) { + _rtl_state = RTL_STATE_HEAD_TO_CENTER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + + break; + + case RTL_STATE_HEAD_TO_CENTER: + + _rtl_state = RTL_STATE_TRANSITION_TO_MC; + + break; + + case RTL_STATE_TRANSITION_TO_MC: + + _rtl_state = RTL_MOVE_TO_LAND_HOVER_VTOL; + + break; + + case RTL_MOVE_TO_LAND_HOVER_VTOL: + + _rtl_state = RTL_STATE_LAND; + + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + _navigator->mode_completed(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL); + break; + + default: + break; + } +} + +rtl_time_estimate_s RtlDirect::calc_rtl_time_estimate() +{ + _global_pos_sub.update(); + + rtl_time_estimate_s rtl_time_estimate{}; + + RTLState start_state_for_estimate{RTL_STATE_NONE}; + + if (isActive()) { + start_state_for_estimate = _rtl_state; + } + + // Calculate RTL time estimate only when there is a valid home position + // TODO: Also check if vehicle position is valid + if (!_navigator->home_global_position_valid()) { + rtl_time_estimate.valid = false; + + } else { + rtl_time_estimate.valid = true; + + // Sum up time estimate for various segments of the landing procedure + switch (start_state_for_estimate) { + case RTL_STATE_NONE: + case RTL_STATE_CLIMB: { + // Climb segment is only relevant if the drone is below return altitude + const float climb_dist = _global_pos_sub.get().alt < _rtl_alt ? (_rtl_alt - _global_pos_sub.get().alt) : 0; + + if (climb_dist > FLT_EPSILON) { + rtl_time_estimate.time_estimate += climb_dist / getClimbRate(); + } + } + + // FALLTHROUGH + case RTL_STATE_RETURN: + + // Add cruise segment to home + rtl_time_estimate.time_estimate += get_distance_to_next_waypoint( + _destination.lat, _destination.lon, _global_pos_sub.get().lat, _global_pos_sub.get().lon) / getCruiseGroundSpeed(); + + // FALLTHROUGH + case RTL_STATE_HEAD_TO_CENTER: + case RTL_STATE_TRANSITION_TO_MC: + case RTL_STATE_DESCEND: { + // when descending, the target altitude is stored in the current mission item + float initial_altitude = 0.f; + float loiter_altitude = 0.f; + + if (start_state_for_estimate == RTL_STATE_DESCEND) { + // Take current vehicle altitude as the starting point for calculation + initial_altitude = _global_pos_sub.get().alt; // TODO: Check if this is in the right frame + loiter_altitude = _mission_item.altitude; // Next waypoint = loiter + + + } else { + // Take the return altitude as the starting point for the calculation + initial_altitude = _rtl_alt; // CLIMB and RETURN + loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + } + + // Add descend segment (first landing phase: return alt to loiter alt) + rtl_time_estimate.time_estimate += fabsf(initial_altitude - loiter_altitude) / getDescendRate(); + } + + // FALLTHROUGH + case RTL_STATE_LOITER: + // Add land delay (the short pause for deploying landing gear) + // TODO: Check if landing gear is deployed or not + rtl_time_estimate.time_estimate += _param_rtl_land_delay.get(); + + // FALLTHROUGH + case RTL_MOVE_TO_LAND_HOVER_VTOL: + case RTL_STATE_LAND: { + float initial_altitude; + + // Add land segment (second landing phase) which comes after LOITER + if (start_state_for_estimate == RTL_STATE_LAND) { + // If we are in this phase, use the current vehicle altitude instead + // of the altitude paramteter to get a continous time estimate + initial_altitude = _global_pos_sub.get().alt; + + + } else { + // If this phase is not active yet, simply use the loiter altitude, + // which is where the LAND phase will start + const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt); + initial_altitude = loiter_altitude; + } + + // Prevent negative times when close to the ground + if (initial_altitude > _destination.alt) { + rtl_time_estimate.time_estimate += (initial_altitude - _destination.alt) / getHoverLandSpeed(); + } + } + + break; + + case RTL_STATE_LANDED: + // Remaining time is 0 + break; + } + + // Prevent negative durations as phyiscally they make no sense. These can + // occur during the last phase of landing when close to the ground. + rtl_time_estimate.time_estimate = math::max(0.f, rtl_time_estimate.time_estimate); + + // Use actual time estimate to compute the safer time estimate with additional scale factor and a margin + rtl_time_estimate.safe_time_estimate = _param_rtl_time_factor.get() * rtl_time_estimate.time_estimate + + _param_rtl_time_margin.get(); + } + + // return message + rtl_time_estimate.timestamp = hrt_absolute_time(); + + return rtl_time_estimate; +} + +float RtlDirect::getCruiseSpeed() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_xy_cruise == PARAM_INVALID || param_get(_param_mpc_xy_cruise, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_airspeed_trim == PARAM_INVALID || param_get(_param_fw_airspeed_trim, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) { + if (_param_rover_cruise_speed == PARAM_INVALID || param_get(_param_rover_cruise_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getHoverLandSpeed() +{ + float ret = 1e6f; + + if (_param_mpc_land_speed == PARAM_INVALID || param_get(_param_mpc_land_speed, &ret) != PX4_OK) { + ret = 1e6f; + } + + return ret; +} + +matrix::Vector2f RtlDirect::get_wind() +{ + _wind_sub.update(); + matrix::Vector2f wind; + + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; + } + + return wind; +} + +float RtlDirect::getClimbRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_up == PARAM_INVALID || param_get(_param_mpc_z_v_auto_up, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + if (_param_fw_climb_rate == PARAM_INVALID || param_get(_param_fw_climb_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getDescendRate() +{ + float ret = 1e6f; + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (_param_mpc_z_v_auto_dn == PARAM_INVALID || param_get(_param_mpc_z_v_auto_dn, &ret) != PX4_OK) { + ret = 1e6f; + } + + } else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + if (_param_fw_sink_rate == PARAM_INVALID || param_get(_param_fw_sink_rate, &ret) != PX4_OK) { + ret = 1e6f; + } + } + + return ret; +} + +float RtlDirect::getCruiseGroundSpeed() +{ + float cruise_speed = getCruiseSpeed(); + + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + const vehicle_global_position_s &global_position = *_navigator->get_global_position(); + matrix::Vector2f wind = get_wind(); + + matrix::Vector2f to_destination_vec; + get_vector_to_next_waypoint(global_position.lat, global_position.lon, _destination.lat, _destination.lon, + &to_destination_vec(0), &to_destination_vec(1)); + + const matrix::Vector2f to_home_dir = to_destination_vec.unit_or_zero(); + + const float wind_towards_home = wind.dot(to_home_dir); + const float wind_across_home = matrix::Vector2f(wind - to_home_dir * wind_towards_home).norm(); + + + // Note: use fminf so that we don't _rely_ on wind towards home to make RTL more efficient + const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_home * wind_across_home) + fminf( + 0.f, wind_towards_home); + + cruise_speed = ground_speed; + } + + return cruise_speed; +} + +void RtlDirect::parameters_update() +{ + if (_parameter_update_sub.updated()) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + // If any parameter updated, call updateParams() to check if + // this class attributes need updating (and do so). + updateParams(); + } +} diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h new file mode 100644 index 0000000000..94ada8f514 --- /dev/null +++ b/src/modules/navigator/rtl_direct.h @@ -0,0 +1,241 @@ +/*************************************************************************** + * + * 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 rtl_direct.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include + +#include "mission_block.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class Navigator; + +class RtlDirect : public MissionBlock, public ModuleParams +{ +public: + /** + * @brief Return to launch position. + * Defines the position and landing yaw for the return to launch destination. + * + */ + struct RtlPosition { + double lat; /**< latitude in WGS84 [rad].*/ + double lon; /**< longitude in WGS84 [rad].*/ + float alt; /**< altitude in MSL [m].*/ + float yaw; /**< final yaw when landed [rad].*/ + }; + + RtlDirect(Navigator *navigator); + + ~RtlDirect() = default; + + /** + * @brief on inactivation + * + */ + void on_inactivation() override; + + /** + * @brief on activation. + * Initialize the return to launch calculations. + * + */ + void on_activation() override; + + /** + * @brief on active + * Update the return to launch calculation and set new setpoints for controller if necessary. + * + */ + void on_active() override; + + /** + * @brief Calculate the estimated time needed to return to launch. + * + * @return estimated time to return to launch. + */ + rtl_time_estimate_s calc_rtl_time_estimate(); + + void setReturnAltMin(bool min) { _enforce_rtl_alt = min; } + void setRtlAlt(float alt) {_rtl_alt = alt;}; + + void setRtlPosition(RtlPosition position) {_destination = position;}; + +private: + /** + * @brief Return to launch heading mode. + * + */ + enum RTLHeadingMode { + RTL_NAVIGATION_HEADING = 0, + RTL_DESTINATION_HEADING, + RTL_CURRENT_HEADING, + }; + + /** + * @brief Return to launch state machine. + * + */ + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_TRANSITION_TO_MC, + RTL_MOVE_TO_LAND_HOVER_VTOL, + RTL_STATE_LAND, + RTL_STATE_LANDED, + RTL_STATE_HEAD_TO_CENTER, + }; + +private: + /** + * @brief Get the horizontal wind velocity + * + * @return horizontal wind velocity. + */ + matrix::Vector2f get_wind(); + + /** + * @brief Set the return to launch control setpoint. + * + */ + void set_rtl_item(); + + /** + * @brief Advance the return to launch state machine. + * + */ + void advance_rtl(); + + /** + * @brief Get the Cruise Ground Speed + * + * @return Ground speed in cruise mode [m/s]. + */ + float getCruiseGroundSpeed(); + + /** + * @brief Get the climb rate + * + * @return Climb rate [m/s] + */ + float getClimbRate(); + + /** + * @brief Get the descend rate + * + * @return descend rate [m/s] + */ + float getDescendRate(); + + /** + * @brief Get the cruise speed + * + * @return cruise speed [m/s] + */ + float getCruiseSpeed(); + + /** + * @brief Get the Hover Land Speed + * + * @return Hover land speed [m/s] + */ + float getHoverLandSpeed(); + + /** + * Check for parameter changes and update them if needed. + */ + void parameters_update(); + + /** Current state in the state machine.*/ + RTLState _rtl_state{RTL_STATE_NONE}; + + bool _enforce_rtl_alt{false}; + + RtlPosition _destination{}; ///< the RTL position to fly to + + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + + DEFINE_PARAMETERS( + (ParamFloat) _param_rtl_return_alt, + (ParamFloat) _param_rtl_descend_alt, + (ParamFloat) _param_rtl_land_delay, + (ParamFloat) _param_rtl_min_dist, + (ParamInt) _param_rtl_pld_md, + (ParamFloat) _param_rtl_loiter_rad, + (ParamInt) _param_rtl_hdg_md, + (ParamFloat) _param_rtl_time_factor, + (ParamInt) _param_rtl_time_margin + ) + + param_t _param_mpc_z_v_auto_up{PARAM_INVALID}; + param_t _param_mpc_z_v_auto_dn{PARAM_INVALID}; + param_t _param_mpc_land_speed{PARAM_INVALID}; + param_t _param_fw_climb_rate{PARAM_INVALID}; + param_t _param_fw_sink_rate{PARAM_INVALID}; + + param_t _param_fw_airspeed_trim{PARAM_INVALID}; + param_t _param_mpc_xy_cruise{PARAM_INVALID}; + param_t _param_rover_cruise_speed{PARAM_INVALID}; + + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionData _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */ + 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 _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< vehicle status subscription */ + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; +}; diff --git a/src/modules/navigator/rtl_direct_mission_land.cpp b/src/modules/navigator/rtl_direct_mission_land.cpp new file mode 100644 index 0000000000..236b7b61c5 --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.cpp @@ -0,0 +1,281 @@ +/*************************************************************************** + * + * 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 rtl_direct_mission_land.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_direct_mission_land.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE = 5; + +RtlDirectMissionLand::RtlDirectMissionLand(Navigator *navigator) : + RtlBase(navigator, DEFAULT_DIRECT_MISSION_LAND_CACHE_SIZE) +{ + +} + +void RtlDirectMissionLand::on_activation() +{ + _land_detected_sub.update(); + _global_pos_sub.update(); + + _needs_climbing = false; + + if (hasMissionLandStart()) { + _is_current_planned_mission_item_valid = (goToItem(_mission.land_start_index, false) == PX4_OK); + + if ((_global_pos_sub.get().alt < _rtl_alt) || _enforce_rtl_alt) { + + // If lower than return altitude, climb up first. + // If enforce_rtl_alt is true then forcing altitude change even if above. + _needs_climbing = true; + + } + + } else { + _is_current_planned_mission_item_valid = false; + } + + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +bool RtlDirectMissionLand::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +void RtlDirectMissionLand::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // 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) + if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + + } else { + _mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT; + } + + _mission_item.lat = _global_pos_sub.get().lat; + _mission_item.lon = _global_pos_sub.get().lon; + _mission_item.altitude = _rtl_alt; + _mission_item.altitude_is_relative = false; + + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL Mission land: climb to %d m\t", + (int)ceilf(_rtl_alt)); + events::send(events::ID("rtl_mission_land_climb"), events::Log::Info, + "RTL Mission Land: climb to {1m_v}", + (int32_t)ceilf(_rtl_alt)); + + _needs_climbing = false; + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + } else if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && + _vehicle_status_sub.get().is_vtol && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Transition to fixed wing if necessary. + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlDirectMissionLand::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_direct_mission_land.h b/src/modules/navigator/rtl_direct_mission_land.h new file mode 100644 index 0000000000..002d26da67 --- /dev/null +++ b/src/modules/navigator/rtl_direct_mission_land.h @@ -0,0 +1,74 @@ +/*************************************************************************** + * + * 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 rtl_direct_mission_land.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlDirectMissionLand : public RtlBase +{ +public: + RtlDirectMissionLand(Navigator *navigator); + ~RtlDirectMissionLand() = default; + + void on_activation() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + + void setReturnAltMin(bool min) override { _enforce_rtl_alt = min; }; + void setRtlAlt(float alt) override {_rtl_alt = alt;}; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + bool _needs_climbing{false}; //< Flag if climbing is required at the start + bool _enforce_rtl_alt{false}; + float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the land position +}; diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp new file mode 100644 index 0000000000..924ef8b8ba --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -0,0 +1,241 @@ +/*************************************************************************** + * + * 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 rtl_mission_fast.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_CACHE_SIZE = 5; + +RtlMissionFast::RtlMissionFast(Navigator *navigator) : + RtlBase(navigator, DEFAULT_MISSION_FAST_CACHE_SIZE) +{ + +} + +void RtlMissionFast::on_activation() +{ + _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; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidad the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFast::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFast::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFast::setNextMissionItem() +{ + return (goToNextPositionItem(true) == PX4_OK); +} + +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(); + + // 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 && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + if (_mission_item.nav_cmd == NAV_CMD_LAND || + _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + int32_t next_mission_item_index; + size_t num_found_items = 0; + getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u); + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed && + ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) + || (_mission_item.nav_cmd == NAV_CMD_LAND)); + bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + needs_to_land; + + if (needs_vtol_landing) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if (needs_to_land) { + /* move to landing waypoint before descent if necessary */ + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land() && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } + } +} + +bool RtlMissionFast::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast.h b/src/modules/navigator/rtl_mission_fast.h new file mode 100644 index 0000000000..576152efe2 --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * 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 rtl_mission_fast.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFast : public RtlBase +{ +public: + RtlMissionFast(Navigator *navigator); + ~RtlMissionFast() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + 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 new file mode 100644 index 0000000000..1b5feb850d --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -0,0 +1,268 @@ +/*************************************************************************** + * + * 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 rtl_mission_fast_reverse.cpp + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "rtl_mission_fast_reverse.h" +#include "navigator.h" + +#include + +static constexpr int32_t DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE = 5; + +RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) : + RtlBase(navigator, -DEFAULT_MISSION_FAST_REVERSE_CACHE_SIZE) +{ + +} + +void RtlMissionFastReverse::on_activation() +{ + _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; + + if (_land_detected_sub.get().landed) { + // already landed, no need to do anything, invalidate the position mission item. + _is_current_planned_mission_item_valid = false; + } + + MissionBase::on_activation(); +} + +void RtlMissionFastReverse::on_active() +{ + _home_pos_sub.update(); + MissionBase::on_active(); +} + +void RtlMissionFastReverse::on_inactive() +{ + _home_pos_sub.update(); + MissionBase::on_inactive(); +} + +bool RtlMissionFastReverse::setNextMissionItem() +{ + return (goToPreviousPositionItem(true) == PX4_OK); +} + +void RtlMissionFastReverse::setActiveMissionItems() +{ + WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // 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 && + !_land_detected_sub.get().landed && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); + _mission_item.yaw = _navigator->get_local_position()->heading; + + // keep current setpoints (FW position controller generates wp to track during transition) + pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF; + + } else if (item_contains_position(_mission_item)) { + int32_t next_mission_item_index; + size_t num_found_items = 0; + getPreviousPositionItems(_mission.current_seq, &next_mission_item_index, num_found_items, 1u); + + // If the current item is a takeoff item or there is no further position item start landing. + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || + num_found_items == 0) { + handleLanding(new_work_item_type); + + } else { + // convert mission item to a simple waypoint, keep loiter to alt + if (_mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT) { + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (num_found_items > 0) { + + const dm_item_t dataman_id = static_cast(_mission.dataman_id); + mission_item_s next_mission_item; + bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index, + reinterpret_cast(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT); + + if (success) { + mission_apply_limitation(next_mission_item); + mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next); + } + } + + mission_apply_limitation(_mission_item); + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + } + + issue_command(_mission_item); + + /* set current work item type */ + _work_item_type = new_work_item_type; + + reset_mission_item_reached(); + + if (_mission_type == MissionType::MISSION_TYPE_MISSION) { + set_mission_result(); + } + + publish_navigator_mission_item(); // for logging + _navigator->set_position_setpoint_triplet_updated(); +} + +void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) +{ + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + bool needs_to_land = !_land_detected_sub.get().landed; + bool vtol_in_fw = _vehicle_status_sub.get().is_vtol && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING); + + if (needs_to_land) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) { + // Go to Take off location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_CLIMB; + + if (!PX4_ISFINITE(_mission_item.altitude)) { + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + } + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + if (vtol_in_fw) { + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB) { + // Go to home location + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + float altitude = _global_pos_sub.get().alt; + + if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + altitude = pos_sp_triplet->current.alt; + } + + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.altitude = altitude; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + _mission_item.vtol_back_transition = true; + + pos_sp_triplet->previous = pos_sp_triplet->current; + } + + /* transition to MC */ + if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) { + + set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC); + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION; + + // make previous setpoint invalid, such that there will be no prev-current line following + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + + } else if ((_work_item_type == WorkItemType::WORK_ITEM_TYPE_CLIMB || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND || + _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) { + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.lat = _home_pos_sub.get().lat; + _mission_item.lon = _home_pos_sub.get().lon; + _mission_item.yaw = NAN; + + if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && + do_need_move_to_land()) { + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND; + + _mission_item.altitude = _global_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0.0f; + + // make previous setpoint invalid, such that there will be no prev-current line following. + // if the vehicle drifted off the path during back-transition it should just go straight to the landing point + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + } else { + _mission_item.altitude = _home_pos_sub.get().alt; + _mission_item.altitude_is_relative = false; + _navigator->reset_position_setpoint(pos_sp_triplet->previous); + } + } + } +} + +bool RtlMissionFastReverse::do_need_move_to_land() +{ + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _global_pos_sub.get().lat, _global_pos_sub.get().lon); + + return d_current > _navigator->get_acceptance_radius(); + +} + +rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate() +{ + rtl_time_estimate_s time_estimate; + time_estimate.valid = false; + time_estimate.timestamp = hrt_absolute_time(); + + return time_estimate; +} diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h new file mode 100644 index 0000000000..acc2893b3a --- /dev/null +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -0,0 +1,71 @@ +/*************************************************************************** + * + * 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 rtl_mission_fast_reverse.h + * + * Helper class for RTL + * + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "rtl_base.h" + +#include +#include +#include + +class Navigator; + +class RtlMissionFastReverse : public RtlBase +{ +public: + RtlMissionFastReverse(Navigator *navigator); + ~RtlMissionFastReverse() = default; + + void on_activation() override; + void on_active() override; + void on_inactive() override; + + rtl_time_estimate_s calc_rtl_time_estimate() override; + +private: + bool setNextMissionItem() override; + void setActiveMissionItems() override; + void handleLanding(WorkItemType &new_work_item_type); + bool do_need_move_to_land(); + + 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 f998b9c1d9..6c5af0de3d 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -98,60 +98,31 @@ Takeoff::set_takeoff_position() { struct position_setpoint_triplet_s *rep = _navigator->get_takeoff_triplet(); - float abs_altitude = 0.0f; + float takeoff_altitude_amsl = 0.f; - float min_abs_altitude; - - // TODO: review this, comments are talking about home pos, the validity is checked but the - // current altitude is used instead. Also, the "else" case does not consider the current altitude at all. - if (_navigator->home_alt_valid()) { //only use home position if it is valid - min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt(); - - } else { //e.g. flow - min_abs_altitude = _navigator->get_takeoff_min_alt(); - } - - // Use altitude if it has been set. If home position is invalid use min_abs_altitude - events::LogLevel log_level = events::LogLevel::Disabled; - - if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_alt_valid()) { - abs_altitude = rep->current.alt; - - // If the altitude suggestion is lower than home + minimum clearance, raise it and complain. - if (abs_altitude < min_abs_altitude) { - if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm - mavlink_log_critical(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); - log_level = events::LogLevel::Warning; - } - - abs_altitude = min_abs_altitude; - } + if (rep->current.valid && PX4_ISFINITE(rep->current.alt)) { + takeoff_altitude_amsl = rep->current.alt; } else { - // Use home + minimum clearance but only notify. - abs_altitude = min_abs_altitude; + takeoff_altitude_amsl = _navigator->get_global_position()->alt + _navigator->get_param_mis_takeoff_alt(); mavlink_log_info(_navigator->get_mavlink_log_pub(), - "Using minimum takeoff altitude: %.2f m\t", (double)_navigator->get_takeoff_min_alt()); - log_level = events::LogLevel::Info; + "Using default takeoff altitude: %.1f m\t", (double)_navigator->get_param_mis_takeoff_alt()); + + events::send(events::ID("navigator_takeoff_default_alt"), {events::Log::Info, events::LogInternal::Info}, + "Using default takeoff altitude: {1:.2m}", + _navigator->get_param_mis_takeoff_alt()); } - if (log_level != events::LogLevel::Disabled) { - events::send(events::ID("navigator_takeoff_min_alt"), {log_level, events::LogInternal::Info}, - "Using minimum takeoff altitude: {1:.2m}", - _navigator->get_takeoff_min_alt()); - } - - if (abs_altitude < _navigator->get_global_position()->alt) { + if (takeoff_altitude_amsl < _navigator->get_global_position()->alt) { // If the suggestion is lower than our current alt, let's not go down. - abs_altitude = _navigator->get_global_position()->alt; + takeoff_altitude_amsl = _navigator->get_global_position()->alt; mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude\t"); events::send(events::ID("navigator_takeoff_already_higher"), {events::Log::Error, events::LogInternal::Info}, "Already higher than takeoff altitude (not descending)"); } // set current mission item to takeoff - set_takeoff_item(&_mission_item, abs_altitude); + set_takeoff_item(&_mission_item, takeoff_altitude_amsl); _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); reset_mission_item_reached(); @@ -181,12 +152,5 @@ Takeoff::set_takeoff_position() memset(rep, 0, sizeof(*rep)); } - if (PX4_ISFINITE(pos_sp_triplet->current.lat) && PX4_ISFINITE(pos_sp_triplet->current.lon)) { - _navigator->set_can_loiter_at_sp(true); - - } else { - _navigator->set_can_loiter_at_sp(false); - } - _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index af106a249d..45aeb30143 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -51,7 +51,7 @@ VtolTakeoff::VtolTakeoff(Navigator *navigator) : void VtolTakeoff::on_activation() { - if (_navigator->home_global_position_valid()) { + if (hrt_elapsed_time(&_navigator->get_global_position()->timestamp) < 1_s) { set_takeoff_position(); _takeoff_state = vtol_takeoff_state::TAKEOFF_HOVER; _navigator->reset_cruising_speed(); @@ -71,8 +71,8 @@ VtolTakeoff::on_active() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat, - _navigator->get_home_position()->lon, _loiter_location(0), _loiter_location(1))); + _mission_item.yaw = wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, + _mission_item.lon, _loiter_location(0), _loiter_location(1))); _mission_item.force_heading = true; mission_apply_limitation(_mission_item); mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); @@ -172,8 +172,8 @@ VtolTakeoff::set_takeoff_position() // set current mission item to takeoff set_takeoff_item(&_mission_item, _transition_alt_amsl); - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; _navigator->get_mission_result()->finished = false; _navigator->set_mission_result_updated(); diff --git a/src/modules/rc_update/RCUpdateTest.cpp b/src/modules/rc_update/RCUpdateTest.cpp index 393ffdae12..d0df1c28be 100644 --- a/src/modules/rc_update/RCUpdateTest.cpp +++ b/src/modules/rc_update/RCUpdateTest.cpp @@ -38,45 +38,121 @@ using namespace rc_update; -TEST(RCUpdateTest, ModeSlotUnassigned) +class TestRCUpdate : public RCUpdate +{ +public: + void UpdateManualSwitches(const hrt_abstime ×tamp_sample) { RCUpdate::UpdateManualSwitches(timestamp_sample); } + void updateParams() { RCUpdate::updateParams(); } + void setChannel(size_t index, float channel_value) { _rc.channels[index] = channel_value; } +}; + +class RCUpdateTest : public ::testing::Test, ModuleParams +{ +public: + RCUpdateTest() : ModuleParams(nullptr) + { + // Disable autosaving parameters to avoid busy loop in param_set() + param_control_autosave(false); + } + + void checkModeSlotSwitch(float channel_value, uint8_t expected_slot) + { + // GIVEN: First channel is configured as mode switch + _param_rc_map_fltmode.set(1); + _param_rc_map_fltmode.commit(); + EXPECT_EQ(_param_rc_map_fltmode.get(), 1); + _rc_update.updateParams(); + // GIVEN: First channel has some value + _rc_update.setChannel(0, channel_value); + + // WHEN: we update the switches two times to pass the simple outlier protection + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); + + // THEN: we receive the expected mode slot + uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; + EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot); + } + + void checkModeSlotButton(uint8_t button_bitmask, uint8_t channel, float channel_value, uint8_t expected_slot) + { + // GIVEN: No mode switch is mapped + _param_rc_map_fltmode.set(0); + _param_rc_map_fltmode.commit(); + EXPECT_EQ(_param_rc_map_fltmode.get(), 0); + // GIVEN: Buttons are configured + _param_rc_map_fltm_btn.set(button_bitmask); + _param_rc_map_fltm_btn.commit(); + EXPECT_EQ(_param_rc_map_fltm_btn.get(), button_bitmask); + _rc_update.updateParams(); + // GIVEN: First channel has some value + _rc_update.setChannel(channel - 1, channel_value); + + // WHEN: we update the switches 4 times: + // - initiate the button press + // - keep the same button pressed + // - hold the button for 50ms + // - pass the simple outlier protection + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(51_ms); + _rc_update.UpdateManualSwitches(51_ms); + + // THEN: we receive the expected mode slot + uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; + EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot); + + // Reset channel value for the next test + _rc_update.setChannel(channel - 1, 0.f); + } + + void checkReturnSwitch(float channel_value, float threshold, uint8_t expected_position) + { + // GIVEN: First channel is configured as return switch + _param_rc_map_return_sw.set(1); + _param_rc_map_return_sw.commit(); + _param_rc_return_th.set(threshold); + _param_rc_return_th.commit(); + _rc_update.updateParams(); + EXPECT_EQ(_param_rc_map_return_sw.get(), 1); + EXPECT_FLOAT_EQ(_param_rc_return_th.get(), threshold); + // GIVEN: First channel has some value + _rc_update.setChannel(0, channel_value); + + // WHEN: we update the switches two times to pass the simple outlier protection + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); + + // THEN: we receive the expected mode slot + uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; + EXPECT_EQ(manual_control_switches_sub.get().return_switch, expected_position); + } + + TestRCUpdate _rc_update; + + DEFINE_PARAMETERS( + (ParamInt) _param_rc_map_fltmode, + (ParamInt) _param_rc_map_fltm_btn, + (ParamInt) _param_rc_map_return_sw, + (ParamFloat) _param_rc_return_th + ) +}; + +TEST_F(RCUpdateTest, ModeSlotUnassigned) { - RCUpdate rc_update; // GIVEN: Default configuration with no assigned mode switch - EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 0); + EXPECT_EQ(_param_rc_map_fltmode.get(), 0); // WHEN: we update the switches two times to pass the simple outlier protection - rc_update.UpdateManualSwitches(0); - rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); // THEN: we receive no mode slot uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; - manual_control_switches_sub.update(); - EXPECT_EQ(manual_control_switches_sub.get().mode_slot, 0); // manual_control_switches_s::MODE_SLOT_NONE } -void checkModeSlotSwitch(float channel_value, uint8_t expected_slot) -{ - RCUpdate rc_update; - - // GIVEN: First channel is configured as mode switch - rc_update._param_rc_map_fltmode.set(1); - EXPECT_EQ(rc_update._param_rc_map_fltmode.get(), 1); - // GIVEN: First channel has some value - rc_update._rc.channels[0] = channel_value; - - // WHEN: we update the switches two times to pass the simple outlier protection - rc_update.UpdateManualSwitches(0); - rc_update.UpdateManualSwitches(0); - - // THEN: we receive the expected mode slot - uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; - manual_control_switches_sub.update(); - - EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot); -} - -TEST(RCUpdateTest, ModeSlotSwitchAllValues) +TEST_F(RCUpdateTest, ModeSlotSwitchAllValues) { checkModeSlotSwitch(-1.f, 1); // manual_control_switches_s::MODE_SLOT_1 checkModeSlotSwitch(-.5f, 2); // manual_control_switches_s::MODE_SLOT_2 @@ -86,36 +162,7 @@ TEST(RCUpdateTest, ModeSlotSwitchAllValues) checkModeSlotSwitch(1.f, 6); // manual_control_switches_s::MODE_SLOT_6 } -void checkModeSlotButton(uint8_t button_configuration, uint8_t channel, float channel_value, uint8_t expected_slot) -{ - RCUpdate rc_update; - - // GIVEN: Buttons are configured - rc_update._param_rc_map_fltm_btn.set(button_configuration); - EXPECT_EQ(rc_update._param_rc_map_fltm_btn.get(), button_configuration); - // GIVEN: buttons are mapped - rc_update.update_rc_functions(); - // GIVEN: First channel has some value - rc_update._rc.channels[channel - 1] = channel_value; - - // WHEN: we update the switches 4 times: - // - initiate the button press - // - keep the same button pressed - // - hold the button for 50ms - // - pass the simple outlier protection - rc_update.UpdateManualSwitches(0); - rc_update.UpdateManualSwitches(0); - rc_update.UpdateManualSwitches(51_ms); - rc_update.UpdateManualSwitches(51_ms); - - // THEN: we receive the expected mode slot - uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; - manual_control_switches_sub.update(); - - EXPECT_EQ(manual_control_switches_sub.get().mode_slot, expected_slot); -} - -TEST(RCUpdateTest, ModeSlotButtonAllValues) +TEST_F(RCUpdateTest, ModeSlotButtonAllValues) { checkModeSlotButton(1, 1, -1.f, 0); // button not pressed -> manual_control_switches_s::MODE_SLOT_NONE checkModeSlotButton(1, 1, 0.f, 0); // button not pressed over threshold -> manual_control_switches_s::MODE_SLOT_NONE @@ -131,3 +178,58 @@ TEST(RCUpdateTest, ModeSlotButtonAllValues) checkModeSlotButton(31, 6, 1.f, 0); // button 6 pressed but not configured -> manual_control_switches_s::MODE_SLOT_NONE checkModeSlotButton(63, 6, 1.f, 6); // button 6 pressed -> manual_control_switches_s::MODE_SLOT_6 } + +TEST_F(RCUpdateTest, ReturnSwitchUnassigned) +{ + // GIVEN: Default configuration with no assigned return switch + EXPECT_EQ(_param_rc_map_return_sw.get(), 0); + + // WHEN: we update the switches two times to pass the simple outlier protection + _rc_update.UpdateManualSwitches(0); + _rc_update.UpdateManualSwitches(0); + + // THEN: we receive an unmapped return switch state + uORB::SubscriptionData manual_control_switches_sub{ORB_ID(manual_control_switches)}; + EXPECT_EQ(manual_control_switches_sub.get().return_switch, 0); // manual_control_switches_s::SWITCH_POS_NONE +} + +TEST_F(RCUpdateTest, ReturnSwitchPositiveThresholds) +{ + checkReturnSwitch(-1.f, 0.5f, 3); // Below threshold -> SWITCH_POS_OFF + checkReturnSwitch(0.f, 0.5f, 3); // On threshold -> SWITCH_POS_OFF + checkReturnSwitch(.001f, 0.5f, 1); // Slightly above threshold -> SWITCH_POS_ON + checkReturnSwitch(1.f, 0.5f, 1); // Above threshold -> SWITCH_POS_ON + + checkReturnSwitch(-1.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF + checkReturnSwitch(0.f, 0.75f, 3); // Below threshold -> SWITCH_POS_OFF + checkReturnSwitch(.5f, 0.75f, 3); // On threshold -> SWITCH_POS_OFF + checkReturnSwitch(.501f, 0.75f, 1); // Slightly above threshold -> SWITCH_POS_ON + checkReturnSwitch(1.f, 0.75f, 1); // Above threshold -> SWITCH_POS_ON + + checkReturnSwitch(-1.f, 0.f, 3); // On minimum threshold -> SWITCH_POS_OFF + checkReturnSwitch(-.999f, 0.f, 1); // Slightly above minimum threshold -> SWITCH_POS_ON + checkReturnSwitch(1.f, 0.f, 1); // Above minimum threshold -> SWITCH_POS_ON + + checkReturnSwitch(-1.f, 1.f, 3); // Below maximum threshold -> SWITCH_POS_OFF + checkReturnSwitch(1.f, 1.f, 3); // On maximum threshold -> SWITCH_POS_OFF +} + +TEST_F(RCUpdateTest, ReturnSwitchNegativeThresholds) +{ + checkReturnSwitch(1.f, -0.5f, 3); // Above threshold -> SWITCH_POS_OFF + checkReturnSwitch(0.f, -0.5f, 3); // On threshold -> SWITCH_POS_OFF + checkReturnSwitch(-.001f, -0.5f, 1); // Slightly below threshold -> SWITCH_POS_ON + checkReturnSwitch(-1.f, -0.5f, 1); // Below threshold -> SWITCH_POS_ON + + checkReturnSwitch(1.f, -0.75f, 3); // Above threshold -> SWITCH_POS_OFF + checkReturnSwitch(.5f, -0.75f, 3); // On threshold -> SWITCH_POS_OFF + checkReturnSwitch(.499f, -0.75f, 1); // Slightly below threshold -> SWITCH_POS_ON + checkReturnSwitch(-1.f, -0.75f, 1); // Below threshold -> SWITCH_POS_ON + + checkReturnSwitch(1.f, -1.f, 3); // On maximum threshold -> SWITCH_POS_OFF + checkReturnSwitch(.999f, -1.f, 1); // Slighly below maximum threshold -> SWITCH_POS_ON + checkReturnSwitch(-1.f, -1.f, 1); // Below minimum threshold -> SWITCH_POS_ON + + checkReturnSwitch(1.f, -.001f, 3); // Above minimum threshold -> SWITCH_POS_OFF + checkReturnSwitch(-1.f, -.001f, 1); // Slightly below minimum threshold -> SWITCH_POS_OFF +} diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp index 365f3b9321..6aa4ffd20b 100644 --- a/src/modules/rc_update/rc_update.cpp +++ b/src/modules/rc_update/rc_update.cpp @@ -101,7 +101,7 @@ RCUpdate::RCUpdate() : } rc_parameter_map_poll(true /* forced */); - parameters_updated(); + updateParams(); // Call is needed to populate the _rc.function array _button_pressed_hysteresis.set_hysteresis_time_from(false, 50_ms); } @@ -123,8 +123,10 @@ bool RCUpdate::init() return true; } -void RCUpdate::parameters_updated() +void RCUpdate::updateParams() { + ModuleParams::updateParams(); + // rc values for (unsigned int i = 0; i < RC_MAX_CHAN_COUNT; i++) { float min = 0.f; @@ -388,7 +390,6 @@ void RCUpdate::Run() // update parameters from storage updateParams(); - parameters_updated(); } rc_parameter_map_poll(); @@ -560,19 +561,16 @@ void RCUpdate::Run() perf_end(_loop_perf); } -switch_pos_t RCUpdate::get_rc_sw2pos_position(uint8_t func, float on_th) const +switch_pos_t RCUpdate::getRCSwitchOnOffPosition(uint8_t function, float threshold) const { - if (_rc.function[func] >= 0) { - const bool on_inv = (on_th < 0.f); + if (_rc.function[function] >= 0) { + float value = 0.5f * _rc.channels[_rc.function[function]] + 0.5f; // Rescale [-1,1] -> [0,1] range - const float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f; - - if (on_inv ? value < on_th : value > on_th) { - return manual_control_switches_s::SWITCH_POS_ON; - - } else { - return manual_control_switches_s::SWITCH_POS_OFF; + if (threshold < 0.f) { + value = -value; } + + return (value > threshold) ? manual_control_switches_s::SWITCH_POS_ON : manual_control_switches_s::SWITCH_POS_OFF; } return manual_control_switches_s::SWITCH_POS_NONE; @@ -639,18 +637,18 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime ×tamp_sample) } } - switches.return_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get()); - switches.loiter_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get()); - switches.offboard_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get()); - switches.kill_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get()); - switches.arm_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get()); - switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get()); - switches.gear_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get()); - switches.engage_main_motor_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR, - _param_rc_eng_mot_th.get()); + switches.return_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_RETURN, _param_rc_return_th.get()); + switches.loiter_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_LOITER, _param_rc_loiter_th.get()); + switches.offboard_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_OFFBOARD, _param_rc_offb_th.get()); + switches.kill_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_KILLSWITCH, _param_rc_killswitch_th.get()); + switches.arm_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ARMSWITCH, _param_rc_armswitch_th.get()); + switches.transition_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get()); + switches.gear_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_GEAR, _param_rc_gear_th.get()); + switches.engage_main_motor_switch = + getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR, _param_rc_eng_mot_th.get()); #if defined(ATL_MANTIS_RC_INPUT_HACKS) - switches.photo_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_3, 0.5f); - switches.video_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_4, 0.5f); + switches.photo_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_3, 0.5f); + switches.video_switch = getRCSwitchOnOffPosition(rc_channels_s::FUNCTION_AUX_4, 0.5f); #endif // last 2 switch updates identical within 1 second (simple protection from bad RC data) diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h index 76ff66833e..edd9e18dfc 100644 --- a/src/modules/rc_update/rc_update.h +++ b/src/modules/rc_update/rc_update.h @@ -89,6 +89,7 @@ public: int print_status() override; +protected: static constexpr uint64_t VALID_DATA_MIN_INTERVAL_US{1_s / 3}; // assume valid RC input is at least 3 Hz void Run() override; @@ -109,7 +110,7 @@ public: /** * Update our local parameter cache. */ - void parameters_updated(); + void updateParams() override; /** * Get and limit value for specified RC function. Returns NAN if not mapped. @@ -117,15 +118,16 @@ public: float get_rc_value(uint8_t func, float min_value, float max_value) const; /** - * Get switch position for specified function. + * Get on/off switch position from the RC channel of the specified function + * + * @param function according to rc_channels_s::FUNCTION_XXX + * @param threshold according to RC_XXX_TH parameters, negative means on and off are flipped */ - switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th) const; + switch_pos_t getRCSwitchOnOffPosition(uint8_t function, float threshold) const; /** * Update parameters from RC channels if the functionality is activated and the * input has changed since the last update - * - * @param */ void set_params_from_rc(); diff --git a/src/modules/replay/Replay.cpp b/src/modules/replay/Replay.cpp index 6218b09e00..3f05082f9a 100644 --- a/src/modules/replay/Replay.cpp +++ b/src/modules/replay/Replay.cpp @@ -65,6 +65,7 @@ #include "ReplayEkf2.hpp" #define PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params.txt" +#define DYNAMIC_PARAMS_OVERRIDE_FILE PX4_ROOTFSDIR "/replay_params_dynamic.txt" using namespace std; using namespace time_literals; @@ -124,6 +125,38 @@ Replay::setupReplayFile(const char *file_name) _replay_file = strdup(file_name); } +void +Replay::setParameter(const string ¶meter_name, const double parameter_value) +{ + param_t handle = param_find(parameter_name.c_str()); + param_type_t param_format = param_type(handle); + + if (param_format == PARAM_TYPE_INT32) { + int32_t orig_value = 0; + param_get(handle, &orig_value); + + int32_t value = (int32_t)parameter_value; + + if (orig_value != value) { + PX4_WARN("Setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); + } + + param_set(handle, (const void *)&value); + + } else if (param_format == PARAM_TYPE_FLOAT) { + float orig_value = 0; + param_get(handle, &orig_value); + + float value = (float)parameter_value; + + if (fabsf(orig_value - value) > FLT_EPSILON) { + PX4_WARN("Setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); + } + + param_set(handle, (const void *)&value); + } +} + void Replay::setUserParams(const char *filename) { @@ -149,39 +182,59 @@ Replay::setUserParams(const char *filename) mystrstream >> pname; mystrstream >> value_string; - double param_value_double = stod(value_string); - - param_t handle = param_find(pname.c_str()); - param_type_t param_format = param_type(handle); _overridden_params.insert(pname); - if (param_format == PARAM_TYPE_INT32) { - int32_t orig_value = 0; - param_get(handle, &orig_value); + double param_value_double = stod(value_string); - int32_t value = (int32_t)param_value_double; - - if (orig_value != value) { - PX4_WARN("setting %s (INT32) %d -> %d", param_name(handle), orig_value, value); - } - - param_set(handle, (const void *)&value); - - } else if (param_format == PARAM_TYPE_FLOAT) { - float orig_value = 0; - param_get(handle, &orig_value); - - float value = (float)param_value_double; - - if (fabsf(orig_value - value) > FLT_EPSILON) { - PX4_WARN("setting %s (FLOAT) %.3f -> %.3f", param_name(handle), (double)orig_value, (double)value); - } - - param_set(handle, (const void *)&value); - } + setParameter(pname, param_value_double); } } +void +Replay::readDynamicParams(const char *filename) +{ + _dynamic_parameter_schedule.clear(); + + string line; + string param_name; + string value_string; + string time_string; + ifstream myfile(filename); + + if (!myfile.is_open()) { + return; + } + + PX4_INFO("Reading dynamic params from %s...", filename); + + while (!myfile.eof()) { + getline(myfile, line); + + if (line.empty() || line[0] == '#') { + continue; + } + + istringstream mystrstream(line); + mystrstream >> param_name; + mystrstream >> value_string; + mystrstream >> time_string; + + _dynamic_parameters.insert(param_name); + + double param_value = stod(value_string); + uint64_t change_timestamp = (uint64_t)(stod(time_string) * 1e6); + + // Construct and store parameter change event + ParameterChangeEvent change_event = {change_timestamp, param_name, param_value}; + _dynamic_parameter_schedule.push_back(change_event); + } + + // Sort by event time + sort(_dynamic_parameter_schedule.begin(), _dynamic_parameter_schedule.end()); + + _next_param_change = 0; +} + bool Replay::readFileHeader(std::ifstream &file) { @@ -611,7 +664,8 @@ Replay::readAndApplyParameter(std::ifstream &file, uint16_t msg_size) string type = key.substr(0, pos); string param_name = key.substr(pos + 1); - if (_overridden_params.find(param_name) != _overridden_params.end()) { + if (_overridden_params.find(param_name) != _overridden_params.end() || + _dynamic_parameters.find(param_name) != _dynamic_parameters.end()) { //this parameter is overridden, so don't apply it return true; } @@ -826,6 +880,7 @@ Replay::readDefinitionsAndApplyParams(std::ifstream &file) } setUserParams(PARAMS_OVERRIDE_FILE); + readDynamicParams(DYNAMIC_PARAMS_OVERRIDE_FILE); return true; } @@ -896,7 +951,7 @@ Replay::run() Subscription &sub = *_subscriptions[next_msg_id]; - if (next_file_time == 0) { + if (next_file_time == 0 || next_file_time < _file_start_time) { //someone didn't set the timestamp properly. Consider the message invalid nextDataMessage(replay_file, sub, next_msg_id); continue; @@ -908,6 +963,17 @@ Replay::run() readAndHandleAdditionalMessages(replay_file, next_additional_message_pos); last_additional_message_pos = next_additional_message_pos; + // Perform scheduled parameter changes + while (_next_param_change < _dynamic_parameter_schedule.size() && + _dynamic_parameter_schedule[_next_param_change].timestamp <= next_file_time) { + const auto param_change = _dynamic_parameter_schedule[_next_param_change]; + PX4_WARN("Performing param change scheduled for t=%.3lf at t=%.3lf.", + (double)param_change.timestamp / 1.e6, + (double)next_file_time / 1.e6); + setParameter(param_change.parameter_name, param_change.parameter_value); + _next_param_change++; + } + const uint64_t publish_timestamp = handleTopicDelay(next_file_time, timestamp_offset); // It's time to publish diff --git a/src/modules/replay/Replay.hpp b/src/modules/replay/Replay.hpp index 94ecc9d416..ef6f0ee97b 100644 --- a/src/modules/replay/Replay.hpp +++ b/src/modules/replay/Replay.hpp @@ -33,6 +33,7 @@ #pragma once +#include #include #include #include @@ -220,6 +221,23 @@ protected: private: std::set _overridden_params; + + struct ParameterChangeEvent { + uint64_t timestamp; + std::string parameter_name; + double parameter_value; + + // Comparison operator such that sorting is done by timestamp + bool operator<(const ParameterChangeEvent &other) const + { + return timestamp < other.timestamp; + } + }; + + std::set _dynamic_parameters; + std::vector _dynamic_parameter_schedule; + size_t _next_param_change; + std::map _file_formats; ///< all formats we read from the file uint64_t _file_start_time; @@ -275,7 +293,9 @@ private: /** get the size of a type that can be an array */ static size_t sizeOfFullType(const std::string &type_name_full); + void setParameter(const std::string ¶meter_name, const double parameter_value); void setUserParams(const char *filename); + void readDynamicParams(const char *filename); std::string parseOrbFields(const std::string &fields); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 9a0a7d2c20..24909280e9 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -138,7 +138,7 @@ RoverPositionControl::manual_control_setpoint_poll() } _att_sp.yaw_body = _manual_yaw_sp; - _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; + _att_sp.thrust_body[0] = _manual_control_setpoint.throttle; Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -152,7 +152,7 @@ RoverPositionControl::manual_control_setpoint_poll() // Set heading from the manual roll input channel _yaw_control = _manual_control_setpoint.roll; // Nominally yaw: _manual_control_setpoint.yaw; // Set throttle from the manual throttle channel - _throttle_control = (_manual_control_setpoint.throttle + 1.f) * .5f; + _throttle_control = _manual_control_setpoint.throttle; _reset_yaw_sp = true; } diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml index 70670f882c..5fa440d5ff 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -360,6 +360,7 @@ parameters: short: Magnetometer ${i} rotation relative to airframe long: | An internal sensor will force a value of -1, so a GCS should only attempt to configure the rotation if the value is greater than or equal to zero. + Set to "Custom Euler Angle" to define the rotation using CAL_MAG${i}_ROLL, CAL_MAG${i}_PITCH and CAL_MAG${i}_YAW. category: System type: enum values: @@ -405,12 +406,52 @@ parameters: 38: Roll 90°, Pitch 68°, Yaw 293° 39: Pitch 315° 40: Roll 90°, Pitch 315° + 100: Custom Euler Angle min: -1 - max: 40 + max: 100 default: -1 num_instances: *max_num_sensor_instances instance_start: 0 + CAL_MAG${i}_ROLL: + description: + short: Magnetometer ${i} Custom Euler Roll Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + CAL_MAG${i}_PITCH: + description: + short: Magnetometer ${i} Custom Euler Pitch Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + + CAL_MAG${i}_YAW: + description: + short: Magnetometer ${i} Custom Euler Yaw Angle + long: Setting this parameter changes CAL_MAG${i}_ROT to "Custom Euler Angle" + category: System + type: float + default: 0.0 + min: -180 + max: 180 + unit: deg + num_instances: *max_num_sensor_instances + instance_start: 0 + CAL_MAG${i}_XOFF: description: short: Magnetometer ${i} X-axis offset diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index 0ecdc0a4e4..3673ca14bb 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -70,7 +70,7 @@ void GpsBlending::update(uint64_t hrt_now_us) // Check for new data on selected GPS, and clear blend offsets for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { _NE_pos_offset_m[i].zero(); - _hgt_offset_mm[i] = 0.0f; + _hgt_offset_m[i] = 0.0; } // Only use a secondary instance if the fallback is allowed @@ -443,38 +443,38 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS // Convert each GPS position to a local NEU offset relative to the reference position Vector2f blended_NE_offset_m{0, 0}; - float blended_alt_offset_mm = 0.0f; + double blended_alt_offset_m = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if ((blend_weights[i] > 0.0f) && (i != gps_best_index)) { // calculate the horizontal offset Vector2f horiz_offset{}; - get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), - (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, + _gps_state[i].latitude_deg, _gps_state[i].longitude_deg, &horiz_offset(0), &horiz_offset(1)); // sum weighted offsets blended_NE_offset_m += horiz_offset * blend_weights[i]; - // calculate vertical offset - float vert_offset = (float)(_gps_state[i].alt - gps_blended_state.alt); + // calculate vertical offset, meters + double vert_offset_m = _gps_state[i].altitude_msl_m - gps_blended_state.altitude_msl_m; // sum weighted offsets - blended_alt_offset_mm += vert_offset * blend_weights[i]; + blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; } } // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; - const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + const double lat_deg_now = gps_blended_state.latitude_deg; + const double lon_deg_now = gps_blended_state.longitude_deg; double lat_deg_res = 0; double lon_deg_res = 0; add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, &lon_deg_res); - gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); - gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); - gps_blended_state.alt += (int32_t)blended_alt_offset_mm; + gps_blended_state.latitude_deg = lat_deg_res; + gps_blended_state.longitude_deg = lon_deg_res; + gps_blended_state.altitude_msl_m += blended_alt_offset_m; // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value int8_t gps_best_yaw_index = -1; @@ -516,30 +516,30 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) // Calculate a filtered position delta for each GPS relative to the blended solution state for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, + gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, &offset(0), &offset(1)); _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); - _hgt_offset_mm[i] = (float)(gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + - _hgt_offset_mm[i] * (1.0f - alpha[i]); + _hgt_offset_m[i] = (gps_blended_state.altitude_msl_m - _gps_state[i].altitude_msl_m) * (double)alpha[i] + + _hgt_offset_m[i] * (1.0 - (double)alpha[i]); } // calculate offset limits from the largest difference between receivers Vector2f max_ne_offset{}; - float max_alt_offset = 0; + double max_alt_offset = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { for (uint8_t j = i; j < GPS_MAX_RECEIVERS_BLEND; j++) { if (i != j) { Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), + get_vector_to_next_waypoint(_gps_state[i].latitude_deg, _gps_state[i].longitude_deg, + _gps_state[j].latitude_deg, _gps_state[j].longitude_deg, &offset(0), &offset(1)); - max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); - max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); - max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); + max_ne_offset(0) = fmax(max_ne_offset(0), fabsf(offset(0))); + max_ne_offset(1) = fmax(max_ne_offset(1), fabsf(offset(1))); + max_alt_offset = fmax(max_alt_offset, fabs(_gps_state[i].altitude_msl_m - _gps_state[j].altitude_msl_m)); } } } @@ -548,7 +548,7 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state) for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); - _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); + _hgt_offset_m[i] = constrain(_hgt_offset_m[i], -max_alt_offset, max_alt_offset); } } @@ -558,26 +558,26 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, // Convert each GPS position to a local NEU offset relative to the reference position // which is defined as the positon of the blended solution calculated from non offset corrected data Vector2f blended_NE_offset_m{0, 0}; - float blended_alt_offset_mm = 0.0f; + double blended_alt_offset_m = 0.0; for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { if (blend_weights[i] > 0.0f) { // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_orig = (double)_gps_state[i].lat * 1.0e-7; - const double lon_deg_orig = (double)_gps_state[i].lon * 1.0e-7; + const double lat_deg_orig = _gps_state[i].latitude_deg; + const double lon_deg_orig = _gps_state[i].longitude_deg; double lat_deg_offset_res = 0; double lon_deg_offset_res = 0; add_vector_to_global_position(lat_deg_orig, lon_deg_orig, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_offset_res, &lon_deg_offset_res); - float alt_offset = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; + double alt_offset_m = _gps_state[i].altitude_msl_m + _hgt_offset_m[i]; // calculate the horizontal offset Vector2f horiz_offset{}; - get_vector_to_next_waypoint((gps_blended_state.lat / 1.0e7), (gps_blended_state.lon / 1.0e7), + get_vector_to_next_waypoint(gps_blended_state.latitude_deg, gps_blended_state.longitude_deg, lat_deg_offset_res, lon_deg_offset_res, &horiz_offset(0), &horiz_offset(1)); @@ -585,23 +585,23 @@ void GpsBlending::calc_gps_blend_output(sensor_gps_s &gps_blended_state, blended_NE_offset_m += horiz_offset * blend_weights[i]; // calculate vertical offset - float vert_offset = alt_offset - gps_blended_state.alt; + double vert_offset_m = alt_offset_m - gps_blended_state.altitude_msl_m; // sum weighted offsets - blended_alt_offset_mm += vert_offset * blend_weights[i]; + blended_alt_offset_m += vert_offset_m * (double)blend_weights[i]; } } // Add the sum of weighted offsets to the reference position to obtain the blended position - const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7; - const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7; + const double lat_deg_now = gps_blended_state.latitude_deg; + const double lon_deg_now = gps_blended_state.longitude_deg; double lat_deg_res = 0; double lon_deg_res = 0; add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, &lon_deg_res); - gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); - gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); - gps_blended_state.alt = gps_blended_state.alt + (int32_t)blended_alt_offset_mm; + gps_blended_state.latitude_deg = lat_deg_res; + gps_blended_state.longitude_deg = lon_deg_res; + gps_blended_state.altitude_msl_m = gps_blended_state.altitude_msl_m + blended_alt_offset_m; } diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp index fa52806632..6d33a643cd 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.hpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.hpp @@ -131,7 +131,7 @@ private: bool _is_new_output_data_available{false}; matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m) - float _hgt_offset_mm[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm) + double _hgt_offset_m[GPS_MAX_RECEIVERS_BLEND] {}; ///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (meters) uint64_t _time_prev_us[GPS_MAX_RECEIVERS_BLEND] {}; ///< the previous value of time_us for that GPS instance - used to detect new data. uint8_t _gps_time_ref_index{0}; ///< index of the receiver that is used as the timing reference for the blending update diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp index 1baaf1da42..af5b1feab7 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending_test.cpp @@ -60,10 +60,10 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData() sensor_gps_s gps_data{}; gps_data.timestamp = _time_now_us - 10e3; gps_data.time_utc_usec = 0; - gps_data.lat = 47e7; - gps_data.lon = 9e7; - gps_data.alt = 800e3; - gps_data.alt_ellipsoid = 800e3; + gps_data.latitude_deg = 47.0; + gps_data.longitude_deg = 9.0; + gps_data.altitude_msl_m = 800.0; + gps_data.altitude_ellipsoid_m = 800.0; gps_data.s_variance_m_s = 0.2f; gps_data.c_variance_rad = 0.5f; gps_data.eph = 0.7f; @@ -213,9 +213,9 @@ TEST_F(GpsBlendingTest, dualReceiverBlendingHPos) EXPECT_FLOAT_EQ(gps_blending.getOutputGpsData().eph, gps_data1.eph); // TODO: should be greater than EXPECT_EQ(gps_blending.getOutputGpsData().timestamp, gps_data0.timestamp); EXPECT_EQ(gps_blending.getOutputGpsData().timestamp_sample, gps_data0.timestamp_sample); - EXPECT_EQ(gps_blending.getOutputGpsData().lat, gps_data0.lat); - EXPECT_EQ(gps_blending.getOutputGpsData().lon, gps_data0.lon); - EXPECT_EQ(gps_blending.getOutputGpsData().alt, gps_data0.alt); + EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); + EXPECT_EQ(gps_blending.getOutputGpsData().latitude_deg, gps_data0.latitude_deg); + EXPECT_EQ(gps_blending.getOutputGpsData().altitude_msl_m, gps_data0.altitude_msl_m); } TEST_F(GpsBlendingTest, dualReceiverFailover) diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index a0ce7de6ae..6bb7c5ef5b 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -259,7 +259,7 @@ void VehicleMagnetometer::UpdateMagCalibration() // State variance assumed for magnetometer bias storage. // This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value. // Larger values cause a larger fraction of the learned biases to be used. - static constexpr float magb_vref = 2.5e-7f; + static constexpr float magb_vref = 2.5e-6f; static constexpr float min_var_allowed = magb_vref * 0.01f; static constexpr float max_var_allowed = magb_vref * 500.f; @@ -275,10 +275,10 @@ void VehicleMagnetometer::UpdateMagCalibration() const Vector3f bias_variance{estimator_sensor_bias.mag_bias_variance}; const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s) - && (estimator_sensor_bias.mag_device_id != 0) && - estimator_sensor_bias.mag_bias_valid && - estimator_sensor_bias.mag_bias_stable && - (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); + && (estimator_sensor_bias.mag_device_id != 0) + && estimator_sensor_bias.mag_bias_valid + && estimator_sensor_bias.mag_bias_stable + && (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); if (valid) { // find corresponding mag calibration @@ -288,8 +288,20 @@ void VehicleMagnetometer::UpdateMagCalibration() _mag_cal[i].device_id = estimator_sensor_bias.mag_device_id; // readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias) - _mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]); + const Vector3f mag_cal_offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + + _calibration_estimator_bias[mag_index]); + if ((mag_cal_offset - _mag_cal[i].offset).longerThan(0.001f)) { + const Vector3f mag_cal_orig{_calibration[mag_index].offset()}; + + PX4_DEBUG("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), + (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), + (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); + } + + _mag_cal[i].offset = mag_cal_offset; _mag_cal[i].variance = bias_variance; _in_flight_mag_cal_available = true; @@ -329,7 +341,7 @@ void VehicleMagnetometer::UpdateMagCalibration() if (_calibration[mag_index].set_offset(mag_cal_offset)) { - PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])", + PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])", mag_index, _calibration[mag_index].device_id(), i, (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), @@ -340,6 +352,14 @@ void VehicleMagnetometer::UpdateMagCalibration() _calibration_estimator_bias[mag_index].zero(); calibration_param_save_needed = true; + + } else { + // new offset not saved + PX4_DEBUG("%d (%" PRIu32 ") EST:%d rejected: [%.3f, %.3f, %.3f]->[%.3f, %.3f, %.3f] (full [%.3f, %.3f, %.3f])", + mag_index, _calibration[mag_index].device_id(), i, + (double)mag_cal_orig(0), (double)mag_cal_orig(1), (double)mag_cal_orig(2), + (double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2), + (double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2)); } } } diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp index 87f356781e..fbebe07ad1 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp @@ -112,15 +112,16 @@ void VehicleOpticalFlow::Run() if (_sensor_flow_sub.update(&sensor_optical_flow)) { // clear data accumulation if there's a gap in data - if (((sensor_optical_flow.timestamp_sample - _flow_timestamp_sample_last) - > sensor_optical_flow.integration_timespan_us * 1.5f) - || (_accumulated_count > 0 && _quality_sum == 0)) { + const uint64_t integration_gap_threshold_us = sensor_optical_flow.integration_timespan_us * 2; + + if ((sensor_optical_flow.timestamp_sample >= _flow_timestamp_sample_last + integration_gap_threshold_us) + || (_accumulated_count > 0 && (sensor_optical_flow.quality > 0) && _quality_sum == 0)) { + ClearAccumulatedData(); } - const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - lroundf( - sensor_optical_flow.integration_timespan_us); + const hrt_abstime timestamp_oldest = sensor_optical_flow.timestamp_sample - sensor_optical_flow.integration_timespan_us; const hrt_abstime timestamp_newest = sensor_optical_flow.timestamp; // delta angle @@ -203,12 +204,7 @@ void VehicleOpticalFlow::Run() const float interval_us = 1e6f / _param_sens_flow_rate.get(); // don't allow publishing faster than SENS_FLOW_RATE - if (sensor_optical_flow.timestamp_sample < _last_publication_timestamp + interval_us) { - publish = false; - } - - // integrate for full interval unless we haven't published recently - if ((hrt_elapsed_time(&_last_publication_timestamp) < 1_ms) && (_integration_timespan_us < interval_us)) { + if (_integration_timespan_us < interval_us) { publish = false; } } @@ -271,8 +267,6 @@ void VehicleOpticalFlow::Run() vehicle_optical_flow.timestamp = hrt_absolute_time(); _vehicle_optical_flow_pub.publish(vehicle_optical_flow); - _last_publication_timestamp = vehicle_optical_flow.timestamp_sample; - // vehicle_optical_flow_vel if distance is available (for logging) if (_distance_sum_count > 0 && PX4_ISFINITE(_distance_sum)) { diff --git a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp index 186af594a8..fb424ea5af 100644 --- a/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp +++ b/src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.hpp @@ -117,8 +117,6 @@ private: uint16_t _quality_sum{0}; uint8_t _accumulated_count{0}; - hrt_abstime _last_publication_timestamp{0}; - int _distance_sensor_selected{-1}; // because we can have several distance sensor instances with different orientations hrt_abstime _last_range_sensor_update{0}; diff --git a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp index 38eeadf11f..c1a88f7bda 100644 --- a/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp +++ b/src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp @@ -53,6 +53,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name) return false; } + _esc_status_pub.advertise(); + ScheduleNow(); return true; diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 1e1b5be22c..5641706345 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -116,7 +116,7 @@ void SensorGpsSim::Run() double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH); - float altitude = gpos.alt + (generate_wgn() * 0.5f); + double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f)); Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f); @@ -132,7 +132,7 @@ void SensorGpsSim::Run() if (_sim_gps_used.get() >= 4) { // fix sensor_gps.fix_type = 3; // 3D fix - sensor_gps.s_variance_m_s = 0.5f; + sensor_gps.s_variance_m_s = 0.4f; sensor_gps.c_variance_rad = 0.1f; sensor_gps.eph = 0.9f; sensor_gps.epv = 1.78f; @@ -153,10 +153,10 @@ void SensorGpsSim::Run() sensor_gps.timestamp_sample = gpos.timestamp_sample; sensor_gps.time_utc_usec = 0; sensor_gps.device_id = device_id.devid; - sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees - sensor_gps.lon = roundf(longitude * 1e7); // Longitude in 1E-7 degrees - sensor_gps.alt = roundf(altitude * 1000.f); // Altitude in 1E-3 meters above MSL, (millimetres) - sensor_gps.alt_ellipsoid = sensor_gps.alt; + sensor_gps.latitude_deg = latitude; // Latitude in degrees + sensor_gps.longitude_deg = longitude; // Longitude in degrees + sensor_gps.altitude_msl_m = altitude; // Altitude in meters above MSL + sensor_gps.altitude_ellipsoid_m = altitude; sensor_gps.noise_per_ms = 0; sensor_gps.jamming_indicator = 0; sensor_gps.vel_m_s = sqrtf(gps_vel(0) * gps_vel(0) + gps_vel(1) * gps_vel(1)); // GPS ground speed, (metres/sec) diff --git a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp index e3bc981805..43c0dfaa59 100644 --- a/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp +++ b/src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp @@ -96,6 +96,8 @@ SimulatorMavlink::SimulatorMavlink() : snprintf(param_name, sizeof(param_name), "%s_%s%d", "PWM_MAIN", "FUNC", i + 1); param_get(param_find(param_name), &_output_functions[i]); } + + _esc_status_pub.advertise(); } void SimulatorMavlink::parameters_update(bool force) @@ -402,10 +404,10 @@ void SimulatorMavlink::handle_message_hil_gps(const mavlink_message_t *msg) if (!_gps_blocked) { sensor_gps_s gps{}; - gps.lat = hil_gps.lat; - gps.lon = hil_gps.lon; - gps.alt = hil_gps.alt; - gps.alt_ellipsoid = hil_gps.alt; + gps.latitude_deg = hil_gps.lat / 1e7; + gps.longitude_deg = hil_gps.lon / 1e7; + gps.altitude_msl_m = hil_gps.alt / 1e3; + gps.altitude_ellipsoid_m = hil_gps.alt / 1e3; gps.s_variance_m_s = 0.25f; gps.c_variance_rad = 0.5f; diff --git a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake index 7b73e03f75..37e4014d20 100644 --- a/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake +++ b/src/modules/simulation/simulator_mavlink/sitl_targets_gazebo-classic.cmake @@ -81,6 +81,8 @@ if(gazebo_FOUND) iris_foggy_lidar iris_irlock iris_obs_avoid + iris_depth_camera + iris_downward_depth_camera iris_opt_flow iris_opt_flow_mockup iris_rplidar diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index ee666d4757..1fa076568b 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -546,6 +546,7 @@ void Sih::publish_ground_truth(const hrt_abstime &time_now_us) local_position.heading = Eulerf(_q).psi(); local_position.heading_good_for_control = true; + local_position.unaided_heading = NAN; local_position.timestamp = hrt_absolute_time(); _local_position_ground_truth_pub.publish(local_position); @@ -645,7 +646,7 @@ int Sih::task_spawn(int argc, char *argv[]) _task_id = px4_task_spawn_cmd("sih", SCHED_DEFAULT, SCHED_PRIORITY_MAX, - 1250, + 1560, (px4_main_t)&run_trampoline, (char *const *)argv); diff --git a/src/modules/temperature_compensation/temperature_calibration/mag.cpp b/src/modules/temperature_compensation/temperature_calibration/mag.cpp index 3609d94f12..714e928af6 100644 --- a/src/modules/temperature_compensation/temperature_calibration/mag.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/mag.cpp @@ -212,7 +212,7 @@ int TemperatureCalibrationMag::finish_sensor_instance(PerSensorData &data, int s for (unsigned axis_index = 0; axis_index < 3; axis_index++) { for (unsigned coef_index = 0; coef_index <= 3; coef_index++) { - sprintf(str, "TC_M%d_X%d_%d", sensor_index, 3 - coef_index, axis_index); + snprintf(str, sizeof(str), "TC_M%d_X%d_%d", sensor_index, 3 - coef_index, axis_index); param = (float)res[axis_index][coef_index]; result = param_set_no_notification(param_find(str), ¶m); diff --git a/src/modules/temperature_compensation/temperature_calibration/task.cpp b/src/modules/temperature_compensation/temperature_calibration/task.cpp index 4262b4a0a2..a447390d5a 100644 --- a/src/modules/temperature_compensation/temperature_calibration/task.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/task.cpp @@ -284,7 +284,7 @@ void TemperatureCalibration::task_main() } param_notify_changes(); - int ret = param_save_default(); + int ret = param_save_default(true); if (ret != 0) { PX4_ERR("Failed to save params (%i)", ret); diff --git a/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client b/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client index 4248559f3b..711aef423e 160000 --- a/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client +++ b/src/modules/uxrce_dds_client/Micro-XRCE-DDS-Client @@ -1 +1 @@ -Subproject commit 4248559f3b111155c783e524e461ccc83e768103 +Subproject commit 711aef423edd1820347b866d1e4164832df35d04 diff --git a/src/modules/uxrce_dds_client/dds_topics.h.em b/src/modules/uxrce_dds_client/dds_topics.h.em index dc79e0fabd..9e4be9832e 100644 --- a/src/modules/uxrce_dds_client/dds_topics.h.em +++ b/src/modules/uxrce_dds_client/dds_topics.h.em @@ -21,68 +21,104 @@ import os #include #include -#include +#include #include +#include @[for include in type_includes]@ #include +#include @[end for]@ +#define UXRCE_DEFAULT_POLL_RATE 10 + +typedef bool (*UcdrSerializeMethod)(const void* data, ucdrBuffer& buf, int64_t time_offset); + +static constexpr int max_topic_size = 512; +@[ for pub in publications]@ +static_assert(sizeof(@(pub['simple_base_type'])_s) <= max_topic_size, "topic too large, increase max_topic_size"); +@[ end for]@ + +struct SendSubscription { + const struct orb_metadata *orb_meta; + uxrObjectId data_writer; + const char* dds_type_name; + uint32_t topic_size; + UcdrSerializeMethod ucdr_serialize_method; +}; + // Subscribers for messages to send struct SendTopicsSubs { + SendSubscription send_subscriptions[@(len(publications))] = { @[ for pub in publications]@ - uORB::Subscription @(pub['topic_simple'])_sub{ORB_ID(@(pub['topic_simple']))}; - uxrObjectId @(pub['topic_simple'])_data_writer{}; + { ORB_ID(@(pub['topic_simple'])), + uxr_object_id(0, UXR_INVALID_ID), + "@(pub['dds_type'])", + ucdr_topic_size_@(pub['simple_base_type'])(), + &ucdr_serialize_@(pub['simple_base_type']), + }, @[ end for]@ + }; + + px4_pollfd_struct_t fds[@(len(publications))] {}; uint32_t num_payload_sent{}; + void init(); void update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace); void reset(); }; +void SendTopicsSubs::init() { + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + fds[idx].fd = orb_subscribe(send_subscriptions[idx].orb_meta); + fds[idx].events = POLLIN; + orb_set_interval(fds[idx].fd, UXRCE_DEFAULT_POLL_RATE); + } +} + void SendTopicsSubs::reset() { num_payload_sent = 0; -@[ for idx, pub in enumerate(publications)]@ - @(pub['topic_simple'])_data_writer = uxr_object_id(0, UXR_INVALID_ID); -@[ end for]@ + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + send_subscriptions[idx].data_writer = uxr_object_id(0, UXR_INVALID_ID); + } }; void SendTopicsSubs::update(uxrSession *session, uxrStreamId reliable_out_stream_id, uxrStreamId best_effort_stream_id, uxrObjectId participant_id, const char *client_namespace) { int64_t time_offset_us = session->time_offset / 1000; // ns -> us -@[ for idx, pub in enumerate(publications)]@ - { - @(pub['simple_base_type'])_s data; + alignas(sizeof(uint64_t)) char topic_data[max_topic_size]; - if (@(pub['topic_simple'])_sub.update(&data)) { - - if (@(pub['topic_simple'])_data_writer.id == UXR_INVALID_ID) { + for (unsigned idx = 0; idx < sizeof(send_subscriptions)/sizeof(send_subscriptions[0]); ++idx) { + if (fds[idx].revents & POLLIN) { + // Topic updated, copy data and send + 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, ORB_ID::@(pub['topic_simple']), client_namespace, "@(pub['topic_simple'])", "@(pub['dds_type'])", @(pub['topic_simple'])_data_writer); + 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, + send_subscriptions[idx].dds_type_name, send_subscriptions[idx].data_writer); } - if (@(pub['topic_simple'])_data_writer.id != UXR_INVALID_ID) { + if (send_subscriptions[idx].data_writer.id != UXR_INVALID_ID) { ucdrBuffer ub; - uint32_t topic_size = ucdr_topic_size_@(pub['simple_base_type'])(); - if (uxr_prepare_output_stream(session, best_effort_stream_id, @(pub['topic_simple'])_data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { - ucdr_serialize_@(pub['simple_base_type'])(data, ub, time_offset_us); + uint32_t topic_size = send_subscriptions[idx].topic_size; + if (uxr_prepare_output_stream(session, best_effort_stream_id, send_subscriptions[idx].data_writer, &ub, topic_size) != UXR_INVALID_REQUEST_ID) { + send_subscriptions[idx].ucdr_serialize_method(&topic_data, ub, time_offset_us); // TODO: fill up the MTU and then flush, which reduces the packet overhead uxr_flash_output_streams(session); num_payload_sent += topic_size; } else { - //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID @(pub['topic_simple'])"); + //PX4_ERR("Error uxr_prepare_output_stream UXR_INVALID_REQUEST_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); } } else { - //PX4_ERR("Error UXR_INVALID_ID @(pub['topic_simple'])"); + //PX4_ERR("Error UXR_INVALID_ID %s", send_subscriptions[idx].subscription.get_topic()->o_name); } } } -@[ end for]@ } // Publishers for received messages @@ -105,7 +141,7 @@ static void on_topic_update(uxrSession *session, uxrObjectId object_id, uint16_t switch (object_id.id) { @[ for idx, sub in enumerate(subscriptions)]@ - case @(idx)+1000: { + case @(idx)+ (65535U / 32U) + 1: { @(sub['simple_base_type'])_s data; if (ucdr_deserialize_@(sub['simple_base_type'])(*ub, data, time_offset_us)) { diff --git a/src/modules/uxrce_dds_client/module.yaml b/src/modules/uxrce_dds_client/module.yaml index 739ae43af1..77078e560a 100644 --- a/src/modules/uxrce_dds_client/module.yaml +++ b/src/modules/uxrce_dds_client/module.yaml @@ -64,3 +64,14 @@ parameters: reboot_required: true default: 2130706433 requires_ethernet: true + + UXRCE_DDS_SYNCT: + description: + short: uXRCE-DDS timestamp synchronization enable + long: When enabled, uxrce_dds_client will synchronize the timestamps + of the incoming and outgoing messages measuring the offset + between the Agent OS time and the PX4 time. + type: boolean + category: System + reboot_required: true + default: 1 diff --git a/src/modules/uxrce_dds_client/utilities.hpp b/src/modules/uxrce_dds_client/utilities.hpp index 91e1996fb8..5c7fb2e9dc 100644 --- a/src/modules/uxrce_dds_client/utilities.hpp +++ b/src/modules/uxrce_dds_client/utilities.hpp @@ -10,9 +10,13 @@ uxrObjectId topic_id_from_orb(ORB_ID orb_id, uint8_t instance = 0) { - if (orb_id != ORB_ID::INVALID) { - uint16_t id = static_cast(orb_id) + (instance * UINT8_MAX); - uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); + // Note that the uxrObjectId.id is a uint16_t so we need to cap the ID, + // and urx does not allow us to use the upper 4 bits. + const unsigned max_id = 65535U / 32U; + const unsigned id = static_cast(orb_id) + (instance * ORB_TOPICS_COUNT); + + if (orb_id != ORB_ID::INVALID && id < max_id) { + uxrObjectId topic_id = uxr_object_id(static_cast(id), UXR_TOPIC_ID); return topic_id; } @@ -94,8 +98,9 @@ static bool create_data_reader(uxrSession *session, uxrStreamId reliable_out_str return false; } - uint16_t id = index + 1000; - + // Use the second half of the available ID space. + // Add 1 so that we get a nice hex starting number: 0x800 instead of 0x7ff. + uint16_t id = index + (65535U / 32U) + 1; uxrObjectId topic_id = uxr_object_id(id, UXR_TOPIC_ID); uint16_t topic_req = uxr_buffer_create_topic_bin(session, reliable_out_stream_id, topic_id, participant_id, topic_name, diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp index b9f1d066e4..f4c9cfd0ce 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.cpp +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.cpp @@ -79,11 +79,18 @@ void on_time(uxrSession *session, int64_t current_time, int64_t received_timesta } } +void on_time_no_sync(uxrSession *session, int64_t current_time, int64_t received_timestamp, int64_t transmit_timestamp, + int64_t originate_timestamp, void *args) +{ + session->time_offset = 0; +} + UxrceddsClient::UxrceddsClient(Transport transport, const char *device, int baudrate, const char *agent_ip, - const char *port, bool localhost_only, bool custom_participant, const char *client_namespace) : + const char *port, bool localhost_only, bool custom_participant, const char *client_namespace, + bool synchronize_timestamps) : ModuleParams(nullptr), _localhost_only(localhost_only), _custom_participant(custom_participant), - _client_namespace(client_namespace) + _client_namespace(client_namespace), _synchronize_timestamps(synchronize_timestamps) { if (transport == Transport::Serial) { @@ -299,12 +306,17 @@ void UxrceddsClient::run() _connected = true; // Set time-callback. - uxr_set_time_callback(&session, on_time, &_timesync); + if (_synchronize_timestamps) { + uxr_set_time_callback(&session, on_time, &_timesync); + + } else { + uxr_set_time_callback(&session, on_time_no_sync, nullptr); + } // Synchronize with the Agent bool synchronized = false; - while (!synchronized) { + while (_synchronize_timestamps && !synchronized) { synchronized = uxr_sync_session(&session, 1000); if (synchronized) { @@ -323,15 +335,37 @@ void UxrceddsClient::run() bool had_ping_reply = false; uint32_t last_num_payload_sent{}; uint32_t last_num_payload_received{}; + int poll_error_counter = 0; + + _subs->init(); while (!should_exit() && _connected) { + /* Wait for topic updates for max 1000 ms (1sec) */ + int poll = px4_poll(&_subs->fds[0], (sizeof(_subs->fds) / sizeof(_subs->fds[0])), 1000); + + /* Handle the poll results */ + if (poll == 0) { + /* Timeout, no updates in selected uorbs */ + continue; + + } else if (poll < 0) { + /* Error */ + if (poll_error_counter < 10 || poll_error_counter % 50 == 0) { + /* Prevent flooding */ + PX4_ERR("ERROR while polling uorbs: %d", poll); + } + + poll_error_counter++; + continue; + } + _subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace); uxr_run_session_timeout(&session, 0); // time sync session - if (hrt_elapsed_time(&last_sync_session) > 1_s) { + if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) { if (uxr_sync_session(&session, 100)) { //PX4_INFO("synchronized with time offset %-5" PRId64 "ns", session.time_offset); last_sync_session = hrt_absolute_time(); @@ -376,7 +410,6 @@ void UxrceddsClient::run() _connected = false; } - px4_usleep(1000); } uxr_delete_session_retries(&session, _connected ? 1 : 0); @@ -694,8 +727,17 @@ UxrceddsClient *UxrceddsClient::instantiate(int argc, char *argv[]) } } + // determines if timestamps should be synchronized + int32_t synchronize_timestamps = 0; + param_get(param_find("UXRCE_DDS_SYNCT"), &synchronize_timestamps); + + if ((synchronize_timestamps != 1) && (synchronize_timestamps != 0)) { + PX4_ERR("UXRCE_DDS_SYNCT must be either 0 or 1"); + } + + return new UxrceddsClient(transport, device, baudrate, agent_ip, port, localhost_only, custom_participant, - client_namespace); + client_namespace, synchronize_timestamps); } int UxrceddsClient::print_usage(const char *reason) diff --git a/src/modules/uxrce_dds_client/uxrce_dds_client.h b/src/modules/uxrce_dds_client/uxrce_dds_client.h index aab381b73d..f757040214 100644 --- a/src/modules/uxrce_dds_client/uxrce_dds_client.h +++ b/src/modules/uxrce_dds_client/uxrce_dds_client.h @@ -49,7 +49,7 @@ public: }; UxrceddsClient(Transport transport, const char *device, int baudrate, const char *host, const char *port, - bool localhost_only, bool custom_participant, const char *client_namespace); + bool localhost_only, bool custom_participant, const char *client_namespace, bool synchornize_timestamps); ~UxrceddsClient(); @@ -77,6 +77,7 @@ private: const bool _localhost_only; const bool _custom_participant; const char *_client_namespace; + const bool _synchronize_timestamps; // max port characters (5+'\0') diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 3be203dd59..c06d7e888e 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -43,12 +43,10 @@ #include "vtol_att_control_main.h" using namespace matrix; -using namespace time_literals; -#define FRONTTRANS_THR_MIN 0.25f -#define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 1.0f -#define BACKTRANS_THROTTLE_UPRAMP_DUR_S 1.0f; -#define BACKTRANS_MOTORS_UPTILT_DUR_S 1.0f; +#define FRONTTRANS_THR_MIN 0.25f +#define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 0.5f +#define BACKTRANS_THROTTLE_UPRAMP_DUR_S 0.5f Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : VtolType(attc) @@ -183,44 +181,8 @@ void Tiltrotor::update_mc_state() { VtolType::update_mc_state(); - /*Motor spin up: define the first second after arming as motor spin up time, during which - * the tilt is set to the value of VT_TILT_SPINUP. This allows the user to set a spin up - * tilt angle in case the propellers don't spin up smoothly in full upright (MC mode) position. - */ - - const int spin_up_duration_p1 = 1000_ms; // duration of 1st phase of spinup (at fixed tilt) - const int spin_up_duration_p2 = 700_ms; // duration of 2nd phase of spinup (transition from spinup tilt to mc tilt) - - // reset this timestamp while disarmed - if (!_v_control_mode->flag_armed) { - _last_timestamp_disarmed = hrt_absolute_time(); - _tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0 - - } else if (_tilt_motors_for_startup) { - // leave motors tilted forward after arming to allow them to spin up easier - if (hrt_absolute_time() - _last_timestamp_disarmed > (spin_up_duration_p1 + spin_up_duration_p2)) { - _tilt_motors_for_startup = false; - } - } - - if (_tilt_motors_for_startup) { - if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) { - _tilt_control = _param_vt_tilt_spinup.get(); - - } else { - // duration phase 2: begin to adapt tilt to multicopter tilt - float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get()); - _tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() - - (_last_timestamp_disarmed + spin_up_duration_p1)); - } - - _mc_yaw_weight = 0.0f; //disable yaw control during spinup - - } else { - // normal operation - _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); - _mc_yaw_weight = 1.0f; - } + _tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get(); + _mc_yaw_weight = 1.0f; } void Tiltrotor::update_fw_state() @@ -321,7 +283,8 @@ void Tiltrotor::update_transition_state() // tilt rotors back once motors are idle if (_time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) { - float progress = (_time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S; + float progress = (_time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / math::max(_param_vt_bt_tilt_dur.get(), + 0.1f); progress = math::constrain(progress, 0.0f, 1.0f); _tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress); } @@ -458,7 +421,7 @@ void Tiltrotor::blendThrottleDuringBacktransition(float scale, float target_thro float Tiltrotor::timeUntilMotorsAreUp() { - return BACKTRANS_THROTTLE_DOWNRAMP_DUR_S + BACKTRANS_MOTORS_UPTILT_DUR_S; + return BACKTRANS_THROTTLE_DOWNRAMP_DUR_S + _param_vt_bt_tilt_dur.get(); } float Tiltrotor::moveLinear(float start, float stop, float progress) diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 369e335684..cac172c2ae 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -91,15 +91,13 @@ private: void blendThrottleDuringBacktransition(const float scale, const float target_throttle); bool isFrontTransitionCompletedBase() override; - hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */ - bool _tilt_motors_for_startup{false}; DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType, (ParamFloat) _param_vt_tilt_mc, (ParamFloat) _param_vt_tilt_trans, (ParamFloat) _param_vt_tilt_fw, - (ParamFloat) _param_vt_tilt_spinup, - (ParamFloat) _param_vt_trans_p2_dur + (ParamFloat) _param_vt_trans_p2_dur, + (ParamFloat) _param_vt_bt_tilt_dur ) }; diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 796d214a6b..92fa558454 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -71,20 +71,6 @@ PARAM_DEFINE_FLOAT(VT_TILT_TRANS, 0.4f); */ PARAM_DEFINE_FLOAT(VT_TILT_FW, 1.0f); -/** - * Tilt when disarmed and in the first second after arming - * - * This specific tilt during spin-up is necessary for some systems whose motors otherwise don't - * spin-up freely. - * - * @min 0.0 - * @max 1.0 - * @increment 0.01 - * @decimal 2 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); - /** * Duration of front transition phase 2 * @@ -98,3 +84,17 @@ PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f); + +/** + * Duration motor tilt up in backtransition + * + * Time in seconds it takes to tilt form VT_TILT_FW to VT_TILT_MC. + * + * @unit s + * @min 0.1 + * @max 10 + * @increment 0.1 + * @decimal 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_BT_TILT_DUR, 1.f); diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 1361f1f5e6..3bcb56e498 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-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 @@ -79,7 +79,6 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f); /** * Maximum duration of a back transition * - * Time in seconds used for a back transition maximally. * Transition is also declared over if the groundspeed drops below MPC_XY_CRUISE. * * @unit s @@ -94,10 +93,6 @@ PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 10.0f); /** * Target throttle value for the transition to fixed-wing flight. * - * standard vtol: pusher - * - * tailsitter, tiltrotor: main throttle - * * @min 0.0 * @max 1.0 * @increment 0.01 @@ -109,7 +104,6 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_THR, 1.0f); /** * Approximate deceleration during back transition * - * The approximate deceleration during a back transition in m/s/s * Used to calculate back transition distance in an auto mode. * For standard vtol and tiltrotors a controller is used to track this value during the transition. * @@ -216,9 +210,9 @@ PARAM_DEFINE_FLOAT(VT_QC_ALT_LOSS, 0.0f); /** * Quad-chute transition altitude loss threshold * - * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight. + * Altitude loss threshold for quad-chute triggering during VTOL transition to fixed-wing flight + * in altitude-controlled flight modes. * Active until 5s after completing transition to fixed-wing. - * Only active if altitude estimate is valid and in altitude-controlled mode. * If the current altitude is more than this value below the altitude at the beginning of the * transition, it will instantly switch back to MC mode and execute behavior defined in COM_QC_ACT. * @@ -344,23 +338,9 @@ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f); */ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f); -/** - * Backtransition deceleration setpoint to pitch feedforward gain. - * - * - * @unit rad s^2/m - * @min 0 - * @max 0.2 - * @decimal 2 - * @increment 0.01 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.f); - /** * Backtransition deceleration setpoint to pitch I gain. * - * * @unit rad s/m * @min 0 * @max 0.3 @@ -373,9 +353,8 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_I, 0.1f); /** * Minimum pitch angle during hover. * - * Minimum pitch angle during hover flight. If the desired pitch angle is is lower than this value - * then the fixed-wing forward actuation can be used to compensate for the missing thrust in forward direction - * (see VT_FW_TRHUST_EN) + * Any pitch setpoint below this value is translated to a forward force by the fixed-wing forward actuation if + * VT_FW_TRHUST_EN is set to 1. * * @unit deg * @min -10.0 @@ -390,8 +369,7 @@ PARAM_DEFINE_FLOAT(VT_PITCH_MIN, -5.0f); * Minimum pitch angle during hover landing. * * Overrides VT_PITCH_MIN when the vehicle is in LAND mode (hovering). - * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings - * generating too much lift and preventing the vehicle from sinking at the desired rate. + * During landing it can be beneficial to reduce the pitch angle to reduce the generated lift in head wind. * * @unit deg * @min -10.0 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 92c3672f0a..0a558b0ec6 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -176,7 +176,7 @@ float VtolType::update_and_get_backtransition_pitch_sp() // get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number) const float accel_error_forward = dec_sp + accel_body_forward; - const float pitch_sp_new = _param_vt_b_dec_ff.get() * dec_sp + _accel_to_pitch_integ; + const float pitch_sp_new = _accel_to_pitch_integ; float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward; diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index d8f3de8620..84dfbe6d00 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -335,7 +335,6 @@ protected: (ParamFloat) _param_vt_fw_difthr_s_y, (ParamFloat) _param_vt_fw_difthr_s_p, (ParamFloat) _param_vt_fw_difthr_s_r, - (ParamFloat) _param_vt_b_dec_ff, (ParamFloat) _param_vt_b_dec_i, (ParamFloat) _param_vt_b_dec_mss, diff --git a/src/systemcmds/gpio/gpio.cpp b/src/systemcmds/gpio/gpio.cpp index 343c1bbd18..20db2a96d0 100644 --- a/src/systemcmds/gpio/gpio.cpp +++ b/src/systemcmds/gpio/gpio.cpp @@ -314,8 +314,8 @@ $ gpio read H4 PULLUP Set the output value on Port E pin 7 to high $ gpio write E7 1 --force -Set the output value on device /dev/gpin1 to high -$ gpio write /dev/gpin1 1 +Set the output value on device /dev/gpio1 to high +$ gpio write /dev/gpio1 1 )DESCR_STR"); diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index 2f7842f126..f1f355514c 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -509,7 +509,7 @@ do_import(const char *param_file_name) static int do_save_default() { - return param_save_default(); + return param_save_default(true); } static int diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index ab948cf8a2..72d99c6708 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -36,7 +36,7 @@ set(srcs test_bezierQuad.cpp test_bitset.cpp test_bson.cpp - test_dataman.c + test_dataman.cpp test_file.c test_file2.c test_float.cpp @@ -93,6 +93,7 @@ px4_add_module( SRCS ${srcs} DEPENDS + dataman_client version ) diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c deleted file mode 100644 index e553323060..0000000000 --- a/src/systemcmds/tests/test_dataman.c +++ /dev/null @@ -1,220 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file test_dataman.c - * Tests for the data manager. - */ - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "tests_main.h" - -#include "dataman/dataman.h" - -static px4_sem_t *sems; -static bool *task_returned_error; -int test_dataman(int argc, char *argv[]); - -#define NUM_MISSIONS_TEST 50 - -#define DM_MAX_DATA_SIZE sizeof(struct mission_s) - -static int -task_main(int argc, char *argv[]) -{ - char buffer[DM_MAX_DATA_SIZE]; - - PX4_INFO("Starting dataman test task %s", argv[2]); - /* try to read an invalid item */ - int my_id = atoi(argv[2]); - - /* try to read an invalid item */ - if (dm_read(DM_KEY_NUM_KEYS, 0, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid item failed", my_id); - goto fail; - } - - /* try to read an invalid index */ - if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) { - PX4_ERR("%d read an invalid index failed", my_id); - goto fail; - } - - srand(hrt_absolute_time() ^ my_id); - unsigned hit = 0; - unsigned miss = 0; - hrt_abstime wstart = hrt_absolute_time(); - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - memset(buffer, my_id, sizeof(buffer)); - buffer[1] = i; - unsigned hash = i ^ my_id; - unsigned len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - int ret = dm_write(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, len); - //PX4_INFO("ret: %d", ret); - - if (ret != len) { - PX4_WARN("task %d: write failed ret=%d, index: %d, length: %d", my_id, ret, hash, len); - goto fail; - } - - if (i % (NUM_MISSIONS_TEST / 10) == 0) { - PX4_INFO("task %d: %.0f%%", my_id, (double)i * 100.0f / NUM_MISSIONS_TEST); - } - - px4_usleep(rand() & ((64 * 1024) - 1)); - } - - hrt_abstime rstart = hrt_absolute_time(); - hrt_abstime wend = rstart; - - for (unsigned i = 0; i < NUM_MISSIONS_TEST; i++) { - unsigned hash = i ^ my_id; - ssize_t len2 = dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, hash, buffer, sizeof(buffer)); - ssize_t len = (hash % (DM_MAX_DATA_SIZE / 2)) + 2; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, ret=%zd, len=%zd", my_id, hash, len2, len); - goto fail; - } - - if (buffer[0] == my_id) { - hit++; - - if (len2 != len) { - PX4_WARN("task %d: read failed length test, index %d, wanted %zd, got %zd", my_id, hash, len, len2); - goto fail; - } - - if (buffer[1] != i) { - PX4_WARN("task %d: data verification failed, index %d, wanted %d, got %d", my_id, hash, my_id, buffer[1]); - goto fail; - } - - } else { - miss++; - } - } - - hrt_abstime rend = hrt_absolute_time(); - PX4_INFO("task %d pass, hit %d, miss %d, io time read %" PRIu64 "ms. write %" PRIu64 "ms.", - my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_TEST / 1000, (wend - wstart) / NUM_MISSIONS_TEST / 1000); - px4_sem_post(sems + my_id); - return 0; - -fail: - PX4_ERR("test_dataman FAILED: task %d, buffer %02x %02x %02x %02x %02x %02x", - my_id, buffer[0], buffer[1], buffer[2], buffer[3], buffer[4], buffer[5]); - px4_sem_post(sems + my_id); - task_returned_error[my_id] = true; - return -1; -} - -int test_dataman(int argc, char *argv[]) -{ - int i = 0; - unsigned num_tasks = 4; - char buffer[DM_MAX_DATA_SIZE]; - - if (argc > 1) { - num_tasks = atoi(argv[1]); - } - - sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t)); - task_returned_error = (bool *)calloc(num_tasks, sizeof(bool)); - PX4_INFO("Running %d tasks", num_tasks); - - for (i = 0; i < num_tasks; i++) { - int task; - - char a[16]; - snprintf(a, 16, "%d", i); - - char *av[] = {"tests_dataman", a, NULL}; - - px4_sem_init(sems + i, 1, 0); - /* sems use case is a signal */ - px4_sem_setprotocol(sems + i, SEM_PRIO_NONE); - - /* start the task */ - if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, task_main, av)) <= 0) { - PX4_ERR("task start failed"); - } - } - - for (i = 0; i < num_tasks; i++) { - px4_sem_wait(sems + i); - px4_sem_destroy(sems + i); - } - - free(sems); - - bool got_error = false; - - for (i = 0; i < num_tasks; i++) { - if (task_returned_error[i]) { - got_error = true; - break; - } - } - - free(task_returned_error); - - if (got_error) { - return -1; - } - - for (i = 0; i < NUM_MISSIONS_TEST; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) { - break; - } - } - - return 0; -} diff --git a/src/systemcmds/tests/test_dataman.cpp b/src/systemcmds/tests/test_dataman.cpp new file mode 100644 index 0000000000..dbd1690088 --- /dev/null +++ b/src/systemcmds/tests/test_dataman.cpp @@ -0,0 +1,1149 @@ +/**************************************************************************** + * + * Copyright (C) 2023 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file test_dataman.cpp + * Tests for Dataman. + */ + +#include + +#include +#include +#include +#include +#include + +#include "dataman_client/DatamanClient.hpp" + +class DatamanTest : public UnitTest +{ +public: + DatamanTest(); + virtual bool run_tests(); + +private: + + enum class State { + Write, + WriteWait, + Read, + ReadWait, + Clear, + ClearWait, + OperationCompleted, + CompareBuffers, + Exit + }; + + //Sync + bool testSyncReadInvalidItem(); + bool testSyncWriteInvalidItem(); + bool testSyncReadInvalidIndex(); + bool testSyncWriteInvalidIndex(); + bool testSyncReadBufferOverflow(); + bool testSyncWriteBufferOverflow(); + bool testSyncMutipleClients(); + bool testSyncWriteReadAllItemsMaxSize(); + bool testSyncClearAll(); + + //Async + bool testAsyncReadInvalidItem(); + bool testAsyncWriteInvalidItem(); + bool testAsyncReadInvalidIndex(); + bool testAsyncWriteInvalidIndex(); + bool testAsyncReadBufferOverflow(); + bool testAsyncWriteBufferOverflow(); + bool testAsyncMutipleClients(); + bool testAsyncWriteReadAllItemsMaxSize(); + bool testAsyncClearAll(); + + //Cache + bool testCache(); + + //This will reset the items but it will not restore the compact key. + bool testResetItems(); + + DatamanClient _dataman_client1{}; + DatamanClient _dataman_client2{}; + DatamanClient _dataman_client3{}; + DatamanClient _dataman_client_thread1{}; + DatamanClient _dataman_client_thread2{}; + DatamanClient _dataman_client_thread3{}; + + DatamanCache _dataman_cache{"test_dm_cache_miss", 10}; + + static void *testAsyncThread(void *arg); + + static constexpr uint32_t DM_MAX_DATA_SIZE{MISSION_ITEM_SIZE}; + static_assert(sizeof(dataman_response_s::data) == DM_MAX_DATA_SIZE, "data size != DM_MAX_DATA_SIZE"); + + uint8_t _buffer_read[DM_MAX_DATA_SIZE]; + uint8_t _buffer_write[DM_MAX_DATA_SIZE]; + + bool _response_success{false}; + + px4::atomic_int _thread_index{0}; + px4::atomic_bool _thread_tests_success{false}; + + uint16_t _max_index[DM_KEY_NUM_KEYS] {}; + + static constexpr uint32_t OVERFLOW_LENGTH = sizeof(_buffer_write) + 1; +}; + +DatamanTest::DatamanTest() +{ + for (uint32_t i = 0; i < DM_KEY_NUM_KEYS; ++i) { + _max_index[i] = g_per_item_max_index[i]; + } + +#ifndef __PX4_NUTTX + _max_index[DM_KEY_WAYPOINTS_OFFBOARD_0] = 200; + _max_index[DM_KEY_WAYPOINTS_OFFBOARD_1] = 200; +#endif + +} + +bool +DatamanTest::testSyncReadInvalidItem() +{ + + bool success = _dataman_client1.readSync(DM_KEY_NUM_KEYS, 0, _buffer_read, 2); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidItem() +{ + bool success = _dataman_client1.writeSync(DM_KEY_NUM_KEYS, 0, _buffer_write, 2); + return !success; +} + + +bool +DatamanTest::testSyncReadInvalidIndex() +{ + bool success = _dataman_client1.readSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + return !success; +} + +bool +DatamanTest::testSyncWriteInvalidIndex() +{ + bool success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); + return !success; +} + +bool +DatamanTest::testSyncReadBufferOverflow() +{ + bool success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_read, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testSyncWriteBufferOverflow() +{ + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, _buffer_write, OVERFLOW_LENGTH); + return !success; +} + +bool +DatamanTest::testSyncMutipleClients() +{ + // Prepare write buffer + for (uint32_t i = 0; i < DM_MAX_DATA_SIZE; ++i) { + _buffer_write[i] = (uint8_t)i; + } + + bool success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x11, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client2.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client3.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x22, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + success = _dataman_client1.readSync(DM_KEY_WAYPOINTS_OFFBOARD_0, 0x33, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + return false; + } + + //Compare content from buffers + for (uint32_t i = 0; i < g_per_item_size[DM_KEY_MISSION_STATE]; ++i) { + if (_buffer_read[i] != _buffer_write[i]) { + return false; + } + } + + return success; +} + +bool +DatamanTest::testSyncWriteReadAllItemsMaxSize() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + // writeSync + for (uint32_t index = 0U; index < _max_index[item]; ++index) { + + // Prepare write buffer + for (uint32_t i = 0; i < g_per_item_size[item]; ++i) { + _buffer_write[i] = (uint8_t)(index % UINT8_MAX); + } + + success = _dataman_client1.writeSync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]); + + if (!success) { + PX4_ERR("writeSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + } + + // readSync + for (uint32_t index = 0U; index < _max_index[item]; ++index) { + + success = _dataman_client1.readSync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]); + + if (!success) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32, item, index); + return false; + } + + // Check read buffer + for (uint32_t i = 0U; i < g_per_item_size[item]; ++i) { + + uint8_t expected_value = (index % UINT8_MAX); + + if (expected_value != _buffer_read[i]) { + PX4_ERR("readSync failed at item = %" PRIu32 ", index = %" PRIu32 ", element= %" PRIu32 ", expected: %" PRIu8 + ", received: %" PRIu8, item, index, i, expected_value, _buffer_read[i]); + return false; + } + } + } + } + + return true; +} + +bool +DatamanTest::testSyncClearAll() +{ + bool success = false; + + // Iterate all items + for (uint32_t item = DM_KEY_SAFE_POINTS; item < DM_KEY_NUM_KEYS; ++item) { + + success = _dataman_client1.clearSync((dm_item_t)item); + + if (!success) { + PX4_ERR("clearSync failed at item = %" PRIu32, item); + return false; + } + } + + return success; +} + +bool +DatamanTest::testAsyncReadInvalidItem() +{ + bool success = true; + + State state = State::Read; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Read: + + state = State::ReadWait; + success = _dataman_client1.readAsync(DM_KEY_NUM_KEYS, 0, _buffer_read, 2); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncWriteInvalidItem() +{ + bool success = true; + + State state = State::Write; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = _dataman_client1.writeAsync(DM_KEY_NUM_KEYS, 0, _buffer_write, 2); + + if (!success) { + return false; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncReadInvalidIndex() +{ + bool success = true; + + State state = State::Read; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Read: + + state = State::ReadWait; + success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, 2); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncWriteInvalidIndex() +{ + bool success = true; + + State state = State::Write; + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, 2); + + if (!success) { + return false; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + //Test ends here + return true; + } + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 1_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return false; +} + +bool +DatamanTest::testAsyncReadBufferOverflow() +{ + bool success = _dataman_client1.readAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_read, OVERFLOW_LENGTH); + + return !success; +} + +bool +DatamanTest::testAsyncWriteBufferOverflow() +{ + bool success = _dataman_client1.writeAsync(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, _buffer_write, OVERFLOW_LENGTH); + + return !success; +} + +bool +DatamanTest::testAsyncMutipleClients() +{ + pthread_t thread1{}; + pthread_t thread2{}; + pthread_t thread3{}; + + _thread_tests_success.store(true); + _thread_index.store(0); + + // Test multiple dataman clients + uint32_t ret = pthread_create(&thread1, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + ret = pthread_create(&thread2, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + ret = pthread_create(&thread3, nullptr, &testAsyncThread, this); + + if (ret != 0) { + printf("pthread_create failed: %" PRIu32 "\n", ret); + _thread_tests_success.store(false); + } + + pthread_join(thread1, nullptr); + pthread_join(thread2, nullptr); + pthread_join(thread3, nullptr); + + return _thread_tests_success.load(); +} + +void *DatamanTest::testAsyncThread(void *arg) +{ + DatamanTest *dataman_test = (DatamanTest *)arg; + const uint32_t index = dataman_test->_thread_index.fetch_add(1); + State state = State::Write; + + hrt_abstime start_time = hrt_absolute_time(); + + uint8_t buffer_read[DM_MAX_DATA_SIZE] = {}; + uint8_t buffer_write[DM_MAX_DATA_SIZE] = {}; + + bool success; + bool response_success; + + // Prepare write buffer + for (uint8_t i = 0; i < g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0]; ++i) { + buffer_write[i] = i * index; + } + + DatamanClient *dataman_client{nullptr}; + + if (index == 0) { + dataman_client = &(dataman_test->_dataman_client_thread1); + + } else if (index == 1) { + dataman_client = &(dataman_test->_dataman_client_thread2); + + } else if (index == 2) { + dataman_client = &(dataman_test->_dataman_client_thread3); + + } else { + PX4_ERR("Unknown thread %" PRIu32 "!", index); + return nullptr; + } + + while (state != State::Exit) { + + dataman_client->update(); + + switch (state) { + + case State::Write: + + state = State::WriteWait; + success = dataman_client->writeAsync(DM_KEY_WAYPOINTS_OFFBOARD_0, index, buffer_write, sizeof(buffer_write)); + + if (!success) { + PX4_ERR("writeAsync failed for index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + break; + + case State::WriteWait: + + if (dataman_client->lastOperationCompleted(response_success)) { + state = State::Read; + + if (!response_success) { + PX4_ERR("writeAsync failed to get success operation complete for the index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + } + + break; + + case State::Read: + + state = State::ReadWait; + success = dataman_client->readAsync(DM_KEY_WAYPOINTS_OFFBOARD_0, index, buffer_read, sizeof(buffer_read)); + + if (!success) { + PX4_ERR("readAsync failed for index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + break; + + case State::ReadWait: + if (dataman_client->lastOperationCompleted(response_success)) { + state = State::CompareBuffers; + + if (!response_success) { + PX4_ERR("readAsync failed to get success operation complete for the index %" PRIu32 "!", index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + } + + break; + + case State::CompareBuffers: + + for (uint32_t i = 0; i < g_per_item_size[DM_KEY_WAYPOINTS_OFFBOARD_0]; ++i) { + if (buffer_write[i] != buffer_read[i]) { + PX4_ERR("buffer are not the same for index %" PRIu32 "!", index); + dataman_test->_thread_tests_success.store(false); + break; + } + } + + state = State::Exit; + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout! index=%" PRIu32, index); + state = State::Exit; + dataman_test->_thread_tests_success.store(false); + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + PX4_INFO("Thread %" PRIu32 " finished!", index); + px4_usleep(200_ms); + + return nullptr; +} + +bool +DatamanTest::testAsyncWriteReadAllItemsMaxSize() +{ + bool success = false; + State state = State::Write; + + uint32_t item = DM_KEY_SAFE_POINTS; + uint32_t index = 0U; + + hrt_abstime start_time = hrt_absolute_time(); + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + switch (state) { + + case State::Write: + + state = State::WriteWait; + + // Prepare write buffer + for (uint32_t i = 0; i < g_per_item_size[item]; ++i) { + _buffer_write[i] = (uint8_t)(index % UINT8_MAX); + } + + success = _dataman_client1.writeAsync((dm_item_t)item, index, _buffer_write, g_per_item_size[item]); + + if (!success) { + return false; + } + + break; + + case State::Read: + state = State::ReadWait; + success = _dataman_client1.readAsync((dm_item_t)item, index, _buffer_read, g_per_item_size[item]); + + if (!success) { + return false; + } + + break; + + case State::ReadWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + if (!_response_success) { + return false; + } + + state = State::CompareBuffers; + } + + break; + + case State::WriteWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + if (!_response_success) { + return false; + } + + state = State::Read; + } + + break; + + case State::CompareBuffers: + state = State::Write; + + for (uint32_t i = 0U; i < g_per_item_size[item]; ++i) { + + if (_buffer_write[i] != _buffer_read[i]) { + PX4_ERR("readAsync failed at item = %" PRIu32 ", index = %" PRIu32 ", element= %" PRIu32 ", expected: %" PRIu8 + ", received: %" PRIu8, item, index, i, _buffer_write[i], _buffer_read[i]); + return false; + } + } + + if (index < _max_index[item] - 1) { + ++index; + + } else { + + if (item < DM_KEY_NUM_KEYS - 1) { + index = 0U; + ++item; + + } else { + state = State::Exit; + } + } + + break; + + default: + break; + } + + if (hrt_elapsed_time(&start_time) > 20_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + + } + + return success; +} + +bool +DatamanTest::testAsyncClearAll() +{ + bool success = true; + + State state = State::Clear; + hrt_abstime start_time = hrt_absolute_time(); + uint32_t item = DM_KEY_SAFE_POINTS; + + //While loop represents a task + while (state != State::Exit) { + + _dataman_client1.update(); + + switch (state) { + + case State::Clear: + + state = State::ClearWait; + success = _dataman_client1.clearAsync((dm_item_t)item); + + if (!success) { + PX4_ERR("Failed at item %" PRIu32, item); + return false; + } + + break; + + case State::ClearWait: + if (_dataman_client1.lastOperationCompleted(_response_success)) { + state = State::OperationCompleted; + + if (!_response_success) { + PX4_ERR("Failed at item %" PRIu32, item); + return false; + } + } + + break; + + case State::OperationCompleted: + if (item < DM_KEY_NUM_KEYS - 1) { + state = State::Clear; + ++item; + + } else { + state = State::Exit; + } + + break; + + default: + break; + + } + + if (hrt_elapsed_time(&start_time) > 5_s) { + PX4_ERR("Test timeout!"); + return false; + } + + //Simulate rescheduling the task after a 1 ms delay to allow time for the dataman task to operate. + px4_usleep(1_ms); + } + + return true; +} + +//Cache +bool +DatamanTest::testCache() +{ + bool success = false; + dm_item_t item = DM_KEY_WAYPOINTS_OFFBOARD_0; + uint32_t uniq_number = 13; // Use this to make sure stored data is from this test + + for (uint32_t index = 0; index < 15; ++index) { + uint8_t value = index + uniq_number; + memset(_buffer_write, value, sizeof(_buffer_write)); + success = _dataman_cache.client().writeSync(item, index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + } + + // Write one extra for loadWait with timeout + uint32_t extra_index = 100; + _buffer_write[0] = 123; + success = _dataman_cache.client().writeSync(item, extra_index, _buffer_write, sizeof(_buffer_write)); + + if (!success) { + return false; + } + + // Load cache + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + if (!_dataman_cache.load(item, index)) { + return false; + } + } + + hrt_abstime start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (_dataman_cache.isLoading()) { + + px4_usleep(1_ms); + _dataman_cache.update(); + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout!"); + return false; + } + } + + // check cached data + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // expected to fail without timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded"); + return false; + } + + // expected to success with timeout set + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read), 100_ms); + + if (!success) { + PX4_ERR("loadWait failed"); + return false; + } + + // expected to success without timeout set (item is now cached) + success = _dataman_cache.loadWait(item, extra_index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("loadWait failed"); + return false; + } + + uint32_t old_cache_size = _dataman_cache.size(); + _dataman_cache.resize(5); + + // check cached data after resize (reduced, the first item got overwritten by extra_index) + for (uint32_t index = 1; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + for (uint32_t index = _dataman_cache.size(); index < old_cache_size; ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + } + + _dataman_cache.invalidate(); + _dataman_cache.resize(15); + + // Load cache + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + _dataman_cache.load(item, index); + } + + start_time = hrt_absolute_time(); + + // loop represents the task, we collect the data + while (_dataman_cache.isLoading()) { + + px4_usleep(1_ms); + _dataman_cache.update(); + + if (hrt_elapsed_time(&start_time) > 2_s) { + PX4_ERR("Test timeout!"); + return false; + } + } + + // check cached data + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + if (!success) { + PX4_ERR("Failed loadWait at index %" PRIu32, index); + return false; + } + + for (uint32_t i = 0; i < sizeof(_buffer_read); ++i) { + if (_buffer_read[i] != value) { + PX4_ERR("Wrong data recived %" PRIu8" , expected %" PRIu8, _buffer_read[i], value); + return false; + } + } + } + + // invalidate and check cached data + _dataman_cache.invalidate(); + + for (uint32_t index = 0; index < _dataman_cache.size(); ++index) { + uint8_t value = index + uniq_number; + success = _dataman_cache.loadWait(item, index, _buffer_read, sizeof(_buffer_read)); + + // expected to fail + if (success) { + PX4_ERR("loadWait unexpectedly succeeded at index %" PRIu32, index); + return false; + } + + } + + return true; +} + +bool +DatamanTest::testResetItems() +{ + bool success = false; + + mission_s mission{}; + mission.timestamp = hrt_absolute_time(); + mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0; + mission.count = 0; + mission.current_seq = 0; + + success = _dataman_client1.writeSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_MISSION_STATE"); + return false; + } + + success = _dataman_client1.readSync(DM_KEY_MISSION_STATE, 0, reinterpret_cast(&mission), sizeof(mission_s)); + + if (!success) { + PX4_ERR("failed to read DM_KEY_MISSION_STATE"); + return false; + } + + mission_stats_entry_s stats; + stats.num_items = 0; + stats.update_counter = 1; + + success = _dataman_client1.writeSync(DM_KEY_FENCE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_FENCE_POINTS"); + return false; + } + + success = _dataman_client1.writeSync(DM_KEY_SAFE_POINTS, 0, reinterpret_cast(&stats), + sizeof(mission_stats_entry_s)); + + if (!success) { + PX4_ERR("failed to reset DM_KEY_SAFE_POINTS"); + return false; + } + + return success; +} + +bool DatamanTest::run_tests() +{ + ut_run_test(testSyncReadInvalidItem); + ut_run_test(testSyncWriteInvalidItem); + ut_run_test(testSyncReadInvalidIndex); + ut_run_test(testSyncWriteInvalidIndex); + ut_run_test(testSyncReadBufferOverflow); + ut_run_test(testSyncWriteBufferOverflow); + ut_run_test(testSyncMutipleClients); + ut_run_test(testSyncWriteReadAllItemsMaxSize); + ut_run_test(testSyncClearAll); + + ut_run_test(testAsyncReadInvalidItem); + ut_run_test(testAsyncWriteInvalidItem); + ut_run_test(testAsyncReadInvalidIndex); + ut_run_test(testAsyncWriteInvalidIndex); + ut_run_test(testAsyncReadBufferOverflow); + ut_run_test(testAsyncWriteBufferOverflow); + ut_run_test(testAsyncMutipleClients); + ut_run_test(testAsyncWriteReadAllItemsMaxSize); + ut_run_test(testAsyncClearAll); + + ut_run_test(testCache); + + ut_run_test(testResetItems); + + return (_tests_failed == 0); +} + +ut_declare_test_c(test_dataman, DatamanTest) diff --git a/src/systemcmds/tests/test_parameters.cpp b/src/systemcmds/tests/test_parameters.cpp index 1d15eae785..77b45ce55e 100644 --- a/src/systemcmds/tests/test_parameters.cpp +++ b/src/systemcmds/tests/test_parameters.cpp @@ -316,7 +316,7 @@ bool ParameterTest::exportImport() } // save - if (param_save_default() != PX4_OK) { + if (param_save_default(true) != PX4_OK) { PX4_ERR("param_save_default failed"); return false; } @@ -461,7 +461,7 @@ bool ParameterTest::exportImportAll() } // save - if (param_save_default() != PX4_OK) { + if (param_save_default(true) != PX4_OK) { PX4_ERR("param_save_default failed"); return false; } @@ -561,7 +561,7 @@ bool ParameterTest::exportImportAll() } // save - if (param_save_default() != PX4_OK) { + if (param_save_default(true) != PX4_OK) { PX4_ERR("param_save_default failed"); return false; } diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index 8a895c157a..af25fd074e 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -81,7 +81,7 @@ const struct { {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, - {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, + {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, {"file2", test_file2, OPT_NOJIGTEST}, {"float", test_float, 0}, {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt index 90b27f943a..3b4be475e3 100644 --- a/test/mavsdk_tests/CMakeLists.txt +++ b/test/mavsdk_tests/CMakeLists.txt @@ -17,6 +17,7 @@ if(MAVSDK_FOUND) test_main.cpp autopilot_tester.cpp autopilot_tester_failure.cpp + autopilot_tester_rtl.cpp # follow-me needs a MAVSDK update: # https://github.com/mavlink/MAVSDK/pull/1770 # autopilot_tester_follow_me.cpp diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp index c58dd28bf6..5811d796d4 100644 --- a/test/mavsdk_tests/autopilot_tester.cpp +++ b/test/mavsdk_tests/autopilot_tester.cpp @@ -268,7 +268,7 @@ void AutopilotTester::execute_mission_and_lose_gps() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorGps, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -280,7 +280,7 @@ void AutopilotTester::execute_mission_and_lose_mag() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -296,7 +296,7 @@ void AutopilotTester::execute_mission_and_lose_baro() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Off, 0) == Failure::Result::Success); @@ -312,7 +312,7 @@ void AutopilotTester::execute_mission_and_get_baro_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorBaro, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -328,7 +328,7 @@ void AutopilotTester::execute_mission_and_get_mag_stuck() { CHECK(_param->set_param_int("SYS_FAILURE_EN", 1) == Param::Result::Success); - start_and_wait_for_first_mission_item(); + start_and_wait_for_mission_sequence(1); CHECK(_failure->inject(Failure::FailureUnit::SensorMag, Failure::FailureType::Stuck, 0) == Failure::Result::Success); @@ -484,6 +484,65 @@ void AutopilotTester::fly_forward_in_altctl() } } } +void AutopilotTester::fly_forward_in_offboard_attitude() +{ + // This test does not depend on valid position estimate. + // Wait for raw gps & stable attitude estimate + CHECK(poll_condition_with_timeout( + [this]() { + auto attitude = _telemetry->attitude_euler(); + return _telemetry->raw_gps().altitude_ellipsoid_m > 0.f && fabsf(attitude.roll_deg) < 5.f + && fabsf(attitude.pitch_deg) < 5.f; + }, std::chrono::seconds(20))); + + const float start_altitude_ellipsoid_m = _telemetry->raw_gps().altitude_ellipsoid_m; + + Offboard::Attitude attitude{}; + _offboard->set_attitude(attitude); + REQUIRE(_offboard->start() == Offboard::Result::Success); + + // Wait until we can arm + CHECK(poll_condition_with_timeout( + [this]() { return _telemetry->health().is_armable; }, std::chrono::seconds(20))); + arm(); + + const unsigned offboard_rate_hz = 50; + + // Climb + const float climb_altitude_m = 10.f; + attitude.thrust_value = 0.8f; + + while (_telemetry->raw_gps().altitude_ellipsoid_m - start_altitude_ellipsoid_m < climb_altitude_m) { + CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz)); + } + + // Fly forward for 3s + attitude.thrust_value = 0.8f; + attitude.pitch_deg = -20.f; + + for (unsigned i = 0; i < 3 * offboard_rate_hz; ++i) { + CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz)); + } + + // Check attitude + auto attitude_estimate = _telemetry->attitude_euler(); + CHECK(fabsf(attitude.roll_deg - attitude_estimate.roll_deg) < 5.f); + CHECK(fabsf(attitude.pitch_deg - attitude_estimate.pitch_deg) < 5.f); + + // Descend + attitude.thrust_value = 0.4f; + attitude.pitch_deg = 0.f; + + for (unsigned i = 0; i < 6 * offboard_rate_hz; ++i) { + CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success); + sleep_for(std::chrono::milliseconds(1000 / offboard_rate_hz)); + } + + attitude.thrust_value = 0.0f; + CHECK(_offboard->set_attitude(attitude) == Offboard::Result::Success); +} void AutopilotTester::start_checking_altitude(const float max_deviation_m) { @@ -501,6 +560,49 @@ void AutopilotTester::stop_checking_altitude() _telemetry->subscribe_position(nullptr); } +void AutopilotTester::check_tracks_mission_raw(float corridor_radius_m, bool reverse) +{ + auto mission_raw = _mission_raw->download_mission(); + CHECK(mission_raw.first == MissionRaw::Result::Success); + + auto mission_items = mission_raw.second; + auto ct = get_coordinate_transformation(); + + _telemetry->set_rate_position_velocity_ned(5); + _telemetry->subscribe_position_velocity_ned([ct, mission_items, corridor_radius_m, reverse, + this](Telemetry::PositionVelocityNed position_velocity_ned) { + auto progress = _mission_raw->mission_progress(); + + + std::function(std::vector, unsigned, mavsdk::geometry::CoordinateTransformation)> + get_waypoint_for_sequence = [](std::vector mission_items, int sequence, auto ct) { + for (auto waypoint : mission_items) { + + if (waypoint.seq == (uint32_t)sequence) { + return get_local_mission_item_from_raw_item(waypoint, ct); + } + } + + return std::array({0.0f, 0.0f, 0.0f}); + }; + + if (progress.current > 0 && progress.current < progress.total) { + // Get shortest distance of current position to 3D line between previous and next waypoint + + std::array current { position_velocity_ned.position.north_m, + position_velocity_ned.position.east_m, + position_velocity_ned.position.down_m }; + std::array wp_prev = get_waypoint_for_sequence(mission_items, + reverse ? progress.current + 1 : progress.current - 1, ct); + std::array wp_next = get_waypoint_for_sequence(mission_items, progress.current, ct); + + float distance_to_trajectory = point_to_line_distance(current, wp_prev, wp_next); + + CHECK(distance_to_trajectory < corridor_radius_m); + } + }); +} + void AutopilotTester::check_tracks_mission(float corridor_radius_m) { auto mission = _mission->download_mission(); @@ -535,6 +637,12 @@ void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m); } +void AutopilotTester::execute_rtl_when_reaching_mission_sequence(int sequence_number) +{ + start_and_wait_for_mission_sequence_raw(sequence_number); + execute_rtl(); +} + std::array AutopilotTester::get_current_position_ned() { mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned(); @@ -642,15 +750,15 @@ bool AutopilotTester::ground_truth_horizontal_position_far_from(const Telemetry: return pass; } -void AutopilotTester::start_and_wait_for_first_mission_item() +void AutopilotTester::start_and_wait_for_mission_sequence(int sequence_number) { auto prom = std::promise {}; auto fut = prom.get_future(); - _mission->subscribe_mission_progress([&prom, this](Mission::MissionProgress progress) { + _mission->subscribe_mission_progress([&prom, this, sequence_number](Mission::MissionProgress progress) { std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; - if (progress.current >= 1) { + if (progress.current >= sequence_number) { _mission->subscribe_mission_progress(nullptr); prom.set_value(); } @@ -661,6 +769,25 @@ void AutopilotTester::start_and_wait_for_first_mission_item() REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); } +void AutopilotTester::start_and_wait_for_mission_sequence_raw(int sequence_number) +{ + auto prom = std::promise {}; + auto fut = prom.get_future(); + + _mission_raw->subscribe_mission_progress([&prom, this, sequence_number](MissionRaw::MissionProgress progress) { + std::cout << time_str() << "Progress: " << progress.current << "/" << progress.total << std::endl; + + if (progress.current >= sequence_number) { + _mission_raw->subscribe_mission_progress(nullptr); + prom.set_value(); + } + }); + + REQUIRE(_mission_raw->start_mission() == MissionRaw::Result::Success); + + REQUIRE(fut.wait_for(std::chrono::seconds(60)) == std::future_status::ready); +} + void AutopilotTester::wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout) { auto prom = std::promise {}; diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h index bb4b826c0e..22a9225628 100644 --- a/test/mavsdk_tests/autopilot_tester.h +++ b/test/mavsdk_tests/autopilot_tester.h @@ -134,12 +134,15 @@ public: void offboard_land(); void fly_forward_in_posctl(); void fly_forward_in_altctl(); + void fly_forward_in_offboard_attitude(); void request_ground_truth(); void check_mission_item_speed_above(int item_index, float min_speed_m_s); void check_tracks_mission(float corridor_radius_m = 1.5f); + void check_tracks_mission_raw(float corridor_radius_m = 1.f, bool reverse = false); void start_checking_altitude(const float max_deviation_m); void stop_checking_altitude(); void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f); + void execute_rtl_when_reaching_mission_sequence(int sequence_number); // Blocking call to get the drone's current position in NED frame std::array get_current_position_ned(); @@ -199,7 +202,8 @@ private: bool ground_truth_horizontal_position_far_from(const Telemetry::GroundTruth &target_pos, float min_distance_m); bool estimated_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m); bool estimated_horizontal_position_close_to(const Offboard::PositionNedYaw &target_pos, float acceptance_radius_m); - void start_and_wait_for_first_mission_item(); + void start_and_wait_for_mission_sequence(int sequence_number); + void start_and_wait_for_mission_sequence_raw(int sequence_number); void wait_for_flight_mode(Telemetry::FlightMode flight_mode, std::chrono::seconds timeout); void wait_for_landed_state(Telemetry::LandedState landed_state, std::chrono::seconds timeout); void wait_for_mission_finished(std::chrono::seconds timeout); diff --git a/test/mavsdk_tests/autopilot_tester_rtl.cpp b/test/mavsdk_tests/autopilot_tester_rtl.cpp new file mode 100644 index 0000000000..b3996a76c4 --- /dev/null +++ b/test/mavsdk_tests/autopilot_tester_rtl.cpp @@ -0,0 +1,57 @@ + +/**************************************************************************** + * + * 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 "autopilot_tester_rtl.h" + +#include "math_helpers.h" +#include +#include +#include +#include + + +void AutopilotTesterRtl::connect(const std::string uri) +{ + AutopilotTester::connect(uri); +} + +void AutopilotTesterRtl::set_rtl_type(int rtl_type) +{ + CHECK(getParams()->set_param_int("RTL_TYPE", rtl_type) == Param::Result::Success); +} + +void AutopilotTesterRtl::set_takeoff_land_requirements(int req) +{ + CHECK(getParams()->set_param_int("MIS_TKO_LAND_REQ", req) == Param::Result::Success); +} diff --git a/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009_registers.hpp b/test/mavsdk_tests/autopilot_tester_rtl.h similarity index 68% rename from platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009_registers.hpp rename to test/mavsdk_tests/autopilot_tester_rtl.h index 1e99a0ea03..6c59d64462 100644 --- a/platforms/nuttx/src/px4/common/gpio/mcp23009/mcp23009_registers.hpp +++ b/test/mavsdk_tests/autopilot_tester_rtl.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 @@ -33,36 +33,23 @@ #pragma once -#include +#include "autopilot_tester.h" -// TODO: move to a central header -static constexpr uint8_t Bit0 = (1 << 0); -static constexpr uint8_t Bit1 = (1 << 1); -static constexpr uint8_t Bit2 = (1 << 2); -static constexpr uint8_t Bit3 = (1 << 3); -static constexpr uint8_t Bit4 = (1 << 4); -static constexpr uint8_t Bit5 = (1 << 5); -static constexpr uint8_t Bit6 = (1 << 6); -static constexpr uint8_t Bit7 = (1 << 7); +#include +#include -namespace Microchip_MCP23009 + +class AutopilotTesterRtl : public AutopilotTester { +public: + AutopilotTesterRtl() = default; + ~AutopilotTesterRtl() = default; -enum class -Register : uint8_t { + void set_rtl_type(int rtl_type); + void set_takeoff_land_requirements(int req); + void connect(const std::string uri); - IODIR = 0x00, - IPOL = 0x01, - GPINTEN = 0x02, - DEFVAL = 0x03, - INTCON = 0x04, - IOCON = 0x05, - GPPU = 0x06, - INTF = 0x07, - INTCAP = 0x08, - GPIO = 0x09, - OLAT = 0x0a +private: + std::unique_ptr _failure{}; }; - -} // namespace Microchip_MCP23009 diff --git a/test/mavsdk_tests/configs/sitl.json b/test/mavsdk_tests/configs/sitl.json index daff0aefc6..791f3d6766 100644 --- a/test/mavsdk_tests/configs/sitl.json +++ b/test/mavsdk_tests/configs/sitl.json @@ -7,9 +7,18 @@ { "model": "iris", "vehicle": "iris", - "test_filter": "[multicopter],[offboard]", + "test_filter": "[multicopter],[offboard][offboard_attitude]", "timeout_min": 10 }, + { + "model": "iris", + "vehicle": "iris", + "test_filter": "[offboard_attitude]", + "timeout_min": 10, + "env": { + "SYS_MC_EST_GROUP": 3 + } + }, { "model": "standard_vtol", "vehicle": "standard_vtol", diff --git a/test/mavsdk_tests/math_helpers.h b/test/mavsdk_tests/math_helpers.h index 34817381ef..3f26d3aa18 100644 --- a/test/mavsdk_tests/math_helpers.h +++ b/test/mavsdk_tests/math_helpers.h @@ -44,6 +44,20 @@ std::array get_local_mission_item(const Mission::MissionItem &item, const return {static_cast(local.north_m), static_cast(local.east_m), -item.relative_altitude_m}; } +template +std::array get_local_mission_item_from_raw_item(const mavsdk::MissionRaw::MissionItem &item, + const CoordinateTransformation &ct) +{ + using GlobalCoordinate = mavsdk::geometry::CoordinateTransformation::GlobalCoordinate; + GlobalCoordinate global; + global.latitude_deg = item.x / 1e7; + global.longitude_deg = item.y / 1e7; + + + auto local = ct.local_from_global(global); + return {static_cast(local.north_m), static_cast(local.east_m), -item.z}; +} + template T sq(T x) { diff --git a/test/mavsdk_tests/mavsdk_test_runner.py b/test/mavsdk_tests/mavsdk_test_runner.py index 47ccd62d58..5b4ce39b5e 100755 --- a/test/mavsdk_tests/mavsdk_test_runner.py +++ b/test/mavsdk_tests/mavsdk_test_runner.py @@ -444,6 +444,8 @@ class Tester: self.debugger, self.verbose, self.build_dir) + for env_key in test.get('env', []): + px4_runner.env[env_key] = str(test['env'][env_key]) self.active_runners.append(px4_runner) mavsdk_tests_runner = ph.TestRunner( diff --git a/test/mavsdk_tests/test_multicopter_offboard.cpp b/test/mavsdk_tests/test_multicopter_offboard.cpp index b8984d4207..2a166e3612 100644 --- a/test/mavsdk_tests/test_multicopter_offboard.cpp +++ b/test/mavsdk_tests/test_multicopter_offboard.cpp @@ -73,3 +73,12 @@ TEST_CASE("Offboard position control", "[multicopter][offboard]") tester.wait_until_disarmed(std::chrono::seconds(120)); tester.check_home_within(1.0f); } + +TEST_CASE("Offboard attitude control", "[multicopter][offboard_attitude]") +{ + AutopilotTester tester; + tester.connect(connection_url); + tester.set_rc_loss_exception(AutopilotTester::RcLossException::Offboard); + tester.fly_forward_in_offboard_attitude(); + tester.wait_until_disarmed(std::chrono::seconds(120)); +} diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp index 5417254fce..e12f583c2c 100644 --- a/test/mavsdk_tests/test_vtol_mission.cpp +++ b/test/mavsdk_tests/test_vtol_mission.cpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include "autopilot_tester.h" +#include "autopilot_tester_rtl.h" TEST_CASE("Fly VTOL mission", "[vtol]") @@ -44,3 +44,47 @@ TEST_CASE("Fly VTOL mission", "[vtol]") tester.execute_mission_raw(); tester.wait_until_disarmed(); } + +TEST_CASE("RTL direct Home", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.store_home(); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan"); + // fly directly to home position + tester.set_rtl_type(0); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.wait_until_disarmed(std::chrono::seconds(90)); + tester.check_home_within(5.0f); +} + +TEST_CASE("RTL with Mission Landing", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan"); + // Vehicle should follow the mission and use the mission landing + tester.set_rtl_type(2); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(2); + tester.check_tracks_mission_raw(35.0f); + tester.wait_until_disarmed(std::chrono::seconds(95)); +} + +TEST_CASE("RTL with Reverse Mission", "[vtol]") +{ + AutopilotTesterRtl tester; + tester.connect(connection_url); + tester.wait_until_ready(); + tester.set_takeoff_land_requirements(0); + tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_without_landing.plan"); + // vehicle should follow the mission in reverse and land at the home position // TODO enable checks again if MAVSDK can handle mission in reverse order + tester.set_rtl_type(2); + tester.arm(); + tester.execute_rtl_when_reaching_mission_sequence(6); + //tester.check_tracks_mission_raw(35.0f); + tester.wait_until_disarmed(std::chrono::seconds(90)); +} diff --git a/test/mavsdk_tests/vtol_mission_without_landing.plan b/test/mavsdk_tests/vtol_mission_without_landing.plan new file mode 100644 index 0000000000..e52e3f6fa4 --- /dev/null +++ b/test/mavsdk_tests/vtol_mission_without_landing.plan @@ -0,0 +1,184 @@ +{ + "fileType": "Plan", + "geoFence": { + "circles": [ + ], + "polygons": [ + ], + "version": 2 + }, + "groundStation": "QGroundControl", + "mission": { + "cruiseSpeed": 15, + "firmwareType": 12, + "globalPlanAltitudeMode": 1, + "hoverSpeed": 5, + "items": [ + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 84, + "doJumpId": 1, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39833113265167, + 8.545508725338607, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 2, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.399332700068925, + 8.54481499384454, + 20 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 3, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39908888031702, + 8.54344001880591, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 4, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39760160279815, + 8.542394178137585, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 5, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396941861414504, + 8.54282818797708, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 6, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.396686401111786, + 8.544419333554089, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 30, + "Altitude": 30, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 7, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.397202447861446, + 8.545440338322464, + 30 + ], + "type": "SimpleItem" + }, + { + "AMSLAltAboveTerrain": 20, + "Altitude": 20, + "AltitudeMode": 1, + "autoContinue": true, + "command": 16, + "doJumpId": 8, + "frame": 3, + "params": [ + 0, + 0, + 0, + null, + 47.39766309343905, + 8.545713820298545, + 20 + ], + "type": "SimpleItem" + } + ], + "plannedHomePosition": [ + 47.39775218584113, + 8.545620889782981, + 489.0021493051957 + ], + "vehicleType": 20, + "version": 2 + }, + "rallyPoints": { + "points": [ + ], + "version": 2 + }, + "version": 1 +}